Diff: robot.cpp

Revision:

7:a852e63cac3d

Parent:

6:c2d4a02115db

Child:

8:5ca76759a67e

--- a/robot.cpp	Thu Nov 24 09:15:48 2016 +0000
+++ b/robot.cpp	Fri Dec 09 12:32:21 2016 +0000
@@ -7,76 +7,64 @@
  Copyright   : Open for all
  Description : Program to serve the platform for Pro1 2016
  ============================================================================
- */
-#include "odometry.h"
+ */ 
+#include "odometry.h" 
 #include "mbed.h"
-#include "HCSR04.h"
-#include "nucleo_servo.h"
 #include "hack_motor.h"
 #include <math.h> 
-#include "PID.h"
-
 
 void startfunction( );
-Ticker T1; // create an object T1 of Ticker
+Ticker T1; // create an object T1 of Ticker 
 
-/*
-----------------------------------------------------------------------------
-                                MAIN FUNCTION
-----------------------------------------------------------------------------*/
 int main()
-{   
-/*
-=============================================================================
-Driving and stopping the robot with member functions from the wheel class
-=============================================================================*/
-    startfunction();    //  1. DRIVE     
+{ 
+/////////////////////////////////////////////////////////////////////////////
+//                          Driving setup                                  // 
+/////////////////////////////////////////////////////////////////////////////
+    while(1)    //Remember to include if statement to stop robot if it's time to recharge
+    {
+    read_analog();
+    startfunction(); 
+    get_to_goal();
+    robot.stop();
+    printf("\n\r DistanceBysensor: %.2f \n", Dcenter()); 
+    wait_ms(2000);
+    
+    read_analog();
+    startfunction();
     get_to_goal();
-    robot.stop(); 
-    printf("\n\rtimeused = %4d tickL= %4d  tickR = %4d \n", t.read_ms(),tickL,
-           tickR);   
-    wait_ms(3000);
+    robot.stop();
+    printf("\n\r DistanceBysensor: %.2f \n", Dcenter());  
+    wait_ms(2000);
+    
+    read_analog();
+    startfunction();
+    get_to_goal();
+    robot.stop();
+    printf("\n\r DistanceBysensor: %.2f \n", Dcenter()); 
+    wait_ms(2000);
+    
+    read_analog();
+    startfunction();
+    get_to_goal();
+    robot.stop();
+    printf("\n\r DistanceBysensor: %.2f \n", Dcenter());  
+    wait_ms(2000);
+    }
 } 
 /*  
 ----------------------------------------------------------------------------
                             END OF MAIN FUNCTION
-----------------------------------------------------------------------------*/
-
+----------------------------------------------------------------------------*/  
 void startfunction()
 {
     T1.attach(&read_analog, 1.0); // attach the address of the read_analog
     //function to read analog in every second
-
     tacho_left.rise(&tickLeft);  // attach the address of the count function
     //to the falling edge
     tacho_right.rise(&tickRight);  // attach the address of the count function
     //to the falling edge
-
-    HCSR04 sensor(PC_5,PC_6); // Create an object of HCSR04 ultrasonic with pin
-    // (echo,trigger) on pin PC_5, PC6
-    Servo servo1(PC_8);     //Create an object of Servo class on pin PC_8
-
-    sensor.setRanges(2, 400); // set the range for Ultrasonic
-
-/*
-=============================================================================
-Demo of the servor and ulta sonic sensor
-=============================================================================*/  
-    wait_ms(2000); // just get time for you to enable your screeen
-    servo1.set_position(0); // Servo right position (angle = 0 degree) for servo
-    wait_ms (500);
-    printf("\r\nDistance: %5.1f mm \n", sensor.getDistance_mm()); // display the
-    //readings from ultra sonic at this position
-    servo1.set_position(180); // Servo left position (angle = 180 degree)
-    //for servo
-    wait_ms (500);
-    printf("\r\nDistance: %5.1f mm \n", sensor.getDistance_mm());
-    servo1.set_position(90); // Straight ahead (angle = 90 degree) for servo
-    wait_ms (500);
-    printf("\r\nDistance: %5.1f mm \n", sensor.getDistance_mm());
-
     init(); // initialise parameters (just for you to remember if you need to)
-
-    wait_ms(1000); //wait 1 secs here before you go
+    wait_ms(2000); //wait 1 secs here before you go
     t.start();      // start timer (just demo of how you can use a timer)
 }
\ No newline at end of file