![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
vf
Dependencies: Servo ServoArm mbed
Fork of PES_Official by
Sources/main.cpp@10:f76476943a6c, 2017-05-03 (annotated)
- Committer:
- beacon
- Date:
- Wed May 03 13:54:51 2017 +0000
- Revision:
- 10:f76476943a6c
- Parent:
- 7:6fed0dcae9c1
- Child:
- 11:292bdbd85a9c
oi
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
beacon | 0:d267b248eff4 | 1 | #include "mbed.h" |
beacon | 0:d267b248eff4 | 2 | #include "Robot.h" |
beacon | 4:67d7177c213f | 3 | #include "Declarations.h" |
beacon | 0:d267b248eff4 | 4 | |
beacon | 0:d267b248eff4 | 5 | #include <cstdlib> |
beacon | 0:d267b248eff4 | 6 | |
beacon | 4:67d7177c213f | 7 | //DistanceSensors related bottom: |
beacon | 0:d267b248eff4 | 8 | Serial pc(SERIAL_TX, SERIAL_RX); |
beacon | 0:d267b248eff4 | 9 | |
beacon | 0:d267b248eff4 | 10 | AnalogIn sensorVoltage(PB_1); |
beacon | 0:d267b248eff4 | 11 | DigitalOut enable(PC_1); |
beacon | 0:d267b248eff4 | 12 | DigitalOut bit0(PH_1); |
beacon | 0:d267b248eff4 | 13 | DigitalOut bit1(PC_2); |
beacon | 0:d267b248eff4 | 14 | DigitalOut bit2(PC_3); |
beacon | 1:388c915756f5 | 15 | |
beacon | 4:67d7177c213f | 16 | //DistanceSensors top: |
beacon | 4:67d7177c213f | 17 | AnalogIn frontS(A1); |
beacon | 4:67d7177c213f | 18 | AnalogIn leftS(A2); |
beacon | 4:67d7177c213f | 19 | AnalogIn rightS(A3); |
beacon | 4:67d7177c213f | 20 | |
beacon | 1:388c915756f5 | 21 | //Leds related: |
beacon | 1:388c915756f5 | 22 | DigitalOut leds[6] = { PC_8, PC_6, PB_12, PA_7, PC_0, PC_9 }; |
beacon | 0:d267b248eff4 | 23 | |
beacon | 1:388c915756f5 | 24 | //motor related: |
beacon | 1:388c915756f5 | 25 | PwmOut left(PA_8); |
beacon | 1:388c915756f5 | 26 | PwmOut right(PA_9); |
beacon | 0:d267b248eff4 | 27 | |
beacon | 1:388c915756f5 | 28 | DigitalOut powerSignal(PB_2); |
beacon | 10:f76476943a6c | 29 | DigitalIn errorMotor(PB_14); |
beacon | 1:388c915756f5 | 30 | DigitalIn overtemperatur(PB_15); |
beacon | 1:388c915756f5 | 31 | |
beacon | 4:67d7177c213f | 32 | //Arm: |
beacon | 10:f76476943a6c | 33 | ServoArm servoArm(PA_6); |
beacon | 10:f76476943a6c | 34 | |
beacon | 10:f76476943a6c | 35 | //Greifer: |
beacon | 10:f76476943a6c | 36 | Servo servoGreifer(PC_7); |
beacon | 4:67d7177c213f | 37 | |
beacon | 1:388c915756f5 | 38 | //Farbsensor: |
beacon | 1:388c915756f5 | 39 | AnalogIn FarbVoltage(A0); |
beacon | 1:388c915756f5 | 40 | |
beacon | 10:f76476943a6c | 41 | Robot sam( &left, &right, &powerSignal, leds, &FarbVoltage, &frontS, &leftS, &rightS, &servoArm, &servoGreifer ); //Implement the Farbsensor into the Robot init function!! |
beacon | 10:f76476943a6c | 42 | |
beacon | 10:f76476943a6c | 43 | void initializeDistanceSensors(){ |
beacon | 10:f76476943a6c | 44 | for( int ii = 0; ii<9; ++ii) { |
beacon | 10:f76476943a6c | 45 | sam.sensors[ii].init(&sensorVoltage, &frontS, &leftS, &rightS, &bit0, &bit1, &bit2, ii); |
beacon | 10:f76476943a6c | 46 | |
beacon | 10:f76476943a6c | 47 | enable = 1; |
beacon | 10:f76476943a6c | 48 | } |
beacon | 10:f76476943a6c | 49 | } |
beacon | 1:388c915756f5 | 50 | |
beacon | 0:d267b248eff4 | 51 | |
beacon | 10:f76476943a6c | 52 | /* */ |
beacon | 1:388c915756f5 | 53 | int main(){ |
beacon | 4:67d7177c213f | 54 | initializeDistanceSensors(); //Initialises IR-Sensors. |
beacon | 4:67d7177c213f | 55 | int counter = 0; //Counts how many times the robot has turned, before driving |
beacon | 4:67d7177c213f | 56 | int timer = 0; //Is used, to reset the robot. Returns time in [0.1s] |
beacon | 4:67d7177c213f | 57 | //int lastFun = -1; //Is used, to check if the same action in Robot::search() was made multiple times. |
beacon | 4:67d7177c213f | 58 | int found = 0; //0:= no block available, 1 := a lego is ready to be picked up |
beacon | 0:d267b248eff4 | 59 | |
beacon | 4:67d7177c213f | 60 | while( 1 ){ |
beacon | 10:f76476943a6c | 61 | |
beacon | 4:67d7177c213f | 62 | if ( timer > TIMEOUT ){ |
beacon | 4:67d7177c213f | 63 | NVIC_SystemReset(); //Resets Sam. |
beacon | 4:67d7177c213f | 64 | } |
beacon | 4:67d7177c213f | 65 | else if (found == 0){ |
beacon | 4:67d7177c213f | 66 | sam.search(&counter, &timer, &found); |
beacon | 4:67d7177c213f | 67 | } |
beacon | 4:67d7177c213f | 68 | else{ |
beacon | 4:67d7177c213f | 69 | //pick up lego |
beacon | 4:67d7177c213f | 70 | //found = 0; |
beacon | 5:1aaf5de776ff | 71 | sam.stop(); //Nur zum Testen |
beacon | 4:67d7177c213f | 72 | } |
beacon | 1:388c915756f5 | 73 | |
beacon | 1:388c915756f5 | 74 | wait( 0.1f ); |
beacon | 0:d267b248eff4 | 75 | } |
beacon | 0:d267b248eff4 | 76 | return 0; |
beacon | 4:67d7177c213f | 77 | } |
beacon | 4:67d7177c213f | 78 | |
beacon | 4:67d7177c213f | 79 | /* * / |
beacon | 4:67d7177c213f | 80 | int main(){ |
beacon | 4:67d7177c213f | 81 | |
beacon | 4:67d7177c213f | 82 | initializeDistanceSensors(); |
beacon | 4:67d7177c213f | 83 | sam.stop(); |
beacon | 4:67d7177c213f | 84 | |
beacon | 4:67d7177c213f | 85 | while ( 1 ){ |
beacon | 4:67d7177c213f | 86 | for (int i=0; i<6; i++){ |
beacon | 4:67d7177c213f | 87 | sam.sensors[i] < NEAR ? sam.leds[i] = 1 : sam.leds[i] = 0; |
beacon | 4:67d7177c213f | 88 | } |
beacon | 4:67d7177c213f | 89 | |
beacon | 4:67d7177c213f | 90 | sam.turnLeftS(); |
beacon | 4:67d7177c213f | 91 | |
beacon | 4:67d7177c213f | 92 | while (sam.sensors[FWD] < NEAR){ |
beacon | 4:67d7177c213f | 93 | wait(0.05f); |
beacon | 4:67d7177c213f | 94 | sam.stop(); |
beacon | 4:67d7177c213f | 95 | } |
beacon | 4:67d7177c213f | 96 | |
beacon | 4:67d7177c213f | 97 | } |
beacon | 4:67d7177c213f | 98 | return 0; |
beacon | 4:67d7177c213f | 99 | } |
beacon | 4:67d7177c213f | 100 | /* */ |
beacon | 4:67d7177c213f | 101 | |
beacon | 4:67d7177c213f | 102 | /* * / |
beacon | 4:67d7177c213f | 103 | int main(){ |
beacon | 4:67d7177c213f | 104 | for(float p=0; p<1.0; p += 0.1) { |
beacon | 4:67d7177c213f | 105 | // arm = p; |
beacon | 4:67d7177c213f | 106 | wait(0.2); |
beacon | 4:67d7177c213f | 107 | } |
beacon | 4:67d7177c213f | 108 | } |
beacon | 6:ba26dd3251b3 | 109 | /* */ |
beacon | 6:ba26dd3251b3 | 110 | |
beacon | 10:f76476943a6c | 111 | /* * / |
beacon | 6:ba26dd3251b3 | 112 | int main(){ |
beacon | 6:ba26dd3251b3 | 113 | sam.stop(); |
beacon | 6:ba26dd3251b3 | 114 | int done = 0; //1:= finished process; 0:= not finished |
beacon | 6:ba26dd3251b3 | 115 | int fun = 0; //just to test. |
EpicG10 | 7:6fed0dcae9c1 | 116 | int start = 0; |
EpicG10 | 7:6fed0dcae9c1 | 117 | |
EpicG10 | 7:6fed0dcae9c1 | 118 | |
beacon | 6:ba26dd3251b3 | 119 | while(1){ |
beacon | 6:ba26dd3251b3 | 120 | if(fun == 0){ |
beacon | 6:ba26dd3251b3 | 121 | done = 0; |
beacon | 6:ba26dd3251b3 | 122 | sam.Arm.collecttoback(&done); |
EpicG10 | 7:6fed0dcae9c1 | 123 | done == 0 ? fun = 0 : fun = 1; |
beacon | 6:ba26dd3251b3 | 124 | } |
beacon | 6:ba26dd3251b3 | 125 | else if(fun == 1){ |
beacon | 6:ba26dd3251b3 | 126 | done = 0; |
beacon | 6:ba26dd3251b3 | 127 | sam.Arm.backtocollect(&done); |
beacon | 6:ba26dd3251b3 | 128 | done == 0 ? fun = 1 : fun = 2; |
beacon | 6:ba26dd3251b3 | 129 | } |
beacon | 6:ba26dd3251b3 | 130 | else if(fun == 2){ |
beacon | 6:ba26dd3251b3 | 131 | done = 0; |
beacon | 6:ba26dd3251b3 | 132 | sam.Arm.collecttodown(&done); |
beacon | 6:ba26dd3251b3 | 133 | done == 0 ? fun = 2 : fun = 3; |
beacon | 6:ba26dd3251b3 | 134 | } |
beacon | 6:ba26dd3251b3 | 135 | else if(fun == 3){ |
beacon | 6:ba26dd3251b3 | 136 | done = 0; |
beacon | 6:ba26dd3251b3 | 137 | sam.Arm.downtocollect(&done); |
beacon | 6:ba26dd3251b3 | 138 | done == 0 ? fun = 3 : fun = 0; |
beacon | 6:ba26dd3251b3 | 139 | } |
beacon | 6:ba26dd3251b3 | 140 | wait(0.1); |
beacon | 6:ba26dd3251b3 | 141 | } |
beacon | 10:f76476943a6c | 142 | } |
beacon | 10:f76476943a6c | 143 | |
beacon | 10:f76476943a6c | 144 | */ |