Diff: main.cpp

Revision:

1:60882db03b0f

Parent:

0:37f0c1e8fa66

Child:

2:f0a9ecb4d049

--- a/main.cpp	Tue Sep 08 13:38:10 2015 +0000
+++ b/main.cpp	Wed Sep 09 09:58:41 2015 +0000
@@ -35,7 +35,8 @@
 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_R = 2.5, I_R = 3.7, D_R = 0;
+//float P_R = 2.5, I_R = 3.7, D_R = 0;
+float P_R = 6, I_R = 0, 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 = 3.2, IY = 0, DY = 0;
@@ -63,6 +64,10 @@
         mbed_reset();
     if (command == '-')
         debug = !debug;
+    if (command == 'w')
+        P_R += 0.1;
+    if (command == 's')
+        P_R -= 0.1;
         
     pc.putc(command);
     LEDs.tilt(2);
@@ -152,19 +157,20 @@
         } else {
             for(int i=0;i<4;i++) // for security reason, set every motor to zero speed
                 ESC[i] = 0;
+            debug = true;
         }
         
         if (debug) {
-            //pc.printf("$STATE,%d,%.3f\r\n", armed, IMU.dt);
-            pc.printf("$RC,%d,%d,%d,%d,%d,%d,%d\r\n", RC[AILERON].read(), RC[ELEVATOR].read(), RC[RUDDER].read(), RC[THROTTLE].read(), RC[CHANNEL6].read(), RC[CHANNEL7].read(), RC[CHANNEL8].read());
-            pc.printf("$GYRO,%.3f,%.3f,%.3f\r\n", IMU.mpu.Gyro[ROLL], IMU.mpu.Gyro[PITCH], IMU.mpu.Gyro[YAW]);
-            pc.printf("$ACC,%.3f,%.3f,%.3f\r\n", IMU.mpu.Acc[ROLL], IMU.mpu.Acc[PITCH], IMU.mpu.Acc[YAW]);
-            pc.printf("$ANG,%.3f,%.3f,%.3f\r\n", IMU.angle[ROLL], IMU.angle[PITCH], IMU.angle[YAW]);
+            pc.printf("$STATE,%d,%.3f\r\n", armed, IMU.dt);
+            //pc.printf("$RC,%d,%d,%d,%d,%d,%d,%d\r\n", RC[AILERON].read(), RC[ELEVATOR].read(), RC[RUDDER].read(), RC[THROTTLE].read(), RC[CHANNEL6].read(), RC[CHANNEL7].read(), RC[CHANNEL8].read());
+            //pc.printf("$GYRO,%.3f,%.3f,%.3f\r\n", IMU.mpu.Gyro[ROLL], IMU.mpu.Gyro[PITCH], IMU.mpu.Gyro[YAW]);
+            //pc.printf("$ACC,%.3f,%.3f,%.3f\r\n", IMU.mpu.Acc[ROLL], IMU.mpu.Acc[PITCH], IMU.mpu.Acc[YAW]);
+            //pc.printf("$ANG,%.3f,%.3f,%.3f\r\n", IMU.angle[ROLL], IMU.angle[PITCH], IMU.angle[YAW]);
             //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("$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]);
 
-            wait(0.4);
+            wait(0.04);
         }
 
         LEDs.rollnext();