Diff: main.cpp

Revision:

12:273752f540be

Parent:

5:b0af0cfb678e

Child:

13:f7a7fe9b5c00

--- a/main.cpp	Sat Apr 30 14:18:43 2016 -0300
+++ b/main.cpp	Sat Apr 30 21:23:13 2016 +0000
@@ -3,24 +3,23 @@
 #include "mbed.h"
 #include "CarPWM.h"
 #include "receiver.h"
-
+#include "Motor.h"
 
 #define PI 3.141592653589793238462
 #define Ts 0.02                         // Seconds
-#define PWM_PERIOD 13.5 // ms
+#define PWM_PERIOD 13.5                 // ms
 #define INITIAL_P 0.452531214933414
 #define INITIAL_I 5.45748932024049
 #define INITIAL_D 0.000233453623255507
 #define INITIAL_N 51.0605584484153
 #define BRAKE_CONSTANT 40
 #define BRAKE_WAIT 0.3
-#define GYRO_OFFSET 0.0152
 #define END_THRESH 4
 #define START_THRESH 10
 #define MINIMUM_VELOCITY 15
+#define GYRO_PERIOD 1300                //us
 
 Serial ser(USBTX, USBRX);    // Initialize Serial port
-PwmOut motor(PTD1);         // Motor connected to pin PTD1
 PwmOut servo(PTD3);         // Servo connected to pin PTD3
 
 FXOS8700Q_mag mag(PTE25,PTE24,FXOS8700CQ_SLAVE_ADDR1);
@@ -34,13 +33,6 @@
 void controlAnglePID(float P, float I, float D, float N);
 void initializeController();
 
-// Gyroscope variables and functions
-float gyro_data[3], gyro_angle;
-Timer t; 
-void processGyroAngle();
-void startGyro();
-void stopGyro();
-
 // Magnetometer variables and functions
 float max_x, max_y, min_x, min_y,x,y;
 MotionSensorDataUnits mag_data;
@@ -48,65 +40,21 @@
 void magCal();
 
 // State variables
-float feedback, velocity = 0;
+float velocity = 0;
 void readProtocol();
 void brakeMotor();
 void reverseMotor(int speed);
 void setVelocity(int new_velocity);
 
-// Test functions
-void debug();
-
 int main(){
-    int teste = 0;
-    setVelocity(0);
-    switch(teste){
-    case 0: // vai para tras duas vezes
-        brakeMotor();
-        wait(1);
-        setVelocity(-1);
-        setVelocity(0);
-        setVelocity(-30);
-        wait(2);
-        brakeMotor();
-        wait(2);
-        setVelocity(-1);
-        setVelocity(0);
-        setVelocity(-30);
-        break;
-    case 1: // vai para a frente e depois para tras
-        brakeMotor();
-        wait(1);
-        setVelocity(20);
-        wait(2);
-        brakeMotor();
-        wait(2);
-        setVelocity(-1);
-        setVelocity(0);
-        setVelocity(-30);
-        break;
-    case 2: // vai para tras e depois para frente
-        brakeMotor();
-        wait(1);
-        setVelocity(-1);
-        setVelocity(0);
-        setVelocity(-30);
-        wait(2);
-        brakeMotor();
-        wait(2);
-        setVelocity(20);
-        break;
-    case 3: // vai para frente duas vezes
-        brakeMotor();
-        wait(1);
-        setVelocity(20);
-        wait(2);
-        brakeMotor();
-        wait(2);
-        setVelocity(20);
-        break;
+    gyro.gyro_config(MODE_2);
+    gyro.start_measure(GYRO_PERIOD);
+    initializeController();
+    while(1){
+        controlAnglePID(P,I,D,N);
+        printf("%f \r\n",gyro.get_angle());
+        wait(Ts);
     }
-    while(1);
 }
 void readProtocol(){
     char msg = ser.getc();
@@ -122,9 +70,8 @@
             break;
         case ANG_RST:
             //ser.printf("sending blue signal to led\r\n");
-            stopGyro();
-            gyro_angle = 0;
-            startGyro();
+            gyro.stop_measure();
+            gyro.start_measure(GYRO_PERIOD);
             return;
             break;
         case ANG_REF:
@@ -156,36 +103,10 @@
     N= INITIAL_N;
 }
 
-/* Start the Gyroscope timer and set the initial configuration */
-void startGyro(){
-    gyro.gyro_config(); 
-    t.start();
-}
-
-/* Stop and reset the Gyroscope */
-void stopGyro(){
-    t.stop();
-    t.reset();
-    gyro_angle = 0;
-}
-
-/* Integrate the Gyroscope to get the angular position (Deegre/seconds) */
-void processGyroAngle(){
-    gyro.acquire_gyro_data_dps(gyro_data);
-    t.stop();
-    gyro_angle = gyro_angle + (gyro_data[2] + GYRO_OFFSET)*(double)(t.read_us())/1000000;
-    t.reset();
-    t.start();
-    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 */
+    float feedback = gyro.get_angle();
     e[1] = e[0];
     e[0] = reference - (feedback*PI/180);
     if(e[0] > PI)
@@ -252,14 +173,6 @@
         velocity--;
         }
 }
-/* Debug functions that prints the sensor and control inputs values. Since it's time consuming it should not be used    */
-/* in the main loop or the controller performance may be affected.                                                      */
-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",feedback);
-}
-
 /* Function to normalize the magnetometer reading */
 void magCal(){
         printf("Starting Calibration");