DERPS
Dependencies: BMP280 BNO055_fusion PowerControl mbed
Fork of STRAIGHT_DRIVE_NO_SEP by
Diff: main.cpp
- Revision:
- 38:16208e003dc9
- Parent:
- 37:e8a6ea255c09
- Child:
- 39:ecc9defe3dc0
- Child:
- 40:9a97c4403c0a
--- a/main.cpp Tue Apr 26 20:09:25 2016 +0000 +++ b/main.cpp Wed Apr 27 04:24:11 2016 +0000 @@ -24,14 +24,13 @@ I2C i2c2(p28, p27); I2C i2c1(p9, p10); BMP280 pres(i2c2); -LOCALIZE loc(i2c1, i2c2, p29, p8, p7, p6, p5, led1, led2, led3, led4); +LOCALIZE loc(i2c1, i2c2, p29, p5, p6, p7, p8, led1, led2, led3, led4); LOCALIZE_xya xya; LOCOMOTION motion(p11, p23, p24, p21, p22, p16, p15, led1, led2, led3, led4); -//LOCOMOTION motion(p11, p21, p22, p23, p24, p15, p16, led1, led2, led3, led4); void BrownOut(); -int xTarget=120; +int xTarget=FRAME_W; int angle_error=1; bool xGood=false; bool yGood=false; @@ -93,33 +92,26 @@ wait(0.5); while(1) { suction.pulsewidth_us(1285); - /*pressure=(int)read[0];//(int)pres.getTemperature(); - if(pressure==88) - led1=1; - else - led1=0;*/ //uncomment this part if you want the robot to just drive down the window with no separtor - if (xState==0) { - motion.mStop(); - safe.kick(); - continue; + if (xya.y>FRAME_H*0.65) { + while(1) + { + suction.pulsewidth_us(1285); + motion.mStop(); + safe.kick(); + } + //continue; } loc.get_xy(&xya); - //motion.check_xya(&xGood,&yGood,&angleGood,xTarget,angleTarget,yTarget,xya,3,3,angle_error); - - //motion.setXstate(&xState,&xTarget,xGood,angleGood,&angleTarget,&angle_error,yTarget); - //motion.setAngleTol(&angle_error,yGood,xGood); - // motion.setYgoal(xGood,angleGood,yGood,&yTarget); - //motion.setXPos(xTarget,xya.x,5,angleTarget,xya.a); xGood=motion.setXPos(xTarget,xya.x,2,0); if(!xGood) motion.setAngle(angleTarget,xya.a,angle_error,ANGLE_BIAS); else { - xTarget=(xTarget==120)?30:120; + xTarget=(xTarget==FRAME_W)?0:FRAME_W; angleTarget=(angleTarget==5)?-5:5; } //motion.setYBias(0,xya.y,2,angleTarget); - //loc.get_xy(&xya); + //loc.get_xy(&xya);5 #if defined(PC_MODE) pc.printf("X: %3d\tY: %3d\tP: %3d\txGood: %d\tAngleGood: %d\tState: %d \n",xya.y,yTarget,xya.a,xGood,angleGood,xState); #endif @@ -131,7 +123,7 @@ void send() { //Print over serial port to WiFi MCU - pc.printf("%c%c%c%c\n",(char)xya.x,(char)xTarget,xya.a/10,xya.a%10); + pc.printf("%c%c%c%c\n",(char)xya.x,(char)xya.y,xya.a/10,xya.a%10); } void BrownOut()