zhaw_st16b_pes2_10 / Mbed 2 deprecated PES_PIXY_Official

k

Dependencies:   Servo ServoArm mbed

main.cpp@0:15a8480061e8, 2017-05-22 (annotated)

Committer:
beacon
Date:
Mon May 22 11:24:46 2017 +0000
Revision:
0:15a8480061e8
o

Who changed what in which revision?

UserRevisionLine numberNew contents of line
beacon 0:15a8480061e8 1 #include "mbed.h"
beacon 0:15a8480061e8 2 #include "Robot.h"
beacon 0:15a8480061e8 3 #include "Declarations.h"
beacon 0:15a8480061e8 4 #include "Ultraschall.h"
beacon 0:15a8480061e8 5 #include "Pixy.h"
beacon 0:15a8480061e8 6 #include "PID_Control.h"
beacon 0:15a8480061e8 7 #include "LowpassFilter.h"
beacon 0:15a8480061e8 8 #include "EncoderCounter.h"
beacon 0:15a8480061e8 9
beacon 0:15a8480061e8 10 #include <cstdlib>
beacon 0:15a8480061e8 11
beacon 0:15a8480061e8 12 //Cam:
beacon 0:15a8480061e8 13 Serial cam(PA_9, PA_10);
beacon 0:15a8480061e8 14 Pixy pixy(cam);
beacon 0:15a8480061e8 15
beacon 0:15a8480061e8 16 //DistanceSensors related bottom:
beacon 0:15a8480061e8 17 Serial pc(SERIAL_TX, SERIAL_RX);
beacon 0:15a8480061e8 18
beacon 0:15a8480061e8 19 AnalogIn sensorVoltage(PB_1);
beacon 0:15a8480061e8 20 DigitalOut enable(PC_1);
beacon 0:15a8480061e8 21 DigitalOut bit0(PH_1);
beacon 0:15a8480061e8 22 DigitalOut bit1(PC_2);
beacon 0:15a8480061e8 23 DigitalOut bit2(PC_3);
beacon 0:15a8480061e8 24
beacon 0:15a8480061e8 25 //DistanceSensors top:
beacon 0:15a8480061e8 26 AnalogIn frontS(A1);
beacon 0:15a8480061e8 27 AnalogIn leftS(A2);
beacon 0:15a8480061e8 28 AnalogIn rightS(A3);
beacon 0:15a8480061e8 29
beacon 0:15a8480061e8 30
beacon 0:15a8480061e8 31 //Ultraschall
beacon 0:15a8480061e8 32 Ultraschall usensor(D6,D5);
beacon 0:15a8480061e8 33
beacon 0:15a8480061e8 34 //Leds related:
beacon 0:15a8480061e8 35 DigitalOut leds[6] = { PC_8, PC_6, PB_12, PA_7, PC_0, PC_9 };
beacon 0:15a8480061e8 36
beacon 0:15a8480061e8 37 //motor related:
beacon 0:15a8480061e8 38 PwmOut left(PA_8);
beacon 0:15a8480061e8 39 PwmOut right(PA_9);
beacon 0:15a8480061e8 40
beacon 0:15a8480061e8 41 DigitalOut powerSignal(PB_2);
beacon 0:15a8480061e8 42 DigitalIn errorMotor(PB_14);
beacon 0:15a8480061e8 43 DigitalIn overtemperatur(PB_15);
beacon 0:15a8480061e8 44
beacon 0:15a8480061e8 45 //Arm:
beacon 0:15a8480061e8 46 ServoArm servoArm(PA_6);
beacon 0:15a8480061e8 47
beacon 0:15a8480061e8 48 //Greifer:
beacon 0:15a8480061e8 49 Servo servoGreifer(PC_7);
beacon 0:15a8480061e8 50
beacon 0:15a8480061e8 51 //Leiste:
beacon 0:15a8480061e8 52 Servo servoLeiste(PB_6);
beacon 0:15a8480061e8 53
beacon 0:15a8480061e8 54 //Button:
beacon 0:15a8480061e8 55 DigitalIn mybutton(USER_BUTTON);
beacon 0:15a8480061e8 56
beacon 0:15a8480061e8 57 //Farbsensor:
beacon 0:15a8480061e8 58 AnalogIn FarbVoltage(A0);
beacon 0:15a8480061e8 59 //DigitalOut led(D2);
beacon 0:15a8480061e8 60
beacon 0:15a8480061e8 61
beacon 0:15a8480061e8 62 Robot sam( &left, &right, &powerSignal, leds, &FarbVoltage, &frontS, &leftS, &rightS, &servoArm, &servoGreifer, &servoLeiste, &usensor, &pixy ); //Implement the Farbsensor into the Robot init function!!
beacon 0:15a8480061e8 63
beacon 0:15a8480061e8 64 void initializeDistanceSensors()
beacon 0:15a8480061e8 65 {
beacon 0:15a8480061e8 66 for( int ii = 0; ii<9; ++ii) {
beacon 0:15a8480061e8 67 sam.sensors[ii].init(&sensorVoltage, &frontS, &leftS, &rightS, &bit0, &bit1, &bit2, ii);
beacon 0:15a8480061e8 68
beacon 0:15a8480061e8 69 enable = 1;
beacon 0:15a8480061e8 70 }
beacon 0:15a8480061e8 71 }
beacon 0:15a8480061e8 72 /* */
beacon 0:15a8480061e8 73 int main(){
beacon 0:15a8480061e8 74 initializeDistanceSensors(); //Initialises IR-Sensors.
beacon 0:15a8480061e8 75 enum states {search = 0, setX, setY, fine, take, test};
beacon 0:15a8480061e8 76 int time = 0; //Time keeps track of time. [time] = ms
beacon 0:15a8480061e8 77
beacon 0:15a8480061e8 78 PID_Control pid;
beacon 0:15a8480061e8 79 pid.setPIDValues( 0.001f, 0.001f, 0.00015f, 0.3f, -0.3f, 1000);
beacon 0:15a8480061e8 80 pc.baud( 115200 );
beacon 0:15a8480061e8 81
beacon 0:15a8480061e8 82 int state = search;
beacon 0:15a8480061e8 83
beacon 0:15a8480061e8 84 sam.stop();
beacon 0:15a8480061e8 85
beacon 0:15a8480061e8 86 /*
beacon 0:15a8480061e8 87 while( 1 ){
beacon 0:15a8480061e8 88 printf("\n\rX: %d,\t Y: %d", pixy.getX(), pixy.getY());
beacon 0:15a8480061e8 89 wait(1.0f);
beacon 0:15a8480061e8 90 }
beacon 0:15a8480061e8 91 */
beacon 0:15a8480061e8 92
beacon 0:15a8480061e8 93 while( 1 ){
beacon 0:15a8480061e8 94 switch( state ){
beacon 0:15a8480061e8 95 case test:
beacon 0:15a8480061e8 96 int x = pixy.getX(), y = pixy.getY();
beacon 0:15a8480061e8 97 if( !mybutton ) printf("\n\rX:%d\tY:%d", x, y);
beacon 0:15a8480061e8 98 break;
beacon 0:15a8480061e8 99
beacon 0:15a8480061e8 100 case search:
beacon 0:15a8480061e8 101 if( sam.search(&time) ) state = found;
beacon 0:15a8480061e8 102 break;
beacon 0:15a8480061e8 103
beacon 0:15a8480061e8 104 case setX:{
beacon 0:15a8480061e8 105 static int messungen[20], i = 0;
beacon 0:15a8480061e8 106 float e = 132.5f - pixy.getX();
beacon 0:15a8480061e8 107 float diff = pid.calc( e, 0.001f );
beacon 0:15a8480061e8 108
beacon 0:15a8480061e8 109 sam.setLeft(0.5f - diff);
beacon 0:15a8480061e8 110 sam.setRight(0.5f - diff);
beacon 0:15a8480061e8 111 break;
beacon 0:15a8480061e8 112 }
beacon 0:15a8480061e8 113 case setY:{
beacon 0:15a8480061e8 114 static int messungen[20], i = 0;
beacon 0:15a8480061e8 115 float e = 121.5f - pixy.getY();
beacon 0:15a8480061e8 116 float diff = pid.calc( e, 0.001f );
beacon 0:15a8480061e8 117
beacon 0:15a8480061e8 118 sam.setLeft(0.5f + diff);
beacon 0:15a8480061e8 119 sam.setRight(0.5f - diff);
beacon 0:15a8480061e8 120 break;
beacon 0:15a8480061e8 121 }
beacon 0:15a8480061e8 122
beacon 0:15a8480061e8 123 case take:{
beacon 0:15a8480061e8 124 //sam.leds[1] = 1;
beacon 0:15a8480061e8 125 sam.stop();
beacon 0:15a8480061e8 126 while (1) wait(0.1f);
beacon 0:15a8480061e8 127 static int down = 0, closed = 0, up = 0;
beacon 0:15a8480061e8 128 if( down || sam.Arm.backToDown() ) down = 1;
beacon 0:15a8480061e8 129 else if( closed || sam.Greifer.take() ) closed = 1;
beacon 0:15a8480061e8 130 else if( up || sam.Arm.downToBack() ) up = 1;
beacon 0:15a8480061e8 131 else if( sam.Greifer.leave() ){
beacon 0:15a8480061e8 132 state = search;
beacon 0:15a8480061e8 133 down = 0;
beacon 0:15a8480061e8 134 closed = 0;
beacon 0:15a8480061e8 135 up = 0;
beacon 0:15a8480061e8 136 }
beacon 0:15a8480061e8 137 break;
beacon 0:15a8480061e8 138 }
beacon 0:15a8480061e8 139 }
beacon 0:15a8480061e8 140 time++;
beacon 0:15a8480061e8 141 wait(0.001f);
beacon 0:15a8480061e8 142 }
beacon 0:15a8480061e8 143
beacon 0:15a8480061e8 144 return 0;
beacon 0:15a8480061e8 145 }
beacon 0:15a8480061e8 146
beacon 0:15a8480061e8 147 /* * /
beacon 0:15a8480061e8 148 int main()
beacon 0:15a8480061e8 149 {
beacon 0:15a8480061e8 150 initializeDistanceSensors(); //Initialises IR-Sensors.
beacon 0:15a8480061e8 151 //int counter = 0; //Counts how many times the robot has turned, before driving
beacon 0:15a8480061e8 152 int timer = 0; //Is used, to reset the robot. Returns time in [0.1s]
beacon 0:15a8480061e8 153 //int lastFun = -1; //Is used, to check if the same action in Robot::search() was made multiple times.
beacon 0:15a8480061e8 154 //int found = 0; //0:= no block available, 1 := a lego is ready to be picked up
beacon 0:15a8480061e8 155 //int done = 0;
beacon 0:15a8480061e8 156 int color;
beacon 0:15a8480061e8 157
beacon 0:15a8480061e8 158 enum states { search = 0, LeisteDown, turn, push, backOff, forward, downward, down, upward, colorS, readyDrop, backward, LeisteUp };
beacon 0:15a8480061e8 159
beacon 0:15a8480061e8 160 int state = search;
beacon 0:15a8480061e8 161
beacon 0:15a8480061e8 162 static float messung = 0;
beacon 0:15a8480061e8 163
beacon 0:15a8480061e8 164 while( 1 ){
beacon 0:15a8480061e8 165 printf("\n\r%f", sam.see(FWD_L));
beacon 0:15a8480061e8 166
beacon 0:15a8480061e8 167 wait(1.0f);
beacon 0:15a8480061e8 168
beacon 0:15a8480061e8 169 }
beacon 0:15a8480061e8 170
beacon 0:15a8480061e8 171 while( 1 ) {
beacon 0:15a8480061e8 172
beacon 0:15a8480061e8 173 if ( timer > TIMEOUT ) {
beacon 0:15a8480061e8 174 NVIC_SystemReset(); //Resets Sam.
beacon 0:15a8480061e8 175 }
beacon 0:15a8480061e8 176
beacon 0:15a8480061e8 177 //if( timer == 0 )
beacon 0:15a8480061e8 178 //printf("\n\rLEFT: %.3f,\tFWD: %.3f,\tRIGHT: %.3f", sam.sensors[LEFT].read(), sam.sensors[FWD].read(), sam.sensors[RIGHT].read());
beacon 0:15a8480061e8 179
beacon 0:15a8480061e8 180
beacon 0:15a8480061e8 181 //printf("\n\rcurrent main state: %d", state);
beacon 0:15a8480061e8 182
beacon 0:15a8480061e8 183 sam.sensors[FWD_L].read() < NEAR ? sam.leds[1] = 1 : sam.leds[1] = 0;
beacon 0:15a8480061e8 184 switch( state ) {
beacon 0:15a8480061e8 185 case search:
beacon 0:15a8480061e8 186 if( sam.search(&timer) ){
beacon 0:15a8480061e8 187 //sam.Greifer.nullPos();
beacon 0:15a8480061e8 188 state = LeisteDown;
beacon 0:15a8480061e8 189 timer = 0;
beacon 0:15a8480061e8 190 }
beacon 0:15a8480061e8 191 break;
beacon 0:15a8480061e8 192
beacon 0:15a8480061e8 193 case LeisteDown:
beacon 0:15a8480061e8 194 int count = 0;
beacon 0:15a8480061e8 195 if( sam.Leiste.upToDown() ){
beacon 0:15a8480061e8 196 //sam.Greifer.leave();
beacon 0:15a8480061e8 197 state = turn;
beacon 0:15a8480061e8 198 timer = 0;
beacon 0:15a8480061e8 199 }
beacon 0:15a8480061e8 200 break;
beacon 0:15a8480061e8 201
beacon 0:15a8480061e8 202 case turn:
beacon 0:15a8480061e8 203 static int i = 0;
beacon 0:15a8480061e8 204 if( i > 7 ){
beacon 0:15a8480061e8 205 sam.stop();
beacon 0:15a8480061e8 206 state = push;
beacon 0:15a8480061e8 207 i = 0;
beacon 0:15a8480061e8 208 }
beacon 0:15a8480061e8 209 else{
beacon 0:15a8480061e8 210 i++;
beacon 0:15a8480061e8 211 sam.turnRight();
beacon 0:15a8480061e8 212 }
beacon 0:15a8480061e8 213 break;
beacon 0:15a8480061e8 214
beacon 0:15a8480061e8 215 case push:{
beacon 0:15a8480061e8 216 static int i = 0;
beacon 0:15a8480061e8 217 if( i > 5 ){
beacon 0:15a8480061e8 218 sam.stop();
beacon 0:15a8480061e8 219 i = 0;
beacon 0:15a8480061e8 220 state = backOff;
beacon 0:15a8480061e8 221 timer = 0;
beacon 0:15a8480061e8 222 }
beacon 0:15a8480061e8 223 else{
beacon 0:15a8480061e8 224 sam.driveSlowly();
beacon 0:15a8480061e8 225 i++;
beacon 0:15a8480061e8 226 }
beacon 0:15a8480061e8 227 break;
beacon 0:15a8480061e8 228 }
beacon 0:15a8480061e8 229
beacon 0:15a8480061e8 230 case backOff:{
beacon 0:15a8480061e8 231 static int i = 0;
beacon 0:15a8480061e8 232 if( i > 1 ){
beacon 0:15a8480061e8 233 sam.stop();
beacon 0:15a8480061e8 234 i = 0;
beacon 0:15a8480061e8 235 state = forward;
beacon 0:15a8480061e8 236 timer = 0;
beacon 0:15a8480061e8 237 }
beacon 0:15a8480061e8 238 else{
beacon 0:15a8480061e8 239 sam.driveBackSlowly();
beacon 0:15a8480061e8 240 i++;
beacon 0:15a8480061e8 241 }
beacon 0:15a8480061e8 242 break;
beacon 0:15a8480061e8 243 }
beacon 0:15a8480061e8 244
beacon 0:15a8480061e8 245
beacon 0:15a8480061e8 246 case forward:
beacon 0:15a8480061e8 247 if( sam.Arm.backToCollect() ){
beacon 0:15a8480061e8 248 state = downward;
beacon 0:15a8480061e8 249 timer = 0;
beacon 0:15a8480061e8 250 }
beacon 0:15a8480061e8 251 break;
beacon 0:15a8480061e8 252
beacon 0:15a8480061e8 253 case downward:
beacon 0:15a8480061e8 254 if( sam.Arm.collectToDown() ){
beacon 0:15a8480061e8 255 state = down;
beacon 0:15a8480061e8 256 timer = 0;
beacon 0:15a8480061e8 257 }
beacon 0:15a8480061e8 258 break;
beacon 0:15a8480061e8 259
beacon 0:15a8480061e8 260 case down:
beacon 0:15a8480061e8 261 if( sam.Greifer.take() ) {
beacon 0:15a8480061e8 262 state = upward;
beacon 0:15a8480061e8 263 timer = 0;
beacon 0:15a8480061e8 264 }
beacon 0:15a8480061e8 265 break;
beacon 0:15a8480061e8 266
beacon 0:15a8480061e8 267 case upward:
beacon 0:15a8480061e8 268 if( sam.Arm.downToCollect() ){
beacon 0:15a8480061e8 269 state = colorS;
beacon 0:15a8480061e8 270 timer = 0;
beacon 0:15a8480061e8 271 }
beacon 0:15a8480061e8 272 break;
beacon 0:15a8480061e8 273
beacon 0:15a8480061e8 274 case colorS:
beacon 0:15a8480061e8 275 led = 1;
beacon 0:15a8480061e8 276 color = sam.FarbVoltage.read();
beacon 0:15a8480061e8 277
beacon 0:15a8480061e8 278 if( color == -1 ){
beacon 0:15a8480061e8 279 //Do nothing
beacon 0:15a8480061e8 280 }
beacon 0:15a8480061e8 281
beacon 0:15a8480061e8 282
beacon 0:15a8480061e8 283 else if( color == 0 || color == GREEN ){
beacon 0:15a8480061e8 284 state = backward;
beacon 0:15a8480061e8 285 led = 0;
beacon 0:15a8480061e8 286 timer = 0;
beacon 0:15a8480061e8 287 }
beacon 0:15a8480061e8 288
beacon 0:15a8480061e8 289 else if( color == RED ){
beacon 0:15a8480061e8 290 state = readyDrop;
beacon 0:15a8480061e8 291 led = 0;
beacon 0:15a8480061e8 292 timer = 0;
beacon 0:15a8480061e8 293 }
beacon 0:15a8480061e8 294
beacon 0:15a8480061e8 295 else{
beacon 0:15a8480061e8 296 //Shit...
beacon 0:15a8480061e8 297 }
beacon 0:15a8480061e8 298 break;
beacon 0:15a8480061e8 299
beacon 0:15a8480061e8 300 case readyDrop:
beacon 0:15a8480061e8 301 if( sam.Greifer.leave() ){
beacon 0:15a8480061e8 302 if( color == GREEN || color == 0 ){
beacon 0:15a8480061e8 303 state = LeisteUp;
beacon 0:15a8480061e8 304 }
beacon 0:15a8480061e8 305 else{
beacon 0:15a8480061e8 306 state = backward;
beacon 0:15a8480061e8 307 }
beacon 0:15a8480061e8 308 timer = 0;
beacon 0:15a8480061e8 309 }
beacon 0:15a8480061e8 310
beacon 0:15a8480061e8 311 break;
beacon 0:15a8480061e8 312
beacon 0:15a8480061e8 313 case backward:
beacon 0:15a8480061e8 314 if( sam.Arm.collectToBack() ){
beacon 0:15a8480061e8 315 //sam.Greifer.nullPos();
beacon 0:15a8480061e8 316 state = LeisteUp;
beacon 0:15a8480061e8 317 timer = 0;
beacon 0:15a8480061e8 318 if( color == GREEN || color == 0 ){
beacon 0:15a8480061e8 319 state = readyDrop;
beacon 0:15a8480061e8 320 sam.Greifer.leave();
beacon 0:15a8480061e8 321 }
beacon 0:15a8480061e8 322 else{
beacon 0:15a8480061e8 323 state = LeisteUp;
beacon 0:15a8480061e8 324 }
beacon 0:15a8480061e8 325 }
beacon 0:15a8480061e8 326 break;
beacon 0:15a8480061e8 327
beacon 0:15a8480061e8 328 case LeisteUp:
beacon 0:15a8480061e8 329 if( sam.Leiste.downToUp() ){
beacon 0:15a8480061e8 330 state = search;
beacon 0:15a8480061e8 331 timer = 0;
beacon 0:15a8480061e8 332 }
beacon 0:15a8480061e8 333 break;
beacon 0:15a8480061e8 334 }
beacon 0:15a8480061e8 335 timer++;
beacon 0:15a8480061e8 336 wait(0.1f);
beacon 0:15a8480061e8 337 }
beacon 0:15a8480061e8 338
beacon 0:15a8480061e8 339 return 0;
beacon 0:15a8480061e8 340 }
beacon 0:15a8480061e8 341 /* */