include AFMotor Библиотека для работы Motor Sheeld volatile int imp_le

  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
#include <AFMotor.h> //Библиотека для работы с Motor Sheeld
volatile int imp_left = 0; //Кол-во импульсов на левом энкодере
volatile int imp_rigth = 0; //Кол-во импульсов на правом энкодере
unsigned int Tpwm = 100; //Период ШИМа
unsigned int Tleft = 0; //Период рабочего цикла для левого колеса
unsigned int Trigth = 0; //Период рабочего цикла для правого колеса
unsigned long time = 0;
//Для управлением двигателями используются выводы D4, D6, D11, D12
//D4, D12 - управляют драйверами двигателей
//D6, D11 - формируют ШИМ
AF_DCMotor motor_left(1); //Объект для левого двигателя на канале М1
AF_DCMotor motor_rigth(3); //Объект для правого двигателя на канале М2
int direction_rotation_left = RISING; //Направление вращения левого двигателя
int direction_rotation_rigth = RISING; //Направление вращения правого двигателя
// | Напр. движения | D7, D9 | D8,D10 |
// | Вперед | HIGH | LOW |
// | Назад | LOW | HIGH |
// | Остановка | LOW | LOW |
// | Недопустимо!!! | HIGH | HIGH |
int left_motor_forward = 7; //Для вращения левого мотора вперед на D7 HIGH
int left_motor_backward = 8; //Для вращения левого мотора назад на D8 HIGH
int rigth_motor_forward = 9; //Для вращения правого мотора вперед на D9 HIGH
int rigth_motor_backward = 10; //Для вращения правого мотора назад на D10 HIGH
char data[20]; //Полученные данные
int buf = 0; //Буферная переменная
int i = 0;
//0 - задание ошибки рассогласования
//1 - установка направления движения
//2 - задание периода ШИМа;
int oper; //Номер команды
double eps = 0; //Ошибка рассоглосования
double VT = 0; //Скорость продольного перемещения
byte r = 40; //Передаточное число редуктора
double K = 11; //Коэффициент передачи усилителя
double omega_nom = 251.327; //Номинальная частота вращения двигателя
double R = 0.15; //Радиус колес в метрах
byte U_nom = 110; //Номинальное напряжение
double U0 = 0; //Определяет связь сигналов управления со скоростью продольного перемещения
double U1 = 0; //Определяет разность напряжений, необходимую для компенсации бокового отклонения
double U_left, U_rigth = 0; //Процент управляющего напряжения для левого и правого двигателей от номинального
volatile int puls_per_turn = 580; //Кол-во импульсов за оборот
char* pStr; //Указатель на строку
void setup(){
Serial.begin(9600); //Открытие последовательного порта настройка скорости на 9600 бит/с
//Левый и правый энкодеры подключаем к D2, D3 соответственно
attachInterrupt(0, inc_left_encoder, RISING); //Настройка прерывания для левого энкодера на переход с LOW на HIGH
attachInterrupt(1, inc_rigth_encoder, RISING); //Настройка прерывания для правого энкодера на переход с LOW на HIGH
//Управление аппаратным ШИМом в текущем случае не возможно, т.к. период стандартного ШИМа составляет порядка 490 Hz
//при оптимальной 10 Hz. Т.о. ШИМ реализован на программном уровне
motor_left.setSpeed(255); //Устанавливаем скорость левого двигателя на максимум
motor_rigth.setSpeed(255); //Устанавливаем скорость правого двигателя на максимум
pinMode(left_motor_forward, OUTPUT); // <--¬
pinMode(left_motor_backward, OUTPUT); // ¦ Устанавливает режим работы выход
pinMode(rigth_motor_forward, OUTPUT); // ¦
pinMode(rigth_motor_backward, OUTPUT); // <---
} //end setup
void loop(){
if (time <= millis() - Tpwm){
time = millis();
send_speeds();
} //end if
if (millis() >= (time + Tpwm - Tleft)){ // <--¬
run_left_motor(); // ¦
} //end if // ¦ Формируем ШИМ на левом двигателе
else { // ¦
stop_left_motor(); // ¦
} //end else // <---
if (millis() >= (time + Tpwm - Trigth)){ // <--¬
run_rigth_motor(); // ¦
} //end if // ¦ Формируем ШИМ на правом двигателе
else { // ¦
stop_rigth_motor(); // ¦
} //end else // <---
if (Serial.available() > 0){ //Если имеются входные данные
oper = Serial.read(); //Считываем номер операции
oper = oper - 48;
i = 0;
memset(data,0,20); //Очищаем буферную строку
while (Serial.available() > 0){ //Считываем парамметры
buf = Serial.read();
if (buf != 13){ //Конец строки
data[i] = buf; //Формируем строку парамметров
i++;
} // end if
else {
break;
} //end else
} //end while
switch (oper){ //Выбор операции
case 0: //Считываем ошибку рассогласования и скорость продольного движения
eps = atof(data); //Считываем ошибку рассогласования
pStr = strchr(data,' '); //Находим указатель на подстроку содержащую скорость продольного движения
VT = atof(pStr); //Считываем скорость продольного движения
break;
case 1: //Считываем направление вращения для левого и правого двигателей
direction_rotation_left = data[1]-48;
direction_rotation_rigth = data[2]-48;
switch (direction_rotation_left){ //Выбор напрвления вращения для левого двигателя
case 1: //Вращение вперед
digitalWrite(left_motor_forward, HIGH);
digitalWrite(left_motor_backward, LOW);
break;
case 2: //Вращение назад
digitalWrite(left_motor_forward, LOW);
digitalWrite(left_motor_backward, HIGH);
break;
default: //Остановка
digitalWrite(left_motor_forward, LOW);
digitalWrite(left_motor_backward, LOW);
} //end switch
switch (direction_rotation_rigth){ //Выбор напрвления вращения для правого двигателя
case 1: //Вращение вперед
digitalWrite(rigth_motor_forward, HIGH);
digitalWrite(rigth_motor_backward, LOW);
break;
case 2: //Вращение назад
digitalWrite(rigth_motor_forward, LOW);
digitalWrite(rigth_motor_backward, HIGH);
break;
default: //Остановка
digitalWrite(rigth_motor_forward, LOW);
digitalWrite(rigth_motor_backward, LOW);
} //end switch
break;
case 3:
Tpwm = atof(data); //Считываем период ШИМа
break;
default: //Отправка сообщения об ошибке
Serial.print("Unknow command:");
Serial.println(oper, DEC);
} //end switch
if ((oper == 1) || (oper == 3)){
U0 = VT*r/R/omega_nom*100;
U1 = eps*K/U_nom/2*100;
U_left = U0 + U1;
if (U_left > 100){
U_left = 100;
} //end if
U_rigth = U0 - U1;
if (U_rigth > 100){
U_rigth = 100;
} //end if
Tleft = round(U_left*Tpwm/100);
Trigth = round(U_rigth*Tpwm/100);
} //end if
} //end if
} //end loop
void inc_left_encoder(){ //Подсчет импульсов с левого энкодера
imp_left++;
} //end inc_left_encoder
void inc_rigth_encoder(){ //Подсчет импульсов с правого энкодера
imp_rigth++;
} //end inc_rigth_encoder
void run_left_motor(){ //Включение левого двигателя
motor_left.run(FORWARD);
} //end run_left_motor
void stop_left_motor(){ //Выключение левого двигателя
motor_left.run(RELEASE);
} //end stop_left_motor
void run_rigth_motor(){ //Включение правого двигателя
motor_rigth.run(FORWARD);
} //end run_rigth_motor
void stop_rigth_motor(){ //Выключение правого двигателя
motor_rigth.run(RELEASE);
} //end stop_rigth_motor
void send_speeds(){
//Скорость колеса (рад/с) равна:
//(кол-во импульсов подсчитанных за период*1000)/(количество импульсов за оборот*период)*2*Pi
double speed_left = (imp_left*1000)/(puls_per_turn*Tpwm)*2*PI; //Скорость левого колеса
double speed_rigth = (imp_rigth*1000)/(puls_per_turn*Tpwm)*2*PI; //Скорость правого колеса
Serial.print("L: ");
Serial.println(speed_left, 4); //Отправка скорости левого колеса
Serial.print("R: ");
Serial.println(speed_rigth, 4); //Отправка скорости правого колеса
} //end send_speeds