main.cpp

00001 #include "mbed.h"
00002 #include "Robot.h"
00003 #include "Declarations.h"
00004 #include "Ultraschall.h"
00005 
00006 #include <cstdlib>
00007 
00008 //DistanceSensors related bottom:
00009 Serial pc(SERIAL_TX, SERIAL_RX);
00010 
00011 AnalogIn sensorVoltage(PB_1);
00012 DigitalOut enable(PC_1);
00013 DigitalOut bit0(PH_1);
00014 DigitalOut bit1(PC_2);
00015 DigitalOut bit2(PC_3);
00016 
00017 //DistanceSensors top:
00018 AnalogIn frontS(A1);
00019 AnalogIn leftS(A2);
00020 AnalogIn rightS(A3);
00021 
00022 //Ultraschall
00023 Ultraschall usensor(D6,D5);
00024 
00025 
00026 //Leds related:
00027 DigitalOut leds[6] = { PC_8, PC_6, PB_12, PA_7, PC_0, PC_9 };
00028 
00029 //motor related:
00030 PwmOut          left(PA_8);
00031 PwmOut          right(PA_9);
00032 
00033 DigitalOut      powerSignal(PB_2);
00034 DigitalIn       errorMotor(PB_14);
00035 DigitalIn       overtemperatur(PB_15);
00036 
00037 //Arm:
00038 ServoArm servoArm(PA_6);
00039 
00040 //Greifer:
00041 Servo servoGreifer(PC_7);
00042 
00043 //Leiste:
00044 Servo servoLeiste(PB_6);
00045 
00046 //Button:
00047 DigitalIn mybutton(USER_BUTTON);
00048 
00049 //Farbsensor:
00050 AnalogIn FarbVoltage(A0);
00051 DigitalOut led(D2);
00052 
00053 Robot sam( &left, &right, &powerSignal, leds, &FarbVoltage, &frontS, &leftS, &rightS, &servoArm, &servoGreifer, &servoLeiste, &usensor ); //Implement the Farbsensor into the Robot init function!!
00054 
00055 void initializeDistanceSensors()
00056 {
00057     for( int ii = 0; ii<9; ++ii) {
00058         sam.sensors[ii].init(&sensorVoltage, &frontS, &leftS, &rightS, &bit0, &bit1, &bit2, ii);
00059 
00060         enable = 1;
00061     }
00062 }
00063 
00064 
00065 /* */
00066 int main()
00067 {
00068     initializeDistanceSensors();        //Initialises IR-Sensors.
00069     //int counter = 0;                    //Counts how many times the robot has turned, before driving
00070     int timer = 0;                      //Is used, to reset the robot. Returns time in [0.1s]
00071     //int lastFun = -1;                   //Is used, to check if the same action in Robot::search() was made multiple times.
00072     //int found = 0;                      //0:= no block available, 1 := a lego is ready to be picked up
00073     //int done = 0;
00074     int color;
00075     
00076     enum states { search = 0, LeisteDown, turn, push, backOff, forward, downward, down, upward, colorS, readyDrop, backward, LeisteUp };
00077 
00078     int state = search;
00079     
00080                 static float messung = 0;
00081                 
00082     while( 1 ){
00083         printf("\n\r%f", sam.see(FWD_L));
00084         
00085         wait(1.0f);
00086             
00087     }
00088     
00089     while( 1 ) {
00090         
00091         if ( timer > TIMEOUT ) {
00092             NVIC_SystemReset();         //Resets Sam.
00093         }
00094         
00095         //if( timer == 0 )
00096         //printf("\n\rLEFT: %.3f,\tFWD: %.3f,\tRIGHT: %.3f", sam.sensors[LEFT].read(), sam.sensors[FWD].read(), sam.sensors[RIGHT].read());
00097         
00098         
00099         //printf("\n\rcurrent main state: %d", state);
00100         
00101         sam.sensors[FWD_L].read() < NEAR ? sam.leds[1] = 1 : sam.leds[1] = 0;
00102         switch( state ) {
00103             case search:
00104                 if( sam.search(&timer) ){
00105                     //sam.Greifer.nullPos();
00106                     state = LeisteDown;
00107                     timer = 0;
00108                 }
00109                 break;
00110        
00111             case LeisteDown:
00112                 int count = 0;
00113                 if( sam.Leiste.upToDown() ){
00114                     //sam.Greifer.leave();
00115                     state = turn;
00116                     timer = 0;
00117                 }
00118                 break;
00119             
00120             case turn:
00121                 static int i = 0;
00122                 if( i > 7 ){
00123                     sam.stop();
00124                     state = push;
00125                     i = 0;
00126                 }
00127                 else{
00128                     i++;
00129                     sam.turnRight();
00130                 }
00131                 break;
00132                 
00133             case push:{
00134                 static int i = 0;
00135                 if( i > 5 ){
00136                     sam.stop();
00137                     i = 0;
00138                     state = backOff;
00139                     timer = 0;
00140                 }
00141                 else{
00142                     sam.driveSlowly();
00143                     i++;
00144                 }
00145                 break;
00146             }
00147             
00148             case backOff:{
00149                 static int i = 0;
00150                 if( i > 1 ){
00151                     sam.stop();
00152                     i = 0;
00153                     state = forward;
00154                     timer = 0;
00155                 }
00156                 else{
00157                     sam.driveBackSlowly();
00158                     i++;
00159                 }
00160                 break;
00161             }
00162                 
00163             
00164             case forward:
00165                 if( sam.Arm.backToCollect() ){
00166                     state = downward;
00167                     timer = 0;
00168                 }
00169                 break;
00170 
00171             case downward:
00172                 if( sam.Arm.collectToDown() ){
00173                     state = down;
00174                     timer = 0;
00175                 }
00176                 break;
00177 
00178             case down:
00179                 if( sam.Greifer.take() ) {
00180                     state = upward;
00181                     timer = 0;
00182                 }
00183                 break;
00184             
00185             case upward:
00186                 if( sam.Arm.downToCollect() ){
00187                     state = colorS;
00188                     timer = 0;
00189                 }
00190                 break;
00191             
00192             case colorS:
00193                 led = 1;
00194                 color = sam.FarbVoltage.read();
00195                 
00196                 if( color == -1 ){
00197                     //Do nothing
00198                 }
00199                 
00200                 
00201                 else if( color == 0 || color == GREEN ){
00202                     state = backward;
00203                     led = 0;
00204                     timer = 0;
00205                 }
00206                 
00207                 else if( color == RED ){
00208                     state = readyDrop;
00209                     led = 0;
00210                     timer = 0;
00211                 }
00212                 
00213                 else{
00214                     //Shit...
00215                 }
00216                 break;
00217              
00218             case readyDrop:
00219                 if( sam.Greifer.leave() ){
00220                     if( color == GREEN || color == 0 ){
00221                         state = LeisteUp;
00222                     }
00223                     else{
00224                         state = backward;
00225                     }
00226                     timer = 0;
00227                 }
00228                 
00229                 break;
00230                 
00231             case backward:
00232                 if( sam.Arm.collectToBack() ){
00233                     //sam.Greifer.nullPos();
00234                     state = LeisteUp;
00235                     timer = 0;
00236                     if( color == GREEN || color == 0 ){
00237                         state = readyDrop;
00238                         sam.Greifer.leave();
00239                     }
00240                     else{
00241                         state = LeisteUp;
00242                     }
00243                 }
00244                 break;
00245                 
00246             case LeisteUp:
00247                 if( sam.Leiste.downToUp() ){
00248                     state = search;
00249                     timer = 0;
00250                 }
00251                 break;
00252         } 
00253         timer++;
00254         wait(0.1f);
00255     }
00256     
00257     return 0;
00258 }