zhaw_st16b_pes2_10 / Mbed 2 deprecated PES_Official

Do NOT modify!

Dependencies:   mbed Servo ServoArm

Fork of PES_Yanick by zhaw_st16b_pes2_10

Sources/main.cpp@12:c0bcb95885dd, 2017-05-10 (annotated)

Committer:
beacon
Date:
Wed May 10 13:30:13 2017 +0000
Revision:
12:c0bcb95885dd
Parent:
11:292bdbd85a9c 
k

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