#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