![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
added
Dependencies: BNO055_fusion PowerControl mbed
Fork of TEAM_G_FLOW_RIDA by
Diff: main.cpp
- Revision:
- 26:5ae5ef623b72
- Parent:
- 25:f3bf8351bbd4
--- a/main.cpp Tue Apr 05 02:30:40 2016 +0000 +++ b/main.cpp Wed Apr 06 03:54:29 2016 +0000 @@ -3,7 +3,7 @@ #include "SAFETY.h" #define WATCHDOG_TIME 10 -//#define PC_MODE 1 +#define PC_MODE 1 #if defined (PC_MODE) Serial pc(USBTX, USBRX); @@ -35,7 +35,7 @@ bool angleGood=false; int xState=X_INCREASE; int angleTarget=0; -int yTarget=100; +int yTarget=30; //bool flag=false; //int target=20; @@ -70,11 +70,16 @@ led4=0; suction.pulsewidth_us(1000); while(1) { + if (yTarget>120){ + motion.mStop(); + safe.kick(); + continue; + } //suction.pulsewidth_us(1900); 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); + motion.setXstate(&xState,&xTarget,xGood,angleGood,&angleTarget,&angle_error); motion.setAngleTol(&angle_error,yGood,xGood); motion.setYgoal(xGood,angleGood,yGood,&yTarget); if(motion.setAngle(angleTarget,xya.a,angle_error,ANGLE_TURN)) { @@ -96,7 +101,7 @@ #if defined(PC_MODE) - pc.printf("X: %3d\tY: %3d\tP: %3d\txGood: %d\tAngleGood: %d\tState: %d \n",xya.x,xya.y,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();