Diff: main.cpp

Revision:

21:0b69fada7c5f

Parent:

20:ed954836d028

Child:

22:973b95478663

--- a/main.cpp	Fri Dec 09 11:10:14 2016 +0000
+++ b/main.cpp	Fri Dec 09 13:19:28 2016 +0000
@@ -34,6 +34,7 @@
 //testing timer
 Timer test;
 
+
 int main() {
     //Set up TFC driver stuff
     TFC_Init();
@@ -48,7 +49,6 @@
     initVariables();
     initSpeedSensors();
               
-
     while(1) {
         
         #if USE_COMMS
@@ -61,27 +61,34 @@
              * the centre */        
             servo_pid.measured_value = findCentreValue(CLOSE_CAMERA, left, right);
             
+            // Check if car is at the stop line
+            handleStartStop();
+            
+            // Send the line scan image over serial
+            //sendImage();
+            
+            
+            //Reset image ready flag
+            TFC_LineScanImageReady=0;
             
             // Slow down, adjust PID values and enable differential before corners.
-            handleCornering();
+            //handleCornering();
             
             // Run the PID controllers and adjust steering/motor accordingly
             PIDController();
             
+            
+                
+            
             #if USE_COMMS
-                // Send the line scan image over serial
-                sendImage();
-                
                 // Send the wheel speeds over serial
                 sendSpeeds();
             #endif
             
-            // Check if car is at the stop line
-            handleStartStop();
+
             
             
-            //Reset image ready flag
-            TFC_LineScanImageReady=0;
+            
         }
     }
 }
@@ -89,8 +96,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.02f, 0.f, 0.f);
-    initPID(&right_motor_pid, 0.02f, 0.f, 0.f);
+    initPID(&left_motor_pid, 0.01f, 0.f, 0.f);
+    initPID(&right_motor_pid, 0.01f, 0.f, 0.f);
     
     right_motor_pid.desired_value=0;
     left_motor_pid.desired_value=0;
@@ -261,6 +268,7 @@
         wR = prevR;
     }
     
+    
     //PID Stuff!
     t.start();
     handlePID(&servo_pid);
@@ -301,6 +309,7 @@
     
     TFC_SetMotorPWM(left_motor_pid.output,right_motor_pid.output);
     
+    
     t.stop();
     t.reset();
     t.start();
@@ -309,22 +318,22 @@
 inline void handleStartStop() {
     //Hacky way to detect the start/stop signal
     if(right - left < 60) {
-        sendString("START STOP!! &d",startstop);//do you mean %d?
+        sendString("START STOP!!");//do you mean %d? - YES!!!!
     
         //lapTime(); 
         //testSpeed(speed) HOLY SHIT ITS DAT BOI!!!!!!!!
-        if(seen) {
+        /*if(seen) {
             seen = false;
         } else {
             startstop++;
             seen = true;    
         }    
-        //If we've done 5 laps, stop the car
-        if(startstop >= 1) {
+
+        if(startstop >= 1) { */
             TFC_SetMotorPWM(0.f,0.f);
             TFC_HBRIDGE_DISABLE;
             startstop = 0;      
-        }
+        
     }    
 }
 
@@ -429,15 +438,6 @@
             switch(curr_cmd) {
                 case 'A':
                     if(xb.cBuffer->available() >= 12) {
-                        /*
-                        char p = xb.cBuffer->read();
-                        char i = xb.cBuffer->read();
-                        char d = xb.cBuffer->read();
-                        servo_pid.Kp = p/25.0f;
-                        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();
@@ -466,9 +466,7 @@
                 case 'F':
                     if(xb.cBuffer->available() >= 1) {
                         char a = xb.cBuffer->read();
-                        //speed = a/255;
                         speed = (a/0.025f)/50.f;
-                        //TFC_SetMotorPWM(speed,speed);  
                         sendString("s = %u %f",a, speed);
                         curr_cmd = 0;   
                     }
@@ -486,9 +484,11 @@
             char cmd = xb.cBuffer->read();
             if(cmd == 'D') {
                 ALIGN_SERVO;
+                right_motor_pid.desired_value=speed;
+                left_motor_pid.desired_value=speed;
                 TFC_HBRIDGE_ENABLE;
-                    right_motor_pid.desired_value=speed;
-                    left_motor_pid.desired_value=speed;
+
+                    
                 servo_pid.integral = 0;
                 test.start();
                 lapNo =0;
@@ -496,7 +496,15 @@
             } else if (cmd == 'C') {
                 TFC_SetMotorPWM(0.0,0.0);
                 right_motor_pid.desired_value=0;
+                right_motor_pid.measured_value = 0;
+                wR = 0;
+                prevR = 0;
+                
                 left_motor_pid.desired_value=0;
+                left_motor_pid.measured_value = 0;
+                wL = 0;
+                prevL = 0;
+                
                 TFC_HBRIDGE_DISABLE;
                 endTest();
             } else if(cmd == 'A') {