zhaw_st16b_pes2_10
a
Dependencies: Servo ServoArm mbed
Fork of
PES_Official-TestF
by 
zhaw_st16b_pes2_10
Diff: Sources/main.cpp
- Revision:
- 17:4e1be70bdedb
- Parent:
- 15:915f8839fe48
- Child:
- 18:a158713a0049
--- 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