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);
         }