Diff: main.cpp

Revision:

6:d868495c1936

Parent:

5:74ca8be12359

--- a/main.cpp	Fri Sep 11 22:24:22 2015 +0000
+++ b/main.cpp	Sat Sep 19 02:23:09 2015 +0000
@@ -20,9 +20,21 @@
 #include "FXOS8700Q.h"
 #include "FXAS21000.h"
 #include "kalman.c"
+#include "arm_math.h"
 
-#define PI             3.1415926535897932384626433832795
-#define Rad2Dree       57.295779513082320876798154814105
+#define PI             3.1415926535897932384626433832795f
+#define Rad2Dree       57.295779513082320876798154814105f
+
+#define PID_ROLL_KP        0.0245f              /* Proporcional */ //0.015f
+#define PID_ROLL_KI        0.000175f              /* Integral */
+#define PID_ROLL_KD        0.0f              /* Derivative */
+
+#define PID_PITCH_KP       0.0245f              /* Proporcional */ //0.015f
+#define PID_PITCH_KI       0.000175f              /* Integral */
+#define PID_PITCH_KD       0.0f              /* Derivative */
+
+#define ROLL_SP            PI/2
+#define PITCH_SP           PI/2
 
 DigitalOut red(LED_RED);
 DigitalOut green(LED_GREEN);
@@ -54,18 +66,26 @@
 long loopStartTime;
 char i;
 char command = ' ';
+char sflag = 0;
 float high;
 
-float ESC1 = 0.0006f;
-float ESC2 = 0.0006f;
-float ESC3 = 0.0006f;
-float ESC4 = 0.0006f;
+float ESC1 = 0.0006f;   //pitch up
+float ESC2 = 0.0006f;   //roll  up
+float ESC3 = 0.0006f;   //roll  down
+float ESC4 = 0.0006f;   //pitch down
+
+float roll_error;
+float pitch_error;
+
+float roll;
+float pitch;
 
 void bt_callback(void);
 void esc_start(void);
 void esc_stop(void);
+float esc_control(float current, float pid, float rate);
 
-int main()
+int main(void)
 {
     float gyro_data[3];
     MotionSensorDataUnits adata;
@@ -75,6 +95,22 @@
     
     printf("\r\nStarting\r\n\r\n");
     
+    arm_pid_instance_f32 RPID;
+    arm_pid_instance_f32 PPID;
+  
+      //Pitch
+    PPID.Kp = PID_PITCH_KP/1000.0f;        /* Proporcional */
+    PPID.Ki = PID_PITCH_KI/1000.0f;        /* Integral */
+    PPID.Kd = PID_PITCH_KD/1000.0f;        /* Derivative */
+    
+    //Roll
+    RPID.Kp = PID_ROLL_KP/1000.0f;        /* Proporcional */
+    RPID.Ki = PID_ROLL_KI/1000.0f;        /* Integral */
+    RPID.Kd = PID_ROLL_KD/1000.0f;        /* Derivative */
+    
+    arm_pid_init_f32(&RPID, 1);
+    arm_pid_init_f32(&PPID, 1);
+    
     red = 0; green= 1;
     GlobalTime.start();
     
@@ -118,12 +154,42 @@
         angle[0] = kalman_get_angle(&filter_pitch);
         angle[1] = kalman_get_angle(&filter_roll);
         
+        if (angle[0]>PI) angle[0] = PI;
+        else if (angle[0]<0) angle[0] = 0.0f;
+        else angle[0] += 0.0f;
+        
+        if (angle[1]>PI) angle[1] = PI;
+        else if (angle[1]<0) angle[1] = 0.0f;
+        else angle[1] += 0.0f;
+        
+        pitch_error = angle[0] - PITCH_SP;
+        roll_error = angle[1] - ROLL_SP;
+        
+        pitch = arm_pid_f32(&PPID, pitch_error);
+        roll = arm_pid_f32(&RPID, roll_error);
+        
         timer = ProgramTimer.read_us();
+        
+        if (!sflag){
+            
+            ESC1 = esc_control(ESC1,-pitch,0.0f);
+            ESC2 = esc_control(ESC2,-roll,0.0f);
+            ESC3 = esc_control(ESC3,roll,0.0f);
+            ESC4 = esc_control(ESC4,pitch,0.0f);
+            
+            M1.pulsewidth(ESC1);
+            M2.pulsewidth(ESC2); 
+            M3.pulsewidth(ESC3); 
+            M4.pulsewidth(ESC4);
+        }
     
         //printf("FXOS8700 Acc:   X:%6.3f Y:%6.3f Z:%6.3f\r\n", adata.x, adata.y, adata.z);
         //printf("FXOS8700 Mag:   X:%6.2f Y:%6.2f Z:%6.2f\r\n", mdata.x, mdata.y, mdata.z);
         //printf("FXAS21000 Gyro: X:%6.2f Y:%6.2f Z:%6.2f\r\n", gyro_data[0], gyro_data[1], gyro_data[2]);
-        printf("Roll Angle X: %.6f   Pitch Angle Y: %.6f \r\n", Rad2Dree * angle[1], Rad2Dree * angle[0]);
+        bt.printf("Roll Angle X: %.6f   Pitch Angle Y: %.6f \r\n", Rad2Dree * angle[1], Rad2Dree * angle[0]);
+        bt.printf("roll = %.6f     pitch = %.6f \r\n",roll,pitch);
+        bt.printf("ESC1 = %.6f     ESC4 = %.6f \r\n", ESC1,ESC4);
+        bt.printf("ESC2 = %.6f     ESC3 = %.6f \r\n", ESC2,ESC3);
         //printf("dist = %.2f \r\n",dist);
         
         wait(0.02f);
@@ -135,6 +201,7 @@
     command = bt.getc();
     //if (command == 'z') bt.printf("start\n\r");
     if (command == 'x') {
+        sflag = 1;
         bt.printf("stop\n\r"); 
         esc_stop();
     }
@@ -144,7 +211,14 @@
     else bt.printf("%c\n\r", command);
 }
 
+float esc_control(float current, float pid, float rate){
+    if ((current + pid + rate)>0.0014f) return 0.0014f;
+    else if ((current + pid + rate)<0.001f) return 0.001f; 
+    else return current + pid + rate;
+}
+
 void esc_start(void) {
+    sflag = 0;
     ESC1 = 0.0006f;
     ESC2 = 0.0006f;
     ESC3 = 0.0006f;
@@ -163,7 +237,7 @@
 }
 
 void esc_stop(void){
-    red = 0; green= 1;
+    red = 0; green= 1; sflag = 1;
     while ((ESC1 > 0.0006f)||(ESC2 > 0.0006f)||(ESC3 > 0.0006f)||(ESC4 > 0.0006f)){
         ESC1 -= 0.0001f;
         ESC2 -= 0.0001f;
@@ -173,7 +247,7 @@
         M2.pulsewidth(ESC2);
         M3.pulsewidth(ESC3);
         M4.pulsewidth(ESC4);
-        wait_ms(1000);   
+        wait_ms(250);   
     }
     if (ESC1 < 0.0006f) ESC1 = 0.0006f;
     if (ESC2 < 0.0006f) ESC2 = 0.0006f;