Diff: main.cpp

Revision:

12:da96e2f87465

Parent:

11:53de69b1840b

Child:

13:4e77264f254a

--- a/main.cpp	Mon Nov 21 16:57:53 2016 +0000
+++ b/main.cpp	Mon Nov 21 17:19:05 2016 +0000
@@ -13,6 +13,8 @@
 #include "TFC.h"
 #include "XBEE.h"
 #include "angular_speed.h"
+#include "motor.h"
+#include "main.h"
 
 #define BAUD_RATE 57600
 #define CAM_THRESHOLD 109
@@ -40,17 +42,21 @@
 uint8_t valBufferIndex;
 
 //Some PID variables
-float Kp;
-float Ki;
-float Kd;
-float dt;
-float p_error;
-float pid_error;
-float integral;
-float measured_value, desired_value, derivative;
-float output;
+pid_instance servo_pid;
 Timer t;
 
+float right_p;
+float right_error;
+float right_perror;
+float right_measured_value, right_desired_value;
+float right_output;
+
+float left_p;
+float left_error;
+float left_perror;
+float left_measured_value, left_desired_value;
+float left_output;
+
 //Speed Sensors variables
 InterruptIn leftHallSensor(D0);
 InterruptIn rightHallSensor(D2);
@@ -94,14 +100,14 @@
     //Initialise/reset PID variables
     initVariables();
     initSpeedSensors();
-    
+              
     while(1) {
         
         handleComms();
         
         //If we have an image ready
         if(TFC_LineScanImageReady>0) {       
-            measured_value = findCentreValue();
+            servo_pid.measured_value = findCentreValue();
             
             PIDController();
             
@@ -145,18 +151,33 @@
 
 void initVariables() {
     //Tunable PID variables
-    Kp = 125   / 25.0f;
-    Ki = 12.0f / 25.0f;
-    Kd = 0.0f;
-    dt = 0;
-    p_error = 0;
-    pid_error = 0;
-    integral = 0;
-    measured_value = 0;
-    desired_value = 0;
-    derivative = 0;
+    servo_pid.Kp = 125   / 25.0f;
+    servo_pid.Ki = 12.0f / 25.0f;
+    servo_pid.Kd = 0.0f;
+    servo_pid.dt = 0;
+    servo_pid.p_error = 0;
+    servo_pid.pid_error = 0;
+    servo_pid.integral = 0;
+    servo_pid.measured_value = 0;
+    servo_pid.desired_value = 0;
+    servo_pid.derivative = 0;
     
     valBufferIndex = 0;
+    
+    // motor p controller init
+    right_p = 1.0;
+    right_error = 0.0;
+    right_perror = 0.0;
+    right_measured_value = 0.0;
+    right_desired_value = 0.0;
+    right_output = 0.0;
+    left_p = 1.0;
+    left_error = 0.0;
+    left_perror = 0.0;
+    left_measured_value = 0.0;
+    left_desired_value = 0.0;
+    left_output = 0.0;
+    
     //Measured value is a float between -1.0 and 1.0 (from left to right)
     //Desired value is always 0.0 (as in, car is in the middle of the road)
 }
@@ -169,29 +190,26 @@
             pc.putc((int8_t)(TFC_LineScanImage0[i] >> 4) & 0xFF);    
         }
         
-
-    }
-    frame_counter++;
-    
-        wL=Get_Speed(Time_L);
-        wR=Get_Speed(Time_R);
-        
-        union {
-            float a;
-            unsigned char bytes[4];
-        } thing;
+        wL = wL/3.0f;
+        wR= wR/3.0f;
+        sendString("wL = %f, wR = %f",wL,wR);
+        wL = 0;
+        wR = 0;
         
