мега 2 шаговых двигателя и 4 датчика

svorog
Offline
Зарегистрирован: 27.04.2017

Здравствуйте ищу исполнителя по доработке кода, код в приложении обеспечивает управление движением двумя шаговыми двигателями по четырем датчикам (первый датчик начало движения первого двигателя с максимальной скоростью на заданное число шагов, потом проверяем второй датчик если ещё не сработал то продолжаем движение с уменьшенной скоростью пока не сработает второй датчик, датчик сработал останавливаемся) Второй двигатель управляется аналогично по третьему четвертому датчику. Есть ещё один пятый датчик который при получении сообщения брак подает высокое напряжение на один из пинов. Код работает однако имеются наводки, т.к. плата расположена в силовом шкафу вместе с драйверами двигателей, при включении двигателей появляются ложные срабатывания датчиков, аппаратно решить проблему наводок нет возможности, поэтому хочу решить проблему програмно.

Задача: прописать в имеющемся коде минимальный интервал срабатывания датчика, который мы будем считать реальной сменой сигнала, все короткие импульсы от наводок мы считаем помехами и игнорируем,  если сигнал датчика  сменился и держится 100..200мс то да мы считаем что датчик реально сработал и начинаем движение. Продолжительность интервала нужно будет настраивать вручную при пусконаладке (по умолчанию 100мс на все датчики).

sistema164@mail.ru

