![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
vf
Dependencies: Servo ServoArm mbed
Fork of PES_Official by
Revision 17:4e1be70bdedb, committed 2017-05-18
- Comitter:
- beacon
- Date:
- Thu May 18 14:27:09 2017 +0000
- Parent:
- 15:915f8839fe48
- Commit message:
- Si compila.
Changed in this revision
diff -r 915f8839fe48 -r 4e1be70bdedb Headers/Declarations.h --- a/Headers/Declarations.h Thu May 11 18:57:45 2017 +0000 +++ b/Headers/Declarations.h Thu May 18 14:27:09 2017 +0000 @@ -2,8 +2,8 @@ #define DECLARATIONS_H //DistanceSensors: -#define NEAR 0.18f //Used for distance Sensors. If they're to near to a wall -> turn -#define NEAR_LEGO 0.20f //If the DistanceSensors are near to a Lego... +#define NEAR 0.16f //Used for distance Sensors. If they're to near to a wall -> turn +#define NEAR_LEGO 0.12f //If the DistanceSensors are near to a Lego... #define LEFT_L 5 //Arrayindex of the left LEGO Sensor & left LED #define FWD_L 0 //Arrayindex of the front LEGO Sensor & front LED
diff -r 915f8839fe48 -r 4e1be70bdedb Headers/Robot.h --- a/Headers/Robot.h Thu May 11 18:57:45 2017 +0000 +++ b/Headers/Robot.h Thu May 18 14:27:09 2017 +0000 @@ -5,6 +5,7 @@ #include <mbed.h> #include "Servo.h" #include "ServoArm.h" +#include <Ultraschall.h> #include "mbed.h" @@ -52,7 +53,7 @@ private: - AnalogIn* FarbVoltage; + AnalogIn* FarbVoltage; }; class Arm{ @@ -84,8 +85,8 @@ void init(Servo* servoLeiste); - int UpToDown(); - int DownToUp(); + int upToDown(); + int downToUp(); private: @@ -112,13 +113,27 @@ }; +class USsensor +{ + public: + + USsensor(); + USsensor(Ultraschall* Usensor); + void init(Ultraschall* Usensor); + float read(); + operator float(); + + private: + Ultraschall* Usensor; +}; + class Robot { public: //Robot related: - Robot(PwmOut* left, PwmOut* right, DigitalOut* powerSignal, DigitalOut* leds, AnalogIn* FarbVoltage, AnalogIn* frontS, AnalogIn* leftS, AnalogIn* rightS, ServoArm* Arm, Servo* Greifer, Servo* Leiste ); + Robot(PwmOut* left, PwmOut* right, DigitalOut* powerSignal, DigitalOut* leds, AnalogIn* FarbVoltage, AnalogIn* frontS, AnalogIn* leftS, AnalogIn* rightS, ServoArm* Arm, Servo* Greifer, Servo* Leiste, Ultraschall* USsensor ); //Drive Functions void drive(); @@ -132,6 +147,8 @@ void stop(); //Functions that use the drive functions + void driveSlowly(); + void driveBackSlowly();/* void wallRight(int* counter, int* timer, int* lastAct); void wallLeft(int* counter, int* timer, int* lastAct); void wallFront(int* counter, int* timer, int* lastAct); @@ -142,13 +159,15 @@ void legoLeft(int* counter, int* timer, int* lastAct, int* legoFound); void nothingFound(int* counter, int* timer, int* lastAct); - + */ int search(int* timer); + float see(int sensor); int getErrorMotor(); //DistanceSensors related: DistanceSensors sensors[9]; + USsensor USsensor; AnalogIn* frontS; AnalogIn* leftS; AnalogIn* rightS; @@ -162,6 +181,7 @@ //Farbsensors related: Farbsensor FarbVoltage; + DigitalOut* led; //Arm related: Arm Arm;
diff -r 915f8839fe48 -r 4e1be70bdedb Headers/Ultraschall.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Headers/Ultraschall.h Thu May 18 14:27:09 2017 +0000 @@ -0,0 +1,47 @@ +#ifndef ULTRASCHALL_H +#define ULTRASCHALL_H + +#include "mbed.h" + +/** Ultraschall Class(es) + */ + +class Ultraschall +{ +public: + /** Create a Ultraschall object connected to the specified pin + * @param pin i/o pin to connect to + */ + Ultraschall(); + Ultraschall(PinName TrigPin,PinName EchoPin); + ~Ultraschall(); + + /** Return the distance from obstacle in cm + * @param distance in cms and returns -1, in case of failure + */ + unsigned int get_dist_cm(void); + /** Return the pulse duration equal to sonic waves travelling to obstacle and back to receiver. + * @param pulse duration in microseconds. + */ + unsigned int get_pulse_us(void); + /** Generates the trigger pulse of 10us on the trigger PIN. + */ + void start(void); + void isr_rise(void); + void isr_fall(void); + void fall (void (*fptr)(void)); + void rise (void (*fptr)(void)); + + + +private: + + Timer pulsetime; + DigitalOut trigger; + InterruptIn echo; + unsigned int pulsedur; + unsigned int distance; +}; + +#endif + \ No newline at end of file
diff -r 915f8839fe48 -r 4e1be70bdedb Sources/Arm.cpp --- a/Sources/Arm.cpp Thu May 11 18:57:45 2017 +0000 +++ b/Sources/Arm.cpp Thu May 18 14:27:09 2017 +0000 @@ -13,7 +13,7 @@ this->arm = arm; this->arm->calibrate(0.0015f, 180.0f); - this->arm->position(COLLECT_POS); + this->arm->position(RELEASE_POS); } int Arm::collectToDown(){
diff -r 915f8839fe48 -r 4e1be70bdedb Sources/DistanceSensors.cpp --- a/Sources/DistanceSensors.cpp Thu May 11 18:57:45 2017 +0000 +++ b/Sources/DistanceSensors.cpp Thu May 18 14:27:09 2017 +0000 @@ -38,7 +38,7 @@ { float Usensor; - + if (number < 6){ *bit0 = number & 1; // Set the first bit of the Sensors MUX *bit1 = number & 2; // Set the second bit of the Sensors MUX @@ -61,8 +61,15 @@ } //Usensor=sensorVoltage->read(); //Read the Voltage from the selected distance sensor - float Distance=-0.38f*sqrt(Usensor)+0.38f; + Usensor *= 3.3f; + float Distance= 5.906*Usensor*Usensor - 30.831*Usensor + 47.628; + Distance /= 100; + + static float distance_filtered = 0.0f; + distance_filtered = 0.55f * distance_filtered + 0.45f * Distance; + return Distance; + return distance_filtered; } DistanceSensors::operator float()
diff -r 915f8839fe48 -r 4e1be70bdedb Sources/Greifer.cpp --- a/Sources/Greifer.cpp Thu May 11 18:57:45 2017 +0000 +++ b/Sources/Greifer.cpp Thu May 18 14:27:09 2017 +0000 @@ -19,14 +19,14 @@ void Greifer::init(Servo* greifer) { this->greifer = greifer; - greifer->calibrate(0.0017f, 180.0f); - greifer->position(60.0f); + greifer->calibrate(0.001f, 90.0f); + greifer->position(65.0f); } int Greifer::take() { - this->greifer->position(-155.0f); + this->greifer->position(-70.0f); static int time = 0; if( time < 10 ){ time++; @@ -40,7 +40,7 @@ int Greifer::leave() { - this->greifer->position(60.0f); + this->greifer->position(65.0f); static int time = 0; if( time < 10 ){ time++;
diff -r 915f8839fe48 -r 4e1be70bdedb Sources/Leiste.cpp --- a/Sources/Leiste.cpp Thu May 11 18:57:45 2017 +0000 +++ b/Sources/Leiste.cpp Thu May 18 14:27:09 2017 +0000 @@ -15,7 +15,7 @@ this->leiste->position(UP_POS); } -int Leiste::UpToDown(){ +int Leiste::upToDown(){ static float pos=UP_POS; if(pos>DOWN_POS) { pos-=3; @@ -28,7 +28,7 @@ } } -int Leiste::DownToUp(){ +int Leiste::downToUp(){ static float pos=DOWN_POS; if(pos<UP_POS) { pos+=3;
diff -r 915f8839fe48 -r 4e1be70bdedb Sources/Robot.cpp --- a/Sources/Robot.cpp Thu May 11 18:57:45 2017 +0000 +++ b/Sources/Robot.cpp Thu May 18 14:27:09 2017 +0000 @@ -3,7 +3,7 @@ /* Work in progress -------------------------------------------- */ -Robot::Robot(PwmOut* left, PwmOut* right, DigitalOut* powerSignal, DigitalOut* leds, AnalogIn* FarbVoltage, AnalogIn* frontS, AnalogIn* leftS, AnalogIn* rightS, ServoArm* Arm, Servo* Greifer, Servo* Leiste ) +Robot::Robot(PwmOut* left, PwmOut* right, DigitalOut* powerSignal, DigitalOut* leds, AnalogIn* FarbVoltage, AnalogIn* frontS, AnalogIn* leftS, AnalogIn* rightS, ServoArm* Arm, Servo* Greifer, Servo* Leiste, Ultraschall* USsensor ) { this->left = left; this->right = right; @@ -23,6 +23,7 @@ this->Arm = Arm; this->Greifer = Greifer; this->Leiste = Leiste; + this->USsensor = USsensor; } @@ -97,7 +98,29 @@ *right= 0.5f; } +void Robot::driveSlowly(){ + static int i = 0; + i++; + if( i % 2 ){ + this->drive(); + } + else{ + this->stop(); + } +} +void Robot::driveBackSlowly(){ + static int i = 0; + i++; + if( i % 2 ){ + this->driveB(); + } + else{ + this->stop(); + } +} + +/* //Functions that use the drive functions void Robot::counterMax(int* counter, int* timer, int* lastAct, int* rando){ if (*lastAct != 0){ //If this wasn't the last called action, reset the timer. @@ -166,7 +189,7 @@ } if (this->sensors[FWD_L] > 0.16f){ - this->drive(); + this->driveSlowly(); } else{ this->stop(); @@ -219,42 +242,50 @@ } this->drive(); -} +}*/ int Robot::search(int* timer){ - //return 1; enum states {neutral = 0, max, wallF, wallL, wallR, legoF, legoL, legoR }; static int state = neutral; - static int counter = 0; //counter is used blabla + static int counter = 0; //counter is used blabla + + static int rando = -1; + + static int lastAct = 0; - static int rando = -1; + //static int stay = -1; //Stay is used to remain in a certain state + /* + this->sensors[FWD_L] < NEAR ? this->leds[4] = 1 : this->leds[4] = 0; + this->sensors[RIGHT_L] < NEAR ? this->leds[RIGHT_L] = 1 : this->leds[RIGHT_L] = 0; + this->sensors[LEFT_L] < NEAR ? this->leds[LEFT_L] = 1 : this->leds[LEFT_L] = 0; + */ - //static int lastAct = 0; + //printf("\n\rcurrent robot state: %d", state); switch( state ){ case neutral: if( counter > MAX ){ state = max; } - else if( this->sensors[FWD] < NEAR ){ + else if( this->see(FWD) < NEAR ){ state = wallF; } - else if( this->sensors[LEFT] < NEAR ){ + else if( this->see(LEFT) < NEAR ){ state = wallL; } - else if( this->sensors[RIGHT] < NEAR ){ + else if( this->see(RIGHT) < NEAR ){ state = wallR; } - else if( this->sensors[FWD_L] < NEAR ){ + else if( this->see(FWD_L) < NEAR_LEGO + 0.015f ){ state = legoF; } - else if( this->sensors[LEFT_L] < NEAR ){ + else if( this->see(LEFT_L) < NEAR_LEGO ){ state = legoL; } - else if( this->sensors[RIGHT_L] < NEAR ){ + else if( this->see(RIGHT_L) < NEAR_LEGO ){ state = legoR; } else{ @@ -265,7 +296,7 @@ case max: { int time = 0; - if( time < 15 && this->sensors[FWD] > NEAR ){ + if( time < 15 && this->see(FWD) > NEAR ){ rando == -1 ? rando = rand() % 2 : rando = rando; this->turnAround(rando); } @@ -279,7 +310,7 @@ case wallF: counter++; - if( this->sensors[FWD] < NEAR ){ + if( this->see(FWD) < NEAR ){ rando == -1 ? rando = rand() % 2 : rando = rando; this->turnAround(rando); } @@ -291,7 +322,7 @@ case wallL: counter++; - if( this->sensors[LEFT] < NEAR ){ + if( this->see(LEFT) < NEAR ){ this->turnRight(); } else{ @@ -301,7 +332,7 @@ case wallR: counter++; - if( this->sensors[RIGHT] < NEAR ){ + if( this->see(RIGHT) < NEAR ){ this->turnLeft(); } else{ @@ -311,11 +342,16 @@ case legoF: //counter++; - if( this->sensors[FWD_L] > 0.17f ){ - this->drive(); + if( this->see(FWD) < NEAR ){ + state = wallF; + //stay = -1; + } + else if( this->see(FWD_L) < 0.19f ){ + this->driveBackSlowly(); } else{ state = neutral; + //stay = -1; counter = 0; this->stop(); return 1; @@ -324,21 +360,25 @@ case legoL: counter++; - if( this->sensors[FWD_L] < NEAR_LEGO + 0.05f ){ + if( this->see(FWD_L) > NEAR_LEGO + 0.05f ){ this->turnLeft(); } else{ - state = legoF; + state = neutral; + this->drive(); + //stay = 1; } break; case legoR: counter++; - if( this->sensors[FWD_L] < NEAR_LEGO + 0.05f ){ + if( this->see(FWD_L) > NEAR_LEGO + 0.05f ){ this->turnRight(); } else{ - state = legoF; + this->drive(); + state = neutral; + //stay = 1; } break; @@ -346,56 +386,15 @@ return 0; } -int Robot::getErrorMotor(){ - return 0; //errorMotor; +float Robot::see(int sensor){ + if( sensor == FWD_L ){ + return this->USsensor.read(); + } + else{ + return this->sensors[sensor].read(); + } } -/*int Robot::searchh(int* timer){ - enum states {neutral = 0, max, wallF, wallL, wallR, legoF, legoL, legoR }; - - static int state = neutral; - - static int counter = 0; //counter is used blabla - - //static int lastAct = 0; - - switch( state ){ - case neutral: - if( counter > MAX ){ - state = max; - } - else if( this->sensora[FWD] < NEAR ){ - state = wallF; - counter = 0; - } - else if( this->sensors[LEFT] < NEAR ){ - state = wallL; - counter = 0; - } - else if( this->sensors[RIGHT] < NEAR ){ - state = wallR; - counter = 0; - } - else if( this->sensors[FWD_L] < NEAR ){ - state = legoF; - counter = 0; - } - else if( this->sensors[LEFT_L] < NEAR ){ - state = legoL; - counter = 0; - } - else if( this->sensors[RIGHT_L] < NEAR ){ - state = legoR; - counter = 0; - } - break; - - case max: - - break; - */ - -//reset -//} - -//Add management for Overpower!! Pin PB_15 \ No newline at end of file +int Robot::getErrorMotor(){ + return 0; //errorMotor; +} \ No newline at end of file
diff -r 915f8839fe48 -r 4e1be70bdedb Sources/USsensor.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Sources/USsensor.cpp Thu May 18 14:27:09 2017 +0000 @@ -0,0 +1,37 @@ +#include <Robot.h> + +USsensor::USsensor() +{ +} + +USsensor::USsensor(Ultraschall* Usensor) +{ + init(Usensor); +} +void USsensor::init(Ultraschall* Usensor) +{ + this->Usensor = Usensor; +} + +float USsensor::read() +{ + static int i = 0; + Usensor->start(); + if( i ){ + float dist = Usensor->get_dist_cm(); + static float distFiltered = dist; + + distFiltered = 0.05f * distFiltered + 0.95f * dist; + return distFiltered / 100; + //return dist/100.0f; + } + else{ + i++; + return 0.25f; + } +} + + USsensor::operator float() +{ + return read(); +} \ No newline at end of file
diff -r 915f8839fe48 -r 4e1be70bdedb Sources/Ultraschall.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Sources/Ultraschall.cpp Thu May 18 14:27:09 2017 +0000 @@ -0,0 +1,53 @@ +#include "Ultraschall.h" + + +Ultraschall::Ultraschall(PinName TrigPin,PinName EchoPin): + trigger(TrigPin), echo(EchoPin) +{ + pulsetime.stop(); + pulsetime.reset(); + echo.rise(this,&Ultraschall::isr_rise); + echo.fall(this,&Ultraschall::isr_fall); + trigger=0; +} + +Ultraschall::~Ultraschall() +{ +} + +void Ultraschall::isr_rise(void) +{ + pulsetime.start(); +} +void Ultraschall::start(void) +{ + trigger=1; + wait_us(10); + trigger=0; +} + +void Ultraschall::isr_fall(void) +{ + pulsetime.stop(); + pulsedur = pulsetime.read_us(); + distance= (pulsedur*343)/20000; + pulsetime.reset(); +} + +void Ultraschall::rise (void (*fptr)(void)) +{ + echo.rise(fptr); +} +void Ultraschall::fall (void (*fptr)(void)) +{ + echo.fall(fptr); +} + +unsigned int Ultraschall::get_dist_cm() +{ + return distance; +} +unsigned int Ultraschall::get_pulse_us() +{ + return pulsedur; +} \ No newline at end of file
diff -r 915f8839fe48 -r 4e1be70bdedb Sources/main.cpp --- a/Sources/main.cpp Thu May 11 18:57:45 2017 +0000 +++ b/Sources/main.cpp Thu May 18 14:27:09 2017 +0000 @@ -1,6 +1,7 @@ #include "mbed.h" #include "Robot.h" #include "Declarations.h" +#include "Ultraschall.h" #include <cstdlib> @@ -18,6 +19,10 @@ AnalogIn leftS(A2); AnalogIn rightS(A3); +//Ultraschall +Ultraschall usensor(D6,D5); + + //Leds related: DigitalOut leds[6] = { PC_8, PC_6, PB_12, PA_7, PC_0, PC_9 }; @@ -38,11 +43,14 @@ //Leiste: Servo servoLeiste(PB_6); +//Button: +DigitalIn mybutton(USER_BUTTON); + //Farbsensor: AnalogIn FarbVoltage(A0); DigitalOut led(D2); -Robot sam( &left, &right, &powerSignal, leds, &FarbVoltage, &frontS, &leftS, &rightS, &servoArm, &servoGreifer, &servoLeiste ); //Implement the Farbsensor into the Robot init function!! +Robot sam( &left, &right, &powerSignal, leds, &FarbVoltage, &frontS, &leftS, &rightS, &servoArm, &servoGreifer, &servoLeiste, &usensor ); //Implement the Farbsensor into the Robot init function!! void initializeDistanceSensors() { @@ -63,41 +71,101 @@ //int lastFun = -1; //Is used, to check if the same action in Robot::search() was made multiple times. //int found = 0; //0:= no block available, 1 := a lego is ready to be picked up //int done = 0; - - enum states { search = 0, LeisteDown, forward, downward, down, upward, color, backwardDrop, readyDrop, backward, LeisteUp }; + int color; + + enum states { search = 0, LeisteDown, turn, push, backOff, forward, downward, down, upward, colorS, readyDrop, backward, LeisteUp }; int state = search; - wait(5); + + static float messung = 0; + + while( 1 ){ + printf("\n\r%f", sam.see(FWD_L)); + + wait(1.0f); + + } + while( 1 ) { if ( timer > TIMEOUT ) { NVIC_SystemReset(); //Resets Sam. } + //if( timer == 0 ) + //printf("\n\rLEFT: %.3f,\tFWD: %.3f,\tRIGHT: %.3f", sam.sensors[LEFT].read(), sam.sensors[FWD].read(), sam.sensors[RIGHT].read()); + + + //printf("\n\rcurrent main state: %d", state); + + sam.sensors[FWD_L].read() < NEAR ? sam.leds[1] = 1 : sam.leds[1] = 0; switch( state ) { case search: - if( 1 || sam.search(&timer) ){ - state = LeisteDown; - timer = 0; + if( sam.search(&timer) ){ + //sam.Greifer.nullPos(); + state = LeisteDown; + timer = 0; } - break; case LeisteDown: - if( sam.Leiste.UpToDown() ){ - sam.Greifer.leave(); - state = forward; - timer = 0; + int count = 0; + if( sam.Leiste.upToDown() ){ + //sam.Greifer.leave(); + state = turn; + timer = 0; + } + break; + + case turn: + static int i = 0; + if( i > 7 ){ + sam.stop(); + state = push; + i = 0; + } + else{ + i++; + sam.turnRight(); } + break; + case push:{ + static int i = 0; + if( i > 5 ){ + sam.stop(); + i = 0; + state = backOff; + timer = 0; + } + else{ + sam.driveSlowly(); + i++; + } break; + } + + case backOff:{ + static int i = 0; + if( i > 1 ){ + sam.stop(); + i = 0; + state = forward; + timer = 0; + } + else{ + sam.driveBackSlowly(); + i++; + } + break; + } + case forward: if( sam.Arm.backToCollect() ){ state = downward; timer = 0; } - break; case downward: @@ -105,7 +173,6 @@ state = down; timer = 0; } - break; case down: @@ -113,20 +180,18 @@ state = upward; timer = 0; } - break; case upward: if( sam.Arm.downToCollect() ){ - state = color; + state = colorS; timer = 0; } - break; - case color: { + case colorS: led = 1; - int color = sam.FarbVoltage.read(); + color = sam.FarbVoltage.read(); if( color == -1 ){ //Do nothing @@ -134,7 +199,7 @@ else if( color == 0 || color == GREEN ){ - state = backwardDrop; + state = backward; led = 0; timer = 0; } @@ -149,19 +214,15 @@ //Shit... } break; - } - - case backwardDrop: - if( sam.Arm.collectToBack() ){ - state = readyDrop; - timer = 0; - } - - break; - + case readyDrop: if( sam.Greifer.leave() ){ - state = backward; + if( color == GREEN || color == 0 ){ + state = LeisteUp; + } + else{ + state = backward; + } timer = 0; } @@ -169,124 +230,29 @@ case backward: if( sam.Arm.collectToBack() ){ - sam.Greifer.nullPos(); + //sam.Greifer.nullPos(); state = LeisteUp; timer = 0; + if( color == GREEN || color == 0 ){ + state = readyDrop; + sam.Greifer.leave(); + } + else{ + state = LeisteUp; + } } - break; case LeisteUp: - if( sam.Leiste.DownToUp() ){ + if( sam.Leiste.downToUp() ){ state = search; timer = 0; - wait(2); } break; - } - + } timer++; wait(0.1f); } return 0; -} - - - - /* - if (sam.search(&counter, &timer)) { - - while(!done) { - sam.Arm.collectToDown(&done); - wait(0.1f); - } - done = 0; - - sam.Greifer.take(); - - while(!done) { - sam.Arm.downToCollect(&done); - sam.leds[1] = 1; - wait(0.1f); - } - done = 0; - - sam.Greifer.leave(); - - found = 0; - } - - - wait( 0.1f ); - } - return 0; -} -*/ - -/* * / -int main(){ - - initializeDistanceSensors(); - sam.stop(); - - while ( 1 ){ - for (int i=0; i<6; i++){ - sam.sensors[i] < NEAR ? sam.leds[i] = 1 : sam.leds[i] = 0; - } - - sam.turnLeftS(); - - while (sam.sensors[FWD] < NEAR){ - wait(0.05f); - sam.stop(); - } - - } - return 0; -} -/* */ - -/* * / -int main(){ - for(float p=0; p<1.0; p += 0.1) { - // arm = p; - wait(0.2); - } -} -/* */ - -/* * / -int main(){ - sam.stop(); - int done = 0; //1:= finished process; 0:= not finished - int fun = 0; //just to test. - int start = 0; - - - while(1){ - if(fun == 0){ - done = 0; - sam.Arm.collecttoback(&done); - done == 0 ? fun = 0 : fun = 1; - } - else if(fun == 1){ - done = 0; - sam.Arm.backtocollect(&done); - done == 0 ? fun = 1 : fun = 2; - } - else if(fun == 2){ - done = 0; - sam.Arm.collecttodown(&done); - done == 0 ? fun = 2 : fun = 3; - } - else if(fun == 3){ - done = 0; - sam.Arm.downtocollect(&done); - done == 0 ? fun = 3 : fun = 0; - } - wait(0.1); - } -} - -*/ \ No newline at end of file +} \ No newline at end of file