Revision 17:ec52258b9472, committed 2017-06-07

Comitter:

obrie829

Date:

Wed Jun 07 11:35:59 2017 +0000

Parent:

16:570134040cdf

Commit message:

v18

Changed in this revision

--- 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);
 }
 
 

--- a/Brobot.h	Mon Jun 05 07:57:07 2017 +0000
+++ b/Brobot.h	Wed Jun 07 11:35:59 2017 +0000
@@ -32,7 +32,7 @@
     void        back();
     void        stop();
     void        rotate(int ammount); //
-    void        approachBrick();
+    int         rotateAndApproach();
     bool        approachHome();
     void        closeGrip();
     void        openGrip();
@@ -56,8 +56,10 @@
     Ticker t1;
     PID_Control pid;
     PID_Control pixypid;
+    PID_Control pixypidS;
     Pixy& pixy;
     UniServ& servo;
+    float e ;
 
 };
 

--- a/PID_Control.h	Mon Jun 05 07:57:07 2017 +0000
+++ b/PID_Control.h	Wed Jun 07 11:35:59 2017 +0000
@@ -49,7 +49,7 @@
     float max;
     float min;
     float iMax;
-    
+
 };
 
 #endif /* COMMON_PID_CONTROL_H_ */

--- a/Pixy.cpp	Mon Jun 05 07:57:07 2017 +0000
+++ b/Pixy.cpp	Wed Jun 07 11:35:59 2017 +0000
@@ -2,8 +2,8 @@
  
 Pixy::Pixy(Serial& _cam) : cam(_cam), detects(0)
 {
-    //cam.baud( 230400 );
-    cam.baud( 460800 );
+    cam.baud( 230400 );
+    //cam.baud( 460800 );
     cam.attach(this, &Pixy::rxCallback, Serial::RxIrq);
 }
  

--- a/SpeedControl.cpp	Mon Jun 05 07:57:07 2017 +0000
+++ b/SpeedControl.cpp	Wed Jun 07 11:35:59 2017 +0000
@@ -35,8 +35,7 @@
     actualSpeedLeft = 0.0f;
     actualSpeedRight = 0.0f;
 
-    t2.attach( callback(this, &SpeedControl::speedCtrl), 0.05f);  // SpeedControl:: allows you to access funtion within the class file
-
+    t2.attach( callback(this, &SpeedControl::speedCtrl), PERIOD);  // SpeedControl:: allows you to access funtion within the class file
 }
 
 //Destructor
@@ -51,47 +50,22 @@
     short valueCounterRight = counterRight->read();
     short countsInPastPeriodLeft = valueCounterLeft-previousValueCounterLeft;
     short countsInPastPeriodRight = valueCounterRight-previousValueCounterRight;
-
+    
     previousValueCounterLeft = valueCounterLeft;
     previousValueCounterRight = valueCounterRight;
     actualSpeedLeft = speedLeftFilter.filter((float)countsInPastPeriodLeft /COUNTS_PER_TURN/PERIOD*60.0f);
     actualSpeedRight = speedRightFilter.filter((float)countsInPastPeriodRight /COUNTS_PER_TURN/PERIOD*60.0f);
 
-
-    //Error of each wheel from Pauls I code
-
-    short eL = desiredSpeedLeft-actualSpeedLeft;
-    short eR = desiredSpeedRight-actualSpeedRight;
+    // Berechnen der Motorspannungen Uout
+    float voltageLeft = KP*(desiredSpeedLeft-actualSpeedLeft)+desiredSpeedLeft/KN;
+    float voltageRight = KP*(desiredSpeedRight-actualSpeedRight)+desiredSpeedRight/KN;
 
-    /*
-    // Increment the error integral for each wheel
-    integralErrorRight += eR;
-    integralErrorLeft += eL;
-    // Limit the integral term
-    if (integralErrorRight > integralErrorMax)
-        integralErrorRight = integralErrorMax;
-    if (integralErrorRight < -integralErrorMax)
-        integralErrorRight = -integralErrorMax;
-    if (integralErrorLeft > integralErrorMax)
-        integralErrorLeft = integralErrorMax;
-    if (integralErrorLeft < -integralErrorMax)
-        integralErrorLeft = -integralErrorMax;
-    //end of Paul's I control
-    */
-    // + KI*integralErrorLeft and KI*integralErrorRight + were removed from below
-    // P + I Control!
-    float voltageLeft = KP*eL  + desiredSpeedLeft/KN;
-    float voltageRight = KP*eR +  desiredSpeedRight/KN;
-    
-    //printf("the voltage on the L and R sides are %f and %f \n\r", voltageLeft, voltageRight);
-    
     // Berechnen, Limitieren und Setzen der Duty-Cycle
