Diff: Brobot.cpp

Revision:

12:9a763d149f61

Parent:

11:05d5539141c8

Child:

14:6a3eb8e8a75e

--- a/Brobot.cpp	Thu Jun 01 08:34:27 2017 +0000
+++ b/Brobot.cpp	Thu Jun 01 09:44:03 2017 +0000
@@ -117,6 +117,49 @@
 
 }
 
+///////////// start new code //////////////////////
+void Brobot::approachHome()
+{        
+    float x_e,diff;
+    int triggered_sensors = 0, rotate_count = 0;
+    bool home_reached = false;
+    //the time to rotate after the brick is released and the distance away from
+    // the way that the robot should stop at
+    float end_rotate_time = 0.75, sensor_trigger_dist=0.3;
+    
+    while(!home_reached){
+        // find the error in the x alignment and calculate the restoring voltage
+        x_e = pixy.getX()-160;
+        diff = pid.calc(x_e,0.05);
+        
+        // if home is detected drive the robot forward while continuously realigning the robot
+        if (pixy.objectDetected() && (pixy.getSignature() == 2)){
+            speedctrl.setDesiredSpeed(5-diff,-5-diff);
+            rotate_count = 0;
+            }
+        // check the distance sensors. Home is reached when 2 sensors read < 0.3m
+        triggered_sensors = (sensor_left<sensor_trigger_dist)+(sensor_right<sensor_trigger_dist)+(sensor_front<sensor_trigger_dist);
+        if (triggered_sensors >= 2)
+            home_reached = true;
+            
+        // if home is not detected spin CW and check again
+        if (!pixy.objectDetected() || (pixy.objectDetected() && pixy.getSignature() == 1)){
+            Brobot::rotate(5);
+            rotate_count++;
+            if (rotate_count > 10)
+                break; //break the loop if the robot rotates enough times in a row without detecting home
+        }
+    }        
+    Brobot::openGrip(); // open the grip and back away
+    wait(0.5); //gives the gripper time to open
+    Brobot::back(); //back up so the gripper clears the brick
+    wait(0.75); // tune so the robot backs up the right distance
+    Brobot::stop();
+    Brobot::rotate(end_rotate_time); // rotate to start searching again
+}
+
+////////////// end new code //////////////////
+
 void Brobot::closeGrip()
 {
     //insert Paul's code here