DERPS
Dependencies: BMP280 BNO055_fusion PowerControl mbed
Fork of STRAIGHT_DRIVE_NO_SEP by
Diff: main.cpp
- Revision:
- 31:69caabc10879
- Parent:
- 30:116cd143fdf7
- Child:
- 33:baf24c59affc
diff -r 116cd143fdf7 -r 69caabc10879 main.cpp --- a/main.cpp Tue Apr 19 02:04:10 2016 +0000 +++ b/main.cpp Wed Apr 20 08:21:21 2016 +0000 @@ -32,13 +32,13 @@ void BrownOut(); int xTarget=120; -int angle_error=5; +int angle_error=2; bool xGood=false; bool yGood=false; bool angleGood=false; -int xState=X_INCREASE; -int angleTarget=5; -int yTarget=80; +int xState=X_BEGINING; +int angleTarget=180; +int yTarget=ROB_SIZE/2; int pressure; //bool flag=false; @@ -78,47 +78,33 @@ led2=0; led3=0; led4=0; - suction.pulsewidth_us(1200); + suction.pulsewidth_us(1000); wait(0.5); - suction.pulsewidth_us(1300); + suction.pulsewidth_us(1050); wait(0.5); - suction.pulsewidth_us(1400); + suction.pulsewidth_us(1100); wait(0.5); - suction.pulsewidth_us(1500); + suction.pulsewidth_us(1150); wait(0.5); - suction.pulsewidth_us(1600); + suction.pulsewidth_us(1200); wait(0.5); while(1) { - suction.pulsewidth_us(1750); - //loc.get_angle(&xya); - loc.get_xy(&xya); - //pressure=(int)read[0];//(int)pres.getTemperature(); - //if(pressure==88) - // led1=1; - //else - // led1=0; - /*if(motion.setAngle(5,xya.a,angle_error,ANGLE_TURN)) - { - motion.setXPos(xTarget,xya.x,2,angleTarget); - motion.setYBias(yTarget,xya.y,2,angleTarget); - }*/ - //motion.moveF(xya.a); - /*if(abs(xya.a-180)<90) - motion.moveB(); + suction.pulsewidth_us(1250); + /*pressure=(int)read[0];//(int)pres.getTemperature(); + if(pressure==88) + led1=1; else - motion.moveB();*/ - //loc.get_xy(&xya); - - /*if (yTarget>120) { + 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; } - 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); + motion.setXstate(&xState,&xTarget,xGood,angleGood,&angleTarget,&angle_error,yTarget); motion.setAngleTol(&angle_error,yGood,xGood); motion.setYgoal(xGood,angleGood,yGood,&yTarget); if(motion.setAngle(angleTarget,xya.a,angle_error,ANGLE_TURN)) {