Hallo,
ich habe einen arduino nano und würde gerne einen flyghtcontroller für meinen vtol machen
mein code ist wie folgt:
#include <Wire.h>
#include <Servo.h>
int throttle;
int aileron;
int elevator;
int rudder;
int tilt;
Servo throttleleftoutput;
Servo throttlerightoutput;
Servo tiltservooutput1;
Servo tiltservooutput2;
int throttleleft;
int throttleright;
int tiltservoleft;
int tiltservoright;
//////////////////////////////////////////////////////////////////
//Gyro Variables
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
//Acc Variables
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
float Total_angle_x, Total_angle_y;
void setup() {
pinMode (8, INPUT);
pinMode (2, INPUT);
pinMode (5, INPUT);
pinMode (6, INPUT);
pinMode (7, INPUT);
throttleleftoutput.attach(9); // throttleleft
throttlerightoutput.attach(10); // throttleright
tiltservooutput1.attach(11); // tiltservo
tiltservooutput2.attach(3); // tiltservo
//////////////////////////////////////////////////////////////////
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);
Serial.begin(9600); //Remember to set this same baud rate to the serial monitor
time = millis(); //Start counting time in milliseconds
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
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
}//end of setup void
void loop() {
aileron = pulseIn(8,HIGH);
elevator = pulseIn(7,HIGH);
throttle = pulseIn(6,HIGH);
rudder = pulseIn(5,HIGH);
tilt = pulseIn(2,HIGH);
aileron=constrain(aileron,1000,2000);
elevator=constrain(elevator,1000,2000);
throttle=constrain(throttle,1000,2000);
rudder=constrain(rudder,1000,2000);
tilt=constrain(tilt,1000,2000);
///////////////////////////////only vertical MIXING///////////////////////////////////////////////////////
aileron =aileron -1500;
elevator =elevator -1500;
throttle =throttle -1500;
rudder =rudder -1500;
throttleleft = constrain(throttle+throttle *(map(aileron,-500,500,-40,40)-
Total_angle_x)/90,-500,500)+1500;
throttleright = constrain(throttle-throttle *(map(aileron,-500,500,-40,40)-
Total_angle_x)/90,-500,500)+1500;
tiltservoleft = 1500 + (1500* (map(elevator,-500,500,-20,20)+Total_angle_y)/200)-rudder/6;
tiltservoright = 1500 + (1500* (map(elevator,-500,500,-20,20)+Total_angle_y)/200)+rudder/6;
/*
Serial.println(tiltservoleft);
Serial.println(tiltservoright);
Serial.println(throttleleft);
Serial.println(throttleright);
*/
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
throttleleftoutput.writeMicroseconds(throttleleft);
throttlerightoutput.writeMicroseconds(throttleright);
tiltservooutput1.writeMicroseconds(throttleleft);
tiltservooutput2.writeMicroseconds(throttleright);
}
den code für den mpu6050 hab ich rausgeschnitten da sonst der code zu lange war aber die Kommunikation funktioniert und das Mixing ist auch kein problem, aber bei
throttleleftoutput.writeMicroseconds(throttleleft);
throttlerightoutput.writeMicroseconds(throttleright);
tiltservooutput1.writeMicroseconds(throttleleft);
tiltservooutput2.writeMicroseconds(throttleright);
habe ich ein problem weil er mir kein servo signal ausgibt, die on time ist immer 0.5 statt 1-2
throttleleft und die anderen variablien sind aber zwischen 1000 und 2000.
Wenn ich nur z.b 1500 reinschreibe ist mein servosignal wieder bei 1,5
hat jemand eine idee?