-    float dutyCycleLeft = 0.5f+0.5f*voltageLeft/MAX_VOLTAGE;    // Max_voltage is 12.0f
+    float dutyCycleLeft = 0.5f+0.5f*voltageLeft/MAX_VOLTAGE;
     if (dutyCycleLeft < MIN_DUTY_CYCLE) dutyCycleLeft = MIN_DUTY_CYCLE;
     else if (dutyCycleLeft > MAX_DUTY_CYCLE) dutyCycleLeft = MAX_DUTY_CYCLE;
 
     *pwmLeft = dutyCycleLeft;
-
     float dutyCycleRight = 0.5f+0.5f*voltageRight/MAX_VOLTAGE;
     if (dutyCycleRight < MIN_DUTY_CYCLE) dutyCycleRight = MIN_DUTY_CYCLE;
     else if (dutyCycleRight > MAX_DUTY_CYCLE) dutyCycleRight = MAX_DUTY_CYCLE;
@@ -105,7 +79,7 @@
 //set desired wheel speed in RPM
 void SpeedControl::setDesiredSpeed( float L, float R)
 {
-    desiredSpeedLeft = L * 1.1f ;
+    desiredSpeedLeft = L * 1.18f ;
     desiredSpeedRight = R;
 }
 

--- a/SpeedControl.h	Mon Jun 05 07:57:07 2017 +0000
+++ b/SpeedControl.h	Wed Jun 07 11:35:59 2017 +0000
@@ -27,12 +27,12 @@
 private:
 
     //static allows you to initialize variables within the header
-    static const float PERIOD = 0.001f;                    // period of control task, given in [s]
+    static const float PERIOD = 0.002f;                    // period of control task, given in [s]
     static const float COUNTS_PER_TURN = 1200.0f;          // resolution of encoder counter
     static const float LOWPASS_FILTER_FREQUENCY = 300.0f;  // frequency of lowpass filter for actual speed values, given in [rad/s]
     static const float KN = 40.0f;                         // speed constant of motor, given in [rpm/V]
     static const float KP = 0.2f;                          // speed controller gain, given in [V/rpm]
-    static const float KI = 0.1f;                          //speed controller integral term gain [V/rpm]
+    //static const float KI = 0.1f;                          //speed controller integral term gain [V/rpm]
     static const float MAX_VOLTAGE = 12.0f;                // supply voltage for power stage in [V]
     static const float MIN_DUTY_CYCLE = 0.02f;             // minimum allowed value for duty cycle (2%)
     static const float MAX_DUTY_CYCLE = 0.98f;             // maximum allowed value for duty cycle (98%)

