zhaw_st16b_pes2_10 / Mbed 2 deprecated PES_Official-US

a

Dependencies:   Servo ServoArm mbed

Fork of PES_Official-TestF by zhaw_st16b_pes2_10

Sources/main.cpp@15:915f8839fe48, 2017-05-11 (annotated)

Committer:
EpicG10
Date:
Thu May 11 18:57:45 2017 +0000
Revision:
15:915f8839fe48
Parent:
14:b61fbd13a7c9
Child:
16:f157e5ccd7d3
Child:
17:4e1be70bdedb
c

Who changed what in which revision?

UserRevisionLine numberNew 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 12:c0bcb95885dd 36 Servo servoGreifer(PC_7);
beacon 4:67d7177c213f 37
EpicG10 14:b61fbd13a7c9 38 //Leiste:
EpicG10 15:915f8839fe48 39 Servo servoLeiste(PB_6);
EpicG10 14:b61fbd13a7c9 40
beacon 1:388c915756f5 41 //Farbsensor:
beacon 1:388c915756f5 42 AnalogIn FarbVoltage(A0);
beacon 12:c0bcb95885dd 43 DigitalOut led(D2);
beacon 1:388c915756f5 44
EpicG10 15:915f8839fe48 45 Robot sam( &left, &right, &powerSignal, leds, &FarbVoltage, &frontS, &leftS, &rightS, &servoArm, &servoGreifer, &servoLeiste ); //Implement the Farbsensor into the Robot init function!!
beacon 10:f76476943a6c 46
beacon 11:292bdbd85a9c 47 void initializeDistanceSensors()
beacon 11:292bdbd85a9c 48 {
beacon 10:f76476943a6c 49 for( int ii = 0; ii<9; ++ii) {
beacon 10:f76476943a6c 50 sam.sensors[ii].init(&sensorVoltage, &frontS, &leftS, &rightS, &bit0, &bit1, &bit2, ii);
beacon 10:f76476943a6c 51
beacon 11:292bdbd85a9c 52 enable = 1;
beacon 10:f76476943a6c 53 }
beacon 10:f76476943a6c 54 }
beacon 1:388c915756f5 55
beacon 0:d267b248eff4 56
beacon 10:f76476943a6c 57 /* */
beacon 11:292bdbd85a9c 58 int main()
beacon 11:292bdbd85a9c 59 {
beacon 4:67d7177c213f 60 initializeDistanceSensors(); //Initialises IR-Sensors.
beacon 11:292bdbd85a9c 61 //int counter = 0; //Counts how many times the robot has turned, before driving
beacon 4:67d7177c213f 62 int timer = 0; //Is used, to reset the robot. Returns time in [0.1s]
beacon 4:67d7177c213f 63 //int lastFun = -1; //Is used, to check if the same action in Robot::search() was made multiple times.
beacon 11:292bdbd85a9c 64 //int found = 0; //0:= no block available, 1 := a lego is ready to be picked up
beacon 11:292bdbd85a9c 65 //int done = 0;
beacon 11:292bdbd85a9c 66
EpicG10 15:915f8839fe48 67 enum states { search = 0, LeisteDown, forward, downward, down, upward, color, backwardDrop, readyDrop, backward, LeisteUp };
beacon 11:292bdbd85a9c 68
EpicG10 13:6c1f8e35b242 69 int state = search;
EpicG10 15:915f8839fe48 70 wait(5);
beacon 11:292bdbd85a9c 71 while( 1 ) {
beacon 10:f76476943a6c 72
beacon 11:292bdbd85a9c 73 if ( timer > TIMEOUT ) {
beacon 4:67d7177c213f 74 NVIC_SystemReset(); //Resets Sam.
beacon 4:67d7177c213f 75 }
beacon 11:292bdbd85a9c 76
beacon 11:292bdbd85a9c 77 switch( state ) {
beacon 11:292bdbd85a9c 78 case search:
EpicG10 13:6c1f8e35b242 79 if( 1 || sam.search(&timer) ){
EpicG10 15:915f8839fe48 80 state = LeisteDown;
EpicG10 15:915f8839fe48 81 timer = 0;
EpicG10 15:915f8839fe48 82 }
EpicG10 15:915f8839fe48 83
EpicG10 15:915f8839fe48 84 break;
EpicG10 15:915f8839fe48 85
EpicG10 15:915f8839fe48 86 case LeisteDown:
EpicG10 15:915f8839fe48 87 if( sam.Leiste.UpToDown() ){
EpicG10 15:915f8839fe48 88 sam.Greifer.leave();
beacon 11:292bdbd85a9c 89 state = forward;
beacon 11:292bdbd85a9c 90 timer = 0;
beacon 11:292bdbd85a9c 91 }
beacon 11:292bdbd85a9c 92
beacon 11:292bdbd85a9c 93 break;
beacon 11:292bdbd85a9c 94
beacon 11:292bdbd85a9c 95 case forward:
beacon 11:292bdbd85a9c 96 if( sam.Arm.backToCollect() ){
beacon 11:292bdbd85a9c 97 state = downward;
beacon 11:292bdbd85a9c 98 timer = 0;
beacon 11:292bdbd85a9c 99 }
beacon 11:292bdbd85a9c 100
beacon 11:292bdbd85a9c 101 break;
beacon 11:292bdbd85a9c 102
beacon 11:292bdbd85a9c 103 case downward:
beacon 11:292bdbd85a9c 104 if( sam.Arm.collectToDown() ){
beacon 11:292bdbd85a9c 105 state = down;
beacon 11:292bdbd85a9c 106 timer = 0;
beacon 11:292bdbd85a9c 107 }
beacon 11:292bdbd85a9c 108
beacon 11:292bdbd85a9c 109 break;
beacon 11:292bdbd85a9c 110
beacon 11:292bdbd85a9c 111 case down:
beacon 11:292bdbd85a9c 112 if( sam.Greifer.take() ) {
beacon 11:292bdbd85a9c 113 state = upward;
beacon 11:292bdbd85a9c 114 timer = 0;
beacon 11:292bdbd85a9c 115 }
beacon 11:292bdbd85a9c 116
beacon 11:292bdbd85a9c 117 break;
beacon 11:292bdbd85a9c 118
beacon 11:292bdbd85a9c 119 case upward:
beacon 11:292bdbd85a9c 120 if( sam.Arm.downToCollect() ){
beacon 11:292bdbd85a9c 121 state = color;
beacon 11:292bdbd85a9c 122 timer = 0;
beacon 11:292bdbd85a9c 123 }
beacon 11:292bdbd85a9c 124
beacon 11:292bdbd85a9c 125 break;
beacon 11:292bdbd85a9c 126
beacon 11:292bdbd85a9c 127 case color: {
beacon 12:c0bcb95885dd 128 led = 1;
beacon 11:292bdbd85a9c 129 int color = sam.FarbVoltage.read();
beacon 11:292bdbd85a9c 130
beacon 12:c0bcb95885dd 131 if( color == -1 ){
beacon 11:292bdbd85a9c 132 //Do nothing
beacon 11:292bdbd85a9c 133 }
beacon 11:292bdbd85a9c 134
beacon 12:c0bcb95885dd 135
beacon 11:292bdbd85a9c 136 else if( color == 0 || color == GREEN ){
beacon 11:292bdbd85a9c 137 state = backwardDrop;
beacon 12:c0bcb95885dd 138 led = 0;
beacon 11:292bdbd85a9c 139 timer = 0;
beacon 11:292bdbd85a9c 140 }
beacon 11:292bdbd85a9c 141
beacon 11:292bdbd85a9c 142 else if( color == RED ){
beacon 11:292bdbd85a9c 143 state = readyDrop;
beacon 12:c0bcb95885dd 144 led = 0;
beacon 11:292bdbd85a9c 145 timer = 0;
beacon 11:292bdbd85a9c 146 }
beacon 11:292bdbd85a9c 147
beacon 11:292bdbd85a9c 148 else{
beacon 11:292bdbd85a9c 149 //Shit...
beacon 11:292bdbd85a9c 150 }
beacon 11:292bdbd85a9c 151 break;
beacon 11:292bdbd85a9c 152 }
beacon 11:292bdbd85a9c 153
beacon 11:292bdbd85a9c 154 case backwardDrop:
beacon 11:292bdbd85a9c 155 if( sam.Arm.collectToBack() ){
beacon 11:292bdbd85a9c 156 state = readyDrop;
beacon 11:292bdbd85a9c 157 timer = 0;
beacon 11:292bdbd85a9c 158 }
beacon 11:292bdbd85a9c 159
beacon 11:292bdbd85a9c 160 break;
beacon 11:292bdbd85a9c 161
beacon 11:292bdbd85a9c 162 case readyDrop:
beacon 11:292bdbd85a9c 163 if( sam.Greifer.leave() ){
beacon 11:292bdbd85a9c 164 state = backward;
beacon 11:292bdbd85a9c 165 timer = 0;
beacon 11:292bdbd85a9c 166 }
beacon 11:292bdbd85a9c 167
beacon 11:292bdbd85a9c 168 break;
beacon 11:292bdbd85a9c 169
beacon 11:292bdbd85a9c 170 case backward:
beacon 11:292bdbd85a9c 171 if( sam.Arm.collectToBack() ){
EpicG10 15:915f8839fe48 172 sam.Greifer.nullPos();
EpicG10 15:915f8839fe48 173 state = LeisteUp;
beacon 11:292bdbd85a9c 174 timer = 0;
beacon 11:292bdbd85a9c 175 }
beacon 11:292bdbd85a9c 176
beacon 11:292bdbd85a9c 177 break;
EpicG10 15:915f8839fe48 178
EpicG10 15:915f8839fe48 179 case LeisteUp:
EpicG10 15:915f8839fe48 180 if( sam.Leiste.DownToUp() ){
EpicG10 15:915f8839fe48 181 state = search;
EpicG10 15:915f8839fe48 182 timer = 0;
EpicG10 15:915f8839fe48 183 wait(2);
EpicG10 15:915f8839fe48 184 }
EpicG10 15:915f8839fe48 185 break;
beacon 4:67d7177c213f 186 }
beacon 1:388c915756f5 187
beacon 11:292bdbd85a9c 188 timer++;
beacon 11:292bdbd85a9c 189 wait(0.1f);
beacon 11:292bdbd85a9c 190 }
beacon 11:292bdbd85a9c 191
beacon 11:292bdbd85a9c 192 return 0;
beacon 11:292bdbd85a9c 193 }
beacon 11:292bdbd85a9c 194
beacon 11:292bdbd85a9c 195
beacon 11:292bdbd85a9c 196
beacon 11:292bdbd85a9c 197 /*
beacon 11:292bdbd85a9c 198 if (sam.search(&counter, &timer)) {
beacon 11:292bdbd85a9c 199
beacon 11:292bdbd85a9c 200 while(!done) {
beacon 11:292bdbd85a9c 201 sam.Arm.collectToDown(&done);
beacon 11:292bdbd85a9c 202 wait(0.1f);
beacon 11:292bdbd85a9c 203 }
beacon 11:292bdbd85a9c 204 done = 0;
beacon 11:292bdbd85a9c 205
beacon 11:292bdbd85a9c 206 sam.Greifer.take();
beacon 11:292bdbd85a9c 207
beacon 11:292bdbd85a9c 208 while(!done) {
beacon 11:292bdbd85a9c 209 sam.Arm.downToCollect(&done);
beacon 11:292bdbd85a9c 210 sam.leds[1] = 1;
beacon 11:292bdbd85a9c 211 wait(0.1f);
beacon 11:292bdbd85a9c 212 }
beacon 11:292bdbd85a9c 213 done = 0;
beacon 11:292bdbd85a9c 214
beacon 11:292bdbd85a9c 215 sam.Greifer.leave();
beacon 11:292bdbd85a9c 216
beacon 11:292bdbd85a9c 217 found = 0;
beacon 11:292bdbd85a9c 218 }
beacon 11:292bdbd85a9c 219
beacon 11:292bdbd85a9c 220
beacon 1:388c915756f5 221 wait( 0.1f );
beacon 0:d267b248eff4 222 }
beacon 0:d267b248eff4 223 return 0;
beacon 4:67d7177c213f 224 }
beacon 11:292bdbd85a9c 225 */
beacon 4:67d7177c213f 226
beacon 4:67d7177c213f 227 /* * /
beacon 4:67d7177c213f 228 int main(){
beacon 11:292bdbd85a9c 229
beacon 4:67d7177c213f 230 initializeDistanceSensors();
beacon 4:67d7177c213f 231 sam.stop();
beacon 11:292bdbd85a9c 232
beacon 4:67d7177c213f 233 while ( 1 ){
beacon 4:67d7177c213f 234 for (int i=0; i<6; i++){
beacon 4:67d7177c213f 235 sam.sensors[i] < NEAR ? sam.leds[i] = 1 : sam.leds[i] = 0;
beacon 4:67d7177c213f 236 }
beacon 11:292bdbd85a9c 237
beacon 4:67d7177c213f 238 sam.turnLeftS();
beacon 11:292bdbd85a9c 239
beacon 4:67d7177c213f 240 while (sam.sensors[FWD] < NEAR){
beacon 4:67d7177c213f 241 wait(0.05f);
beacon 4:67d7177c213f 242 sam.stop();
beacon 4:67d7177c213f 243 }
beacon 11:292bdbd85a9c 244
beacon 4:67d7177c213f 245 }
beacon 4:67d7177c213f 246 return 0;
beacon 4:67d7177c213f 247 }
beacon 4:67d7177c213f 248 /* */
beacon 4:67d7177c213f 249
beacon 4:67d7177c213f 250 /* * /
beacon 4:67d7177c213f 251 int main(){
beacon 4:67d7177c213f 252 for(float p=0; p<1.0; p += 0.1) {
beacon 4:67d7177c213f 253 // arm = p;
beacon 4:67d7177c213f 254 wait(0.2);
beacon 4:67d7177c213f 255 }
beacon 4:67d7177c213f 256 }
beacon 6:ba26dd3251b3 257 /* */
beacon 6:ba26dd3251b3 258
beacon 10:f76476943a6c 259 /* * /
beacon 6:ba26dd3251b3 260 int main(){
beacon 6:ba26dd3251b3 261 sam.stop();
beacon 6:ba26dd3251b3 262 int done = 0; //1:= finished process; 0:= not finished
beacon 6:ba26dd3251b3 263 int fun = 0; //just to test.
beacon 11:292bdbd85a9c 264 int start = 0;
beacon 11:292bdbd85a9c 265
beacon 11:292bdbd85a9c 266
beacon 6:ba26dd3251b3 267 while(1){
beacon 6:ba26dd3251b3 268 if(fun == 0){
beacon 6:ba26dd3251b3 269 done = 0;
beacon 6:ba26dd3251b3 270 sam.Arm.collecttoback(&done);
beacon 11:292bdbd85a9c 271 done == 0 ? fun = 0 : fun = 1;
beacon 6:ba26dd3251b3 272 }
beacon 6:ba26dd3251b3 273 else if(fun == 1){
beacon 6:ba26dd3251b3 274 done = 0;
beacon 6:ba26dd3251b3 275 sam.Arm.backtocollect(&done);
beacon 6:ba26dd3251b3 276 done == 0 ? fun = 1 : fun = 2;
beacon 6:ba26dd3251b3 277 }
beacon 6:ba26dd3251b3 278 else if(fun == 2){
beacon 6:ba26dd3251b3 279 done = 0;
beacon 6:ba26dd3251b3 280 sam.Arm.collecttodown(&done);
beacon 6:ba26dd3251b3 281 done == 0 ? fun = 2 : fun = 3;
beacon 6:ba26dd3251b3 282 }
beacon 6:ba26dd3251b3 283 else if(fun == 3){
beacon 6:ba26dd3251b3 284 done = 0;
beacon 6:ba26dd3251b3 285 sam.Arm.downtocollect(&done);
beacon 6:ba26dd3251b3 286 done == 0 ? fun = 3 : fun = 0;
beacon 6:ba26dd3251b3 287 }
beacon 6:ba26dd3251b3 288 wait(0.1);
beacon 6:ba26dd3251b3 289 }
beacon 10:f76476943a6c 290 }
beacon 10:f76476943a6c 291
beacon 10:f76476943a6c 292 */