Matthias Grob
/
FlyBed2
My fully self designed first stable working Quadrocopter Software.
Diff: main.cpp
- Revision:
- 7:ac2895479e34
- Parent:
- 5:06e978fd147a
- Child:
- 8:e79c7939d6de
--- a/main.cpp Sun Jul 06 09:25:09 2014 +0000 +++ b/main.cpp Sat Jul 12 12:21:11 2014 +0000 @@ -10,7 +10,7 @@ #define PPM_FREQU 495 // Hz Frequency of PPM Signal for ESCs (maximum <500Hz) #define INTEGRAL_MAX 300 // maximal output offset that can result from integrating errors #define RC_SENSITIVITY 30 // maximal angle from horizontal that the PID is aming for -#define YAWSPEED 0.2 // maximal speed of yaw rotation in degree per Rate +#define YAWSPEED 1.0 // maximal speed of yaw rotation in degree per Rate #define AILERON 0 // RC #define ELEVATOR 1 #define RUDDER 2 @@ -22,16 +22,19 @@ #define PITCH 1 #define YAW 2 +//#define CONSTRAIN(VAL,LIMIT) ((VAL)<(-LIMIT)?(-LIMIT):((VAL)>(LIMIT)?(LIMIT):(VAL))) + bool armed = false; // is for security (when false no motor rotates any more) bool debug = true; // shows if we want output for the computer bool RC_present = false; // shows if an RC is present -float P = 0, I = 0, D = 0; +float P_R = 4, I_R = 11, D_R = 0; +float P_A = 1.865, I_A = 1.765, D_A = 0; //float P = 13.16, I = 8, D = 2.73; // PID values -float PY = 0, IY = 0, DY = 0; +float PY = 3.2, IY = 0, DY = 0; //float PY = 5.37, IY = 0, DY = 3; // PID values for Yaw float RC_angle[] = {0,0,0}; // Angle of the RC Sticks, to steer the QC float Motor_speed[4] = {0,0,0,0}; // Mixed Motorspeeds, ready to send -float * command_pointer = &D; +//float * command_pointer = &D; // TODO: pointer to varible that's going to be changed by UART command /*float max[3] = {-10000,-10000,-10000}; float min[3] = {10000,10000,10000};*/ @@ -41,7 +44,8 @@ //PC pc(p9, p10, 115200); // Bluetooth IMU_10DOF IMU(p28, p27); RC_Channel RC[] = {RC_Channel(p8,1), RC_Channel(p7,2), RC_Channel(p5,4), RC_Channel(p6,3), RC_Channel(p15,2), RC_Channel(p16,4), RC_Channel(p17,3)}; // no p19/p20 ! -PID Controller[] = {PID(P, I, D, INTEGRAL_MAX), PID(P, I, D, INTEGRAL_MAX), PID(PY, IY, DY, INTEGRAL_MAX)}; // 0:X:Roll 1:Y:Pitch 2:Z:Yaw +PID Controller_Rate[] = {PID(P_R, I_R, D_R, INTEGRAL_MAX), PID(P_R, I_R, D_R, INTEGRAL_MAX), PID(PY, IY, DY, INTEGRAL_MAX)}; // 0:X:Roll 1:Y:Pitch 2:Z:Yaw +PID Controller_Angle[] = {PID(P_A, I_A, D_A, INTEGRAL_MAX), PID(P_A, I_A, D_A, INTEGRAL_MAX), PID(0, 0, 0, INTEGRAL_MAX)}; Servo ESC[] = {Servo(p21,PPM_FREQU), Servo(p22,PPM_FREQU), Servo(p23,PPM_FREQU), Servo(p24,PPM_FREQU)}; // use any DigitalOit Pin extern "C" void mbed_reset(); @@ -87,12 +91,14 @@ // Setting PID Values from auxiliary RC channels if (RC[CHANNEL8].read() > 0 && RC[CHANNEL8].read() < 1000) - D = 0 + (((float)RC[CHANNEL8].read()) * 15 / 1000); - /*if (RC[CHANNEL7].read() > 0 && RC[CHANNEL7].read() < 1000) - D = 2 + (((float)RC[CHANNEL7].read()) * 4 / 1000);*/ + P_R = 0 + (((float)RC[CHANNEL8].read()) * 5 / 1000); + if (RC[CHANNEL7].read() > 0 && RC[CHANNEL7].read() < 1000) + I_R = 0 + (((float)RC[CHANNEL7].read()) * 12 / 1000); + for(int i=0;i<3;i++) + Controller_Angle[i].setPID(P_A,I_A,D_A); for(int i=0;i<2;i++) - Controller[i].setPID(P,I,D); // give the new PID values to roll and pitch controller - Controller[YAW].setPID(PY,IY,DY); + Controller_Rate[i].setPID(P_R,I_R,D_R); // give the new PID values to roll and pitch controller + Controller_Rate[YAW].setPID(PY,IY,DY); // RC Angle ROLL-PITCH-Part for(int i=0;i<2;i++) { // calculate new angle we want the QC to have @@ -103,22 +109,48 @@ } // RC Angle YAW-Part + if (RC_present && RC[THROTTLE].read() > 20) + RC_angle[YAW] -= (RC[RUDDER].read()-500)*YAWSPEED/500; + float RC_yaw_adding; // temporary variable to take the desired yaw adjustment if (RC_present && RC[THROTTLE].read() > 20) RC_yaw_adding = -(RC[RUDDER].read()-500)*YAWSPEED/500; else RC_yaw_adding = 0; - + + RC_angle[YAW] = RC_angle[YAW] + RC_yaw_adding < -180 ? RC_angle[YAW] + 360 + RC_yaw_adding : RC_angle[YAW] + RC_yaw_adding; + RC_angle[YAW] = RC_angle[YAW] + RC_yaw_adding > 180 ? RC_angle[YAW] - 360 + RC_yaw_adding : RC_angle[YAW] + RC_yaw_adding; + + /*float RC_yaw_adding; // temporary variable to take the desired yaw adjustment + if (RC_present && RC[THROTTLE].read() > 20) + RC_yaw_adding = -(RC[RUDDER].read()-500)*YAWSPEED/500; + else + RC_yaw_adding = 0; + while(RC_angle[YAW] + RC_yaw_adding < -180 || RC_angle[YAW] + RC_yaw_adding > 180) { // make shure it's in the cycle -180 to 180 if(RC_angle[YAW] + RC_yaw_adding < -180) RC_yaw_adding += 360; if(RC_angle[YAW] + RC_yaw_adding > 180) RC_yaw_adding -= 360; } - RC_angle[YAW] += RC_yaw_adding; // the yaw angle is integrated from stick input + RC_angle[YAW] += RC_yaw_adding; // the yaw angle is integrated from stick input*/ // Controlling for(int i=0;i<2;i++) { + Controller_Rate[i].setIntegrate(armed); // only integrate in controller when armed, so the value is not totally odd from not flying + Controller_Rate[i].compute((RC[i].read()-500.0)*100.0/500.0, IMU.Sensor.data_gyro[i]); // give the controller the actual gyro values and get his advice to correct + } + Controller_Rate[2].setIntegrate(armed); // only integrate in controller when armed, so the value is not totally odd from not flying + Controller_Rate[2].compute(-(RC[2].read()-500.0)*100.0/500.0, IMU.Sensor.data_gyro[2]); // give the controller the actual gyro values and get his advice to correct + /*for(int i=0;i<3;i++) { + Controller_Angle[i].setIntegrate(armed); // only integrate in controller when armed, so the value is not totally odd from not flying + Controller_Angle[i].compute(RC_angle[i], IMU.angle[i]); // give the controller the actual gyro values and get his advice to correct + Controller_Rate[i].setIntegrate(armed); // only integrate in controller when armed, so the value is not totally odd from not flying + Controller_Rate[i].compute(-Controller_Angle[i].Value, IMU.Sensor.data_gyro[i]); // give the controller the actual gyro values and get his advice to correct + }*/ + + // OLD Controlling + /*for(int i=0;i<2;i++) { Controller[i].setIntegrate(armed); // only integrate in controller when armed, so the value is not totally odd from not flying Controller[i].compute(RC_angle[i], IMU.angle[i], IMU.Sensor.data_gyro[i]); // give the controller the actual gyro values for D and angle for P,I and get his advice to correct } @@ -129,25 +161,30 @@ else Controller[YAW].compute(RC_angle[YAW] + 360, IMU.angle[YAW], IMU.Sensor.data_gyro[YAW]); else - Controller[YAW].compute(RC_angle[YAW], IMU.angle[YAW], IMU.Sensor.data_gyro[YAW]); + Controller[YAW].compute(RC_angle[YAW], IMU.angle[YAW], IMU.Sensor.data_gyro[YAW]);*/ // Mixing - Motor_speed[2] = RC[THROTTLE].read() + Controller[PITCH].Value; - Motor_speed[0] = RC[THROTTLE].read() - Controller[PITCH].Value; - Motor_speed[1] = RC[THROTTLE].read() + Controller[ROLL].Value; - Motor_speed[3] = RC[THROTTLE].read() - Controller[ROLL].Value; + Motor_speed[2] = RC[THROTTLE].read() + Controller_Rate[PITCH].Value; + Motor_speed[0] = RC[THROTTLE].read() - Controller_Rate[PITCH].Value; + Motor_speed[1] = RC[THROTTLE].read() + Controller_Rate[ROLL].Value; + Motor_speed[3] = RC[THROTTLE].read() - Controller_Rate[ROLL].Value; - Motor_speed[0] -= Controller[YAW].Value; - Motor_speed[2] -= Controller[YAW].Value; - Motor_speed[3] += Controller[YAW].Value; - Motor_speed[1] += Controller[YAW].Value; + Motor_speed[0] -= Controller_Rate[YAW].Value; + Motor_speed[2] -= Controller_Rate[YAW].Value; + Motor_speed[3] += Controller_Rate[YAW].Value; + Motor_speed[1] += Controller_Rate[YAW].Value; if (armed) // for SECURITY! { - ESC[0] = (int)Motor_speed[0]; - ESC[2] = (int)Motor_speed[2]; - //for(int i=0;i<4;i++) // Set new motorspeeds - //ESC[i] = (int)Motor_speed[i]; + debug = false; + // PITCH + //ESC[0] = (int)Motor_speed[0]>50 ? (int)Motor_speed[0] : 50; + //ESC[2] = (int)Motor_speed[2]>50 ? (int)Motor_speed[2] : 50; + // ROLL + //ESC[1] = (int)Motor_speed[1]>50 ? (int)Motor_speed[1] : 50; + //ESC[3] = (int)Motor_speed[3]>50 ? (int)Motor_speed[3] : 50; + for(int i=0;i<4;i++) // Set new motorspeeds + ESC[i] = (int)Motor_speed[i]>50 ? (int)Motor_speed[i] : 50; } else { for(int i=0;i<4;i++) // for security reason, set every motor to zero speed @@ -165,7 +202,8 @@ pc.printf("$GYRO,%.3f,%.3f,%.3f\r\n", IMU.Sensor.data_gyro[ROLL], IMU.Sensor.data_gyro[PITCH], IMU.Sensor.data_gyro[YAW]); pc.printf("$ACC,%.3f,%.3f,%.3f\r\n", IMU.Sensor.data_acc[ROLL], IMU.Sensor.data_acc[PITCH], IMU.Sensor.data_acc[YAW]); pc.printf("$ANG,%.3f,%.3f,%.3f\r\n", IMU.angle[ROLL], IMU.angle[PITCH], IMU.angle[YAW]); - pc.printf("$CONT,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f\r\n", Controller[ROLL].Value, Controller[PITCH].Value, Controller[YAW].Value, P, I, D); + pc.printf("$RCANG,%.3f,%.3f,%.3f\r\n", RC_angle[ROLL], RC_angle[PITCH], RC_angle[YAW]); + pc.printf("$CONT,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f\r\n", Controller_Rate[ROLL].Value, Controller_Rate[PITCH].Value, Controller_Rate[YAW].Value, P_R, I_R, D_R); pc.printf("$MOT,%d,%d,%d,%d\r\n", (int)Motor_speed[0], (int)Motor_speed[1], (int)Motor_speed[2], (int)Motor_speed[3]); /*for (int i=0;i<3;i++) { min[i] = IMU.Sensor.data_gyro[i]<min[i] ? IMU.Sensor.data_gyro[i] : min[i]; @@ -174,6 +212,24 @@ //pc.printf("%.5f\r\n", IMU.dt); //pc.printf("%d,%d,%d,%d,%d,%d,%d,%d,%d\r\n", IMU.Sensor.raw_gyro[ROLL], IMU.Sensor.raw_gyro[PITCH], IMU.Sensor.raw_gyro[YAW], min[0], min[1], min[2], max[0], max[1], max[2]); //pc.printf("%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f\r\n", IMU.Sensor.data_gyro[ROLL], IMU.Sensor.data_gyro[PITCH], IMU.Sensor.data_gyro[YAW], min[0], min[1], min[2], max[0], max[1], max[2]); + //pc.printf("%.3f,%.3f,%.3f\r\n", IMU.Sensor.data_gyro[ROLL], IMU.Sensor.data_gyro[PITCH], IMU.Sensor.data_gyro[YAW]); + + // SimPlot output + /*int16_t sendvalue[4]; //Buffer to hold the packet, note it is 16bit data type + sendvalue[0] = (int16_t) IMU.Sensor.data_gyro[ROLL]; //Channel 1 data. 16bit signed integer + sendvalue[1] = (int16_t) IMU.Sensor.data_gyro[PITCH]; //Channel 2 data. 16bit signed integer + sendvalue[2] = (int16_t) IMU.Sensor.data_gyro[YAW]; //Channel 3 data. 16bit signed integer + sendvalue[3] = (int16_t) 0; //Channel 4 data. 16bit signed integer + + pc.putc(0xAB); // header + pc.putc(0xCD); + pc.putc(0x08); // size LSB + pc.putc(0x00); // size MSB + for(int i=0; i<4; i++) { + pc.putc((char)sendvalue[i]); // LSB + pc.putc((char)(sendvalue[i] >> 8)); // MSB + }*/ + wait(0.04); }