мега 2 шаговых двигателя и 4 датчика
- Войдите на сайт для отправки комментариев
Здравствуйте ищу исполнителя по доработке кода, код в приложении обеспечивает управление движением двумя шаговыми двигателями по четырем датчикам (первый датчик начало движения первого двигателя с максимальной скоростью на заданное число шагов, потом проверяем второй датчик если ещё не сработал то продолжаем движение с уменьшенной скоростью пока не сработает второй датчик, датчик сработал останавливаемся) Второй двигатель управляется аналогично по третьему четвертому датчику. Есть ещё один пятый датчик который при получении сообщения брак подает высокое напряжение на один из пинов. Код работает однако имеются наводки, т.к. плата расположена в силовом шкафу вместе с драйверами двигателей, при включении двигателей появляются ложные срабатывания датчиков, аппаратно решить проблему наводок нет возможности, поэтому хочу решить проблему програмно.
Задача: прописать в имеющемся коде минимальный интервал срабатывания датчика, который мы будем считать реальной сменой сигнала, все короткие импульсы от наводок мы считаем помехами и игнорируем, если сигнал датчика сменился и держится 100..200мс то да мы считаем что датчик реально сработал и начинаем движение. Продолжительность интервала нужно будет настраивать вручную при пусконаладке (по умолчанию 100мс на все датчики).
001 | #define DEBUG //enable debug messages |
002 |
003 | #include "AccelStepper.h" // #include <AccelStepper.h> |
004 |
005 | AccelStepper stepper1(AccelStepper::FULL2WIRE, 2,3); |
006 | AccelStepper 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 |
022 | uint8_t sensor_pin[6] = {0, 14,43,32,33,42}; // pin numbers, sensor_pin[0] not used !!! |
023 | uint8_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 |
030 | uint8_t sensor_old[6]; // previous value, see setup |
031 | uint8_t sensor_new[6]; // current value |
032 |
033 | // SENSORS ANTI-BOUNCE TIME IN MILLISEC !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! |
034 | uint32_t sensor_bounce[6] = {0, 90,90,90,90,90}; |
035 | uint32_t sensor_start_time[6] = {0}; |
036 |
037 | //*************************** |
038 | float SM1_speed; |
039 | float SM2_speed; |
040 |
041 | uint8_t moving1 = 0; |
042 | uint8_t moving2 = 0; |
043 |
044 | //*************************** |
045 | char inp_buf[8]; // serial input buffer for "brak" |
046 | uint8_t indx = 0; |
047 |
048 | //*************************** |
049 | uint8_t timer_flag[6] = {0,0,0,0,0,0}; |
050 | uint32_t timer_start[6]; |
051 | uint32_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 | //************************************************** |
064 | void 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 | //************************************************** |
083 | bool 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 | //************************************************** |
102 | void 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 | //************************************************** |
116 | bool 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 | //**************************************************************************** |
132 | void 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 | //**************************************************************************** |
156 | void 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 | } |
Задача: прописать в имеющемся коде минимальный интервал срабатывания датчика, который мы будем считать реальной сменой сигнала, все короткие импульсы от наводок мы считаем помехами и игнорируем, если сигнал датчика сменился и держится 100..200мс то да мы считаем что датчик реально сработал и начинаем движение. Продолжительность интервала нужно будет настраивать вручную при пусконаладке (по умолчанию 100мс на все датчики).
если я правильно понял, ваши датчики представляют из себя обычные цифровые пины, то есть фактически кнопки. А то, что вы описываете - называется антидребезг. Пример кода антидребезга можно найти в любой приличной библиотеке работы с кнопками.
Более того, в вашем коде он уже прописан - смотрите процедуру sensor() в районе 80-х строк. Время реакции пина. как вы его называете - в вашем коде 90мс для всех сенсоров.
Так что я не очень понимаю, что именно вы хотите от исполнителя. Или задача - в подборе конкретного дебонсинг-интервала? - вряд ли это можно сделать удаленно.