--- a/mainV12.cpp	Mon Jun 05 07:57:07 2017 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,115 +0,0 @@
-#include "mbed.h"
-#include "Brobot.h"
-//#include "PID_Control.h"
-#include "SpeedControl.h"
-
-//communication
-Serial pc(USBTX, USBRX);
-Serial cam(PA_9, PA_10);  // we are only using PA_9 to recieve
-
-//Perophery for distance sensors
-AnalogIn distance(PB_1);
-DigitalIn button(USER_BUTTON);
-DigitalOut enable(PC_1);  // sensors
-DigitalOut bit0(PH_1);
-DigitalOut bit1(PC_2);
-DigitalOut bit2(PC_3);
-DigitalOut led(LED1);
-Pixy pixy(cam);
-
-
-//indicator leds arround robot
-DigitalOut leds[] = { PC_8, PC_6, PB_12, PA_7, PC_0, PC_9 };
-
-//motor stuff
-DigitalOut enableMotorDriver(PB_2);
-PwmOut pwmL(PA_8);
-PwmOut pwmR(PA_9);
-UniServ servo(PC_4); // Digital Out Pin
-
-EncoderCounter counterLeft(PB_6, PB_7);
-EncoderCounter counterRight(PA_6, PC_7);
-
-SpeedControl speedctrl(&pwmL, &pwmR, &counterLeft, &counterRight);
-
-Brobot stingray(speedctrl, distance, enable, bit0, bit1, bit2, leds, pixy, servo);  // passing in variables connected to PIN
-// should see LEDshow once constructed
-
-int main()
-{
-    enableMotorDriver = 0;  //safety precaution
-    enable = 1 ;            //for debugging and output
-
-    enum robot_states {idle=0, search, approach, goHome, atHome};   //so that we can reference states by name vs number
-    int robot_state=idle;    // define and start in idle state
-    int count = 0 ;
-
-    while(1) {
-        wait( 0.02f ); //required to allow time for ticker
-
-        stingray.startLeds();
-
-        switch(robot_state) {
-            case idle:
-                if(button == 0) {       // 0 is pressed
-                    enableMotorDriver =1;
-                    robot_state=search;         // next state
-                    //printf("leaving the idle state \n\r");
-                    //wait(0.1);
-                }
-                break;
-
-            case search:
-                /*
-                while( !stingray.foundGreenBrick() ) {
-                    stingray.avoidObstacleAndMove(20);  // in rpm
-                }
-                */
-
-                stingray.avoidObstacleAndMove(20);  // in rpm
-                //robot_state=approach;
-                break;
-
-            case approach:
-            
-                stingray.stop();
-
-                //stingray.approachBrick();
-                //stingray.closeGrip();
-                // find way to determine if lego is successfully grabbed before switching states
-                // robot_state=goHome;
-
-                break;
-
-            case goHome:
-
-                //printf("In the GoHome state \n\r");
-
-                if(stingray.foundHome() == false && count<=5) { // tune 5 so that robot rotates ~360 degrees
-                    stingray.rotate(10);
-                    count++;
-                }
-                while( stingray.foundHome() == false && count>5 ) {
-                    stingray.avoidObstacleAndMove(20);  // search until home is found
-                }
-                // if this point is reached, we have foundHome() == true
-                stingray.approachHome();
-
-                robot_state = atHome;
-                count =0;
-
-                break;
-
-            case atHome:
-
-                stingray.openGrip();
-                wait(0.5);  // may not be necessary but allows grip time
-                stingray.back(); // goes at 15rpms
-                wait(1);
-                stingray.rotate(10);  // enter rotational velocity in rpm between +-25
-                wait(0.5);  // tune so that rotation is about 180deg
-                robot_state=search;  // restart at searching state
-
-        }
-    }
-}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mainV18.cpp	Wed Jun 07 11:35:59 2017 +0000
@@ -0,0 +1,111 @@
+#include "mbed.h"
+#include "Brobot.h"
+#include "SpeedControl.h"
+
+//communication
+Serial pc(USBTX, USBRX);
+Serial cam(PB_10, PC_5);  // we are only using PA_9 to recieve
+
+//Perophery for distance sensors
+AnalogIn distance(PB_1);
+DigitalIn button(USER_BUTTON);
+DigitalOut enable(PC_1);  // sensors
+DigitalOut bit0(PH_1);
+DigitalOut bit1(PC_2);
+DigitalOut bit2(PC_3);
+DigitalOut led(LED1);
+Pixy pixy(cam);
+
+
+//indicator leds arround robot
+DigitalOut leds[] = { PC_8, PC_6, PB_12, PA_7, PC_0, PC_9 };
+
+//motor stuff
+DigitalOut enableMotorDriver(PB_2);
+PwmOut pwmL(PA_8);
+PwmOut pwmR(PA_9);
+UniServ servo(PC_4); // Digital Out Pin
+
+EncoderCounter counterLeft(PB_6, PB_7);
+EncoderCounter counterRight(PA_6, PC_7);
+
+SpeedControl speedctrl(&pwmL, &pwmR, &counterLeft, &counterRight);
+
+Brobot stingray(speedctrl, distance, enable, bit0, bit1, bit2, leds, pixy, servo);  // passing in variables connected to PIN
+
+int main()
+{
+    enableMotorDriver = 0;  //safety precaution
+    enable = 1 ;            //for debugging and output
+
+    enum robot_states {idle=0, search, approach, goHome, atHome};   //so that we can reference states by name vs number
+    int robot_state  = idle;    // define and start in idle state
+    int count = 0 ;
+    int timer = 0;
+
+    while(1) {
+        wait( 0.01f ); //required to allow time for ticker
+
+        if( ++timer%100==0)
+            led=!led;
+
+        switch(robot_state) {
+            case idle:
+                if(button == 0) {       // 0 is pressed
+                    enableMotorDriver = 1;
+                    robot_state=search;         // next state
+                }
+                break;
+
+            case search:
+
+                stingray.avoidObstacleAndMove(12);  // in rpm
+                stingray.openGrip();  //just trying to get lucky
+
+                if(stingray.foundGreenBrick()) {
+                    stingray.stop();
+                    leds[3] = 1;
+                    robot_state = approach;
+                }
+                break;
+
+            case approach:
+
+                int rotateAndApproachState = stingray.rotateAndApproach();
+                if(rotateAndApproachState== 1) robot_state = goHome ; // gripper has block
+                else if(rotateAndApproachState == 2) robot_state = search ; // block never made it to gripper\
+                count = 0;
+
+                break; // continue approach
+
+            case goHome:
+
+                if(stingray.foundHome() == false && count <= 750) { // tune 5 so that robot rotates ~360 degrees
+                    stingray.rotate(5);
+                    count++;
+                }
+                if( stingray.foundHome() == false && count > 750 ) {
+                    stingray.avoidObstacleAndMove(10);  // search until home is found
+                }
+                static bool foundHomeFlag=false;
+                // if this point is reached, we have foundHome() == true
+                if( stingray.foundHome() || foundHomeFlag ) {
+                    foundHomeFlag=true;
+                    if(stingray.approachHome())
+                        robot_state = atHome ;
+                }
+                break;
+
+            case atHome:
+
+                stingray.openGrip();
+                wait(0.5);  // may not be necessary but allows grip time
+                stingray.back(); // goes at 15rpms
+                wait(1);
+                stingray.rotate(10);  // enter rotational velocity in rpm between +-25
+                wait(0.5);  // tune so that rotation is about 180deg
+                robot_state=search;  // restart at searching state
+
+        }
+    }
+}