Diff: main.cpp

Revision:

20:ed954836d028

Parent:

19:65f0b6febc23

Child:

21:0b69fada7c5f

Child:

24:15264aee54d1

--- a/main.cpp	Wed Dec 07 15:10:59 2016 +0000
+++ b/main.cpp	Fri Dec 09 11:10:14 2016 +0000
@@ -62,7 +62,6 @@
             servo_pid.measured_value = findCentreValue(CLOSE_CAMERA, left, right);
             
             
-            
             // Slow down, adjust PID values and enable differential before corners.
             handleCornering();
             
@@ -78,7 +77,7 @@
             #endif
             
             // Check if car is at the stop line
-            //handleStartStop();
+            handleStartStop();
             
             
             //Reset image ready flag
@@ -90,8 +89,8 @@
 void initVariables() {
     // Initialise three PID controllers for the servo and each wheel.
     initPID(&servo_pid, 2.2f, 0.6f, 0.f);
-    initPID(&left_motor_pid, 0.01f, 0.f, 0.f);
-    initPID(&right_motor_pid, 0.01f, 0.f, 0.f);
+    initPID(&left_motor_pid, 0.02f, 0.f, 0.f);
+    initPID(&right_motor_pid, 0.02f, 0.f, 0.f);
     
     right_motor_pid.desired_value=0;
     left_motor_pid.desired_value=0;
@@ -108,6 +107,11 @@
     turning = 0;
     keepTurning = 0;
     slow = false;
+    
+    wL = 0;
+    wR = 0;
+    prevL = 0;
+    prevR = 0;
  
 }
 
@@ -240,8 +244,22 @@
     // Read the angular velocity of both wheels
     wL=Get_Speed(Time_L);
     wR=Get_Speed(Time_R);
-    left_motor_pid.measured_value = wL;
-    right_motor_pid.measured_value = wR;
+    
+    // Check if left wheel is slipping/giving an abnormal reading and ignore reading
+    if(wL - prevL < 1.2/0.025) {
+        left_motor_pid.measured_value = wL;
+        prevL = wL;  
+    } else {
+        wL = prevL; 
+    }
+    
+    // Same as above for right
+    if(wR - prevR < 1.2/0.025) {
+        right_motor_pid.measured_value = wR;
+        prevR = wR;
+    } else {
+        wR = prevR;
+    }
     
     //PID Stuff!
     t.start();
@@ -267,31 +285,21 @@
     }    
     
     
-    // need to get thepid to set the duty cycle 
-    // need to have the desired value set0
-    //MOTORS
-    if((0<=left_motor_pid.output)&&(0<=right_motor_pid.output))
-    {
-        TFC_SetMotorPWM(left_motor_pid.output,right_motor_pid.output);
-        
+    if(left_motor_pid.output > 1.0f) {
+       left_motor_pid.output = 1.0f; 
     }
-    else
-    {
-        if(0 > right_motor_pid.output)
-        {
-            TFC_SetMotorPWM(left_motor_pid.output,0);
-        }
-        if(0 > left_motor_pid.output)
-        {
-            TFC_SetMotorPWM(0,right_motor_pid.output);
-        }
+    if(left_motor_pid.output < -1.0f) {
+       left_motor_pid.output = 0.f; 
     }
     
-    
-    
+    if(right_motor_pid.output > 1.0f) {
+        right_motor_pid.output = 1.0f;
+    }
+    if(right_motor_pid.output < -1.0f) {
+        right_motor_pid.output = 0.f;
+    }
     
-    
-    
+    TFC_SetMotorPWM(left_motor_pid.output,right_motor_pid.output);
     
     t.stop();
     t.reset();
@@ -303,7 +311,7 @@
     if(right - left < 60) {
         sendString("START STOP!! &d",startstop);//do you mean %d?
     
-        lapTime(); 
+        //lapTime(); 
         //testSpeed(speed) HOLY SHIT ITS DAT BOI!!!!!!!!
         if(seen) {
             seen = false;
@@ -317,8 +325,7 @@
             TFC_HBRIDGE_DISABLE;
             startstop = 0;      
         }
-    }   
-     
+    }    
 }
 
 
@@ -404,12 +411,12 @@
     
 
     pc.putc('B');
-    thing.a = wL * WHEEL_RADIUS;
+    thing.a = wL * WHEEL_RADIUS;//left_motor_pid.output; //
     pc.putc(thing.bytes[0]);
     pc.putc(thing.bytes[1]);
     pc.putc(thing.bytes[2]);
     pc.putc(thing.bytes[3]);
-    thing.a = wR * WHEEL_RADIUS;
+    thing.a = wR * WHEEL_RADIUS; // right_motor_pid.output; //
     pc.putc(thing.bytes[0]);
     pc.putc(thing.bytes[1]);
     pc.putc(thing.bytes[2]);
@@ -421,7 +428,8 @@
     if(curr_cmd != 0) {
             switch(curr_cmd) {
                 case 'A':
-                    if(xb.cBuffer->available() >= 3) {
+                    if(xb.cBuffer->available() >= 12) {
+                        /*
                         char p = xb.cBuffer->read();
                         char i = xb.cBuffer->read();
                         char d = xb.cBuffer->read();
@@ -429,6 +437,27 @@
                         servo_pid.Ki = i/25.0f;
                         servo_pid.Kd = d/25.0f;
                         sendString("pid= Kp: %f, Ki: %f, Kd: %f, p: %u, i: %u, d: %u", servo_pid.Kp, servo_pid.Ki, servo_pid.Kd, p, i, d);
+                        */
+                    
+                        thing.bytes[0] = xb.cBuffer->read();
+                        thing.bytes[1] = xb.cBuffer->read();
+                        thing.bytes[2] = xb.cBuffer->read();
+                        thing.bytes[3] = xb.cBuffer->read();
+                        servo_pid.Kp = thing.a; 
+                        
+                        thing.bytes[0] = xb.cBuffer->read();
+                        thing.bytes[1] = xb.cBuffer->read();
+                        thing.bytes[2] = xb.cBuffer->read();
+                        thing.bytes[3] = xb.cBuffer->read();
+                        servo_pid.Ki = thing.a; 
+                        
+                        thing.bytes[0] = xb.cBuffer->read();
+                        thing.bytes[1] = xb.cBuffer->read();
+                        thing.bytes[2] = xb.cBuffer->read();
+                        thing.bytes[3] = xb.cBuffer->read();
+                        servo_pid.Kd = thing.a; 
+                        
+                        sendString("pid= Kp: %f, Ki: %f, Kd: %f",  servo_pid.Kp, servo_pid.Ki, servo_pid.Kd);
                         
                         curr_cmd = 0;        
                     }