Diff: LineFollowingRobot/main.cpp

Revision:

5:bb0bec710c91

Parent:

4:645b5d648c64

Child:

6:b457f1e2fff8

--- a/LineFollowingRobot/main.cpp	Wed Feb 06 14:02:07 2019 +0000
+++ b/LineFollowingRobot/main.cpp	Thu Mar 07 14:00:42 2019 +0000
@@ -38,15 +38,15 @@
 
 
 Ticker IRsampling;
-
-
+Timer t;
+DigitalOut myled(LED_RED);
 
 int main(){
 
 
 bool toggleIR;
 
-IRSensor IRSensor(PTC6, PTC5, PTC4, PTC3, PTC0, 0.0, 0.0, 0.0);
+IRSensor IRSensor(PTC6, PTC5, PTC4, PTC3, PTC0, 0.27, 0.0, 0.0);
 
 IRsampling.attach(callback(&IRSensor, &IRSensor::Sample), 0.01);
 
@@ -54,28 +54,80 @@
 RobotControl controlLeft(PTA12,PTA4,PTA5);
 RobotControl controlRight(PTA13,PTD5,PTD0);
 
+myled = 0;
+
 
  
  while(1){
 
+    /*controlLeft.SetDirection(0);
+    controlRight.SetDirection(0);
+    
+    controlLeft.SetSpeed(0.3);
+    controlRight.SetSpeed(0.3);
+    
+    myled =1;
+    
+    wait(1);
+    
+    controlLeft.SetDirection(1);
+    controlRight.SetDirection(0);
+    
+    controlLeft.SetSpeed(0.3);
+    controlRight.SetSpeed(0.3);
+    
+    myled =0;
+    
+    wait(1);
+    
+    controlLeft.SetDirection(0);
+    controlRight.SetDirection(1);
+    
+    controlLeft.SetSpeed(0.3);
+    controlRight.SetSpeed(0.3);
+    
+    myled = 1;
+    
+    wait(1);
+    
+    controlLeft.SetDirection(1);
+    controlRight.SetDirection(1);
+    
+    controlLeft.SetSpeed(0.3);
+    controlRight.SetSpeed(0.3);
+    
+    myled = 0;
+    
+    wait(1); */
+    
+    
     //waits for an interrupt to occur, then performs all the calculations
     if(toggleIR != IRSensor.m_toggle){
         
-        //IRSensor.WeightPID();
-        //IRSensor.CalculatePID();
-        IRSensor.MotorControl(controlLeft,controlRight);
+        t.start();
+        IRSensor.WeightPID();
+        IRSensor.CalculatePID();
+        IRSensor.MotorControl();
         
         if(IRSensor.m_dirL != IRSensor.m_prevDirL) controlLeft.SetDirection(IRSensor.m_dirL);
         if(IRSensor.m_dirR != IRSensor.m_prevDirR) controlRight.SetDirection(IRSensor.m_dirR);
     
-        controlLeft.SetSpeed(abs(IRSensor.m_speedL));
-        controlRight.SetSpeed(abs(IRSensor.m_speedR));
+        controlLeft.SetSpeed(fabs(IRSensor.m_speedL));
+        controlRight.SetSpeed(fabs(IRSensor.m_speedR));
     
         IRSensor.m_prevDirL = IRSensor.m_dirL;
         IRSensor.m_prevDirR = IRSensor.m_dirR;
         
-        printf("toggle %d", toggleIR);
+        t.stop();
+        //printf("timer : %f \n", t.read());
+        t.reset();
+        //printf("rightSpeed : %f, leftSpeed : %f \r", IRSensor.m_speedL, IRSensor.m_speedR);
+        
+        
+        
         toggleIR = IRSensor.m_toggle;
         }
+        
+    
     }       
 }
\ No newline at end of file