Do NOT modify!
Dependencies: mbed Servo ServoArm
Fork of PES_Yanick by
Revision 12:c0bcb95885dd, committed 2017-05-10
- Comitter:
- beacon
- Date:
- Wed May 10 13:30:13 2017 +0000
- Parent:
- 11:292bdbd85a9c
- Commit message:
- k
Changed in this revision
diff -r 292bdbd85a9c -r c0bcb95885dd Headers/Declarations.h --- a/Headers/Declarations.h Sat May 06 13:33:23 2017 +0000 +++ b/Headers/Declarations.h Wed May 10 13:30:13 2017 +0000 @@ -17,16 +17,16 @@ #define BLEFT 4 //Arrayindex of the backsward left Sensor & right LED //ColorSensors: -#define RED_UPLIMIT 500 //Default limit in mV -#define GREEN_DOWNLIMIT 501 // -#define GREEN_UPLIMIT 1200 // +#define RED_UPLIMIT 1300 //Default limit in mV +#define GREEN_DOWNLIMIT 1301// +#define GREEN_UPLIMIT 2000 // #define GREEN 1 //Will be used to operate arm functions #define NOBLOCK 0 // -#define RED -1 // +#define RED 2 // //Greifer: -#define PC_7 SERVO0 +//#define PC_7 SERVO0 //Arm #define COLLECT_POS 60.0f @@ -34,7 +34,7 @@ #define TAKE_POS -65.0f //Misc: -#define TIMEOUT 140 //if the timer reaches TIMEOUT ([TIMEOUT] = 0.1s), the robot will reset. +#define TIMEOUT 1400 //if the timer reaches TIMEOUT ([TIMEOUT] = 0.1s), the robot will reset. #define MAX 50 //Once the counter reaches MAX, it will turn around. #endif \ No newline at end of file
diff -r 292bdbd85a9c -r c0bcb95885dd Sources/Arm.cpp --- a/Sources/Arm.cpp Sat May 06 13:33:23 2017 +0000 +++ b/Sources/Arm.cpp Wed May 10 13:30:13 2017 +0000 @@ -24,6 +24,7 @@ return 0; } else{ + pos = COLLECT_POS; return 1; } } @@ -36,6 +37,7 @@ return 0; } else{ + pos = TAKE_POS; return 1; } } @@ -48,6 +50,7 @@ return 0; } else{ + pos = COLLECT_POS; return 1; } } @@ -60,6 +63,7 @@ return 0; } else{ + pos = RELEASE_POS; return 1; } } \ No newline at end of file
diff -r 292bdbd85a9c -r c0bcb95885dd Sources/Farbsensor.cpp --- a/Sources/Farbsensor.cpp Sat May 06 13:33:23 2017 +0000 +++ b/Sources/Farbsensor.cpp Wed May 10 13:30:13 2017 +0000 @@ -18,28 +18,31 @@ int Farbsensor::read() { - int farbe; - float Messungen=0.0; - for(int i=0;i<10;i++){ - Messungen+=FarbVoltage->read(); - wait(0.2); - } - float Ufarbsensor = Messungen/10.0; - Ufarbsensor = Ufarbsensor*3300; //Set the Voltage between 0mV und 3300mV - - if ((Ufarbsensor > GREEN_DOWNLIMIT) && (Ufarbsensor < GREEN_UPLIMIT)) - { - farbe=1; + static int time=0; + static float Messungen=0.0f; + if( time < 10 ){ + Messungen += FarbVoltage->read(); + time++; + return -1; + } + else{ + time = 0; + float Ufarbsensor = Messungen/10.0; + Ufarbsensor = Ufarbsensor*3300; //Set the Voltage between 0mV und 3300mV + + if ((Ufarbsensor > GREEN_DOWNLIMIT) && (Ufarbsensor < GREEN_UPLIMIT)) + { + return GREEN; + } + else if(Ufarbsensor < RED_UPLIMIT) + { + return RED; + } + else + { + return NOBLOCK; + } } - else if(Ufarbsensor < RED_UPLIMIT) - { - farbe=-1; - } - else - { - farbe=0; - } - return farbe; } Farbsensor::operator int()
diff -r 292bdbd85a9c -r c0bcb95885dd Sources/Greifer.cpp --- a/Sources/Greifer.cpp Sat May 06 13:33:23 2017 +0000 +++ b/Sources/Greifer.cpp Wed May 10 13:30:13 2017 +0000 @@ -19,8 +19,8 @@ void Greifer::init(Servo* greifer) { this->greifer = greifer; - greifer->calibrate(0.0015f, 180.0f); - greifer->position(85.0f); + greifer->calibrate(0.0017f, 180.0f); + greifer->position(60.0f); } int Greifer::take() @@ -32,19 +32,21 @@ return 0; } else{ + time = 0; return 1; } } int Greifer::leave() { - this->greifer->position(85.0f); + this->greifer->position(60.0f); static int time = 0; if( time < 10 ){ time++; return 0; } else{ + time = 0; return 1; } } \ No newline at end of file
diff -r 292bdbd85a9c -r c0bcb95885dd Sources/main.cpp --- a/Sources/main.cpp Sat May 06 13:33:23 2017 +0000 +++ b/Sources/main.cpp Wed May 10 13:30:13 2017 +0000 @@ -33,10 +33,11 @@ ServoArm servoArm(PA_6); //Greifer: -Servo servoGreifer(PB_7); +Servo servoGreifer(PC_7); //Farbsensor: AnalogIn FarbVoltage(A0); +DigitalOut led(D2); Robot sam( &left, &right, &powerSignal, leds, &FarbVoltage, &frontS, &leftS, &rightS, &servoArm, &servoGreifer ); //Implement the Farbsensor into the Robot init function!! @@ -62,7 +63,7 @@ enum states { search = 0, forward, downward, down, upward, color, backwardDrop, readyDrop, backward }; - int state = search; + int state = downward; while( 1 ) { @@ -112,19 +113,23 @@ break; case color: { + led = 1; int color = sam.FarbVoltage.read(); - if( color == -1){ + if( color == -1 ){ //Do nothing } + else if( color == 0 || color == GREEN ){ state = backwardDrop; + led = 0; timer = 0; } else if( color == RED ){ state = readyDrop; + led = 0; timer = 0; } @@ -152,7 +157,7 @@ case backward: if( sam.Arm.collectToBack() ){ - state = search; + state = downward; timer = 0; }