DERPS
Dependencies: BMP280 BNO055_fusion PowerControl mbed
Fork of STRAIGHT_DRIVE_NO_SEP by
Diff: main.cpp
- Revision:
- 36:dc69442c4c47
- Parent:
- 35:68917796c30a
- Child:
- 37:e8a6ea255c09
diff -r 68917796c30a -r dc69442c4c47 main.cpp --- a/main.cpp Sun Apr 24 22:37:07 2016 +0000 +++ b/main.cpp Mon Apr 25 17:47:51 2016 +0000 @@ -32,7 +32,7 @@ void BrownOut(); int xTarget=120; -int angle_error=0; +int angle_error=1; bool xGood=false; bool yGood=false; bool angleGood=false; @@ -95,11 +95,11 @@ wait(0.5);*/ while(1) { suction.pulsewidth_us(1300); - /*pressure=(int)read[0];//(int)pres.getTemperature(); + /*pressure=(int)read[0];//(int)pres.getTemperature(); if(pressure==88) - led1=1; + led1=1; else - led1=0;*/ + led1=0;*/ //uncomment this part if you want the robot to just drive down the window with no separtor if (xState==0) { motion.mStop(); @@ -107,17 +107,22 @@ 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.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); - motion.setAngle(angleTarget,xya.a,angle_error,ANGLE_BIAS); + // 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(0,xya.a,angle_error,ANGLE_BIAS); + else { + xTarget=(xTarget==120)?30:120; + } //motion.setYBias(0,xya.y,2,angleTarget); - //loc.get_xy(&xya); + //loc.get_xy(&xya); #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); + pc.printf("X: %3d\tY: %3d\tP: %3d\txGood: %d\tAngleGood: %d\tState: %d \n",xya.y,yTarget,xya.a,xGood,angleGood,xState); #endif //Feed the dog safe.kick(); @@ -127,7 +132,7 @@ void send() { //Print over serial port to WiFi MCU - pc.printf("%c%c%c%c\n",(char)xya.x,(char)xGood,xya.a/10,xya.a%10); + pc.printf("%c%c%c%c\n",(char)xya.x,(char)xTarget,xya.a/10,xya.a%10); } void BrownOut()