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@17:4e1be70bdedb, 2017-05-18 (annotated)

Committer:
beacon
Date:
Thu May 18 14:27:09 2017 +0000
Revision:
17:4e1be70bdedb
Parent:
15:915f8839fe48
Child:
18:a158713a0049
Si compila.

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
beacon 17:4e1be70bdedb 76 enum states { search = 0, LeisteDown, turn, push, backOff, forward, downward, down, upward, colorS, readyDrop, backward, LeisteUp };
beacon 11:292bdbd85a9c 77
EpicG10 13:6c1f8e35b242 78 int state = search;
beacon 17:4e1be70bdedb 79
beacon 17:4e1be70bdedb 80 static float messung = 0;
beacon 17:4e1be70bdedb 81
beacon 17:4e1be70bdedb 82 while( 1 ){
beacon 17:4e1be70bdedb 83 printf("\n\r%f", sam.see(FWD_L));
beacon 17:4e1be70bdedb 84
beacon 17:4e1be70bdedb 85 wait(1.0f);
beacon 17:4e1be70bdedb 86
beacon 17:4e1be70bdedb 87 }
beacon 17:4e1be70bdedb 88
beacon 11:292bdbd85a9c 89 while( 1 ) {
beacon 10:f76476943a6c 90
beacon 11:292bdbd85a9c 91 if ( timer > TIMEOUT ) {
beacon 4:67d7177c213f 92 NVIC_SystemReset(); //Resets Sam.
beacon 4:67d7177c213f 93 }
beacon 11:292bdbd85a9c 94
beacon 17:4e1be70bdedb 95 //if( timer == 0 )
beacon 17:4e1be70bdedb 96 //printf("\n\rLEFT: %.3f,\tFWD: %.3f,\tRIGHT: %.3f", sam.sensors[LEFT].read(), sam.sensors[FWD].read(), sam.sensors[RIGHT].read());
beacon 17:4e1be70bdedb 97
beacon 17:4e1be70bdedb 98
beacon 17:4e1be70bdedb 99 //printf("\n\rcurrent main state: %d", state);
beacon 17:4e1be70bdedb 100
beacon 17:4e1be70bdedb 101 sam.sensors[FWD_L].read() < NEAR ? sam.leds[1] = 1 : sam.leds[1] = 0;
beacon 11:292bdbd85a9c 102 switch( state ) {
beacon 11:292bdbd85a9c 103 case search:
beacon 17:4e1be70bdedb 104 if( sam.search(&timer) ){
beacon 17:4e1be70bdedb 105 //sam.Greifer.nullPos();
beacon 17:4e1be70bdedb 106 state = LeisteDown;
beacon 17:4e1be70bdedb 107 timer = 0;
EpicG10 15:915f8839fe48 108 }
EpicG10 15:915f8839fe48 109 break;
EpicG10 15:915f8839fe48 110
EpicG10 15:915f8839fe48 111 case LeisteDown:
beacon 17:4e1be70bdedb 112 int count = 0;
beacon 17:4e1be70bdedb 113 if( sam.Leiste.upToDown() ){
beacon 17:4e1be70bdedb 114 //sam.Greifer.leave();
beacon 17:4e1be70bdedb 115 state = turn;
beacon 17:4e1be70bdedb 116 timer = 0;
beacon 17:4e1be70bdedb 117 }
beacon 17:4e1be70bdedb 118 break;
beacon 17:4e1be70bdedb 119
beacon 17:4e1be70bdedb 120 case turn:
beacon 17:4e1be70bdedb 121 static int i = 0;
beacon 17:4e1be70bdedb 122 if( i > 7 ){
beacon 17:4e1be70bdedb 123 sam.stop();
beacon 17:4e1be70bdedb 124 state = push;
beacon 17:4e1be70bdedb 125 i = 0;
beacon 17:4e1be70bdedb 126 }
beacon 17:4e1be70bdedb 127 else{
beacon 17:4e1be70bdedb 128 i++;
beacon 17:4e1be70bdedb 129 sam.turnRight();
beacon 11:292bdbd85a9c 130 }
beacon 17:4e1be70bdedb 131 break;
beacon 11:292bdbd85a9c 132
beacon 17:4e1be70bdedb 133 case push:{
beacon 17:4e1be70bdedb 134 static int i = 0;
beacon 17:4e1be70bdedb 135 if( i > 5 ){
beacon 17:4e1be70bdedb 136 sam.stop();
beacon 17:4e1be70bdedb 137 i = 0;
beacon 17:4e1be70bdedb 138 state = backOff;
beacon 17:4e1be70bdedb 139 timer = 0;
beacon 17:4e1be70bdedb 140 }
beacon 17:4e1be70bdedb 141 else{
beacon 17:4e1be70bdedb 142 sam.driveSlowly();
beacon 17:4e1be70bdedb 143 i++;
beacon 17:4e1be70bdedb 144 }
beacon 11:292bdbd85a9c 145 break;
beacon 17:4e1be70bdedb 146 }
beacon 17:4e1be70bdedb 147
beacon 17:4e1be70bdedb 148 case backOff:{
beacon 17:4e1be70bdedb 149 static int i = 0;
beacon 17:4e1be70bdedb 150 if( i > 1 ){
beacon 17:4e1be70bdedb 151 sam.stop();
beacon 17:4e1be70bdedb 152 i = 0;
beacon 17:4e1be70bdedb 153 state = forward;
beacon 17:4e1be70bdedb 154 timer = 0;
beacon 17:4e1be70bdedb 155 }
beacon 17:4e1be70bdedb 156 else{
beacon 17:4e1be70bdedb 157 sam.driveBackSlowly();
beacon 17:4e1be70bdedb 158 i++;
beacon 17:4e1be70bdedb 159 }
beacon 17:4e1be70bdedb 160 break;
beacon 17:4e1be70bdedb 161 }
beacon 17:4e1be70bdedb 162
beacon 11:292bdbd85a9c 163
beacon 11:292bdbd85a9c 164 case forward:
beacon 11:292bdbd85a9c 165 if( sam.Arm.backToCollect() ){
beacon 11:292bdbd85a9c 166 state = downward;
beacon 11:292bdbd85a9c 167 timer = 0;
beacon 11:292bdbd85a9c 168 }
beacon 11:292bdbd85a9c 169 break;
beacon 11:292bdbd85a9c 170
beacon 11:292bdbd85a9c 171 case downward:
beacon 11:292bdbd85a9c 172 if( sam.Arm.collectToDown() ){
beacon 11:292bdbd85a9c 173 state = down;
beacon 11:292bdbd85a9c 174 timer = 0;
beacon 11:292bdbd85a9c 175 }
beacon 11:292bdbd85a9c 176 break;
beacon 11:292bdbd85a9c 177
beacon 11:292bdbd85a9c 178 case down:
beacon 11:292bdbd85a9c 179 if( sam.Greifer.take() ) {
beacon 11:292bdbd85a9c 180 state = upward;
beacon 11:292bdbd85a9c 181 timer = 0;
beacon 11:292bdbd85a9c 182 }
beacon 11:292bdbd85a9c 183 break;
beacon 11:292bdbd85a9c 184
beacon 11:292bdbd85a9c 185 case upward:
beacon 11:292bdbd85a9c 186 if( sam.Arm.downToCollect() ){
beacon 17:4e1be70bdedb 187 state = colorS;
beacon 11:292bdbd85a9c 188 timer = 0;
beacon 11:292bdbd85a9c 189 }
beacon 11:292bdbd85a9c 190 break;
beacon 11:292bdbd85a9c 191
beacon 17:4e1be70bdedb 192 case colorS:
beacon 12:c0bcb95885dd 193 led = 1;
beacon 17:4e1be70bdedb 194 color = sam.FarbVoltage.read();
beacon 11:292bdbd85a9c 195
beacon 12:c0bcb95885dd 196 if( color == -1 ){
beacon 11:292bdbd85a9c 197 //Do nothing
beacon 11:292bdbd85a9c 198 }
beacon 11:292bdbd85a9c 199
beacon 12:c0bcb95885dd 200
beacon 11:292bdbd85a9c 201 else if( color == 0 || color == GREEN ){
beacon 17:4e1be70bdedb 202 state = backward;
beacon 12:c0bcb95885dd 203 led = 0;
beacon 11:292bdbd85a9c 204 timer = 0;
beacon 11:292bdbd85a9c 205 }
beacon 11:292bdbd85a9c 206
beacon 11:292bdbd85a9c 207 else if( color == RED ){
beacon 11:292bdbd85a9c 208 state = readyDrop;
beacon 12:c0bcb95885dd 209 led = 0;
beacon 11:292bdbd85a9c 210 timer = 0;
beacon 11:292bdbd85a9c 211 }
beacon 11:292bdbd85a9c 212
beacon 11:292bdbd85a9c 213 else{
beacon 11:292bdbd85a9c 214 //Shit...
beacon 11:292bdbd85a9c 215 }
beacon 11:292bdbd85a9c 216 break;
beacon 17:4e1be70bdedb 217
beacon 11:292bdbd85a9c 218 case readyDrop:
beacon 11:292bdbd85a9c 219 if( sam.Greifer.leave() ){
beacon 17:4e1be70bdedb 220 if( color == GREEN || color == 0 ){
beacon 17:4e1be70bdedb 221 state = LeisteUp;
beacon 17:4e1be70bdedb 222 }
beacon 17:4e1be70bdedb 223 else{
beacon 17:4e1be70bdedb 224 state = backward;
beacon 17:4e1be70bdedb 225 }
beacon 11:292bdbd85a9c 226 timer = 0;
beacon 11:292bdbd85a9c 227 }
beacon 11:292bdbd85a9c 228
beacon 11:292bdbd85a9c 229 break;
beacon 11:292bdbd85a9c 230
beacon 11:292bdbd85a9c 231 case backward:
beacon 11:292bdbd85a9c 232 if( sam.Arm.collectToBack() ){
beacon 17:4e1be70bdedb 233 //sam.Greifer.nullPos();
EpicG10 15:915f8839fe48 234 state = LeisteUp;
beacon 11:292bdbd85a9c 235 timer = 0;
beacon 17:4e1be70bdedb 236 if( color == GREEN || color == 0 ){
beacon 17:4e1be70bdedb 237 state = readyDrop;
beacon 17:4e1be70bdedb 238 sam.Greifer.leave();
beacon 17:4e1be70bdedb 239 }
beacon 17:4e1be70bdedb 240 else{
beacon 17:4e1be70bdedb 241 state = LeisteUp;
beacon 17:4e1be70bdedb 242 }
beacon 11:292bdbd85a9c 243 }
beacon 11:292bdbd85a9c 244 break;
EpicG10 15:915f8839fe48 245
EpicG10 15:915f8839fe48 246 case LeisteUp:
beacon 17:4e1be70bdedb 247 if( sam.Leiste.downToUp() ){
EpicG10 15:915f8839fe48 248 state = search;
EpicG10 15:915f8839fe48 249 timer = 0;
EpicG10 15:915f8839fe48 250 }
EpicG10 15:915f8839fe48 251 break;
beacon 17:4e1be70bdedb 252 }
beacon 11:292bdbd85a9c 253 timer++;
beacon 11:292bdbd85a9c 254 wait(0.1f);
beacon 11:292bdbd85a9c 255 }
beacon 11:292bdbd85a9c 256
beacon 11:292bdbd85a9c 257 return 0;
beacon 17:4e1be70bdedb 258 }