Diff: main.cpp

Revision:

19:65f0b6febc23

Parent:

18:0095a3a8f8e4

Child:

20:ed954836d028

Child:

23:b234e8fb51b3

--- a/main.cpp	Tue Dec 06 14:15:55 2016 +0000
+++ b/main.cpp	Wed Dec 07 15:10:59 2016 +0000
@@ -61,9 +61,7 @@
              * the centre */        
             servo_pid.measured_value = findCentreValue(CLOSE_CAMERA, left, right);
             
-            // Read the angular velocity of both wheels
-            wL=Get_Speed(Time_L);
-            wR=Get_Speed(Time_R);
+            
             
             // Slow down, adjust PID values and enable differential before corners.
             handleCornering();
@@ -80,7 +78,7 @@
             #endif
             
             // Check if car is at the stop line
-            handleStartStop();
+            //handleStartStop();
             
             
             //Reset image ready flag
@@ -92,11 +90,15 @@
 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, 1.0f, 0.f, 0.f);
-    initPID(&right_motor_pid, 1.0f, 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;
+    
+    // intialise the maximum speed interms of the angular speed.
     valBufferIndex = 0;
-    speed = 0.3;
+    speed = 50;
     
     //Start stop
     startstop = 0;
@@ -106,6 +108,7 @@
     turning = 0;
     keepTurning = 0;
     slow = false;
+ 
 }
 
 void initPID(pid_instance* pid, float Kp, float Ki, float Kd) {
@@ -173,7 +176,7 @@
         //default
         //TFC_SetMotorPWM(0.4,0.4);
         
-        dutyCycleCorner(speed,servo_pid.output);
+        //dutyCycleCorner(speed,servo_pid.output);
         
         //may want to have just to set cornering speed at different if going to be slowing down for cornering.
         //dutyCycleCorner(float cornerspeed, servo_pid.output);
@@ -230,8 +233,13 @@
     }
 }
 
+
+
 inline void PIDController() {
     // update motor measurements
+    // 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;
     
@@ -259,6 +267,32 @@
     }    
     
     
+    // 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);
+        
+    }
+    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);
+        }
+    }
+    
+    
+    
+    
+    
+    
+    
     t.stop();
     t.reset();
     t.start();
@@ -352,7 +386,7 @@
 
 inline void sendSpeeds() {
   
-    float en = getLineEntropy();
+    /*float en = getLineEntropy();
     
     if(onTrack) {
         if(en <= 14000) {
@@ -366,16 +400,16 @@
             onTrack = true;
             sendString("ON TRACK");
         }    
-    }
+    }*/
     
 
     pc.putc('B');
-    thing.a = en;//wL * WHEEL_RADIUS;
+    thing.a = wL * WHEEL_RADIUS;
     pc.putc(thing.bytes[0]);
     pc.putc(thing.bytes[1]);
     pc.putc(thing.bytes[2]);
     pc.putc(thing.bytes[3]);
-    thing.a = en; //wR * WHEEL_RADIUS;
+    thing.a = wR * WHEEL_RADIUS;
     pc.putc(thing.bytes[0]);
     pc.putc(thing.bytes[1]);
     pc.putc(thing.bytes[2]);
@@ -403,8 +437,9 @@
                 case 'F':
                     if(xb.cBuffer->available() >= 1) {
                         char a = xb.cBuffer->read();
-                        speed = a/256.0f;
-                        TFC_SetMotorPWM(speed,speed);  
+                        //speed = a/255;
+                        speed = (a/0.025f)/50.f;
+                        //TFC_SetMotorPWM(speed,speed);  
                         sendString("s = %u %f",a, speed);
                         curr_cmd = 0;   
                     }
@@ -423,13 +458,16 @@
             if(cmd == 'D') {
                 ALIGN_SERVO;
                 TFC_HBRIDGE_ENABLE;
-                TFC_SetMotorPWM(RIGHT_MOTOR_COMPENSATION_RATIO*speed,speed);
+                    right_motor_pid.desired_value=speed;
+                    left_motor_pid.desired_value=speed;
                 servo_pid.integral = 0;
                 test.start();
                 lapNo =0;
                 
             } else if (cmd == 'C') {
                 TFC_SetMotorPWM(0.0,0.0);
+                right_motor_pid.desired_value=0;
+                left_motor_pid.desired_value=0;
                 TFC_HBRIDGE_DISABLE;
                 endTest();
             } else if(cmd == 'A') {
@@ -469,14 +507,14 @@
 
 
 int lapTime()
-{
+{   
     // function which sends the lap time back to the telemetry.
     float newTime= test.read();
     lapNo += 1;
     float lapTime= newTime-oldTime;
     float avgTime= newTime/lapNo;
     
-    sendString("For lap number: %d  Lap Time: %f  Avergae time: %f ", lapNo,lapTime,avgTime);
+    sendString("For lap number: %d  Lap Time: %f  Avergae time: %f \n\r", lapNo,lapTime,avgTime);
     
      // OH WHAT UP IT'S DAT BOI!!!!
     return 0;
@@ -488,11 +526,11 @@
     
     float time= test.read();
     
-    sendString("Laps done: %d  Time elapsed: %f   Average time: %f",lapNo, time,float (time/lapNo));
+    sendString("Laps done: %d  Time elapsed: %f   Average time: %f \n\r",lapNo, time,float (time/lapNo));
     test.stop();
     
     
-}
+}