Здравейте първо да се представя казвам се Светослав Вичев от Варна по принцип не се занимавам с роботика и микроконтролери но имам извесни познания в областа. Хобито ми е авиомоделизъм. От известно време на сам се опитвам да подкарам един таймер за кордови акробати на базата на Ардуино и не ми се получават нещата.Скеча го намерих в руски форум но така и не можахме да се разберем с човека как се настроива,защот не го е писал той,а някой му го е дал.Става въпрос за това че не мога да променя времето за полет. В този си вид ''лети'' около 40 секунди;
//*************************************************************************************************************
//
// Тагильцев Геннадий aka Marks
// версия v3
// изменения v2:
// - добавлена функция boolean Engine_Proc (int, int)
// параметры
// - требуемое положение газа
// - условная задержка (в циклах программы)
// возвращает
// true - при достижении требуемого значения
// изменения v3
// - убрал процедуру таймера, вся обработка в одном цикле.
// изменения v4
// - расчет углов только на основании данных акселерометра
//
//**************************************************************************************************************
#include <Servo.h>
//#include <SoftwareSerial.h>
#include <Wire.h>
#include "Kalman.h"
#define Landing_Gear // если ретрактов нет, ставим коммент на эту стоку строку
#define Button_Pin 12 // порт к которому подключена кнопка старт/стоп или перемычка
#define Port_Motor_ESC 2 // порт к которому подключен ESC
#define Motor_ESC_Max_THR 2200 // максимальные обороты двигателя
#define Motor_ESC_Normall_THR 1800 // номинальные (полетные) обороты
#define Motor_ESC_Landing_THR 1500 // посадочный режим
#define Motor_ESC_Min_THR 800 // двигатель стоп
#define Time_Of_Calibrate 2 // время между Макс газ и мин газ при необходимости калибровке регулятора
#define Takeoff 5 // время отведенное на взлет и уборку шасси, в течении этого времени процедура старта может быть отменена (т.е. время до уборки шасси) - 10 секунд
#define GearUp 5 // время время после старта, начало уборки шасси - 5 секунд
#define InFly 25 // время полета в секундах с момента завершения процедуры старта - сейчас стоит 60 секунд
#define Landing 10 // время отводимое на посадку, в течении этого времени будут выпущены шасси и снижены обороты мотора до посадочных - 60 секунд
#define delay_g 100 // коэффициент торможения ))
#ifdef Landing_Gear
#define Port_Left_Wheel 6 // порт к которому подключена серва ретракта левого шасси
#define Port_Right_Wheel 7 // порт к которому подключена серва ретракта правого шасси
#define Left_Wheel_Down 2000
#define Left_Wheel_Up 1000
#define Right_Wheel_Down 1000
#define Right_Wheel_Up 2000
#define Up 1
#define Down 0
#endif
Servo Motor_ESC; // объект Servo для регулятора мотора
#ifdef Landing_Gear
Servo Left_Wheel; // объект Servo для ретракта левого шасси
Servo Right_Wheel; // объект Servo для ретракта правого шасси
#endif
// SoftwareSerial softSerial(2, 3); // RX, TX>
//**************************************************************************************
// переменные определяющие ветвление программы
// проще говоря, состояние модели
//**************************************************************************************
boolean Abort = false; // процедура старта может быть прервана повторным нажатием кнопки
//************************************************************************************
// блок объявления переменных для процедуры кальман
//************************************************************************************
Kalman kalmanX;
Kalman kalmanY;
uint8_t IMUAddress = 0x68;
/* IMU Data */
int16_t accX;
int16_t accY;
int16_t accZ;
int16_t tempRaw;
int16_t gyroX;
int16_t gyroY;
int16_t gyroZ;
double accXangle; // Angle calculate using the accelerometer
double accYangle;
double temp;
double gyroXangle = 180; // Angle calculate using the gyro
double gyroYangle = 180;
double compAngleX = 180; // Calculate the angle using a Kalman filter
double compAngleY = 180;
double kalAngleX; // Calculate the angle using a Kalman filter
double kalAngleY;
uint32_t timer;
//************************************************************************************
//
//************************************************************************************
int Stack [] {
Motor_ESC_Min_THR,
Motor_ESC_Landing_THR,
Motor_ESC_Normall_THR,
Motor_ESC_Max_THR,
Time_Of_Calibrate,
Takeoff,
GearUp,
InFly,
Landing
};
//************************************************************************************
// временные переменные
//************************************************************************************
unsigned long Current_Time; // текущее время, мало ли где то пригодится
unsigned long Start_Time; // время старта по таймеру (т.е. момент нажатия на кнопку)
unsigned long Takeoff_Time; // время начала полета, шасси уже убраны
unsigned long GearUp_Time; // время начала уборки шасси (т.е. с момента нажатия на кнопку)
unsigned long Fly_Time; // время завершения полета
unsigned long Landing_Time; // время останова двигателя
//*************************************************************************************
//*************************************************************************************
// эти переменные нужны что бы не задаваться вопросом "А как у нас работают сервы"
//*************************************************************************************
int Step_Left;
int Step_Right;
//*************************************************************************************
void setup() {
pinMode(Button_Pin, INPUT); // привязали кнопку старт к порту
digitalWrite(Button_Pin, HIGH); // подтянули кнопку к +5V
Motor_ESC.attach(Port_Motor_ESC,Motor_ESC_Min_THR,Motor_ESC_Max_THR); // привязали регулятор к каналу
//**********************************************************************************
// если при включении нажата кнопка
// программа войдет в режим настройки
//**********************************************************************************
if (digitalRead(Button_Pin) == LOW) Prog_Mode();
while (digitalRead(Button_Pin) == LOW) {
}
Motor_ESC.writeMicroseconds(Motor_ESC_Min_THR);
delay (300);
//**********************************************************************************
//************************************************************************************
int i = Left_Wheel_Down-Left_Wheel_Up;
if (i>0) {
Step_Left = -1;
}
else {
Step_Left = 1;
}
i = Right_Wheel_Down-Right_Wheel_Up;
if (i>0) {
Step_Right = -1;
}
else {
Step_Right = 1;
}
/*
при подключении питания сервы встанут в крайнее положение, соответствующее Down
*/
#ifdef Landing_Gear
Left_Wheel.attach(Port_Left_Wheel);
delay(500);
Right_Wheel.attach(Port_Right_Wheel);
delay(500);
while (!Gear_Proc(Down));
#endif
//**********************************************************************************
// вышли из режима программирования
//*********************************************************************************
while (digitalRead(Button_Pin) == HIGH) {
}
delay (500);
Start_Time = millis();
Takeoff_Time = Start_Time + Stack [5]*1000;
GearUp_Time = Takeoff_Time + Stack [6]*1000;
Fly_Time = GearUp_Time + Stack [7]*1000;
Landing_Time = Fly_Time + Stack [8]*1000;
//*********************************************************************************
//**********************************************************************************
// блок setup для kalman
//**********************************************************************************
Serial.begin(115200);
Wire.begin();
i2cWrite(0x6B,0x00); // Disable sleep mode
if(i2cRead(0x75,1)[0] != 0x68) { // Read "WHO_AM_I" register
Serial.print(F("MPU-6050 with address 0x"));
Serial.print(IMUAddress,HEX);
Serial.println(F(" is not connected"));
while(1);
}
kalmanX.setAngle(180); // Set starting angle
kalmanY.setAngle(180);
timer = micros();
// **********************************************************************************
}
void loop() {
while (millis() < Takeoff_Time) Engine_Proc(Stack[2],10);
while (millis() < GearUp_Time) Gear_Proc(Up);
while (millis() < Fly_Time) {
int currentTHR = Motor_ESC.readMicroseconds();
Angel();
if (accXangle>190 && accXangle<350) {
if (currentTHR != Stack[3]) {
Motor_ESC.writeMicroseconds(Stack[3]);
}
}
else if (accXangle<170 && accXangle>10) {
if (currentTHR != Stack[1]) {
Motor_ESC.writeMicroseconds(Stack[1]);
}
}
else {
Motor_ESC.writeMicroseconds(Stack[2]);
}
}
while(millis() < Landing_Time) {
if (Engine_Proc(Stack[1],10)) Gear_Proc(Down);
}
while (millis() >= Landing_Time) Engine_Proc(Stack[0],10) ;
}
// вошли в режим программирования
// пока тут только калибровка регулятора
//*************************************************************************************
void Prog_Mode () {
Motor_ESC.writeMicroseconds(Stack[3]);
delay (Stack [4]*1000); // немного ждем и газ в ноль
Motor_ESC.writeMicroseconds(Stack[0]);
delay (500);
}
//*************************************************************************************
// процедура уборки/выпуска шасси
// результат
// - TRUE - завершено
// - FALSE - не завершена
// параметр - int -
// - 0 - выпускам
// - 1 - убираем
//*************************************************************************************
boolean Gear_Proc(int l) {
static int i = delay_g;
int j = Left_Wheel.readMicroseconds();
int k = Right_Wheel.readMicroseconds();
if (l == 0) {
// шасси выпускаем
if (j == Left_Wheel_Down && k == Right_Wheel_Down) return true;
if (i==0) {
if (j > Left_Wheel_Down) j--;
else j++;
Left_Wheel.writeMicroseconds(j);
}
if (i==0) {
if (k > Right_Wheel_Down) k--;
else k++;
Right_Wheel.writeMicroseconds(k);
}
if (!i--) i=delay_g;
}
else {
// шасси выпускаем
if (j == Left_Wheel_Up && k == Right_Wheel_Up) return true;
if (i==0) {
if (j > Left_Wheel_Up) j--;
else j++;
Left_Wheel.writeMicroseconds(j);
}
if (i==0) {
if (k > Right_Wheel_Up) k--;
else k++;
Right_Wheel.writeMicroseconds(k);
}
if (!i--) i=delay_g;
}
return false;
}
//*************************************************************************************
// Процедура работы с регуляторм
// возвращает true когда результат достигнут
// первый параметр - int - значение до которого надо изменить газ
// второй параметр - int - задержка
//*************************************************************************************
boolean Engine_Proc(int j1, int i1) {
static int i = 0;
int j = Motor_ESC.readMicroseconds();
if (j==j1) return true;
if (i == i1 && j<j1) {
j++;
Motor_ESC.writeMicroseconds(j);
}
if (i == i1 && j>j1) {
j--;
Motor_ESC.writeMicroseconds(j);
}
if (!i--) i=i1;
if (digitalRead(Button_Pin) == LOW) Abort = true ;
return false;
}
//***************************************************************************************
// кальман для гироскопа
// считает углы ))
//***************************************************************************************
void Angel() {
uint8_t* data = i2cRead(0x3B,14);
accX = ((data[0] <<
| data[1]);
accY = ((data[2] <<
| data[3]);
accZ = ((data[4] <<
| data[5]);
/* Calculate the angls based on the different sensors and algorithm */
accYangle = (atan2(accX,accZ)+PI)*RAD_TO_DEG;
accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;
delay (1);
}
void i2cWrite(uint8_t registerAddress, uint8_t data){
Wire.beginTransmission(IMUAddress);
Wire.write(registerAddress);
Wire.write(data);
Wire.endTransmission(); // Send stop
}
uint8_t* i2cRead(uint8_t registerAddress, uint8_t nbytes) {
uint8_t data[nbytes];
Wire.beginTransmission(IMUAddress);
Wire.write(registerAddress);
Wire.endTransmission(false); // Don't release the bus
Wire.requestFrom(IMUAddress, nbytes); // Send a repeated start and then release the bus after reading
for(uint8_t i = 0; i < nbytes; i++)
data
= Wire.read();
return data;
}
Не знам на кой ред и какво да променя за да го накарам да ''лети'' 5-6 минути колкото ми е необходимо.
Ще съм Ви много благодарен ако някой може да ми помогне. Благодаря.
Последни публикации
