Diff: main.cpp

Revision:

4:8ead8ada8a8b

Parent:

3:e213c44a9f6c

Child:

6:c930555fd872

--- a/main.cpp	Mon Apr 11 21:24:52 2016 +0000
+++ b/main.cpp	Sun Apr 17 01:01:17 2016 +0000
@@ -45,7 +45,7 @@
 void magCal();
 
 // State variables
-float sensor, velocity;
+float feedback, velocity;
 void readProtocol();
 void brakeMotor();
 // Test functions
@@ -68,13 +68,13 @@
     startGyro();
     // main loop
         while (true){
-        processGyroAngle();
-        controlAnglePID(P,I,D,N);
-        //debug();
-        if(t.read_us() < Ts*1000000)
-            wait_us(Ts*1000000 - t.read_us());
-        if(ser.readable())
-            readProtocol();
+            processGyroAngle();
+            controlAnglePID(P,I,D,N);
+            //debug();
+            if(t.read_us() < Ts*1000000)
+                wait_us(Ts*1000000 - t.read_us());
+            if(ser.readable())
+                readProtocol();
         }
 }
 
@@ -146,18 +146,18 @@
     gyro_angle = gyro_angle + (gyro_data[2] + GYRO_OFFSET)*(double)(t.read_us())/1000000;
     t.reset();
     t.start();
-    sensor = gyro_angle;
-    if(sensor > 180)
-        sensor = sensor - 360;
-    if(sensor < -180)
-        sensor = sensor + 360;
+    feedback = gyro_angle;
+    if(feedback > 180)
+        feedback = feedback - 360;
+    if(feedback < -180)
+        feedback = feedback + 360;
 }
 
 /* PID controller for angular position */
 void controlAnglePID(float P, float I, float D, float N){ 
     /* Getting error */
     e[1] = e[0];
-    e[0] = reference - (sensor*PI/180);
+    e[0] = reference - (feedback*PI/180);
     if(e[0] > PI)
       e[0]= e[0] - 2*PI;
     if(e[0] < -PI)
@@ -194,7 +194,7 @@
 void debug(){
     //printf("ERROR: %f Up: %f Ui: %f Ud: %f U: %f \r\n", e[0]*180/3.1415, up[0]*100/(3.1415/8), ui[0]*100/(3.1415/8), ud[0]*100/(3.1415/8),u*100/(3.1415/8));
     //printf("Erro: %f  Sensor: %f Magnetometer: %f \r\n",e[0]*180/PI,sensor,processMagAngle(0)*180/PI);
-    printf(" %f \r\n",sensor);
+    printf(" %f \r\n",feedback);
 }
 
 /* Function to normalize the magnetometer reading */