==== Отработка движения ====
- В движение платформу приводит двигатель мощностью 225w, прямая передача, соотношение зубов 21/64. \\
- Поворот рулевых колёс происходит с помощью простого механизма тяги, сервопривод вращает 15 мм плечо , которое прикреплено к регулируемому рычагу, который в свою очередь прикрепляется к тяге рулевых колёс.\\
- Точность поворота зависит от текущего веса робота, чем тяжелее, тем больше раз нужно подать команду поворота на сервопривод, существуют разумные границы поворота, чтобы не вывести из строя тягу или сервопривод. \\
#include
#define Engine 2
#define Engine_pwm 3
#define R 5
#define L 4
int Steering_Angle = 15;
int Engine_Speed_Now = 30;
int Steering_Pos = 60;
double flag_st = 0; // флаг для сервопривода
Servo Steering_Serv;
void Engine_Parking_without_brake() { // отключение питания двигателя, остановка за счёт трения
Serial.println("Engine_Parking_without_brake");
digitalWrite(R, LOW);
digitalWrite(L, LOW);
digitalWrite(Engine, LOW);
delay(1000);
digitalWrite(Engine, HIGH);
flag_st = 0;
}
void Engine_Forward() { // прямое вращение двигателя
Serial.println("En_Forward");
digitalWrite(L, LOW);X
digitalWrite(R, HIGH);
analogWrite(Engine_pwm, Engine_Speed_Now);
flag_st = 1;
}
void Engine_Back() { // обратное вращение двигателя
Serial.println("Engine_Back");
digitalWrite(L, HIGH);
digitalWrite(R, LOW);
analogWrite(Engine_pwm, Engine_Speed_Now);
flag_st = 1;
}
void Engine_Speed_Up() {
if (Engine_Speed_Now <= 245) {
Engine_Speed_Now += 15;
Serial.print(Engine_Speed_Now);
Serial.println(" Engine_Speed_Up ");
}
else
Engine_Speed_Now = 255;
}
void Engine_Speed_Down() {
if (Engine_Speed_Now > 40) {
Engine_Speed_Now -= 15;
Serial.print(Engine_Speed_Now);
Serial.println(" Engine_Speed_Down ");
}
else
Engine_Speed_Now = 40;
}
void Steering_Forward() {
if (flag_st == 0) { // флаг 0, выравниваемся способом №1
Steering_Pos = 60;
Serial.println(" Steering_Forward(60) ");
Steering_Serv.write(70);
delay(200);
Steering_Serv.write(35);
delay(200);
Steering_Serv.write(70);
delay(200);
Steering_Serv.write(35);
delay(200);
Steering_Serv.write(Steering_Pos + 2);
}
if (flag_st == 1) { // флаг 1, выравниваемся способом №2
if (Steering_Pos > 60 && Steering_Pos < 130) { // программная проверка позиции сервы
Steering_Pos = 60; // выравнивание для правого поворота
Serial.println(" Steering_Forward(60) ");
Steering_Serv.write(75);
delay(90);
Steering_Serv.write(33);
delay(90);
Steering_Serv.write(75);
delay(90);
Steering_Serv.write(33);
delay(90);
Steering_Serv.write(45);
}
if (Steering_Pos < 60 && Steering_Pos > -5) { // программная проверка позиции сервы
Steering_Pos = 60; // выравнивание для левого поворота
Steering_Serv.write(75);
delay(90);
Steering_Serv.write(33);
delay(90);
Steering_Serv.write(75);
delay(90);
Steering_Serv.write(33);
delay(90);
Steering_Serv.write(60);
}
}
}
void Steering_Right() {
if (flag_st == 0) { // если стоим, то до упора поворот
Steering_Pos = 0;
Steering_Serv.write(Steering_Pos);
Serial.println("MAX R");
}
if (flag_st == 1) {
if (Steering_Pos > 5) { // если едем, то плавно рулим
Steering_Pos -= Steering_Angle;
Serial.println(Steering_Pos);
Steering_Serv.write(Steering_Pos);
}
}
}
void Steering_Left() {
if (flag_st == 0) { // если стоим, то до упора поворот
Steering_Pos = 120;
Steering_Serv.write(Steering_Pos);
Serial.println("MAX L");
}
if (flag_st == 1) { // если едем, то плавно рулим
if (Steering_Pos < 120) {
Steering_Pos += Steering_Angle;
Serial.println(Steering_Pos);
Steering_Serv.write(Steering_Pos);
}
}
}
void setup() {
Serial.begin(38400);
pinMode(Engine, OUTPUT);
pinMode(Engine_pwm, OUTPUT);
pinMode(R, OUTPUT);
pinMode(L, OUTPUT);
digitalWrite(Engine, HIGH);
Steering_Serv.attach(8);
Steering_Forward();
}
void loop() {
char key;
if (Serial.available() > 0)
key = Serial.read();
switch (key) {
case 'W':
Engine_Forward();
break;
case 'D':
Serial.print(" Steering_Right ");
Steering_Right();
break;
case 'S':
Engine_Parking_without_brake();
break;
case 'A':
Serial.print(" Steering_Left ");
Steering_Left();
break;
case 'Q':
Engine_Speed_Down();
break;
case 'E':
Engine_Speed_Up();
break;
case 'X':
Engine_Back();
break;
case 'F':
Steering_Forward();
break;
}
}
\\
==== Разработка необходимой телеметрии ====
Так как на платформе будут установлено 3 аккумулятора (7.2V 6Ah на всю логику; 14.8V 10,8Аh двигатель , пневматика и манипулятор; 14,8V 3,4Ah резерв для двигателя,подключаемый)\\
- 5 Вольтметров с пиком в 25V \\
- 4 Амперметра ACS712-30A \\
- 10-осевой датчик от "Amperka.ru" (гироскоп,акселерометр,магнитометр,датчик давления) \\
- Датчик давления BPM180(внутри воздушного резервуара) \\
- Датчик давления и влажности BME280(внешний) \\
- 8 Датчиков температуры(двигатель,драйвер двигателя,сервопривод на рулевой,3 на аккумуляторы, 1 на бортовую эмв, 2 контрольный)\\
- 2 Датчика влажности DHT22\\
Данные будут записываться на постоянной основе в бортовую эвм, а при утере сигнала Wi-fi будут передаваться по запросу оператору с помощью NRF20L01. \\
#include
#include
#include
#include // более оптимизированная библиотека для bme280
#include // библиотека dht ,модель выбиратется константой
#include // библиотека мультиплексора, не нужно внутри кода перебирать каналы, выбирается нужный
#define DHT1PIN 8
#define DHT2PIN 9
#define DHTTYPE DHT22
Gyroscope gyro; // объекты для работы с 10 осевым датчиком от amperka
Accelerometer accel;
Compass compass;
Barometer barometer;
DHT dht1(DHT1PIN, DHTTYPE);
DHT dht2(DHT2PIN, DHTTYPE);
CD74HC4067 Multiplexor_pin(4, 5, 6, 7); // подключенные пины мультиплексора
iarduino_Pressure_BMP BPM180;
const int signal_input = A0; // входной сигнал с мультиплексора на 16 каналов
BME280 bme; // переменная для датчика давления bme280
float gx, gy, gz, ax, ay, az, mx, my, mz;
// полученные в калибровочной матрице из примера «compassCalibrateMatrix»
const double compassCalibrationBias[3] = {
524.21,
3352.214,
-1402.236
};
const double compassCalibrationMatrix[3][3] = {
{1.757, 0.04, -0.028},
{0.008, 1.767, -0.016},
{-0.018, 0.077, 1.782}
};
void setup() {
gyro.begin();
accel.begin();
compass.begin();
barometer.begin();
BPM180.begin();
compass.calibrateMatrix(compassCalibrationMatrix, compassCalibrationBias);
compass.setRange(RANGE_4_GAUSS);
bme.settings.commInterface = I2C_MODE;
bme.settings.I2CAddress = 0x76;
bme.settings.runMode = 3;
bme.settings.tStandby = 0;
bme.settings.filter = 0;
bme.settings.tempOverSample = 1;
bme.settings.pressOverSample = 1;
bme.settings.humidOverSample = 1;
}
void Scan_BPM180(){
float Pressure,Temp;
if(BPM180.read(1)) {
Pressure = BPM180.pressure;
Temp = BPM180.temperature;
Serial.print(Pressure);
Serial.print("/t");
Serial.print(Temp);
}
else{
Serial.println("HET OTBETA OT CEHCOPA");
}
}
void Scan_IMU(){
// считываем данные с акселерометра в единицах G
accel.readGXYZ(&ax, &ay, &az);
// считываем данные с гироскопа в радианах в секунду
gyro.readRadPerSecXYZ(&gx, &gy, &gz);
// считываем данные с компаса в Гауссах потому что можем
compass.readCalibrateGaussXYZ(&mx, &my, &mz);
// считываем азимут относительно оси Z
compass.readAzimut();
// вывод угловой скорости в градусах в секунду относительно оси X
Serial.print(gyro.readDegPerSecX());
Serial.print("\t");
// вывод угловой скорости в градусах в секунду относительно оси Y
Serial.print(gyro.readDegPerSecY());
Serial.print("\t");
// вывод угловой скорости в градусах в секунду относительно оси Z
Serial.print(gyro.readDegPerSecZ());
Serial.print("\t\t");
// вывод направления и величины ускорения в м/с² по оси X
Serial.print(accel.readAX());
Serial.print("\t");
// вывод направления и величины ускорения в м/с² по оси Y
Serial.print(accel.readAY());
Serial.print("\t");
// вывод направления и величины ускорения в м/с² по оси Z
Serial.print(accel.readAZ());
Serial.print("\t\t");
// выводим азимут относительно оси Z
Serial.print(compass.readAzimut());
Serial.print(" Degrees\t");
// вывод значения абсолютного давления
Serial.print(barometer.readPressureMillibars());
Serial.print("\t");
// вывод значения температуры окружающей среды
Serial.print(barometer.readTemperatureC());
Serial.print("\t");
Serial.println("");
}
void Scan_Multiplexor() {
float voltage_engine, voltage_logic, voltage_5v_line, voltage_backup_battery, voltage5;
float amper_engine, amper_logic, amper_manip, amper_pump;
float Temp_Engine, Temp_logic_battery, Temp_engine_battery, Temp_driver_engine, Temp_backup_battery, Temp_serv, Temp_Computer;
Multiplexor_pin.channel(0);
delay(1);
voltage_engine = analogRead(signal_input) / 40.92;
Multiplexor_pin.channel(1);
delay(1);
voltage_logic = analogRead(signal_input) / 40.92;
Multiplexor_pin.channel(2);
delay(1);
voltage_5v_line = analogRead(signal_input) / 40.92;
Multiplexor_pin.channel(3);
delay(1);
voltage_backup_battery = analogRead(signal_input) / 40.92;
Multiplexor_pin.channel(4);
delay(1);
voltage5 = analogRead(signal_input) / 40.92;
Multiplexor_pin.channel(5);
delay(1);
amper_engine = ((((analogRead(signal_input) / 1023) * 5000) - 2500) / 66);
Multiplexor_pin.channel(6);
delay(1);
amper_logic = ((((analogRead(signal_input) / 1023) * 5000) - 2500) / 66);
Multiplexor_pin.channel(7);
delay(1);
amper_manip = ((((analogRead(signal_input) / 1023) * 5000) - 2500) / 66);
Multiplexor_pin.channel(8);
delay(1);
amper_pump = ((((analogRead(signal_input) / 1023) * 5000) - 2500) / 66);
Multiplexor_pin.channel(9);
delay(1);
Temp_Engine = ((analogRead(signal_input) * 0.004888) - 0.5) * 100;
Multiplexor_pin.channel(10);
delay(1);
Temp_logic_battery = ((analogRead(signal_input) * 0.004888) - 0.5) * 100;
Multiplexor_pin.channel(11);
delay(1);
Temp_engine_battery = ((analogRead(signal_input) * 0.004888) - 0.5) * 100;
Multiplexor_pin.channel(12);
delay(1);
Temp_driver_engine = ((analogRead(signal_input) * 0.004888) - 0.5) * 100;
Multiplexor_pin.channel(13);
delay(1);
Temp_backup_battery = ((analogRead(signal_input) * 0.004888) - 0.5) * 100;
Multiplexor_pin.channel(14);
delay(1);
Temp_serv = ((analogRead(signal_input) * 0.004888) - 0.5) * 100;
Multiplexor_pin.channel(15);
delay(1);
Temp_Computer = ((analogRead(signal_input) * 0.004888) - 0.5) * 100;
}
void Scan_DHT22() {
float h1, h2, t1, t2;
h1 = dht1.readHumidity(); // влажность с датчика dht22
t1 = dht1.readTemperature(); // температура с датчика dht22
h2 = dht2.readHumidity();
t2 = dht2.readTemperature();
}
void scan_Bme() {
int Temp, Press, Humidity;
Temp = bme.readTempC(); // температура с датчика bme280
Press = bme.readFloatPressure() / 133.3; // давление с датчика bme280
Humidity = bme.readFloatHumidity(); // влажность с датчика bme280
}
void loop() {
//P.S. оптимизация и вывод будут позже(после проверки на реальном стенде)
// ссылки на библиотеки будут после проверки работоспособности скетча на деле :D
}
\\
==== Разработка,конструирование и отладка системы пневматических стоек ====
Данная система разрабатывается для стабилизации положения робота при работе с манипулятором.\\
В данной системе используются следующие элементы:
|Наименование|Количество|
|Пневматический цилиндр CDJ2B16 | 4 |
|Штуцер М5х0,8| 8 |
|Коннектор на Х6| 2 |
|Шланг 3/5мм| ? |
|Биполярный электромагнитный клапан| 1 |
|Электромагнитный клапан| 2 |
|Воздушный насос| 1 |
|Механический обратный клапан| 1 |
#define Pump_Engine 24 // насос
#define Valve_Out 25 // впускной клапан обратный ход
#define Valve_Input 26 // выпускной клапан выдвижение
#define Valve_way_1 27 // направление на выдвижение поршня
#define Valve_way_2 28 // направление на задвиг поршня
void upload_air(){
digitalWrite(Valve_way_1,LOW); // открытие направления на выдвижение
digitalWrite(Valve_way_2,HIGH);// открытие направления на выдвижение
digitalWrite(Valve_Input,HIGH);// закр вып кл на выд,поршнешь в рабочем положении
digitalWrite(Valve_Output,LOW);
digitalWrite(Pump_Engine,HIGH); // включение воздушного насоса
}
void Pump_Out_air(){
digitalWrite(Valve_way_1,HIGH); // открытие направления на выдвижение
digitalWrite(Valve_way_2,LOW); // открытие направления на выдвижение
digitalWrite(Valve_Input,LOW);
digitalWrite(Valve_Output,HIGH); // зак вып кл на задв,поршень в начальном положении
digitalWrite(Pump_Engine,HIGH); // включение воздушного насоса
}
void Off_pump_system(){
digitalWrite(Valve_way_1,LOW);
digitalWrite(Valve_way_2,LOW);
digitalWrite(Valve_Input,LOW);
digitalWrite(Valve_Output,LOW);
digitalWrite(Pump_Engine,LOW);
}
void setup() {
Serial.begin(38400);
pinMode(Valve_Out, OUTPUT);
pinMode(Valve_Input, OUTPUT);
pinMode(Valve_way_1, OUTPUT);
pinMode(Valve_way_2, OUTPUT);
}
/* требуется прописать гистерезис для включения двигателя( по данным датчика давления внутри резервуара )
* для поддержания нужного уровня давления в рабочем положении,а так же команды по которым
* будут производиться какие-либо манипуляции с системой пневмо-стоек
* Требуется нарисовать схему работы данной системы для более подробного описания
*/
void loop() {
}
\\
==== Разработка и отладка управления при помощи модуля приёмопередатчика NRF24L01 ====
==== Отработка получения телеметрии по запросу при помощи модуля приёмопередатчика NRF24L01 ====
==== Разработка и конструирования собственного 6-осевого манипулятора ====
==== Сборка всех элементов ====