DERPS
Dependencies: BMP280 BNO055_fusion PowerControl mbed
Fork of STRAIGHT_DRIVE_NO_SEP by
Revision 38:16208e003dc9, committed 2016-04-27
- Comitter:
- 12104404
- Date:
- Wed Apr 27 04:24:11 2016 +0000
- Parent:
- 37:e8a6ea255c09
- Child:
- 39:ecc9defe3dc0
- Child:
- 40:9a97c4403c0a
- Commit message:
- good good;
Changed in this revision
--- a/LOCALIZE.cpp Tue Apr 26 20:09:25 2016 +0000 +++ b/LOCALIZE.cpp Wed Apr 27 04:24:11 2016 +0000 @@ -84,30 +84,30 @@ if(_xya.a<R_ERROR || _xya.a>360-R_ERROR) { _rx=_rx_p<_rx_n? _rx_p+RX_OFF : FRAME_W-RX_OFF-_rx_n; _ry=_ry_p<_ry_n? _ry_p+RY_OFF : FRAME_H-RY_OFF-_ry_n; - if(!_sw1 && !_sw2) + if(!_sw1 || !_sw2) _rx=0;//RX_OFF; - else if(!_sw3 && !_sw4) + else if(!_sw3 || !_sw4) _rx=FRAME_W;//-RX_OFF; } else if(abs(_xya.a-270)<R_ERROR) { _rx=_ry_p<_ry_n? _ry_p+RY_OFF : FRAME_W-RY_OFF-_ry_n; _ry=_rx_p<_rx_n? FRAME_H-RX_OFF-_rx_p : _rx_n+RX_OFF; - if(!_sw1 && !_sw2) + if(!_sw1 || !_sw2) _ry=FRAME_H;//-RY_OFF; - else if(!_sw3 && !_sw4) + else if(!_sw3 || !_sw4) _ry=0;//RY_OFF; } else if(abs(_xya.a-180)<R_ERROR) { _rx=_rx_p<_rx_n? FRAME_W-RX_OFF-_rx_p : _rx_n+RX_OFF; _ry=_ry_p<_ry_n? FRAME_H-RY_OFF-_ry_p : _ry_n+RY_OFF; - if(!_sw1 && !_sw2) + if(!_sw1 || !_sw2) _rx=FRAME_W;//-RX_OFF; - else if(!_sw3 && !_sw4) + else if(!_sw3 || !_sw4) _rx=0;//RX_OFF; } else if(abs(_xya.a-90)<R_ERROR) { _rx=_ry_p<_ry_n? FRAME_W-RY_OFF-_ry_p : _ry_n+RY_OFF; _ry=_rx_p<_rx_n? _rx_p+RX_OFF : FRAME_H-RX_OFF-_rx_n; - if(!_sw1 && !_sw2) + if(!_sw1 || !_sw2) _ry=0;//RY_OFF; - else if(!_sw3 && !_sw4) + else if(!_sw3 || !_sw4) _ry=FRAME_H;//-RY_OFF; } else { //error last value
--- a/LOCOMOTION.cpp Tue Apr 26 20:09:25 2016 +0000 +++ b/LOCOMOTION.cpp Wed Apr 27 04:24:11 2016 +0000 @@ -56,7 +56,7 @@ s=SPEED_X_MIN; else s=((SPEED_X_MAX-SPEED_X_MIN)*abs(current-target)/FRAME_W)+SPEED_X_MIN;*/ - if(abs(current-target)<25) + if(abs(current-target)<30) s=((SPEED_X_MAX-SPEED_X_MIN)*abs(current-target)/FRAME_W)+SPEED_X_MIN; else s=SPEED_X_MAX;
--- 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()