Mechatronics Robotics
/
BrobotV1
These are the core files for the Robot at Team conception.
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
diff -r 570134040cdf -r ec52258b9472 Brobot.cpp --- 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); }
diff -r 570134040cdf -r ec52258b9472 Brobot.h --- 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 ; };
diff -r 570134040cdf -r ec52258b9472 PID_Control.h --- 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_ */
diff -r 570134040cdf -r ec52258b9472 Pixy.cpp --- 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); }
diff -r 570134040cdf -r ec52258b9472 SpeedControl.cpp --- 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; }
diff -r 570134040cdf -r ec52258b9472 SpeedControl.h --- 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%)
diff -r 570134040cdf -r ec52258b9472 mainV12.cpp --- 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 - - } - } -}
diff -r 570134040cdf -r ec52258b9472 mainV18.cpp --- /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 + + } + } +}