Diff: functions.cpp

Revision:

6:af897173cb75

Parent:

4:0507835a3dce

Child:

7:d94d23c55015

Child:

9:7770a84228c0

--- a/functions.cpp	Sat May 23 15:08:23 2015 +0000
+++ b/functions.cpp	Sat May 23 16:09:43 2015 +0000
@@ -16,8 +16,8 @@
 
 // Setting pins for motor, as follows:
 // Example: Motor____(PWM, Forward, Reverse)
-Motor MotorLeft(p23, p28, p27);
-Motor MotorRight(p22, p29, p30);
+Motor MotorLeft(p23, p27, p28);
+Motor MotorRight(p22, p30, p29);
 
 // Setting pins for ultrasonic sensor, as follows:
 // Example: usensor(Trigger, Echo)
@@ -26,8 +26,7 @@
 // Returns value from the QRE1113
 // lower numbers mean more refleacive
 // more than 3000 means nothing was reflected.
-int read_line1()
-{
+int read_line1() {
     // Time to record how long input is active
     Timer temp_t;
     
@@ -46,8 +45,7 @@
     return temp_t.read_us();
 }
 
-int read_line2()
-{
+int read_line2() {
     // Time to record how long input is active
     Timer temp_t;
     
@@ -66,8 +64,7 @@
     return temp_t.read_us();
 }
 
-int detect_line()
-{
+int detect_line() {
     int line1val = read_line1();
     int line2val = read_line2();
     
@@ -83,8 +80,7 @@
     }
 }
 
-void reverse(float speed)
-{
+void reverse(float speed) {
     printf("Reverse\n");
     MotorLeft.speed((speed));
     MotorRight.speed(-(speed));
@@ -92,8 +88,7 @@
     return;
 }
 
-void reverseandturn(float speed)
-{
+void reverseandturn(float speed) {
     printf("Reverse and turn\n");
     MotorLeft.speed((speed-0.3));
     MotorRight.speed(-(speed-0.1));
@@ -101,36 +96,34 @@
     return;
 }
 
-void charge(float speed)
-{
+void charge(float speed) {
     MotorLeft.speed(speed);
     MotorRight.speed(speed);
     return;
 }
 
-void stop()
-{
+void stop() {
     MotorLeft.speed(0.0);
     MotorRight.speed(0.0);
     return;
 }
 
-int detect_object(int range, float speed)
-{
+int detect_object(int range, float speed) {
     // Start a timer - finds an object for 10 seconds
     // if it doesn't find anything returns 0
-    Timer usensor_t;
+    Timer usensor_t, inner_t;
     usensor_t.start();
     
     // Variable to store sensed value
-    int sense, dist;
+    unsigned int sense, dist, reverse;
     sense = 0;
     dist = 0;
+    reverse = 0;
         
-    while (usensor_t.read_ms() < 10000)
-    {
+    while (usensor_t.read_ms() < 15000) {
         // Start the ultrasonic sensor
         usensor.start();
+        inner_t.start();
         dist = usensor.get_dist_cm();
         
         // If an object is detected based on out set range return 1
@@ -142,6 +135,20 @@
             MotorLeft.speed(speed);
             MotorRight.speed(-(speed));
         }
+        
+        if (inner_t.read_ms() >=100) {
+            if (reverse == 2) {
+                speed = 0.7;
+                reverse = 0;
+            } else {
+                speed = 0.0;
+            }
+            reverse++;
+            inner_t.reset();
+        }
     }
+    
+    usensor_t.stop();
+    usensor_t.reset();
     return sense;
 }
\ No newline at end of file