Diff: main.cpp

Revision:

23:550b68587eb5

Parent:

22:5e7356dc1d71

Child:

24:2c7787fc4fde

--- a/main.cpp	Wed Nov 27 19:23:04 2019 +0000
+++ b/main.cpp	Fri Dec 06 00:49:17 2019 +0000
@@ -39,18 +39,18 @@
 /* Includes */
 #include "mbed.h"
 #include "XNucleoIKS01A2.h"
+#include "PID.h"
 #include <iostream>
 #include <cmath>
 
-const float MAX_SPEED = 0.0003f;
-const float MIN_SPEED = 0.024095f;
-//const float MIN_SPEED = 0.5f;
+const float MAX_SPEED = 0.00075f;
+const float MIN_SPEED = 0.01000f;
 
 
-const float MAX_DEGREES = 10.0f;
+const float MAX_DEGREES = 3.0f;
 const float MIN_DEGREES = 0.0f;
 
-
+float Acc_angle_error = 0;
 
 float clamp(float degrees)
 {
@@ -81,7 +81,9 @@
 static LSM303AGRAccSensor *accelerometer = mems_expansion_board->accelerometer;
 
 
-
+float unnormalize(float t){
+    return (MIN_SPEED - (t * ( MIN_SPEED - MAX_SPEED))); 
+}
 
 PwmOut M1_step(D3);
 DigitalOut M1_dir(D2);
@@ -118,39 +120,70 @@
     return radians * 57.2957795131f;
 }
 
+void calc_accel_error() {
+    Vec3 vec3;
+    
+    for(int i = 0 ; i < 200; i++){
+        vec3 = get_accel();
+        Acc_angle_error += rad_to_deg(atan(vec3.y / sqrtf(vec3.x*vec3.x + vec3.z*vec3.z)));
+
+    }
+    Acc_angle_error /= 200;
+
+}
 
 
 
 int main()
 {
-    /* Enable all sensors */
     magnetometer->enable();
     accelerometer->enable();
     acc_gyro->enable_x();
     acc_gyro->enable_g();
-
+    
+    calc_accel_error();
     Vec3 vec3;
 
     M1_dir = 1;
     M2_dir = 0;
-
+    float p;
     M1_step = 0.5f;
     M2_step = 0.5f;
+    PID Lpid(1.0, 0.006, 0.003, 0.05);
+    Lpid.setInputLimits(-45.0,45.0);
+    Lpid.setOutputLimits(0.0,1.0);
+    Lpid.setSetPoint(-1.0);
+    Lpid.setMode(AUTO_MODE);
+    PID Rpid(1.0, 0.006, 0.003, 0.05);
+    Rpid.setInputLimits(-45.0,45.0);
+    Rpid.setOutputLimits(0.0,1.0);
+    Rpid.setSetPoint(-1.0);
+    Rpid.setMode(AUTO_MODE);
 
     float angle;
     float period;
-    
-    float test = 1.05f;
-
+    float prev = 0;
+    float L = 0, R = 0;
     for(;;) {
         vec3 = get_accel();
-        angle = rad_to_deg(atan(vec3.y / sqrtf(vec3.x*vec3.x + vec3.z*vec3.z)));
+        int mean = 0;
+        for (int i = 0; i < 5; i++) {
+            angle = rad_to_deg(atan(vec3.y / sqrtf(vec3.x*vec3.x + vec3.z*vec3.z))) - Acc_angle_error;
+            mean += angle;
+            wait(.01);
+        }
+        mean /= 5;
+        angle = mean;
+        
         period = (degrees_to_period(angle));
-
-
-
-//Change direction.
-        if(angle > 0) {
+        Lpid.setProcessValue(angle);
+        Rpid.setProcessValue(angle * -1);
+        L = Lpid.compute();
+        R = Rpid.compute();
+        
+        p = abs( L-R);
+        //Change direction.
+        if(angle < 0) {
             M1_dir = 0;
             M2_dir = 1;
         } else {
@@ -158,15 +191,13 @@
             M2_dir = 0;
         }
         
+        M1_step.period(unnormalize(p));
+        M2_step.period(unnormalize(p));
+        
+        prev = angle;
 
-        //Control stepper motor.
-        out.printf("%f %f %f\r\n", period, angle, degrees_to_period(MAX_DEGREES/test));
-        //if(angle < 2.0f || angle > -2.0f)
-        M1_step.period(period);
-        M2_step.period(period);
-        //M1_step.period(0.0003);
-        //M2_step.period(0.0003);
-        //M1_step.period(degrees_to_period(MAX_DEGREES/test));
-        //M2_step.period(degrees_to_period(MAX_DEGREES/test));        
+        out.printf("%f  %f  %f  %f\r\n", p, L, R , angle);
+
+        
     }
 }
\ No newline at end of file