Diff: main.cpp

Revision:

0:88faaa1afb83

Child:

1:3f923c2862c9

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Apr 11 05:20:40 2016 +0000
@@ -0,0 +1,231 @@
+#include "FXAS21002.h"
+#include "FXOS8700Q.h"  
+#include "mbed.h"
+#include "CarPWM.h"
+#include "receiver.h"
+
+
+#define PI 3.141592653589793238462
+#define Ts 0.02                         // Seconds
+#define INITIAL_P 0.452531214933414
+#define INITIAL_I 5.45748932024049
+#define INITIAL_D 0.000233453623255507
+#define INITIAL_N 51.0605584484153
+#define GYRO_OFFSET 0.0152
+#define END_THRESH 4
+#define START_THRESH 10
+#define MINIMUM_VELOCITY 15
+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);
+FXAS21002 gyro(PTE25,PTE24);
+
+
+
+// PID controller parameters and functions
+float e[2], u, up[1],ui[2], ud[2]; // The vector coeficient means a time delay, for exemple e[a] = e(k-a) -> z^(-a)e(k)
+float P, I, D, N, reference = 0;
+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;
+float processMagAngle();
+void magCal();
+
+// State variables
+float sensor, velocity;
+void readProtocol();
+void brakeMotor();
+// Test functions
+void debug();
+
+int main(){
+    // Initializing serial communication
+    ser.baud(9600);
+    ser.format(8, SerialBase::None, 1);
+    // Initializing controller
+    printf("Initializing controller....\r\n\r\n");
+    initializeController();
+    printf("Controller Initialized. \r\n");
+    // Calibrating magnetometer and setting the initial position
+    magCal();
+    gyro_angle = processMagAngle();
+    // Start moving the robot and integrating the gyroscope
+    velocity = MINIMUM_VELOCITY;
+    setMotorPWM(velocity,motor);
+    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();
+        }
+}
+
+void readProtocol(){
+    char msg = ser.getc();
+    switch(msg)
+    {
+        case NONE:
+            //ser.printf("sending red signal to led\r\n");
+            return;
+            break;          
+        case BRAKE:
+            //ser.printf("sending green signal to led\r\n");
+            brakeMotor();
+            break;
+        case ANG_RST:
+            //ser.printf("sending blue signal to led\r\n");
+            stopGyro();
+            gyro_angle = 0;
+            startGyro();
+            return;
+            break;
+        case ANG_REF:
+            reference = get_ang_ref(ser);     
+            break;
+        case GND_SPEED:
+            velocity = get_gnd_speed(ser);       
+            setMotorPWM(velocity,motor);
+            break;
+        case PID_PARAMS:
+            ser.putc('p');
+            get_pid_params(ser, &P, &I, &D, &N);     
+            break;
+        default:
+           // ser.flush();
+
+    }
+}
+/* Initialize the controller parameter P, I, D and N with the initial values and set the error and input to 0.          */
+void initializeController(){
+    for(int i =0; i<2; i++){
+        e[i] = 0;
+        ui[i] = 0;
+        ud[i] = 0;
+    }
+    P= INITIAL_P;
+    I= INITIAL_I;
+    D= INITIAL_D;
+    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();
+    sensor = gyro_angle;
+    if(sensor > 180)
+        sensor = sensor - 360;
+    if(sensor < -180)
+        sensor = sensor + 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);
+    if(e[0] > PI)
+      e[0]= e[0] - 2*PI;
+    if(e[0] < -PI)
+      e[0] = e[0] + 2*PI;
+    /* Proportinal Part */
+    up[0] = e[0]*P;  
+    /* Integral Part */
+    ui[1] = ui[0];
+    if(abs(u) < PI/8){
+      ui[0] = (P*I*Ts)*e[1] + ui[1];    
+    }
+    else if(u > 0)
+      ui[0] = PI/8 - up[0];
+    else if(u < 0)
+      ui[0] = -PI/8 - up[0];  
+    /* Derivative Part */
+    ud[1] = ud[0];
+    ud[0] = P*D*N*(e[0] - e[1]) - ud[1]*(N*Ts -1); 
+    /** Controller **/ 
+    u = up[0] + ud[0] + ui[0];
+    setServoPWM(u*100/(PI/8), servo);
+}
+/* Brake function, braking while the gyroscope is still integrating will cause considerably error in the measurement. */
+void brakeMotor(){
+    if(velocity > 0)
+        setMotorPWM(-30, motor);
+    else if(velocity < 0)
+        setMotorPWM(30, motor);
+    wait(0.5);
+    setMotorPWM(0,motor);
+}
+/* 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",sensor);
+}
+
+/* Function to normalize the magnetometer reading */
+void magCal(){
+        printf("Starting Calibration");
+        mag.enable(); 
+        wait(0.01);
+        mag.getAxis(mag_data);
+        float x0 = max_x = min_y = mag_data.x;
+        float y0 = max_y = min_y = mag_data.y;
+        bool began = false;
+        while(!(began && abs(mag_data.x - x0) < END_THRESH && abs(mag_data.y - y0) < END_THRESH)){
+            mag.getAxis(mag_data);
+            if(mag_data.x > max_x)
+                max_x = mag_data.x;
+            if(mag_data.y > max_y)
+                max_y = mag_data.y;
+            if(mag_data.y < min_y)
+                min_y = mag_data.y;
+            if(mag_data.x < min_x)
+                min_x = mag_data.x;    
+            if(abs(mag_data.x-x0)>START_THRESH && abs(mag_data.y-y0) > START_THRESH)
+                began = true;   
+            printf("began: %d  X-X0: %f , Y-Y0: %f \n\r", began, abs(mag_data.x-x0),abs(mag_data.y-y0));
+        }
+        printf("Calibration Completed: X_MAX = %f , Y_MAX = %f , X_MIN = %f and Y_MIN = %f \n\r",max_x,max_y,min_x,min_y);
+}
+
+/* Function to transform the magnetometer reading in angle(rad/s).*/
+float processMagAngle(){
+    mag.getAxis(mag_data);
+    x = 2*(mag_data.x-min_x)/float(max_x-min_x) - 1;
+    y = 2*(mag_data.y-min_y)/float(max_y-min_y) - 1;
+    return atan2(y,x);
+}
\ No newline at end of file