Diff: Brobot.cpp

Revision:

17:ec52258b9472

Parent:

15:4efc66de795a

--- a/Brobot.cpp	Mon Jun 05 07:57:07 2017 +0000
+++ b/Brobot.cpp	Wed Jun 07 11:35:59 2017 +0000
@@ -20,15 +20,16 @@
 
     //                      kp        ki       kd      min     max
     //pid.setPIDValues(      0.025f,    0.00005f,  0.001f,   0.5f, -0.5f, 1000);
-    pid.setPIDValues(      0.4f,    0.001f,  0.001f,   0.5f, -0.5f, 1000);
-    pixypid.setPIDValues(  0.0002f, 0.000f, 0.000f, 0.3f, -0.3f, 1000);
+    pid.setPIDValues(      0.4f,    0.001f, 0.001f, 0.5f, -0.5f, 1000);
+    pixypid.setPIDValues(  0.005f, 0.0f, 0.000f, 25.0f, -25.0f, 1000);
+    //pixypidS.setPIDValues( 0.01f,   0.0f,   0.0f, 20.0f,-20.0f,   1000);
+    //e = 0 ;
 
 }
 
 void Brobot::avoidObstacleAndMove(int vtrans)
 {
     float vrot = 0;   // rpm
-    static float e = 0.0f ;
 
     if(sensor_left.read() <0.3f || sensor_right.read() <0.3f) { // to avoid obstacles
 
@@ -37,40 +38,33 @@
 
         vrot = diff*50; //turn
 
-        if( vrot<= -20)
-            vrot=-20;
-        else if (vrot > 20)
-            vrot=20;
+        if( vrot<= -10)
+            vrot=-10;
+        else if (vrot > 10)
+            vrot=10;
     }
 
     speedctrl.setDesiredSpeed( vtrans - vrot, -vtrans - vrot );
-    int delay = vtrans/100 ;
 
     if(sensor_front.read() <=0.25f) { // when approaching normal to wall
         speedctrl.setDesiredSpeed( 0, 0 );
 
         int direction=rand()%2 ;    // so there is variablility in the robots path
         if(direction==0) {
-            //stingray.rotate(10);
-            speedctrl.setDesiredSpeed(15, 15 );
-            wait(delay);
+            speedctrl.setDesiredSpeed(5, 5 );
+            wait(0.5);
         } else if (direction==1) {
-            //stingray.rotate(-10);
-            speedctrl.setDesiredSpeed( -15, -15 );
-            wait(delay);
+            speedctrl.setDesiredSpeed( -5, -5 );
+            wait(0.5);
         }
     }
 }
 
-void Brobot::startLeds()
-{
-    t1.attach( callback(this, &Brobot::ledShow), 0.05f );
-}
-
 void Brobot::ledDistance()
 {
     for( int ii = 0; ii<6; ++ii)
         sensors[ii]< 0.1f ? leds[ii].write(1) : leds[ii].write(0);   // an if statement in one line
+    //printf("the L and R sensor values are %f and %f \n\r", sensor_left, sensor_right);
 }
 
 void Brobot::ledShow()
