В своём предыдущем проекте пульта, для управления использовались два двухосевых джойстика. В этом пульте было решено использовать один двухосевой джойстик и инерциальный датчик, что даёт возможность реализовать управление движением машинкой, как от джойстика, так и с помощью инерциального датчика, просто вращая контроллер.
Обычно для беспроводного канала управления я использую модуль NRF24, а в этот раз хочу показать, как аналогичное можно реализовать с помощью модуля прозрачного радиомоста HC-12.
Вы узнаете, как получать и обрабатывать данные с инерциального датчика, настраивать радиомодуль, передавать данные и управлять коллекторными моторами или устройствами с ШИМ управлением (сервоприводы, контроллеры оборотов и т.д.).
Список деталей:
2 х Arduino NANO
2 х Радиомодуль HC-12 или подобный (HC-05, HC-06 и т.д.)
1 х Модуль инерциального датчика MPU6050 (трёх-осевые гироскоп и акселерометр)
1 х Модуль двухканального драйвера мотора
1 х Двухосевой джойстик
2 х Крона (алкалайновая батарея или NiMh аккумулятор)
2 х Разъем для кроны
1 х Выключатель
1 х Макетная плата
1 х Шасси с двумя моторами
Это пример реализации, который не сложно адаптировать под другие детали, которые больше нравятся или есть в наличии. Возможно вам захочется вместо Arduino NANO, использовать Mini, Micro, Uno и т.д. Вместо MX1508 взять DRV8833, L293 или ещё какой. Крону можно заменить на несколько AA-аккумуляторов или одну-две Li-Ion. С уарт-интерфейсом есть и другие популярные модули - HC-05, HC-06 и т.д. Вместо подобного шасси можно взять танк. Шасси в готовых радиоуправляемых моделях не всегда хорошо подходят даже для такой простой переделки, а если со временем планируете сделать, например, лайнтрейсер, автономного робота (с датчиками, манипулятором и т.д.), подобрать или напечатать более подходящее колёсное или гусеничное шасси. И т.д.
Для управления всем, у нас есть Arduino NANO. Модуль MPU6050, с помощью которого будут определяться углы наклона, подключается к Arduino по интерфейсу I2C.
В качестве источника питания в данном случае используется крона, подключенная через выключатель, чтобы мы могли выключать и выключать питание.
Также есть джойстик, подключаемый к аналоговым входам, но на этом этапе он не задействован. Управление от джойстика будет реализовано позже, когда мы добавим еще два канала.
Наконец, для отправки данных используется радио-модуль HC-12, который подключен по последовательному интерфейсу (UART) и работает как прозрачный радиомост. NRF24 работает на частоте 2.4ГГц, а данный модуль на частоте 433МГц. На частоте 433МГц теоретическая дальность связи может быть намного больше. Радиоэфир на 2.4ГГц иногда бывает очень сильно забит, из-за чего могут быть проблемы с передачей данных и использование радиомодуля на 433МГц может решить эту проблему.
Вместо модуля HC-12 можно другие подобные модули с UART интерфейсом, например, HC-05, HC-06 и т.д. Приём и передача данных происходит также, но в зависимости от модуля, настройка может немного отличаться.
Если использовать радиомодули (NRF24, ZigBee, LoRa, некоторые Bluetooth, WiFi и т.д.), код нужно будет немного адаптировать с учётом особенностей их работы.
Передатчик собран, теперь время перейти к коду. Т.к. джойстик пока не задействован, пока будет использоваться только инерциальный датчик. Алгоритм работы очень простой. Сначала считываются данные от MPU6050. Затем на их основе вычисляются углы наклона датчика, т.е. пульта. С помощью радиомодуля данные отправляются на машину.
//Libraries #include <Wire.h> #include <SoftwareSerial.h> SoftwareSerial HC12(11, 10); // D11 is HC-12 TX Pin, D10 is HC-12 RX Pin ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //Variables for the Gyro data float elapsedTime, time, timePrev; //Variables for time control int gyro_error = 0; //We use this variable to only calculate once the gyro data error float Gyr_rawX, Gyr_rawY, Gyr_rawZ; //Here we store the raw data read float Gyro_angle_x, Gyro_angle_y; //Here we store the angle value obtained with Gyro data float Gyro_raw_error_x, Gyro_raw_error_y; //Here we store the initial gyro data error //Variables for acceleration data int acc_error = 0; //We use this variable to only calculate once the Acc data error float rad_to_deg = 180 / 3.141592654; //This value is for pasing from radians to degrees values float Acc_rawX, Acc_rawY, Acc_rawZ; //Here we store the raw data read float Acc_angle_x, Acc_angle_y; //Here we store the angle value obtained with Acc data float Acc_angle_error_x, Acc_angle_error_y; //Here we store the initial Acc data error //Total angle data float Total_angle_x, Total_angle_y; //Data to send int x_send, y_send; ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void setup() { Serial.begin(9600); //Start serial com in order to print valeus on monitor HC12.begin(9600); //Start the HC12 serial communication at 9600 bauds. Change bauds if needed begin_MPU6050(); //Call this function and start the MPU6050 module time = millis(); //Start counting time in milliseconds calculate_IMU_error(); //Call this function and het the acc and gyro error at the beginning }//end of setup void ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void loop() { timePrev = time; // the previous time is stored before the actual time read time = millis(); // actual time read elapsedTime = (time - timePrev) / 1000; //divide by 1000 in order to obtain seconds //////////////////////////////////////Gyro read///////////////////////////////////// Wire.beginTransmission(0x68); //begin, Send the slave adress (in this case 68) Wire.write(0x43); //First adress of the Gyro data Wire.endTransmission(false); Wire.requestFrom(0x68, 4, true); //We ask for just 4 registers Gyr_rawX = Wire.read() << 8 | Wire.read(); //Once again we shif and sum Gyr_rawY = Wire.read() << 8 | Wire.read(); /*Now in order to obtain the gyro data in degrees/seconds we have to divide first the raw value by 32.8 because that's the value that the datasheet gives us for a 1000dps range*/ /*---X---*/ Gyr_rawX = (Gyr_rawX / 32.8) - Gyro_raw_error_x; /*---Y---*/ Gyr_rawY = (Gyr_rawY / 32.8) - Gyro_raw_error_y; /*Now we integrate the raw value in degrees per seconds in order to obtain the angle If you multiply degrees/seconds by seconds you obtain degrees */ /*---X---*/ Gyro_angle_x = Gyr_rawX * elapsedTime; /*---X---*/ Gyro_angle_y = Gyr_rawY * elapsedTime; //////////////////////////////////////Acc read///////////////////////////////////// Wire.beginTransmission(0x68); //begin, Send the slave adress (in this case 68) Wire.write(0x3B); //Ask for the 0x3B register- correspond to AcX Wire.endTransmission(false); //keep the transmission and next Wire.requestFrom(0x68, 6, true); //We ask for next 6 registers starting withj the 3B /*We have asked for the 0x3B register. The IMU will send a brust of register. The amount of register to read is specify in the requestFrom function. In this case we request 6 registers. Each value of acceleration is made out of two 8bits registers, low values and high values. For that we request the 6 of them and just make then sum of each pair. For that we shift to the left the high values register (<<) and make an or (|) operation to add the low values. If we read the datasheet, for a range of+-8g, we have to divide the raw values by 4096*/ Acc_rawX = (Wire.read() << 8 | Wire.read()) / 4096.0 ; //each value needs two registres Acc_rawY = (Wire.read() << 8 | Wire.read()) / 4096.0 ; Acc_rawZ = (Wire.read() << 8 | Wire.read()) / 4096.0 ; /*Now in order to obtain the Acc angles we use euler formula with acceleration values after that we substract the error value found before*/ /*---X---*/ Acc_angle_x = (atan((Acc_rawY) / sqrt(pow((Acc_rawX), 2) + pow((Acc_rawZ), 2))) * rad_to_deg) - Acc_angle_error_x; /*---Y---*/ Acc_angle_y = (atan(-1 * (Acc_rawX) / sqrt(pow((Acc_rawY), 2) + pow((Acc_rawZ), 2))) * rad_to_deg) - Acc_angle_error_y; //////////////////////////////////////Total angle and filter///////////////////////////////////// /*---X axis angle---*/ Total_angle_x = 0.98 * (Total_angle_x + Gyro_angle_x) + 0.02 * Acc_angle_x; /*---Y axis angle---*/ Total_angle_y = 0.98 * (Total_angle_y + Gyro_angle_y) + 0.02 * Acc_angle_y; Serial.print("Xº: "); Serial.print(Total_angle_x); Serial.print(" | "); Serial.print("Yº: "); Serial.print(Total_angle_y); Serial.println(" "); x_send = -Total_angle_x; //pass from flaot to int in order to send the values, also invert X angle y_send = Total_angle_y; //pass from flaot to int in order to send the values HC12.print(x_send); HC12.write("X"); //Send the x angle value and then the "X" character HC12.print(y_send); HC12.write("Y"); //Send the y angle value and then the "Y" character delay(50); //Add a delay of 50. This might affect the transmission }//End of void loop ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void begin_MPU6050() { Wire.begin(); //begin the wire comunication Wire.beginTransmission(0x68); //begin, Send the slave adress (in this case 68) Wire.write(0x6B); //make the reset (place a 0 into the 6B register) Wire.write(0x00); Wire.endTransmission(true); //end the transmission //Gyro config Wire.beginTransmission(0x68); //begin, Send the slave adress (in this case 68) Wire.write(0x1B); //We want to write to the GYRO_CONFIG register (1B hex) Wire.write(0x10); //Set the register bits as 00010000 (1000dps full scale) Wire.endTransmission(true); //End the transmission with the gyro //Acc config Wire.beginTransmission(0x68); //Start communication with the address found during search. Wire.write(0x1C); //We want to write to the ACCEL_CONFIG register Wire.write(0x10); //Set the register bits as 00010000 (+/- 8g full scale range) Wire.endTransmission(true); } void calculate_IMU_error() { /*Here we calculate the acc data error before we start the loop I make the mean of 200 values, that should be enough*/ if (acc_error == 0) { for (int a = 0; a < 200; a++) { Wire.beginTransmission(0x68); Wire.write(0x3B); //Ask for the 0x3B register- correspond to AcX Wire.endTransmission(false); Wire.requestFrom(0x68, 6, true); Acc_rawX = (Wire.read() << 8 | Wire.read()) / 4096.0 ; //each value needs two registres Acc_rawY = (Wire.read() << 8 | Wire.read()) / 4096.0 ; Acc_rawZ = (Wire.read() << 8 | Wire.read()) / 4096.0 ; /*---X---*/ Acc_angle_error_x = Acc_angle_error_x + ((atan((Acc_rawY) / sqrt(pow((Acc_rawX), 2) + pow((Acc_rawZ), 2))) * rad_to_deg)); /*---Y---*/ Acc_angle_error_y = Acc_angle_error_y + ((atan(-1 * (Acc_rawX) / sqrt(pow((Acc_rawY), 2) + pow((Acc_rawZ), 2))) * rad_to_deg)); if (a == 199) { Acc_angle_error_x = Acc_angle_error_x / 200; Acc_angle_error_y = Acc_angle_error_y / 200; acc_error = 1; } } }//end of acc error calculation /*Here we calculate the gyro data error before we start the loop I make the mean of 200 values, that should be enough*/ if (gyro_error == 0) { for (int i = 0; i < 200; i++) { Wire.beginTransmission(0x68); //begin, Send the slave adress (in this case 68) Wire.write(0x43); //First adress of the Gyro data Wire.endTransmission(false); Wire.requestFrom(0x68, 4, true); //We ask for just 4 registers Gyr_rawX = Wire.read() << 8 | Wire.read(); //Once again we shif and sum Gyr_rawY = Wire.read() << 8 | Wire.read(); /*---X---*/ Gyro_raw_error_x = Gyro_raw_error_x + (Gyr_rawX / 32.8); /*---Y---*/ Gyro_raw_error_y = Gyro_raw_error_y + (Gyr_rawY / 32.8); if (i == 199) { Gyro_raw_error_x = Gyro_raw_error_x / 200; Gyro_raw_error_y = Gyro_raw_error_y / 200; gyro_error = 1; } } }//end of gyro error calculation }
MPU6050 подключен по интерфейсу I2c и для работы с ним подключается библиотека Wire. В цикле loop сначала считываются данные гироскопа и акселерометра. Затем с помощью формул и фильтра, на основе этих данных вычисляются углы наклона для оси X и Y. После передачи данных для организации задержки вызывается delay(50).
В скетче для отправки данных об углах наклона, кроме значения самих углов также отправляются символы «X» и «Y». Это даёт нам возможность при приёме определять, какие данные для оси X, а какие для оси Y.
Аппаратный последовательный интерфейс (UART) используется для вывода данных, которые можно посмотреть на компьютере. В этом скетче выводится данные об углах. Реализуя дополнительный функционал, можно будет добавить вывод других данных или даже реализовать возможность получать команды, к примеру, для настройки, переключения режимов или запроса на вывод данных, которые не нужно выводить постоянно.
Взаимодействие с радиомодулем также происходит по последовательному интерфейсу. Аппаратный UART уже используется для вывода данных. Для радиомодуля используется программный UART, который реализован в библиотеке SoftwareSerial.
После подключения библиотеки SoftwareSerial, строчкой «SoftwareSerial HC12(11, 10)» настраивается на каких выводах Arduino задействовать TX и RX. В данном случае задействуются выводы D11 и D10. Подключив модуль HC-12 к этим выводам, можно будет отправлять данные также, как и аппаратным последовательным интерфейсом, только вместо вызова Serial.print или Serial.write будем вызывать HC12.print и HC12.write.
Модули HC-12 обычно изначально настроены на передачу 9600 бод. В этом скетче взаимодействие с модулем также настроено на 9600бод. При необходимости можно и перенастроить. Для перенастройки модуль нужно подключить следующим образом:
Без подключенного вывода «SET» мы не можем перевести модуль в режим конфигурации.
Загрузите в Arduino следующий скетч:
/*Code to change the HC-12 baud rate Arduino | HC-12 D11 TxD D10 RxD GND GND GND SET 5V VCC */ #include <SoftwareSerial.h> SoftwareSerial hc12(11, 10); void setup() { pinMode(7, OUTPUT); digitalWrite(7, LOW); // enter AT command mode Serial.begin(9600); hc12.begin(9600); hc12.print(F("AT+C001")); // set to channel 1 delay(100); digitalWrite(7, LOW); // enter transparent mode } void loop() { if (Serial.available()) hc12.write(Serial.read()); if (hc12.available()) Serial.write(hc12.read()); }
Теперь в Arduino IDE из меню «Инструменты» откройте «Монитор порта» и в нём установите скорость передачи 9600. Теперь введите AT + ___, где ____ нужная вам команда. Ниже приведены описания нескольких полезных команд.
1. Изменение скорость передачи:
Введите «AT+Bxxxx», чтобы изменить скорость передачи данных на xxxx бод/с.
Пример: введите «AT+B19200», и модуль будет работать со скоростью 19200 бод/с и вернет «OK+B19200».
Доступные значения xxxx: 1200, 2400, 4800, 9600, 19200, 38400, 57600, 115200.
Бодрейт по умолчанию составляет 9600 бод/с.
2. Изменить канал связи:
Введите «AT+Cyyy», чтобы изменить канал связи на yyy.
Пример: введите «AT+C021» и модуль переключится на канал 021, вернув «OK+C021».
Доступные значения yyy: 001, 002, 003,…, 127.
Канал по умолчанию - 001.
3. Сброс к заводским настройкам:
Введя «AT+DEFAULT», модуль вернется к настройкам по умолчанию и вернет «OK+DEFAULT».
Для демонстрации и что бы не усложнять схему, в качестве источника питания также используется крона, подключенная к выводу Vin. Моторы могут очень быстро разряжать крону, поэтому для более частого или долгого использования лучше использовать аккумуляторы.
Напряжение NiMh пальчиковых аккумуляторов составляет примерно 1.2В и, например, соединив последовательно 4шт, выходное напряжение будет примерно 5В.
Для питания можно использовать и другие популярные типы аккумуляторов, например, Li-Ion. Номинальное напряжение одной Li-Ion банки составляет примерно 3.7В, а ёмкость может доходить до 2.3Ач-3.4Ач. Если использовать одну банку, после неё нужно установить импульсный повышающий стабилизатор. При использовании двух, соединённых последовательно, можно оставить такое же подключение, как и с кроной. При использовании трёх и более, как минимум перед Arduino, понадобиться импульсный понижающий стабилизатор.
//Libraries #include <SoftwareSerial.h> SoftwareSerial HC12(11, 10); // D11 is HC-12 TX Pin, D10 is HC-12 RX Pin ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //Inputs and outputs int pwm_front = 3; int pwm_back = 5; int pwm_left = 6; int pwm_right = 9; //Used variables for receive data and control motors byte incomingByte; String readBuffer = ""; String readBufferX = ""; String readBufferY = ""; int X_val = 0; int Y_val = 0; int i = 0; bool x_ready = false; bool y_ready = false; bool x_passed = false; bool data_received = false; int pwm_x = 0; int pwm_y = 0; void setup() { Serial.begin(9600); // Open serial port HC12.begin(9600); // Open serial port to HC12 pinMode(pwm_front, OUTPUT); //Define the pins as outputs pinMode(pwm_back, OUTPUT); pinMode(pwm_left, OUTPUT); pinMode(pwm_right, OUTPUT); digitalWrite(pwm_front, LOW); //Set the pins to low digitalWrite(pwm_back, LOW); digitalWrite(pwm_left, LOW); digitalWrite(pwm_right, LOW); } void loop() { //First we store the entire incoming data into "readBuffer" while (HC12.available() > 0) { // If the HC-12 has data in incomingByte = HC12.read(); // Store the data byte by byte readBuffer += char(incomingByte); // Add each byte to ReadBuffer total string variable } delay(100); //This delay has to be equal or higher than the dalay in transmitter /* We know we first send the X angle data then the Y. So we store the number till we receive an "X". If "X" is received we stop adn we then get the x angle data till we receive an "Y". */ while (i <= sizeof(readBuffer)) { if (readBuffer[i] == 'X') { x_ready = true; } if (readBuffer[i] == 'Y') { y_ready = true; } if (!x_ready) { readBufferX = readBufferX + (readBuffer[i] ); } if (x_passed && !y_ready) { readBufferY = readBufferY + (readBuffer[i] ); } if (x_ready) { x_passed = true; } i = i + 1; } data_received = true; X_val = readBufferX.toInt(); //Pass the data from string to int so we could use it Y_val = readBufferY.toInt(); if (data_received) { Serial.print("X_val: "); Serial.print(X_val); Serial.print(" Y_val "); Serial.println(Y_val); //Now we reset all variables readBuffer = ""; //Reset the buffer to empty readBufferX = ""; //Reset the buffer to empty x_ready = false; //Reset the other values x_passed = false; readBufferY = ""; //Reset the buffer to empty y_ready = false; i = 0; } data_received = false; /////////////////////////////////////////////////////////////////////////// ////////////////////////////front and back///////////////////////////////// /////////////////////////////////////////////////////////////////////////// //Now we control the PWM signal for each motor only if the angle of the //controller is higher ol lower than - 4 if (X_val > 4) { pwm_x = map(X_val, 0, 35, 0, 200); //Map the angle to a PWM signal from 0 tow 255 constrain(pwm_x, 0, 200); //Constrain the values analogWrite(pwm_front, pwm_x); //Write the PWM signal and activate the motor analogWrite(pwm_back, LOW); } //We do the same for all 4 PWM pins if (X_val < -4) { pwm_x = map(X_val, 0, -35, 0, 200); constrain(pwm_x, 0, 200); analogWrite(pwm_front, LOW); analogWrite(pwm_back, pwm_x); } if (X_val > -4 && X_val < 4) { analogWrite(pwm_front, LOW); analogWrite(pwm_back, LOW); } /////////////////////////////////////////////////////////////////////////// ////////////////////////////left and right///////////////////////////////// /////////////////////////////////////////////////////////////////////////// if (Y_val > 10) { analogWrite(pwm_right, 200); analogWrite(pwm_left, LOW); } if (Y_val < -10) { analogWrite(pwm_right, LOW); analogWrite(pwm_left, 200); } if (Y_val > -10 && Y_val < 10) { analogWrite(pwm_right, LOW); analogWrite(pwm_left, LOW); } /////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////// }//End of void loop
В скетче приёмника, как и в коде передатчика, для работы с модулем HC-12 используется SoftwareSerial, а для вывода данных на компьютер аппаратный UART.
Принимаемые с помощью модуля HC-12 данные сначала байт за байтом сохраняются в буфере. Затем содержимое буфера анализируется и переменным X_val и Y_val присваиваются соответствующие значения.
Проверяя в условиях значения переменных X_val и Y_val, определяется в каком направлении нужно вращать моторы.
Чтобы управлять скоростью вращения моторов используется ШИМ. Функции analogWrite для указания периода ШИМ, нужно передавать значение в диапазоне от 0 до 255, т.е. сразу передать в неё X_val и Y_val не получится. В библиотеках Arduino есть функции map и constrain, с помощью которых можно преобразовывать значение из одного диапазона в другой. В данном случае мы преобразуем значения в диапазон от 0 до 200.
Хорошо, загружаем вышеприведенный код в Arduino приёмника и с помощью пульта пробуем управлять машинкой. Чем больше угол наклона, тем выше скорость вращения задних колёс или угол поворота передних.
Для четырёхканального передатчика схема остаётся прежней, но код немного отличается.
//Libraries #include <Wire.h> #include <SoftwareSerial.h> SoftwareSerial HC12(11, 10); // D11 is HC-12 TX Pin, D10 is HC-12 RX Pin ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //Inputs int Joy_x = A0; int Joy_y = A1; //Variables for the Gyro data float elapsedTime, time, timePrev; //Variables for time control int gyro_error = 0; //We use this variable to only calculate once the gyro data error float Gyr_rawX, Gyr_rawY, Gyr_rawZ; //Here we store the raw data read float Gyro_angle_x, Gyro_angle_y; //Here we store the angle value obtained with Gyro data float Gyro_raw_error_x, Gyro_raw_error_y; //Here we store the initial gyro data error //Variables for acceleration data int acc_error = 0; //We use this variable to only calculate once the Acc data error float rad_to_deg = 180 / 3.141592654; //This value is for pasing from radians to degrees values float Acc_rawX, Acc_rawY, Acc_rawZ; //Here we store the raw data read float Acc_angle_x, Acc_angle_y; //Here we store the angle value obtained with Acc data float Acc_angle_error_x, Acc_angle_error_y; //Here we store the initial Acc data error //Total angle data float Total_angle_x, Total_angle_y; //Data to send int x_send, y_send, joy_x_send, joy_y_send; ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void setup() { pinMode(Joy_x, INPUT); pinMode(Joy_y, INPUT); Serial.begin(9600); //Start serial com in order to print valeus on monitor HC12.begin(9600); //Start the HC12 serial communication at 9600 bauds. Change bauds if needed begin_MPU6050(); //Call this function and start the MPU6050 module time = millis(); //Start counting time in milliseconds calculate_IMU_error(); //Call this function and het the acc and gyro error at the beginning }//end of setup void ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void loop() { timePrev = time; // the previous time is stored before the actual time read time = millis(); // actual time read elapsedTime = (time - timePrev) / 1000; //divide by 1000 in order to obtain seconds //////////////////////////////////////Gyro read///////////////////////////////////// Wire.beginTransmission(0x68); //begin, Send the slave adress (in this case 68) Wire.write(0x43); //First adress of the Gyro data Wire.endTransmission(false); Wire.requestFrom(0x68, 4, true); //We ask for just 4 registers Gyr_rawX = Wire.read() << 8 | Wire.read(); //Once again we shif and sum Gyr_rawY = Wire.read() << 8 | Wire.read(); /*Now in order to obtain the gyro data in degrees/seconds we have to divide first the raw value by 32.8 because that's the value that the datasheet gives us for a 1000dps range*/ /*---X---*/ Gyr_rawX = (Gyr_rawX / 32.8) - Gyro_raw_error_x; /*---Y---*/ Gyr_rawY = (Gyr_rawY / 32.8) - Gyro_raw_error_y; /*Now we integrate the raw value in degrees per seconds in order to obtain the angle If you multiply degrees/seconds by seconds you obtain degrees */ /*---X---*/ Gyro_angle_x = Gyr_rawX * elapsedTime; /*---X---*/ Gyro_angle_y = Gyr_rawY * elapsedTime; //////////////////////////////////////Acc read///////////////////////////////////// Wire.beginTransmission(0x68); //begin, Send the slave adress (in this case 68) Wire.write(0x3B); //Ask for the 0x3B register- correspond to AcX Wire.endTransmission(false); //keep the transmission and next Wire.requestFrom(0x68, 6, true); //We ask for next 6 registers starting withj the 3B /*We have asked for the 0x3B register. The IMU will send a brust of register. The amount of register to read is specify in the requestFrom function. In this case we request 6 registers. Each value of acceleration is made out of two 8bits registers, low values and high values. For that we request the 6 of them and just make then sum of each pair. For that we shift to the left the high values register (<<) and make an or (|) operation to add the low values. If we read the datasheet, for a range of+-8g, we have to divide the raw values by 4096*/ Acc_rawX = (Wire.read() << 8 | Wire.read()) / 4096.0 ; //each value needs two registres Acc_rawY = (Wire.read() << 8 | Wire.read()) / 4096.0 ; Acc_rawZ = (Wire.read() << 8 | Wire.read()) / 4096.0 ; /*Now in order to obtain the Acc angles we use euler formula with acceleration values after that we substract the error value found before*/ /*---X---*/ Acc_angle_x = (atan((Acc_rawY) / sqrt(pow((Acc_rawX), 2) + pow((Acc_rawZ), 2))) * rad_to_deg) - Acc_angle_error_x; /*---Y---*/ Acc_angle_y = (atan(-1 * (Acc_rawX) / sqrt(pow((Acc_rawY), 2) + pow((Acc_rawZ), 2))) * rad_to_deg) - Acc_angle_error_y; //////////////////////////////////////Total angle and filter///////////////////////////////////// /*---X axis angle---*/ Total_angle_x = 0.98 * (Total_angle_x + Gyro_angle_x) + 0.02 * Acc_angle_x; /*---Y axis angle---*/ Total_angle_y = 0.98 * (Total_angle_y + Gyro_angle_y) + 0.02 * Acc_angle_y; joy_x_send = analogRead(Joy_x); joy_y_send = analogRead(Joy_y); x_send = -Total_angle_x; //pass from flaot to int in order to send the values, also invert X angle y_send = Total_angle_y; //pass from flaot to int in order to send the values Serial.print("IMU Xº: "); Serial.print(Total_angle_x); Serial.print(" IMU Yº: "); Serial.print(Total_angle_y); Serial.print(" | "); Serial.print("Joy X: "); Serial.print(joy_x_send); Serial.print(" Joy Y: "); Serial.print(joy_x_send); Serial.println(" "); HC12.print(x_send); HC12.write("X"); //Send the x angle value and then the "X" character HC12.print(y_send); HC12.write("Y"); //Send the y angle value and then the "Y" character HC12.print(joy_x_send); HC12.write("W"); //Send the y angle value and then the "Y" character HC12.print(joy_y_send); HC12.write("T"); //Send the y angle value and then the "Y" character delay(20); //Add a delay of 50. This might affect the transmission }//End of void loop ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void begin_MPU6050() { Wire.begin(); //begin the wire comunication Wire.beginTransmission(0x68); //begin, Send the slave adress (in this case 68) Wire.write(0x6B); //make the reset (place a 0 into the 6B register) Wire.write(0x00); Wire.endTransmission(true); //end the transmission //Gyro config Wire.beginTransmission(0x68); //begin, Send the slave adress (in this case 68) Wire.write(0x1B); //We want to write to the GYRO_CONFIG register (1B hex) Wire.write(0x10); //Set the register bits as 00010000 (1000dps full scale) Wire.endTransmission(true); //End the transmission with the gyro //Acc config Wire.beginTransmission(0x68); //Start communication with the address found during search. Wire.write(0x1C); //We want to write to the ACCEL_CONFIG register Wire.write(0x10); //Set the register bits as 00010000 (+/- 8g full scale range) Wire.endTransmission(true); } void calculate_IMU_error() { /*Here we calculate the acc data error before we start the loop I make the mean of 200 values, that should be enough*/ if (acc_error == 0) { for (int a = 0; a < 200; a++) { Wire.beginTransmission(0x68); Wire.write(0x3B); //Ask for the 0x3B register- correspond to AcX Wire.endTransmission(false); Wire.requestFrom(0x68, 6, true); Acc_rawX = (Wire.read() << 8 | Wire.read()) / 4096.0 ; //each value needs two registres Acc_rawY = (Wire.read() << 8 | Wire.read()) / 4096.0 ; Acc_rawZ = (Wire.read() << 8 | Wire.read()) / 4096.0 ; /*---X---*/ Acc_angle_error_x = Acc_angle_error_x + ((atan((Acc_rawY) / sqrt(pow((Acc_rawX), 2) + pow((Acc_rawZ), 2))) * rad_to_deg)); /*---Y---*/ Acc_angle_error_y = Acc_angle_error_y + ((atan(-1 * (Acc_rawX) / sqrt(pow((Acc_rawY), 2) + pow((Acc_rawZ), 2))) * rad_to_deg)); if (a == 199) { Acc_angle_error_x = Acc_angle_error_x / 200; Acc_angle_error_y = Acc_angle_error_y / 200; acc_error = 1; } } }//end of acc error calculation /*Here we calculate the gyro data error before we start the loop I make the mean of 200 values, that should be enough*/ if (gyro_error == 0) { for (int i = 0; i < 200; i++) { Wire.beginTransmission(0x68); //begin, Send the slave adress (in this case 68) Wire.write(0x43); //First adress of the Gyro data Wire.endTransmission(false); Wire.requestFrom(0x68, 4, true); //We ask for just 4 registers Gyr_rawX = Wire.read() << 8 | Wire.read(); //Once again we shif and sum Gyr_rawY = Wire.read() << 8 | Wire.read(); /*---X---*/ Gyro_raw_error_x = Gyro_raw_error_x + (Gyr_rawX / 32.8); /*---Y---*/ Gyro_raw_error_y = Gyro_raw_error_y + (Gyr_rawY / 32.8); if (i == 199) { Gyro_raw_error_x = Gyro_raw_error_x / 200; Gyro_raw_error_y = Gyro_raw_error_y / 200; gyro_error = 1; } } }//end of gyro error calculation }
Двухосевой джойстик подключен к аналоговым входам A0 b A1. В начале скетча эти входы настраиваются как аналоговые входы. Затем в цикле loop с помощью функции analogRead, данные о положении осей джойстика считываются и присваиваются переменным joy_x_send и joy_y_send. После отправки денных об углах по осям X и Y, в коде добавлена отправка данных о положении осей джойстика:
HC12.print(joy_x_send); HC12.write("W");Ранее два канала использовались для управления двумя коллекторными моторами. Дополнительные каналы могут понадобиться, если на шасси установить ещё что-нибудь. При переделке танка, это может быть коллекторный мотор поворота башни. На шасси можно установить механизм поворота/наклона, на котором будет установлен дальномер. Установив сервопривод, к его качалке можно прикрепить шлангу от небольшой водяной помпы. И т.д.
С помощью ШИМ можно управлять не только скоростью вращения коллекторных моторов. Также можно управлять яркостью свечения светодиодов, паро или дымогенераторами (по крайней мере некоторыми, в которых используется нагревательный элемент) и т.д.
Ещё ШИМ управление является чуть ли не единственным вариантом для электроники и исполнительных устройств, используемых в радиомоделизме. Сервоприводы, контроллеры оборотов для коллекторных и BLDC моторов (лопасти, колёса, гидравлика) и т.д., используемые для создания машин, танков, лодок, самолётов, квадрокоптеров и прочего, в основном используют ШИМ управление.
В робототехнике ШИМ управление это всего лишь один из многих вариантов, и он не самый популярный. Намного более популярные это, к примеру, UART, I2C, SPI, параллельный интерфейсы + при передаче данных используется большое количество разнообразных протоколов. А ШИМ управление в основном (пока ещё?) используется для сервоприводов или контроллеров оборотов BLDC моторов.
Пример схемы подключения:
Только модуль HC-12, Arduino и 4 выхода ШИМ. Возможно вы уже заметили, что для питания нарисованы две стрелочки. Если от источника питания на Arduino идёт 5В, можно подключить к выводу 5V. А если больше, нужно подключать к Vin (на плате этот вывод подключен к линейному 5В стабилизатору, от которого уже и подаётся питание на микроконтроллер).
У аккумуляторных сборок, используемые в радиомоделизме, выходное напряжение может быть слишком большим и, если оно превысит максимум для линейного стабилизатора, установленного на плате Arduino, он выйдет из строя. При этом цепь может как разорвать, так и закоротить (очень часто). Во втором случае скорей всего выйдет из строя ещё микроконтроллер на Arduino и/или радиомодуль.
Параметры линейных стабилизаторов, которые устанавливаются на платы Arduino могут отличаться, но очень часто максимальное входное напряжение примерно в диапазоне 12-15В. Если не уверены, какой максимум или у аккумулятора более высокое напряжение, используйте дополнительно понижающий импульсный стабилизатор.
Если на вашем шасси с помощью драйвера нужно управлять коллекторным мотором водяной помпы, механизма поворота башни и т.д., сделайте аналогично, как и в прошлом примере. А в следующем примере будет рассмотрен вариант ШИМ управления для сервоприводов или контроллеров оборотов.
//Libraries #include <SoftwareSerial.h> SoftwareSerial HC12(11, 10); // D11 is HC-12 TX Pin, D10 is HC-12 RX Pin #include <Servo.h> ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //Inputs and outputs Servo throttle; Servo yaw; Servo pitch; Servo roll; //Used variables for receive data and control motors byte incomingByte; String readBuffer = ""; String readBufferX = ""; String readBufferY = ""; String readBufferW = ""; String readBufferT = ""; int X_val = 0; int Y_val = 0; int Yaw_val = 0; int Throttle_val = 0; int i = 0; bool x_ready = false; bool y_ready = false; bool yaw_ready = false; bool throttle_ready = false; bool x_passed = false; bool y_passed = false; bool yaw_passed = false; bool data_received = false; int pwm_x = 0; int pwm_y = 0; int pwm_throttle = 0; int pwm_yaw = 1500; int pwm_roll = 1500; int pwm_pitch = 1500; bool first = false; void setup() { throttle.attach(3); yaw.attach(5); pitch.attach(6); roll.attach(9); Serial.begin(9600); // Open serial port HC12.begin(9600); // Open serial port to HC12 throttle.writeMicroseconds(1000); yaw.writeMicroseconds(1500); pitch.writeMicroseconds(1500); roll.writeMicroseconds(1500); } void loop() { //First we store the entire incoming data into "readBuffer" while (HC12.available() > 0) { // If the HC-12 has data in incomingByte = HC12.read(); // Store the data byte by byte readBuffer += char(incomingByte); // Add each byte to ReadBuffer total string variable } delay(22); //This delay has to be equal or higher than the dalay in transmitter /* We know we first send the X angle data then the Y. So we store the number till we receive an "X". If "X" is received we stop adn we then get the x angle data till we receive an "Y". */ while (i <= sizeof(readBuffer)) { if (readBuffer[i] == 'X') { x_ready = true; } if (readBuffer[i] == 'Y') { y_ready = true; } if (readBuffer[i] == 'W') { yaw_ready = true; } if (readBuffer[i] == 'T') { throttle_ready = true; } if (!x_ready) { readBufferX = readBufferX + (readBuffer[i] ); } if (x_passed && !y_ready) { readBufferY = readBufferY + (readBuffer[i] ); } if (y_passed && !yaw_ready) { readBufferW = readBufferW + (readBuffer[i] ); } if (yaw_passed && !throttle_ready) { readBufferT = readBufferT + (readBuffer[i] ); } if (x_ready) { x_passed = true; } if (y_ready) { y_passed = true; } if (yaw_ready) { yaw_passed = true; } i = i + 1; } data_received = true; X_val = readBufferX.toInt(); //Pass the data from string to int so we could use it Y_val = readBufferY.toInt(); Yaw_val = readBufferW.toInt(); Throttle_val = readBufferT.toInt(); if (data_received) { /////////////////////////////////////////////////////////////////////////// //Uncomment the lines below if you want to print the data on serial monitor //Serial.print(X_val); //Serial.print(" "); //Serial.println(Y_val); /////////////////////////////////////////////////////////////////////////// //Now we reset all variables //Reset the buffer to empty readBufferX = ""; //Reset the buffer to empty readBufferW = ""; //Reset the buffer to empty readBufferT = ""; //Reset the buffer to empty x_ready = false; //Reset the other values y_ready = false; yaw_ready = false; //Reset the other values throttle_ready = false; //Reset the other values x_passed = false; y_passed = false; yaw_passed = false; readBufferY = ""; //Reset the buffer to empty i = 0; data_received = false; readBuffer = ""; } /////////////////////////////////////////////////////////////////////////// ////////////////////////////front and back///////////////////////////////// /////////////////////////////////////////////////////////////////////////// //Now we control the PWM signal for each output pwm_throttle = map (Throttle_val, 0, 1024, 1000, 2000); pwm_yaw = map (Yaw_val, 0, 1024, 1000, 2000); pwm_roll = map (X_val, -60, 60, 1000, 2000); //We give a maximum angle of 60 degrees pwm_pitch = map (Y_val, -60, 60, 1000, 2000); Serial.println(pwm_yaw); throttle.writeMicroseconds(pwm_throttle); yaw.writeMicroseconds(pwm_yaw); roll.writeMicroseconds(pwm_roll); pitch.writeMicroseconds(pwm_pitch); /////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////// }//End of void loop
В сервоприводах или контроллерах оборотов (очень часто, но не во всех) нужно подавать управляющий сигнал с частотой 50Гц, т.е. период следования составляет 20мс. Скважность (длительность управляющего импульса) может быть разная, к примеру 850..2250мкс, 500..1900мкс, 500..2000мкс и т.д., но обычно не выходит за пределы 500..2500мкс. В следующем коде я реализовал для диапазона 1000..2000мкс.
Загрузив код в Arduino и подключив осциллограф к соответствующим выводам, мы сможем увидеть сигнал ШИМ:
