vf
Dependencies: Servo ServoArm mbed
Fork of PES_Official by
Diff: Sources/main.cpp
- Revision:
- 4:67d7177c213f
- Parent:
- 2:01e9de508316
- Child:
- 5:1aaf5de776ff
diff -r fa61be4ecaa6 -r 67d7177c213f Sources/main.cpp --- a/Sources/main.cpp Wed Mar 22 13:33:07 2017 +0000 +++ b/Sources/main.cpp Wed Apr 19 12:23:52 2017 +0000 @@ -1,9 +1,10 @@ #include "mbed.h" #include "Robot.h" +#include "Declarations.h" #include <cstdlib> -//DistanceSensors related: +//DistanceSensors related bottom: Serial pc(SERIAL_TX, SERIAL_RX); AnalogIn sensorVoltage(PB_1); @@ -12,6 +13,11 @@ DigitalOut bit1(PC_2); DigitalOut bit2(PC_3); +//DistanceSensors top: +AnalogIn frontS(A1); +AnalogIn leftS(A2); +AnalogIn rightS(A3); + //Leds related: DigitalOut leds[6] = { PC_8, PC_6, PB_12, PA_7, PC_0, PC_9 }; @@ -23,14 +29,17 @@ DigitalIn errorSignal(PB_14); DigitalIn overtemperatur(PB_15); +//Arm: +//Servo arm(PC_7); + //Farbsensor: AnalogIn FarbVoltage(A0); -Robot sam( &left, &right, &powerSignal, leds, &FarbVoltage ); //Implement the Farbsensor into the Robot init function!! +Robot sam( &left, &right, &powerSignal, leds, &FarbVoltage, &frontS, &leftS, &rightS ); //Implement the Farbsensor into the Robot init function!! void initializeDistanceSensors(){ - for( int ii = 0; ii<6; ++ii) { - sam.sensors[ii].init(&sensorVoltage, &bit0, &bit1, &bit2, ii); + for( int ii = 0; ii<9; ++ii) { + sam.sensors[ii].init(&sensorVoltage, &frontS, &leftS, &rightS, &bit0, &bit1, &bit2, ii); enable = 1; } @@ -38,19 +47,57 @@ /* */ int main(){ - Timer t; - t.start(); + initializeDistanceSensors(); //Initialises IR-Sensors. + int counter = 0; //Counts how many times the robot has turned, before driving + int timer = 0; //Is used, to reset the robot. Returns time in [0.1s] + //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 - initializeDistanceSensors(); - int counter = 0; //counts how many times the robot has turned, before driving + while( 1 ){ + if ( timer > TIMEOUT ){ + NVIC_SystemReset(); //Resets Sam. + } + else if (found == 0){ + sam.search(&counter, &timer, &found); + } + else{ + //pick up lego + //found = 0; + } - while( 1 ){ - if ( t.read() > TIMEOUT ){ - NVIC_SystemReset(); //Resets Sam. - } - - sam.search(&counter, &t); wait( 0.1f ); } return 0; -} \ No newline at end of file +} + +/* * / +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); + } +} +/* */ \ No newline at end of file