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@18:a158713a0049, 2017-05-19 (annotated)

Committer:
EpicG10
Date:
Fri May 19 06:40:33 2017 +0000
Revision:
18:a158713a0049
Parent:
17:4e1be70bdedb 
a

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 17:4e1be70bdedb 4 #include "Ultraschall.h"
beacon 0:d267b248eff4 5
beacon 0:d267b248eff4 6 #include <cstdlib>
beacon 0:d267b248eff4 7
beacon 4:67d7177c213f 8 //DistanceSensors related bottom:
beacon 0:d267b248eff4 9 Serial pc(SERIAL_TX, SERIAL_RX);
beacon 0:d267b248eff4 10
beacon 0:d267b248eff4 11 AnalogIn sensorVoltage(PB_1);
beacon 0:d267b248eff4 12 DigitalOut enable(PC_1);
beacon 0:d267b248eff4 13 DigitalOut bit0(PH_1);
beacon 0:d267b248eff4 14 DigitalOut bit1(PC_2);
beacon 0:d267b248eff4 15 DigitalOut bit2(PC_3);
beacon 1:388c915756f5 16
beacon 4:67d7177c213f 17 //DistanceSensors top:
beacon 4:67d7177c213f 18 AnalogIn frontS(A1);
beacon 4:67d7177c213f 19 AnalogIn leftS(A2);
beacon 4:67d7177c213f 20 AnalogIn rightS(A3);
beacon 4:67d7177c213f 21
beacon 17:4e1be70bdedb 22 //Ultraschall
beacon 17:4e1be70bdedb 23 Ultraschall usensor(D6,D5);
beacon 17:4e1be70bdedb 24
beacon 17:4e1be70bdedb 25
beacon 1:388c915756f5 26 //Leds related:
beacon 1:388c915756f5 27 DigitalOut leds[6] = { PC_8, PC_6, PB_12, PA_7, PC_0, PC_9 };
beacon 0:d267b248eff4 28
beacon 1:388c915756f5 29 //motor related:
beacon 1:388c915756f5 30 PwmOut left(PA_8);
beacon 1:388c915756f5 31 PwmOut right(PA_9);
beacon 0:d267b248eff4 32
beacon 1:388c915756f5 33 DigitalOut powerSignal(PB_2);
beacon 10:f76476943a6c 34 DigitalIn errorMotor(PB_14);
beacon 1:388c915756f5 35 DigitalIn overtemperatur(PB_15);
beacon 1:388c915756f5 36
beacon 4:67d7177c213f 37 //Arm:
beacon 10:f76476943a6c 38 ServoArm servoArm(PA_6);
beacon 10:f76476943a6c 39
beacon 10:f76476943a6c 40 //Greifer:
beacon 12:c0bcb95885dd 41 Servo servoGreifer(PC_7);
beacon 4:67d7177c213f 42
EpicG10 14:b61fbd13a7c9 43 //Leiste:
EpicG10 15:915f8839fe48 44 Servo servoLeiste(PB_6);
EpicG10 14:b61fbd13a7c9 45
beacon 17:4e1be70bdedb 46 //Button:
beacon 17:4e1be70bdedb 47 DigitalIn mybutton(USER_BUTTON);
beacon 17:4e1be70bdedb 48
beacon 1:388c915756f5 49 //Farbsensor:
beacon 1:388c915756f5 50 AnalogIn FarbVoltage(A0);
beacon 12:c0bcb95885dd 51 DigitalOut led(D2);
beacon 1:388c915756f5 52
beacon 17:4e1be70bdedb 53 Robot sam( &left, &right, &powerSignal, leds, &FarbVoltage, &frontS, &leftS, &rightS, &servoArm, &servoGreifer, &servoLeiste, &usensor ); //Implement the Farbsensor into the Robot init function!!
beacon 10:f76476943a6c 54
beacon 11:292bdbd85a9c 55 void initializeDistanceSensors()
beacon 11:292bdbd85a9c 56 {
beacon 10:f76476943a6c 57 for( int ii = 0; ii<9; ++ii) {
beacon 10:f76476943a6c 58 sam.sensors[ii].init(&sensorVoltage, &frontS, &leftS, &rightS, &bit0, &bit1, &bit2, ii);
beacon 10:f76476943a6c 59
beacon 11:292bdbd85a9c 60 enable = 1;
beacon 10:f76476943a6c 61 }
beacon 10:f76476943a6c 62 }
beacon 1:388c915756f5 63
beacon 0:d267b248eff4 64
beacon 10:f76476943a6c 65 /* */
beacon 11:292bdbd85a9c 66 int main()
beacon 11:292bdbd85a9c 67 {
beacon 4:67d7177c213f 68 initializeDistanceSensors(); //Initialises IR-Sensors.
beacon 11:292bdbd85a9c 69 //int counter = 0; //Counts how many times the robot has turned, before driving
beacon 4:67d7177c213f 70 int timer = 0; //Is used, to reset the robot. Returns time in [0.1s]
beacon 4:67d7177c213f 71 //int lastFun = -1; //Is used, to check if the same action in Robot::search() was made multiple times.
beacon 11:292bdbd85a9c 72 //int found = 0; //0:= no block available, 1 := a lego is ready to be picked up
beacon 11:292bdbd85a9c 73 //int done = 0;
beacon 17:4e1be70bdedb 74 int color;
beacon 17:4e1be70bdedb 75
EpicG10 18:a158713a0049 76 enum states { search = 0, LeisteDown, turn, push, pos, backOff, forward, downward, down, upward, colorS, readyDrop, backward, LeisteUp };
beacon 11:292bdbd85a9c 77
EpicG10 13:6c1f8e35b242 78 int state = search;
beacon 17:4e1be70bdedb 79
EpicG10 18:a158713a0049 80 // static float messung = 0;
beacon 17:4e1be70bdedb 81
EpicG10 18:a158713a0049 82
beacon 17:4e1be70bdedb 83
beacon 11:292bdbd85a9c 84 while( 1 ) {
beacon 10:f76476943a6c 85
beacon 11:292bdbd85a9c 86 if ( timer > TIMEOUT ) {
beacon 4:67d7177c213f 87 NVIC_SystemReset(); //Resets Sam.
beacon 4:67d7177c213f 88 }
beacon 11:292bdbd85a9c 89
beacon 17:4e1be70bdedb 90 //if( timer == 0 )
beacon 17:4e1be70bdedb 91 //printf("\n\rLEFT: %.3f,\tFWD: %.3f,\tRIGHT: %.3f", sam.sensors[LEFT].read(), sam.sensors[FWD].read(), sam.sensors[RIGHT].read());
beacon 17:4e1be70bdedb 92
beacon 17:4e1be70bdedb 93
EpicG10 18:a158713a0049 94 printf("\n\rcurrent main state: %d", state);
beacon 17:4e1be70bdedb 95
beacon 17:4e1be70bdedb 96 sam.sensors[FWD_L].read() < NEAR ? sam.leds[1] = 1 : sam.leds[1] = 0;
beacon 11:292bdbd85a9c 97 switch( state ) {
beacon 11:292bdbd85a9c 98 case search:
beacon 17:4e1be70bdedb 99 if( sam.search(&timer) ){
beacon 17:4e1be70bdedb 100 //sam.Greifer.nullPos();
beacon 17:4e1be70bdedb 101 state = LeisteDown;
beacon 17:4e1be70bdedb 102 timer = 0;
EpicG10 15:915f8839fe48 103 }
EpicG10 15:915f8839fe48 104 break;
EpicG10 15:915f8839fe48 105
EpicG10 15:915f8839fe48 106 case LeisteDown:
beacon 17:4e1be70bdedb 107 int count = 0;
beacon 17:4e1be70bdedb 108 if( sam.Leiste.upToDown() ){
beacon 17:4e1be70bdedb 109 //sam.Greifer.leave();
beacon 17:4e1be70bdedb 110 state = turn;
beacon 17:4e1be70bdedb 111 timer = 0;
beacon 17:4e1be70bdedb 112 }
beacon 17:4e1be70bdedb 113 break;
beacon 17:4e1be70bdedb 114
beacon 17:4e1be70bdedb 115 case turn:
EpicG10 18:a158713a0049 116 /* static int i = 0;
EpicG10 18:a158713a0049 117 if( i > 4 ){
beacon 17:4e1be70bdedb 118 sam.stop();
beacon 17:4e1be70bdedb 119 state = push;
beacon 17:4e1be70bdedb 120 i = 0;
beacon 17:4e1be70bdedb 121 }
beacon 17:4e1be70bdedb 122 else{
beacon 17:4e1be70bdedb 123 i++;
beacon 17:4e1be70bdedb 124 sam.turnRight();
EpicG10 18:a158713a0049 125 }*/
EpicG10 18:a158713a0049 126 state = push;
beacon 17:4e1be70bdedb 127 break;
beacon 11:292bdbd85a9c 128
beacon 17:4e1be70bdedb 129 case push:{
beacon 17:4e1be70bdedb 130 static int i = 0;
EpicG10 18:a158713a0049 131 if( i > 6 ){
EpicG10 18:a158713a0049 132 sam.stop();
EpicG10 18:a158713a0049 133 i = 0;
EpicG10 18:a158713a0049 134 state = pos;
EpicG10 18:a158713a0049 135 timer = 0;
EpicG10 18:a158713a0049 136 }
EpicG10 18:a158713a0049 137 else{
EpicG10 18:a158713a0049 138 sam.drive();
EpicG10 18:a158713a0049 139 i++;
EpicG10 18:a158713a0049 140 }
EpicG10 18:a158713a0049 141 break;
EpicG10 18:a158713a0049 142 }
EpicG10 18:a158713a0049 143
EpicG10 18:a158713a0049 144 case pos:{
EpicG10 18:a158713a0049 145 static int i = 0;
EpicG10 18:a158713a0049 146 if( i > 4 ){
beacon 17:4e1be70bdedb 147 sam.stop();
beacon 17:4e1be70bdedb 148 i = 0;
beacon 17:4e1be70bdedb 149 state = backOff;
beacon 17:4e1be70bdedb 150 timer = 0;
beacon 17:4e1be70bdedb 151 }
beacon 17:4e1be70bdedb 152 else{
EpicG10 18:a158713a0049 153 sam.turnRightS();
EpicG10 18:a158713a0049 154 i++;
EpicG10 18:a158713a0049 155 }
EpicG10 18:a158713a0049 156
EpicG10 18:a158713a0049 157 break;
EpicG10 18:a158713a0049 158 }
EpicG10 18:a158713a0049 159
EpicG10 18:a158713a0049 160 case backOff:{
EpicG10 18:a158713a0049 161 static int i = 0;
EpicG10 18:a158713a0049 162 if( i > 2 ){
EpicG10 18:a158713a0049 163 sam.stop();
EpicG10 18:a158713a0049 164 i = 0;
EpicG10 18:a158713a0049 165 state = forward;
EpicG10 18:a158713a0049 166 timer = 0;
EpicG10 18:a158713a0049 167 }
EpicG10 18:a158713a0049 168 else{
beacon 17:4e1be70bdedb 169 sam.driveSlowly();
beacon 17:4e1be70bdedb 170 i++;
beacon 17:4e1be70bdedb 171 }
EpicG10 18:a158713a0049 172 //state = forward;
beacon 17:4e1be70bdedb 173 break;
beacon 17:4e1be70bdedb 174 }
beacon 17:4e1be70bdedb 175
beacon 11:292bdbd85a9c 176
beacon 11:292bdbd85a9c 177 case forward:
beacon 11:292bdbd85a9c 178 if( sam.Arm.backToCollect() ){
beacon 11:292bdbd85a9c 179 state = downward;
beacon 11:292bdbd85a9c 180 timer = 0;
beacon 11:292bdbd85a9c 181 }
beacon 11:292bdbd85a9c 182 break;
beacon 11:292bdbd85a9c 183
beacon 11:292bdbd85a9c 184 case downward:
beacon 11:292bdbd85a9c 185 if( sam.Arm.collectToDown() ){
beacon 11:292bdbd85a9c 186 state = down;
beacon 11:292bdbd85a9c 187 timer = 0;
beacon 11:292bdbd85a9c 188 }
beacon 11:292bdbd85a9c 189 break;
beacon 11:292bdbd85a9c 190
beacon 11:292bdbd85a9c 191 case down:
beacon 11:292bdbd85a9c 192 if( sam.Greifer.take() ) {
beacon 11:292bdbd85a9c 193 state = upward;
beacon 11:292bdbd85a9c 194 timer = 0;
beacon 11:292bdbd85a9c 195 }
beacon 11:292bdbd85a9c 196 break;
beacon 11:292bdbd85a9c 197
beacon 11:292bdbd85a9c 198 case upward:
beacon 11:292bdbd85a9c 199 if( sam.Arm.downToCollect() ){
beacon 17:4e1be70bdedb 200 state = colorS;
beacon 11:292bdbd85a9c 201 timer = 0;
beacon 11:292bdbd85a9c 202 }
beacon 11:292bdbd85a9c 203 break;
beacon 11:292bdbd85a9c 204
beacon 17:4e1be70bdedb 205 case colorS:
beacon 12:c0bcb95885dd 206 led = 1;
beacon 17:4e1be70bdedb 207 color = sam.FarbVoltage.read();
beacon 11:292bdbd85a9c 208
beacon 12:c0bcb95885dd 209 if( color == -1 ){
beacon 11:292bdbd85a9c 210 //Do nothing
beacon 11:292bdbd85a9c 211 }
beacon 11:292bdbd85a9c 212
beacon 12:c0bcb95885dd 213
beacon 11:292bdbd85a9c 214 else if( color == 0 || color == GREEN ){
beacon 17:4e1be70bdedb 215 state = backward;
beacon 12:c0bcb95885dd 216 led = 0;
beacon 11:292bdbd85a9c 217 timer = 0;
beacon 11:292bdbd85a9c 218 }
beacon 11:292bdbd85a9c 219
beacon 11:292bdbd85a9c 220 else if( color == RED ){
beacon 11:292bdbd85a9c 221 state = readyDrop;
beacon 12:c0bcb95885dd 222 led = 0;
beacon 11:292bdbd85a9c 223 timer = 0;
beacon 11:292bdbd85a9c 224 }
beacon 11:292bdbd85a9c 225
beacon 11:292bdbd85a9c 226 else{
beacon 11:292bdbd85a9c 227 //Shit...
beacon 11:292bdbd85a9c 228 }
beacon 11:292bdbd85a9c 229 break;
beacon 17:4e1be70bdedb 230
beacon 11:292bdbd85a9c 231 case readyDrop:
beacon 11:292bdbd85a9c 232 if( sam.Greifer.leave() ){
beacon 17:4e1be70bdedb 233 if( color == GREEN || color == 0 ){
beacon 17:4e1be70bdedb 234 state = LeisteUp;
beacon 17:4e1be70bdedb 235 }
beacon 17:4e1be70bdedb 236 else{
beacon 17:4e1be70bdedb 237 state = backward;
beacon 17:4e1be70bdedb 238 }
beacon 11:292bdbd85a9c 239 timer = 0;
beacon 11:292bdbd85a9c 240 }
beacon 11:292bdbd85a9c 241
beacon 11:292bdbd85a9c 242 break;
beacon 11:292bdbd85a9c 243
beacon 11:292bdbd85a9c 244 case backward:
beacon 11:292bdbd85a9c 245 if( sam.Arm.collectToBack() ){
beacon 17:4e1be70bdedb 246 //sam.Greifer.nullPos();
EpicG10 15:915f8839fe48 247 state = LeisteUp;
beacon 11:292bdbd85a9c 248 timer = 0;
beacon 17:4e1be70bdedb 249 if( color == GREEN || color == 0 ){
beacon 17:4e1be70bdedb 250 state = readyDrop;
beacon 17:4e1be70bdedb 251 sam.Greifer.leave();
beacon 17:4e1be70bdedb 252 }
beacon 17:4e1be70bdedb 253 else{
beacon 17:4e1be70bdedb 254 state = LeisteUp;
beacon 17:4e1be70bdedb 255 }
beacon 11:292bdbd85a9c 256 }
beacon 11:292bdbd85a9c 257 break;
EpicG10 15:915f8839fe48 258
EpicG10 15:915f8839fe48 259 case LeisteUp:
beacon 17:4e1be70bdedb 260 if( sam.Leiste.downToUp() ){
EpicG10 15:915f8839fe48 261 state = search;
EpicG10 15:915f8839fe48 262 timer = 0;
EpicG10 15:915f8839fe48 263 }
EpicG10 15:915f8839fe48 264 break;
beacon 17:4e1be70bdedb 265 }
beacon 11:292bdbd85a9c 266 timer++;
beacon 11:292bdbd85a9c 267 wait(0.1f);
beacon 11:292bdbd85a9c 268 }
beacon 11:292bdbd85a9c 269
beacon 11:292bdbd85a9c 270 return 0;
beacon 17:4e1be70bdedb 271 }