-        pc.putc('B');
-        thing.a = wL;
-        pc.putc(thing.bytes[0]);
-        pc.putc(thing.bytes[1]);
-        pc.putc(thing.bytes[2]);
-        pc.putc(thing.bytes[3]);
-        thing.a = wR;
-        pc.putc(thing.bytes[0]);
-        pc.putc(thing.bytes[1]);
-        pc.putc(thing.bytes[2]);
-        pc.putc(thing.bytes[3]);
+             union {
+        float a;
+        unsigned char bytes[4];
+    } thing;
+    thing.a = TFC_ReadBatteryVoltage();
+    pc.putc('J');        
+    pc.putc(thing.bytes[0]);
+    pc.putc(thing.bytes[1]);
+    pc.putc(thing.bytes[2]);
+    pc.putc(thing.bytes[3]);
+    }
+        wL+=Get_Speed(Time_L);
+        wR+=Get_Speed(Time_R);
+    frame_counter++;
 }
 
 inline void handleComms() {
@@ -202,11 +220,11 @@
                         char p = xb.cBuffer->read();
                         char i = xb.cBuffer->read();
                         char d = xb.cBuffer->read();
-                        Kp = p/25.0f;
-                        Ki = i/25.0f;
-                        Kd = d/25.0f;
+                        servo_pid.Kp = p/25.0f;
+                        servo_pid.Ki = i/25.0f;
+                        servo_pid.Kd = d/25.0f;
                         pc.putc('E');
-                        pc.printf("pid change, Kp: %f, Ki: %f, Kd: %f, p: %u, i: %u, d: %u", Kp, Ki, Kd, p, i, d);
+                        pc.printf("pid change, Kp: %f, Ki: %f, Kd: %f, p: %u, i: %u, d: %u", servo_pid.Kp, servo_pid.Ki, servo_pid.Kd, p, i, d);
                         pc.putc(0);
                         
                         curr_cmd = 0;        
@@ -237,7 +255,8 @@
                 TFC_InitServos(0.00052,0.00122,0.02);
                 TFC_HBRIDGE_ENABLE;
                 TFC_SetMotorPWM(RIGHT_MOTOR_COMPENSATION_RATIO*speed,speed);
-                integral = 0;
+                servo_pid.integral = 0;
+                
                 
             } else if (cmd == 'C') {
                 TFC_SetMotorPWM(0.0,0.0);
@@ -286,26 +305,30 @@
     return measuredValue;
 }
 
+void handlePID(pid_instance *pid) {
+    pid->dt = t.read();
+    pid->pid_error = pid->desired_value - pid->measured_value;
+    pid->integral = pid->integral + pid->pid_error * pid->dt;
+    pid->derivative = (pid->pid_error - pid->p_error) / pid->dt;
+    pid->output = pid->Kp * pid->pid_error + pid->Ki * pid->integral + pid->Kd * pid->derivative;
+    pid->p_error = pid->pid_error;
+    
+    if(pid->integral > 1.0f) {
+        pid->integral = 1.0f;   
+    }
+    if(pid->integral < -1.0f) {
+        pid->integral = -1.0f;   
+    }
+}
+
 inline void PIDController() {
     //PID Stuff!
     t.start();
-    dt = t.read();
-    pid_error = desired_value - measured_value;
-    integral = integral + pid_error * dt;
-    derivative = (pid_error - p_error) / dt;
-    output = Kp * pid_error + Ki * integral + Kd * derivative;
-    p_error = pid_error;
-    
-    if(integral > 1.0f) {
-        integral = 1.0f;   
-    }
-    if(integral < -1.0f) {
-        integral = -1.0f;   
-    }
-    
-    if((-1.0 <= output) && (output <= 1.0))
+    handlePID(&servo_pid);
+
+    if((-1.0 <= servo_pid.output) && (servo_pid.output <= 1.0))
     {
-        TFC_SetServo(0, output);
+        TFC_SetServo(0, servo_pid.output);
     }
     else //Unhappy PID state
     {        
@@ -313,19 +336,33 @@
         pc.printf("pid unhappy");
         pc.putc(0);
         pc.putc('E');
-        pc.printf("out = %f p_err = %f", output, p_error);
+        pc.printf("out = %f p_err = %f", servo_pid.output, servo_pid.p_error);
         pc.putc(0);
         TFC_InitServos(0.00052, 0.00122, 0.02);
         //output, pid_error, p_error, integral, derivative = 0;
         
-        if(output >= 1.0f) {
+        if(servo_pid.output >= 1.0f) {
             TFC_SetServo(0, 0.9f);
-            output = 1.0f;
+            servo_pid.output = 1.0f;
         } else {
             TFC_SetServo(0, -0.9f);
-            output = -1.0f;
+            servo_pid.output = -1.0f;
         }
     }
+   
+    /*
+    right_error = right_desired_value - right_measured_value;
+    right_output = right_p * right_error;
+    right_perror = right_error;
+    
+    left_error = left_desired_value - left_measured_value;
+    left_output = left_p * left_error;
+    left_perror = left_error;
+    
+    TFC_SetMotorPWM(right_output,left_output);
+    
+    dutyCycleCorner(speed,output);
+    */
     
     t.stop();
     t.reset();