@@ -82,15 +76,14 @@
     //quit ticker and start led distance show
     if( ++timer > 10) {
         t1.detach();
-        t1.attach( callback(this, &Brobot::ledDistance), 0.01f );
+        t1.attach( callback(this, &Brobot::ledDistance), 0.05f );
     }
 }
 
 bool Brobot::foundGreenBrick()
 {
-
     if( pixy.objectDetected() ) {
-        if( pixy.getSignature() == 1) {  // 1 is the green brick
+        if( pixy.getSignature() == 1) { // 1 is the green brick
             return true;
         }
     }
@@ -100,67 +93,97 @@
 bool Brobot::foundHome()
 {
     if( pixy.objectDetected() ) {
-        if( pixy.getSignature() == 2) {  // 2 is the home
+        if( pixy.getSignature() == 2) {  // 2 is the red home
             return true;
         }
     }
     return false;
 }
 
-void Brobot::approachBrick()
+int Brobot::rotateAndApproach()
 {
-    // Crystal's code
-    float e = 160-pixy.getX();
-    float diff = pixypid.calc( e, 0.005f );
-    speedctrl.setDesiredSpeed( -(diff*50.0f), -(diff*50.0f)) ;
-    // function assumes Brick is in open grips
+    static float objectDetected = 1.0f;  // assumes we can already detect object
+
+    objectDetected = 0.95f * objectDetected + 0.05f * pixy.objectDetected();// filtering for flickering pixy
+
+    if( pixy.getSignature() == 1 && objectDetected > 0.2f) {
+
+        leds[3] = !leds[3] ;
+
+        float pixyX = pixy.getX();
+        float pixyY = pixy.getY();
+        float error = 160-pixyX;
+        float vrot = error * 0.25f ;
+        float vtrans = (200.0f - pixyY) * 0.25f;
+
+        if( vrot > 5) vrot = 5;
+        if( vrot < -5) vrot = -5;
+        if( vtrans > 5) vtrans = 5;
+        if( vtrans < -5) vtrans = -5;
+
+        //speedctrl.setDesiredSpeed( vtrans -vrot , -vtrans -vrot ) ;
+        speedctrl.setDesiredSpeed( -vrot , -vrot ) ;
+
+        if ((error < 15)&&(error>-15)) {
+            speedctrl.setDesiredSpeed( vtrans -vrot , -vtrans -vrot ) ;
+        }
+
+        if( pixyY >= 180) { // assumes centered on brick
+            speedctrl.setDesiredSpeed(8.0f, -8.0f); // blindly drive forward
+            wait(0.5);
+            Brobot::closeGrip();
+            return 1; // need to transfer to GoHome state
+        }
+        return 3;  // continue the approach function
+    } else return 2; // return to search state, no object is detected
 }
 
 bool 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 sensor_trigger_dist=0.3;
+    static float objectDetected = 1.0f;  // assumes we can already detect object
 
-    while(!home_reached) {
-        // find the error in the x alignment and calculate the restoring voltage
-        x_e = pixy.getX()-160;
-        diff = 50*pid.calc(x_e,0.05);
+    objectDetected = 0.98f * objectDetected + 0.02f * pixy.objectDetected();// filtering for flickering pixy
+    leds[4]=0;
+    if(objectDetected>0.1f) leds[4]=1;
+    
+    if( (pixy.getSignature() == 2) && (objectDetected > 0.1f)) {
+
+        float pixyX = pixy.getX();
+        float pixyY = pixy.getY();
+        float error = 160-pixyX;
+        float vrot = error * 0.25f ;
+        float vtrans = (200.0f - pixyY) * 0.25f;
+
 
-        // if home is detected drive the robot forward while continuously realigning the robot
-        if (pixy.objectDetected() && (pixy.getSignature() == 2)) {
-            speedctrl.setDesiredSpeed(15-diff,-15-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( pixyY >= 50) return true; // need to transfer to atHome state
+        if( vrot > 4) vrot = 4;
+        if( vrot < -4) vrot = -4;
+        if( vtrans > 4) vtrans = 4;
+        if( vtrans < -4) vtrans = -4;
 
-        // 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
+        //speedctrl.setDesiredSpeed( vtrans -vrot , -vtrans -vrot ) ;
+        speedctrl.setDesiredSpeed( -vrot , -vrot ) ;
+
+        if ((error <= 25) && (error>= -25)) {
+            speedctrl.setDesiredSpeed( vtrans -vrot , -vtrans -vrot ) ;
         }
-    }
-    return home_reached;
+
+        return false; 
+    } else return false; // return to goHome case, no object is detected
 }
 
 void Brobot::closeGrip()
 {
-    int close=600;
+    int close=1400;
     servo.write_us(close);
+    wait(0.5);
 }
 
 void Brobot::openGrip()
 {
-    int open=2100;
+    int open=1900;
     servo.write_us(open);
+    wait(0.5);
 }