001#define DEBUG //enable debug messages
002 
003#include "AccelStepper.h" // #include <AccelStepper.h>
004 
005AccelStepper stepper1(AccelStepper::FULL2WIRE, 2,3);
006AccelStepper stepper2(AccelStepper::FULL2WIRE, 4,5);
007 
008//***************************
009//***************************
010// ANALOG INPUTS
011#define R1_DELAY_PIN A2
012#define R1_SPEED_PIN A1
013#define R2_DELAY_PIN A4
014#define R2_SPEED_PIN A3
015 
016//***************************
017// output pin to relay
018#define OUT_BRAK_PIN 46
019 
020//***************************
021// SENSORS PINS AND HIGH/LOW TRIGGER LEVEL
022uint8_t sensor_pin[6] = {0, 14,43,32,33,42};    // pin numbers, sensor_pin[0] not used !!!
023uint8_t sensor_level[6] = {0, 0,0,0,0,0};       // working level: 0 - LOW TRIGGER LEVEL, 1 - HIGH
024// sensor #1 - ШД_1
025// sensor #2 - ШД_1
026// sensor #3 - ШД_2
027// sensor #4 - ШД_2
028// sensor #5 - "+1"
029 
030uint8_t sensor_old[6];      // previous value, see setup
031uint8_t sensor_new[6];      // current value
032 
033// SENSORS ANTI-BOUNCE TIME IN MILLISEC !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
034uint32_t sensor_bounce[6] = {0, 90,90,90,90,90};
035uint32_t sensor_start_time[6] = {0};
036 
037//***************************
038float SM1_speed;
039float SM2_speed;
040 
041uint8_t moving1 = 0;
042uint8_t moving2 = 0;
043 
044//***************************
045char inp_buf[8]; // serial input buffer for "brak"
046uint8_t indx = 0;
047 
048//***************************
049uint8_t timer_flag[6] = {0,0,0,0,0,0};
050uint32_t timer_start[6];
051uint32_t timer_delay[6];
052// timer #1 - пауза для первого ШД (регулируется потенциометром)
053// timer #2 - пауза для второго ШД (регулируется потенциометром)
054// timer #3 - при получении сообщения «brak» выжидаем паузу 20мс
055// timer #4 - 500мс на пин который будет управлять реле
056 
057//***************************
058#ifdef DEBUG
059 char data[80];
060#endif
061 
062//**************************************************
063//**************************************************
064void test_serial_for_BRAK() // read serial one byte
065{
066    char inp_byte = Serial.read();
067        if (inp_byte=='\n') { // it's the end of line
068        inp_buf[indx] = '\0'; // for C-string
069        indx = 0;
070        if (strncmp(inp_buf, "brak", 4)==0) timer_begin( 3, 20 );
071        return;
072    };
073 
074    if (inp_byte>=' ') { // buffer consists only letters
075        inp_buf[indx] = inp_byte;
076        indx++;
077        indx &= 0x07; // 8 bytes buffer
078        return;
079    };
080};
081 
082//**************************************************
083bool sensor(uint8_t i)
084{
085    if ( millis() - sensor_start_time[i] < sensor_bounce[i] ) return false; // sensor bounce
086     
087    sensor_new[i] = digitalRead(sensor_pin[i]);
088    if ( sensor_new[i] == sensor_old[i] ) return false;
089     
090    sensor_old[i] = sensor_new[i];  //input level changed
091    sensor_start_time[i] = millis();
092 
093#ifdef DEBUG
094 sprintf(data, "sensor_%d %s", i, (sensor_new[i]==sensor_level[i])?("ON"):("OFF"));
095 Serial.println(data);
096#endif
097 
098    return ( sensor_new[i] == sensor_level[i] );
099};
100 
101//**************************************************
102void timer_begin(uint8_t i, uint32_t tm)
103{
104 
105#ifdef DEBUG
106 sprintf(data, "timer_%u BEGIN %lums", i, tm);
107 Serial.println(data);
108#endif
109 
110    timer_flag[i] = 1; // timer is active now
111    timer_start[i] = millis();
112    timer_delay[i] = tm;
113};
114 
115//**************************************************
116bool check_timer(uint8_t i) // test timer for the end
117{
118    if ( timer_flag[i] == 0 ) return false;
119    if ( (millis() - timer_start[i]) < timer_delay[i] ) return false;
120    timer_flag[i] = 0;
121 
122#ifdef DEBUG
123 sprintf(data, "timer_%u END", i);
124 Serial.println(data);
125#endif
126 
127    return true; // end of timer[i]
128};
129 
130//****************************************************************************
131//****************************************************************************
132void setup()
133{
134    for (uint8_t i = 1; i <= 5; i++) {
135        pinMode(sensor_pin[i], INPUT_PULLUP); // !!! PULLUP !!!
136        sensor_old[i] = (1-sensor_level[i]);
137    };
138 
139    pinMode(OUT_BRAK_PIN, OUTPUT);
140 
141    Serial.begin(9600);
142 
143    stepper1.setMaxSpeed(3000.0);
144    stepper1.setAcceleration(10000.0);
145 
146    stepper2.setMaxSpeed(5000.0);
147    stepper2.setAcceleration(10000.0);
148 
149#ifdef DEBUG
150 Serial.println("PROGRAM STARTING...\n");
151#endif
152 
153}
154 
155//****************************************************************************
156void loop()
157{
158//******************************* stepper1
159    if ( sensor(1) ) {
160        timer_begin( 1, analogRead(R1_DELAY_PIN) );
161    };
162 
163    if ( check_timer(1) ) {
164        stepper1.setCurrentPosition(0);
165        SM1_speed = (float)map(analogRead(R1_SPEED_PIN), 0, 1023, 3000, 150);
166        stepper1.setMaxSpeed(SM1_speed);
167        stepper1.move(1000);
168        moving1 = 1;
169    };
170 
171    if ( moving1 == 1 && stepper1.distanceToGo() == 0 ) {
172        stepper1.setMaxSpeed(SM1_speed/2.0);
173        stepper1.move(1000000);
174 
175#ifdef DEBUG
176 Serial.println("stepper_1 half speed");
177#endif
178 
179    };
180 
181    if ( sensor(2) ) {
182        stepper1.stop();
183        moving1 = 0;
184    };
185 
186//******************************* stepper2
187    if ( sensor(3) ) {
188        timer_begin( 2, analogRead(R2_DELAY_PIN) );
189    };
190 
191    if ( check_timer(2) ) {
192        stepper2.setCurrentPosition(0);
193        SM2_speed = (float)map(analogRead(R2_SPEED_PIN), 0, 1023, 5000, 150);
194        stepper2.setMaxSpeed(SM2_speed);
195        stepper2.move(1000);
196        moving2 = 1;
197    };
198 
199    if ( moving2 == 1 && stepper2.distanceToGo() == 0 ) {
200        stepper2.setMaxSpeed(SM2_speed/2.0);
201        stepper2.move(1000000);
202 
203#ifdef DEBUG
204 Serial.println("stepper_2 half speed");
205#endif
206 
207    };
208 
209    if ( sensor(4) ) {
210        stepper2.stop();
211        moving2 = 0;
212    };
213 
214//******************************* send "+1"
215    if ( sensor(5) ) {
216        Serial.println("+1");
217    };
218 
219//******************************* catch "brak" with timer_begin( 3, 20 );
220    if ( Serial.available() ) test_serial_for_BRAK();
221 
222//******************************* brak 500ms pulse
223    if ( check_timer(3) ) {
224        digitalWrite(OUT_BRAK_PIN, HIGH);
225        timer_begin( 4, 500 );
226    };
227    if ( check_timer(4) ) {
228        digitalWrite(OUT_BRAK_PIN, LOW);
229    };
230 
231//******************************* RUN-RUN-RUN... STEPPERS
232    stepper1.run();
233    stepper2.run();
234}

 

b707
Offline
Зарегистрирован: 26.05.2017

svorog пишет:

Задача: прописать в имеющемся коде минимальный интервал срабатывания датчика, который мы будем считать реальной сменой сигнала, все короткие импульсы от наводок мы считаем помехами и игнорируем,  если сигнал датчика  сменился и держится 100..200мс то да мы считаем что датчик реально сработал и начинаем движение. Продолжительность интервала нужно будет настраивать вручную при пусконаладке (по умолчанию 100мс на все датчики).

если я правильно понял, ваши датчики представляют из себя обычные цифровые пины, то есть фактически кнопки. А то, что вы описываете - называется антидребезг. Пример кода антидребезга можно найти в любой приличной библиотеке работы с кнопками.

Более того, в вашем коде он уже прописан - смотрите процедуру sensor() в районе 80-х строк. Время реакции пина. как вы его называете - в вашем коде 90мс для всех сенсоров.

Так что я не очень понимаю, что именно вы хотите от исполнителя. Или задача - в подборе конкретного дебонсинг-интервала? - вряд ли это можно сделать удаленно.