Sooner Competitive Robotics / Mbed 2 deprecated IEEE_14_Freescale

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

Fork of IEEE_14_Freescale by IEEE 2014 Mbed

robot.cpp@26:ade7c813538f, 2014-03-14 (annotated)

Committer:
sswatek
Date:
Fri Mar 14 00:38:51 2014 +0000
Revision:
26:ade7c813538f
Parent:
17:e247d58d9f42
Child:
27:688409727452
removed old control system variables and added prototype shape polling function

Who changed what in which revision?

UserRevisionLine numberNew contents of line
soonerbot 1:c28fac16a109 1 #include "robot.h"
soonerbot 1:c28fac16a109 2
soonerbot 1:c28fac16a109 3 #define MIN(a,b) ((b<a)?(b):(a))
soonerbot 1:c28fac16a109 4 #define MAX(a,b) ((b>a)?(b):(a))
soonerbot 1:c28fac16a109 5
soonerbot 7:3b2cf7efe5d1 6 //this is the main place pinouts are put
soonerbot 7:3b2cf7efe5d1 7 //also sets up the control system constants
soonerbot 3:a223b0bf8256 8 robot::robot() : spi(PTD2, PTD3, PTD1), bigenc(spi,PTD0), gyro(PTE0, PTE1),
sswatek 17:e247d58d9f42 9 /* right(bigenc,0,1,PTA5), left(bigenc,2,3,PTA4), */motors(spi,PTD0), pingLeft(PTA13,PTD5),
sswatek 17:e247d58d9f42 10 BTLink(PTC4,PTC3) {
soonerbot 1:c28fac16a109 11
soonerbot 11:967469d7e01c 12 //bigenc.setDirections(1,-1,1,1);
soonerbot 15:b10859606504 13 //left.setReversed(1);
soonerbot 1:c28fac16a109 14 x=y=rot=0;
soonerbot 7:3b2cf7efe5d1 15
soonerbot 11:967469d7e01c 16 //set our set point in memory to match the current set point of the encoders
soonerbot 11:967469d7e01c 17 const int *referenceCounts = bigenc.getReferences();
soonerbot 11:967469d7e01c 18 motors.setSetPoint(referenceCounts[0],referenceCounts[1]);
soonerbot 11:967469d7e01c 19
sswatek 26:ade7c813538f 20 //storage for shape positions
sswatek 26:ade7c813538f 21 circleX=circleY=rectX=rectY=rectRot=triX=triY=triRot=0;
soonerbot 1:c28fac16a109 22 }
soonerbot 1:c28fac16a109 23
soonerbot 10:926f142f16a3 24 //driveforward, but set up so that
soonerbot 10:926f142f16a3 25 int robot::absDriveForward(double desangle, int distance){
soonerbot 15:b10859606504 26 return smoothMove(distance, 0, 5);
soonerbot 13:c2d14bf733a5 27 /*Timer tim;
soonerbot 11:967469d7e01c 28 int dir=1;
soonerbot 12:925f52da3ba9 29 // if we are traveling forward a negative distance, go backwards
soonerbot 11:967469d7e01c 30 if(distance < 0){
soonerbot 12:925f52da3ba9 31 dir = -1; //is multiplied by the final output
soonerbot 11:967469d7e01c 32 distance = -distance;
soonerbot 11:967469d7e01c 33 }
soonerbot 10:926f142f16a3 34 int i,move;
soonerbot 10:926f142f16a3 35 int maxSpeed=41;
soonerbot 13:c2d14bf733a5 36 int stepsPerInc=2;
soonerbot 10:926f142f16a3 37 int stepsToMax=(maxSpeed-1)*stepsPerInc;
soonerbot 10:926f142f16a3 38 int distToMax=(maxSpeed*(maxSpeed-1)*stepsPerInc)/2;
soonerbot 10:926f142f16a3 39 int stopRaise,startFall;
soonerbot 10:926f142f16a3 40 if(distance<distToMax*2){
soonerbot 12:925f52da3ba9 41 // can't get up to full speed before needing to slow down
soonerbot 12:925f52da3ba9 42 //find new max speed, time to get there, and distance there
soonerbot 10:926f142f16a3 43 maxSpeed = ((2*stepsPerInc)+sqrt((double)(2*stepsPerInc)*((2*stepsPerInc)+8*distance)))/(4*stepsPerInc);
soonerbot 10:926f142f16a3 44 stepsToMax=(maxSpeed-1)*stepsPerInc;
soonerbot 10:926f142f16a3 45 distToMax=(maxSpeed*(maxSpeed-1)*stepsPerInc)/2;
soonerbot 10:926f142f16a3 46 stopRaise = stepsToMax;
soonerbot 10:926f142f16a3 47 startFall = stopRaise+(distance-distToMax*2)/(maxSpeed)+1;
soonerbot 10:926f142f16a3 48 DBGPRINT("Insufficent Ramp-Up\r\n",1);
soonerbot 10:926f142f16a3 49 } else {
soonerbot 12:925f52da3ba9 50 // can get up to full speed, so we will
soonerbot 10:926f142f16a3 51 stopRaise=stepsToMax;
soonerbot 12:925f52da3ba9 52 startFall=stopRaise+(distance-distToMax*2)/maxSpeed+1;
soonerbot 10:926f142f16a3 53 DBGPRINT("Sufficent Ramp-Up\r\n",1);
soonerbot 10:926f142f16a3 54 }
soonerbot 10:926f142f16a3 55 DBGPRINT("[%d %d %d] {%d, %d}\n\r",maxSpeed,stepsToMax,distToMax,stopRaise,startFall);
soonerbot 10:926f142f16a3 56 tim.start();
soonerbot 10:926f142f16a3 57 move = 0;
soonerbot 10:926f142f16a3 58 int totalMove=0;
soonerbot 12:925f52da3ba9 59 gyro.stop();
soonerbot 10:926f142f16a3 60 for(i=0;i<startFall+stepsToMax;i++){
soonerbot 12:925f52da3ba9 61 //start clock for this frame
soonerbot 10:926f142f16a3 62 tim.reset();
soonerbot 10:926f142f16a3 63 if(i<=stopRaise && i%stepsPerInc==0){
soonerbot 12:925f52da3ba9 64 //increase speed every (stepsPerInc) steps up to maxSpeed
soonerbot 10:926f142f16a3 65 move++;
soonerbot 10:926f142f16a3 66 }
soonerbot 10:926f142f16a3 67 if(i==startFall-1){
soonerbot 12:925f52da3ba9 68 //calibration step before we start falling to get an exact result
soonerbot 10:926f142f16a3 69 move = distance-totalMove-distToMax;
soonerbot 10:926f142f16a3 70 }
soonerbot 10:926f142f16a3 71 if(i>=startFall && (i-startFall)%stepsPerInc==0){
soonerbot 12:925f52da3ba9 72 if(i==startFall) //reset move after calibration step
soonerbot 10:926f142f16a3 73 move=maxSpeed;
soonerbot 12:925f52da3ba9 74
soonerbot 12:925f52da3ba9 75 //decrement every stepsPerInc steps until stopped
soonerbot 10:926f142f16a3 76 move--;
soonerbot 10:926f142f16a3 77 }
soonerbot 10:926f142f16a3 78 totalMove+=move;
soonerbot 11:967469d7e01c 79 motors.moveForward(move*dir);
soonerbot 11:967469d7e01c 80 //DBGPRINT("%d: %d\t%d\r\n",i,move,totalMove);
soonerbot 12:925f52da3ba9 81 while(tim.read_ms()<5);
soonerbot 12:925f52da3ba9 82 gyro.gyroUpkeep();
soonerbot 12:925f52da3ba9 83 addforward(double(move*dir)*0.0035362);
soonerbot 12:925f52da3ba9 84 //waits until 10ms have passed for the 100hz timing
soonerbot 10:926f142f16a3 85 while(tim.read_ms()<10);
soonerbot 10:926f142f16a3 86 }
soonerbot 12:925f52da3ba9 87 gyro.start();
soonerbot 12:925f52da3ba9 88 return totalMove*dir;
soonerbot 13:c2d14bf733a5 89 */
soonerbot 13:c2d14bf733a5 90 }
soonerbot 13:c2d14bf733a5 91
soonerbot 13:c2d14bf733a5 92 int robot::smoothMove(int distance, int rotate, int maxSpeed){
soonerbot 13:c2d14bf733a5 93 Timer tim;
soonerbot 13:c2d14bf733a5 94 int dir=1;
soonerbot 13:c2d14bf733a5 95 // if we are traveling forward a negative distance, go backwards
soonerbot 13:c2d14bf733a5 96 if(distance < 0){
soonerbot 13:c2d14bf733a5 97 dir = -1; //is multiplied by the final output
soonerbot 13:c2d14bf733a5 98 distance = -distance;
soonerbot 13:c2d14bf733a5 99 }
soonerbot 13:c2d14bf733a5 100 if(rotate)
soonerbot 13:c2d14bf733a5 101 rotate=-1;
soonerbot 13:c2d14bf733a5 102 else
soonerbot 13:c2d14bf733a5 103 rotate=1;
soonerbot 13:c2d14bf733a5 104 int i,move;
soonerbot 13:c2d14bf733a5 105 //int maxSpeed=41;
soonerbot 13:c2d14bf733a5 106 int stepsPerInc=2;
soonerbot 13:c2d14bf733a5 107 int stepsToMax=(maxSpeed-1)*stepsPerInc;
soonerbot 13:c2d14bf733a5 108 int distToMax=(maxSpeed*(maxSpeed-1)*stepsPerInc)/2;
soonerbot 13:c2d14bf733a5 109 int stopRaise,startFall;
soonerbot 13:c2d14bf733a5 110 if(distance<distToMax*2){
soonerbot 13:c2d14bf733a5 111 // can't get up to full speed before needing to slow down
soonerbot 13:c2d14bf733a5 112 //find new max speed, time to get there, and distance there
soonerbot 13:c2d14bf733a5 113 maxSpeed = ((2*stepsPerInc)+sqrt((double)(2*stepsPerInc)*((2*stepsPerInc)+8*distance)))/(4*stepsPerInc);
soonerbot 13:c2d14bf733a5 114 stepsToMax=(maxSpeed-1)*stepsPerInc;
soonerbot 13:c2d14bf733a5 115 distToMax=(maxSpeed*(maxSpeed-1)*stepsPerInc)/2;
soonerbot 13:c2d14bf733a5 116 stopRaise = stepsToMax;
soonerbot 13:c2d14bf733a5 117 startFall = stopRaise+(distance-distToMax*2)/(maxSpeed)+1;
soonerbot 13:c2d14bf733a5 118 DBGPRINT("Insufficent Ramp-Up\r\n",1);
soonerbot 13:c2d14bf733a5 119 } else {
soonerbot 13:c2d14bf733a5 120 // can get up to full speed, so we will
soonerbot 13:c2d14bf733a5 121 stopRaise=stepsToMax;
soonerbot 13:c2d14bf733a5 122 startFall=stopRaise+(distance-distToMax*2)/maxSpeed+1;
soonerbot 13:c2d14bf733a5 123 DBGPRINT("Sufficent Ramp-Up\r\n",1);
soonerbot 13:c2d14bf733a5 124 }
soonerbot 13:c2d14bf733a5 125 DBGPRINT("[%d %d %d] {%d, %d}\n\r",maxSpeed,stepsToMax,distToMax,stopRaise,startFall);
soonerbot 13:c2d14bf733a5 126 tim.start();
soonerbot 13:c2d14bf733a5 127 move = 0;
soonerbot 13:c2d14bf733a5 128 int totalMove=0;
soonerbot 13:c2d14bf733a5 129 gyro.stop();
soonerbot 13:c2d14bf733a5 130 for(i=0;i<startFall+stepsToMax;i++){
soonerbot 13:c2d14bf733a5 131 //start clock for this frame
soonerbot 13:c2d14bf733a5 132 tim.reset();
soonerbot 13:c2d14bf733a5 133 if(i<=stopRaise && i%stepsPerInc==0){
soonerbot 13:c2d14bf733a5 134 //increase speed every (stepsPerInc) steps up to maxSpeed
soonerbot 13:c2d14bf733a5 135 move++;
soonerbot 13:c2d14bf733a5 136 }
soonerbot 13:c2d14bf733a5 137 if(i==startFall-1){
soonerbot 13:c2d14bf733a5 138 //calibration step before we start falling to get an exact result
soonerbot 13:c2d14bf733a5 139 move = distance-totalMove-distToMax;
soonerbot 13:c2d14bf733a5 140 }
soonerbot 13:c2d14bf733a5 141 if(i>=startFall && (i-startFall)%stepsPerInc==0){
soonerbot 13:c2d14bf733a5 142 if(i==startFall) //reset move after calibration step
soonerbot 13:c2d14bf733a5 143 move=maxSpeed;
soonerbot 13:c2d14bf733a5 144
soonerbot 13:c2d14bf733a5 145 //decrement every stepsPerInc steps until stopped
soonerbot 13:c2d14bf733a5 146 move--;
soonerbot 13:c2d14bf733a5 147 }
soonerbot 13:c2d14bf733a5 148 totalMove+=move;
soonerbot 13:c2d14bf733a5 149 //motors.moveForward(move*dir);
soonerbot 13:c2d14bf733a5 150 motors.moveWheels(move*dir,move*dir*rotate);
soonerbot 13:c2d14bf733a5 151 //DBGPRINT("%d: %d\t%d\r\n",i,move,totalMove);
soonerbot 13:c2d14bf733a5 152 while(tim.read_ms()<5);
soonerbot 13:c2d14bf733a5 153 gyro.gyroUpkeep();
soonerbot 13:c2d14bf733a5 154 addforward(double(move*dir)*0.0035362);
soonerbot 13:c2d14bf733a5 155 //waits until 10ms have passed for the 100hz timing
soonerbot 13:c2d14bf733a5 156 while(tim.read_ms()<10);
soonerbot 13:c2d14bf733a5 157 }
soonerbot 13:c2d14bf733a5 158 gyro.start();
soonerbot 13:c2d14bf733a5 159 return totalMove*dir;
soonerbot 15:b10859606504 160 tim.stop();//may not need
soonerbot 10:926f142f16a3 161 }
soonerbot 10:926f142f16a3 162
soonerbot 7:3b2cf7efe5d1 163 //this is the main thing that both turns and goes forward
soonerbot 8:c23cd84befa2 164 // desangle is a angle in degrees to head towards (this is relative to the direction the robot starts pointing in
soonerbot 8:c23cd84befa2 165 // distance is a distance to head in that direction in units of encoder ticks
soonerbot 15:b10859606504 166 /*int robot::driveForward(double desangle, int distance){
soonerbot 1:c28fac16a109 167 bigenc.resetAll();
soonerbot 1:c28fac16a109 168 const int* constbuf = bigenc.getVals();
soonerbot 1:c28fac16a109 169 int prev[4]={0,0,0,0};
soonerbot 1:c28fac16a109 170 int distTraveled=0, i;
soonerbot 6:62d498ee97cf 171 double maxPow=0.4;
soonerbot 4:adc885f4ab75 172 int loopcount=0;
soonerbot 4:adc885f4ab75 173 double minmain=0.05;
soonerbot 4:adc885f4ab75 174 double minalt=0.05;
soonerbot 4:adc885f4ab75 175
soonerbot 7:3b2cf7efe5d1 176 //find a point in front of where we're heading
soonerbot 4:adc885f4ab75 177 int targetang = desangle*4050000.0/360.0;//gyro.getZ();
soonerbot 4:adc885f4ab75 178 int startang = targetang;
soonerbot 4:adc885f4ab75 179 double angle=double(startang)*2*3.14159/4050000.0;
soonerbot 6:62d498ee97cf 180 double targx = x + double((distance==0)?10000:distance)*0.0035362*cos(angle)*1.5;
soonerbot 15:b10859606504 181 double targy = y + double((
soonerbot 15:b10859606504 182
soonerbot 15:b10859606504 183 ==0)?10000:distance)*0.0035362*sin(angle)*1.5;
soonerbot 4:adc885f4ab75 184 int invfactor = 0;
soonerbot 7:3b2cf7efe5d1 185
soonerbot 7:3b2cf7efe5d1 186 //if going backwards, point away from the point
soonerbot 6:62d498ee97cf 187 if(distance<0) {
soonerbot 4:adc885f4ab75 188 invfactor = 2025000;
soonerbot 4:adc885f4ab75 189 }
soonerbot 6:62d498ee97cf 190 double realfac=0.1;
soonerbot 4:adc885f4ab75 191
soonerbot 4:adc885f4ab75 192 int pmain=distance;
soonerbot 4:adc885f4ab75 193 int imain=0;
soonerbot 4:adc885f4ab75 194 int dmain=0;
soonerbot 4:adc885f4ab75 195
soonerbot 4:adc885f4ab75 196 int dterm=0;
soonerbot 4:adc885f4ab75 197 const int ptol = 75;
soonerbot 4:adc885f4ab75 198 const int dtol = 10;
soonerbot 15:b10859606504 199 while((pmain <= -ptol || pmain >= ptol) || (dmain <= -dtol || dmain >= dtol)|| (realfac <= -0.03 || realfac >= 0.03) //|| (fmod(rot*180.0/3.14159-desangle+3600000.0,360.0) >= 1 && fmod(rot*180.0/3.14159-desangle+3600000.0,360.0) <= 359)){
soonerbot 8:c23cd84befa2 200
soonerbot 4:adc885f4ab75 201 //DBGPRINT("=%d of %d [%f] (%d, %d) \t{%f,\t%f,/t%f}",distTraveled,distance, maxPow, constbuf[0], constbuf[1],x,y,rot*180.0/3.14159);
soonerbot 1:c28fac16a109 202 for(i=0;i<4;i++)
soonerbot 1:c28fac16a109 203 prev[i]=constbuf[i];
soonerbot 1:c28fac16a109 204 //wait(0.05);
soonerbot 1:c28fac16a109 205 constbuf = bigenc.getVals();
soonerbot 4:adc885f4ab75 206
soonerbot 14:a30aa3b29a2e 207 //control system (Proportional, Integral, Derivative) = PID controller
soonerbot 5:74c8ce39334b 208 pmain = distance - (constbuf[0]+constbuf[1])/2;
soonerbot 14:a30aa3b29a2e 209 imain += pmain;
soonerbot 5:74c8ce39334b 210 dmain = ((constbuf[0]+constbuf[1]) - (prev[0]+prev[1]))/2;
soonerbot 4:adc885f4ab75 211
soonerbot 6:62d498ee97cf 212 //DBGPRINT("%f like %f [%f] {%d,%d,%f,%f}\n\r", gyro.compZ(invfactor+atan2(targy-y,targx-x)*4050000.0/(2*3.14159))*360.0/4050000.0,angle*180/3.14159,rot,pmain,dmain,fmod(rot*180.0/3.14159-desangle+3600000.0,360.0), realfac);\
soonerbot 5:74c8ce39334b 213
soonerbot 7:3b2cf7efe5d1 214 //finds the difference between the angle to the imaginary point we're headed to and the current angle and turns it into a power level
soonerbot 5:74c8ce39334b 215 realfac = (gyro.compZ(invfactor+atan2(targy-y,targx-x)*4050000.0/(2*3.14159)))*angfac;
soonerbot 7:3b2cf7efe5d1 216 realfac = MAX(MIN(realfac,0.3),-0.3); // limits how much the motors can turn to fix the angle
soonerbot 4:adc885f4ab75 217
soonerbot 7:3b2cf7efe5d1 218 //uses PID control for the forward/back motions and adds in the angular component
soonerbot 7:3b2cf7efe5d1 219 //the forward/back motions is limited to a certain speed
soonerbot 5:74c8ce39334b 220 double leftpow = MAX(MIN(pfac*pmain+ifac*imain+dfac*dmain,maxPow),-maxPow)-realfac;
soonerbot 5:74c8ce39334b 221 double rightpow = MAX(MIN(pfac*pmain+ifac*imain+dfac*dmain,maxPow),-maxPow)+realfac;
soonerbot 7:3b2cf7efe5d1 222
soonerbot 7:3b2cf7efe5d1 223 //if we haven't settled, but also aren't moving, then speed up until it moves
soonerbot 7:3b2cf7efe5d1 224 if((pmain <= -ptol || pmain >= ptol) || (dmain <= -dtol || dmain >= dtol) || (realfac <= -0.02 || realfac >= 0.02) ){
soonerbot 4:adc885f4ab75 225 if (leftpow>0){
soonerbot 4:adc885f4ab75 226 if (leftpow<minalt){
soonerbot 4:adc885f4ab75 227 leftpow=minalt;
soonerbot 4:adc885f4ab75 228 }
soonerbot 4:adc885f4ab75 229 }else if (leftpow>-minalt){
soonerbot 4:adc885f4ab75 230 leftpow=-minalt;
soonerbot 4:adc885f4ab75 231 }
soonerbot 4:adc885f4ab75 232 if (rightpow>0){
soonerbot 4:adc885f4ab75 233 if (rightpow<minmain){
soonerbot 4:adc885f4ab75 234 rightpow=minmain;
soonerbot 4:adc885f4ab75 235 }
soonerbot 4:adc885f4ab75 236 }else if (rightpow>-minmain){
soonerbot 4:adc885f4ab75 237 rightpow=-minmain;
soonerbot 4:adc885f4ab75 238 }
soonerbot 4:adc885f4ab75 239 }
soonerbot 4:adc885f4ab75 240 left.setPower(leftpow);
soonerbot 4:adc885f4ab75 241 right.setPower(rightpow);
soonerbot 7:3b2cf7efe5d1 242
soonerbot 7:3b2cf7efe5d1 243 //how far we've moved in the last timestep
soonerbot 4:adc885f4ab75 244 int deltaTraveled=(constbuf[0]-prev[0]+constbuf[1]-prev[1])/2;
soonerbot 4:adc885f4ab75 245 //DBGPRINT("\t %d\r\n",deltaTraveled);
soonerbot 7:3b2cf7efe5d1 246 addforward(double(deltaTraveled)*0.0035362); //update our position
soonerbot 1:c28fac16a109 247 distTraveled+=deltaTraveled;
soonerbot 4:adc885f4ab75 248 loopcount++;
soonerbot 7:3b2cf7efe5d1 249
soonerbot 7:3b2cf7efe5d1 250 //increase min speed so that it will actually move
soonerbot 4:adc885f4ab75 251 if((dmain<5&&dmain>-5)&&minmain<0.2){
soonerbot 5:74c8ce39334b 252 minmain+=0.003;
soonerbot 4:adc885f4ab75 253 } else if (minmain>=0.05) {
soonerbot 5:74c8ce39334b 254 minmain-=0.003;
soonerbot 4:adc885f4ab75 255 }
soonerbot 4:adc885f4ab75 256 if((dterm<5&&dterm>-5)&&minalt<0.2){
soonerbot 5:74c8ce39334b 257 minalt+=0.003;
soonerbot 4:adc885f4ab75 258 } else if (minalt>=0.05) {
soonerbot 5:74c8ce39334b 259 minalt-=0.003;
soonerbot 4:adc885f4ab75 260 }
soonerbot 1:c28fac16a109 261 }
soonerbot 1:c28fac16a109 262 left.brake();
soonerbot 1:c28fac16a109 263 right.brake();
soonerbot 6:62d498ee97cf 264 DBGPRINT("Loops: %d\r\n",loopcount);
soonerbot 7:3b2cf7efe5d1 265
soonerbot 7:3b2cf7efe5d1 266 //catch the slowdown movement
soonerbot 6:62d498ee97cf 267 wait(0.2);
soonerbot 5:74c8ce39334b 268 for(i=0;i<4;i++)
soonerbot 5:74c8ce39334b 269 prev[i]=constbuf[i];
soonerbot 5:74c8ce39334b 270 //wait(0.05);
soonerbot 5:74c8ce39334b 271 constbuf = bigenc.getVals();
soonerbot 6:62d498ee97cf 272 addforward(double(constbuf[0]-prev[0]+constbuf[1]-prev[1])*0.0035362/2.0);
soonerbot 5:74c8ce39334b 273 DBGPRINT("loss of %d and %d\n\r",constbuf[0]-prev[0],constbuf[1]-prev[1]);
soonerbot 1:c28fac16a109 274 return 0;
soonerbot 15:b10859606504 275 }*/
soonerbot 1:c28fac16a109 276
soonerbot 7:3b2cf7efe5d1 277 //add the motion in the direction that the robot is facing
soonerbot 4:adc885f4ab75 278 void robot::addforward(double dist){
soonerbot 4:adc885f4ab75 279 double angle=double(gyro.getZ())*2*3.14159/4050000.0;
soonerbot 1:c28fac16a109 280 x+=dist*cos(angle);
soonerbot 1:c28fac16a109 281 y+=dist*sin(angle);
soonerbot 1:c28fac16a109 282 rot=angle;
soonerbot 1:c28fac16a109 283 }
soonerbot 1:c28fac16a109 284
soonerbot 7:3b2cf7efe5d1 285 //doesn't work yet
soonerbot 15:b10859606504 286 /*int robot::moveTo(double xInches, double yInches){
soonerbot 1:c28fac16a109 287 double power=.2;
soonerbot 1:c28fac16a109 288 turntowards(xInches,yInches);
soonerbot 1:c28fac16a109 289 double distance=sqrt(pow(xInches-x,2)+pow(yInches-y,2))/0.0035362;
soonerbot 1:c28fac16a109 290 double angle=atan2(xInches-x,yInches-y)*180.0/3.14159;
soonerbot 4:adc885f4ab75 291 double currangle=double(gyro.getZ())*2*3.14159/4050000.0;
soonerbot 1:c28fac16a109 292 bigenc.resetAll();
soonerbot 1:c28fac16a109 293 const int* constbuf = bigenc.getVals();
soonerbot 1:c28fac16a109 294 int prev[4]={0,0,0,0};
soonerbot 1:c28fac16a109 295 int distTraveled=0, i;
soonerbot 1:c28fac16a109 296 double maxPow;
soonerbot 1:c28fac16a109 297 DBGPRINT("going %f at angle %f from current of %f\r\n",distance,angle,currangle);
soonerbot 1:c28fac16a109 298 while(distTraveled<distance){
soonerbot 1:c28fac16a109 299 angle=atan2(xInches-x,yInches-y)*180.0/3.14159;
soonerbot 4:adc885f4ab75 300 currangle=double(gyro.getZ())*2*3.14159/4050000.0;
soonerbot 1:c28fac16a109 301 maxPow=MAX(double(distance-distTraveled-2000)/15000.0,0.1);
soonerbot 1:c28fac16a109 302 if(currangle>angle+2.0){ //too far to the right, brake left
soonerbot 1:c28fac16a109 303 left.brake();
soonerbot 1:c28fac16a109 304 } else {
soonerbot 1:c28fac16a109 305 left.setPower(MIN(power,maxPow));
soonerbot 1:c28fac16a109 306 }
soonerbot 1:c28fac16a109 307 if(currangle<angle-2){
soonerbot 1:c28fac16a109 308 right.brake();
soonerbot 1:c28fac16a109 309 } else {
soonerbot 14:a30aa3b29a2e 310 right.setPo
soonerbot 14:a30aa3b29a2e 311
soonerbot 14:a30aa3b29a2e 312 wer(MIN(power,maxPow));
soonerbot 1:c28fac16a109 313 }
soonerbot 1:c28fac16a109 314 DBGPRINT("=%d of %d [%f] (%d, %d, %d, %d) \t{%f,\t%f,\t%f}\r\n",distTraveled,distance, maxPow, constbuf[0], constbuf[1], constbuf[2], constbuf[3],x,y,rot*180.0/3.14159);
soonerbot 1:c28fac16a109 315 for(i=0;i<4;i++)
soonerbot 1:c28fac16a109 316 prev[i]=constbuf[i];
soonerbot 1:c28fac16a109 317 //wait(0.05);
soonerbot 1:c28fac16a109 318 constbuf = bigenc.getVals();
soonerbot 1:c28fac16a109 319 int deltaTraveled=MAX((MIN(-constbuf[0]+prev[0],-constbuf[1]+prev[1])+MIN(constbuf[2]-prev[2],constbuf[3]-prev[3]))/2,0);
soonerbot 1:c28fac16a109 320 addforward(double(deltaTraveled)*0.0035362);
soonerbot 1:c28fac16a109 321 distTraveled+=deltaTraveled;
soonerbot 1:c28fac16a109 322 //angle=atan2(xInches-x,yInches-y)*180.0/3.14159;
soonerbot 1:c28fac16a109 323 }
soonerbot 1:c28fac16a109 324 left.brake();
soonerbot 1:c28fac16a109 325 right.brake();
soonerbot 1:c28fac16a109 326 return 0;
soonerbot 1:c28fac16a109 327
soonerbot 15:b10859606504 328 }*/
soonerbot 1:c28fac16a109 329
soonerbot 7:3b2cf7efe5d1 330 //also doesn't work
soonerbot 15:b10859606504 331 /*int robot::turntowards(double xInches, double yInches){
soonerbot 4:adc885f4ab75 332 double currangle=double(gyro.getZ())*2*3.14159/4050000.0;
soonerbot 1:c28fac16a109 333 double angle=atan2(xInches-x,yInches-y)*180.0/3.14159;
soonerbot 1:c28fac16a109 334 double finangle=angle;
soonerbot 1:c28fac16a109 335 /*if(int(currangle-angle)%360>180){ //needs to turn positive degrees
soonerbot 1:c28fac16a109 336 finangle=currangle+double(int(angle-currangle)%360);
soonerbot 1:c28fac16a109 337 } else {//negative degrees
soonerbot 1:c28fac16a109 338 finangle=currangle-double(int(currangle-angle)%360);
soonerbot 15:b10859606504 339 }
soonerbot 1:c28fac16a109 340 double acc=turn(0.4,finangle);
soonerbot 1:c28fac16a109 341 if(acc<-0.75 && acc>0.75){
soonerbot 1:c28fac16a109 342 acc=turn(0.3,finangle);
soonerbot 1:c28fac16a109 343 }
soonerbot 1:c28fac16a109 344
soonerbot 1:c28fac16a109 345
soonerbot 1:c28fac16a109 346 return 1;
soonerbot 15:b10859606504 347 }*/
soonerbot 1:c28fac16a109 348
soonerbot 7:3b2cf7efe5d1 349 //still no
soonerbot 15:b10859606504 350 /*double robot::turn(double power, double degrees){
soonerbot 1:c28fac16a109 351 bigenc.resetAll();
soonerbot 1:c28fac16a109 352 const int* constbuf = bigenc.getVals();
soonerbot 1:c28fac16a109 353 int prev[4]={0,0,0,0};
soonerbot 1:c28fac16a109 354 int startz;
soonerbot 4:adc885f4ab75 355 startz=gyro.getZ();
soonerbot 1:c28fac16a109 356 int gyroticks=(degrees*4050000)/360;
soonerbot 1:c28fac16a109 357 double maxPow;
soonerbot 1:c28fac16a109 358 int nowz=startz;
soonerbot 1:c28fac16a109 359 int dir=0;
soonerbot 1:c28fac16a109 360 if(gyroticks>startz){
soonerbot 1:c28fac16a109 361 right.setPower(-power);
soonerbot 1:c28fac16a109 362 left.setPower(power);
soonerbot 1:c28fac16a109 363 dir=1;
soonerbot 1:c28fac16a109 364 } else {
soonerbot 1:c28fac16a109 365 right.setPower(power);
soonerbot 1:c28fac16a109 366 left.setPower(-power);
soonerbot 1:c28fac16a109 367 dir=-1;
soonerbot 1:c28fac16a109 368 }
soonerbot 1:c28fac16a109 369 while((gyroticks-nowz)*dir>0){
soonerbot 1:c28fac16a109 370 maxPow=MAX(double(abs(gyroticks-nowz))/4050000.0,0.25);
soonerbot 1:c28fac16a109 371 if(gyroticks<nowz){
soonerbot 1:c28fac16a109 372 right.setPower(-MIN(power,maxPow));
soonerbot 1:c28fac16a109 373 left.setPower(MIN(power,maxPow));
soonerbot 1:c28fac16a109 374 } else {
soonerbot 1:c28fac16a109 375 right.setPower(MIN(power,maxPow));
soonerbot 1:c28fac16a109 376 left.setPower(-MIN(power,maxPow));
soonerbot 1:c28fac16a109 377 }
soonerbot 1:c28fac16a109 378 DBGPRINT("_%d of %d {%f, %f, %f}\r\n",(gyroticks-nowz)*dir, gyroticks,x,y,rot*180.0/3.14159);
soonerbot 1:c28fac16a109 379 for(int i=0;i<4;i++)
soonerbot 1:c28fac16a109 380 prev[i]=constbuf[i];
soonerbot 1:c28fac16a109 381 constbuf = bigenc.getVals();
soonerbot 1:c28fac16a109 382 int deltaTraveled;
soonerbot 1:c28fac16a109 383 if(gyroticks<nowz){
soonerbot 1:c28fac16a109 384 deltaTraveled=(MIN(-constbuf[0]+prev[0],-constbuf[1]+prev[1])-MIN(-constbuf[2]+prev[2],-constbuf[3]+prev[3]))/2;
soonerbot 1:c28fac16a109 385 } else {
soonerbot 1:c28fac16a109 386 deltaTraveled=(-MIN(constbuf[0]-prev[0],constbuf[1]-prev[1])+MIN(constbuf[2]-prev[2],constbuf[3]-prev[3]))/2;
soonerbot 1:c28fac16a109 387 }
soonerbot 1:c28fac16a109 388 addforward(double(deltaTraveled)*0.0035362);
soonerbot 4:adc885f4ab75 389 nowz=gyro.getZ();
soonerbot 1:c28fac16a109 390 }
soonerbot 15:b10859606504 391 //right.brake();
soonerbot 15:b10859606504 392 //left.brake();
soonerbot 1:c28fac16a109 393 return (gyroticks-nowz)*dir;
sswatek 26:ade7c813538f 394 }*/
sswatek 26:ade7c813538f 395
sswatek 26:ade7c813538f 396 int robot::pollForShapes(){
sswatek 26:ade7c813538f 397 int response=0;
sswatek 26:ade7c813538f 398 int gotAck=0;
sswatek 26:ade7c813538f 399 int responseData[16];
sswatek 26:ade7c813538f 400 int testdata[]={'H','e','l','l','o'};
sswatek 26:ade7c813538f 401 stepTimer.start();
sswatek 26:ade7c813538f 402 while(1){ //put the phone in mode 1 to start finding shapes
sswatek 26:ade7c813538f 403 response=BTLink.sendCmd(0x01,testdata,5);
sswatek 26:ade7c813538f 404 stepTimer.reset();
sswatek 26:ade7c813538f 405 while(1){
sswatek 26:ade7c813538f 406 gotAck=BTLink.getAck(response);
sswatek 26:ade7c813538f 407 if(gotAck || stepTimer.read_ms()>=500)
sswatek 26:ade7c813538f 408 break;
sswatek 26:ade7c813538f 409 }
sswatek 26:ade7c813538f 410 DBGPRINT("Mode1=%d, %d, %d [%d]\n\r",response,BTLink.bufSize(), gotAck, stepTimer.read_ms());
sswatek 26:ade7c813538f 411 if(gotAck)
sswatek 26:ade7c813538f 412 break;
sswatek 26:ade7c813538f 413 }
sswatek 26:ade7c813538f 414 while(1){ //scan the information we get back to find valid data
sswatek 26:ade7c813538f 415 stepTimer.reset();
sswatek 26:ade7c813538f 416 gotAck=0;
sswatek 26:ade7c813538f 417 while(1){
sswatek 26:ade7c813538f 418 BTLink.procBuf(0x01);
sswatek 26:ade7c813538f 419 gotAck=BTLink.getData(0x01, responseData);
sswatek 26:ade7c813538f 420 if(gotAck || stepTimer.read_ms()>=4000)
sswatek 26:ade7c813538f 421 break;
sswatek 26:ade7c813538f 422 }
sswatek 26:ade7c813538f 423 if(responseData[15]==1&&gotAck){
sswatek 26:ade7c813538f 424 for(int i=0;i<16;i++)
sswatek 26:ade7c813538f 425 DBGPRINT("%d,",responseData[i]);
sswatek 26:ade7c813538f 426 DBGPRINT("\n\r",1);
sswatek 26:ade7c813538f 427 circleX=circleY=rectX=rectY=rectRot=triX=triY=triRot=0;
sswatek 26:ade7c813538f 428 rectX=(responseData[4]<<8)|responseData[3];
sswatek 26:ade7c813538f 429 rectY=(responseData[6]<<8)|responseData[5];
sswatek 26:ade7c813538f 430 rectRot=responseData[2];
sswatek 26:ade7c813538f 431 circleX=(responseData[12]<<8)|responseData[11];
sswatek 26:ade7c813538f 432 circleY=(responseData[14]<<8)|responseData[13];
sswatek 26:ade7c813538f 433 triX=(responseData[8]<<8)|responseData[7];
sswatek 26:ade7c813538f 434 triY=(responseData[10]<<8)|responseData[9];
sswatek 26:ade7c813538f 435 triRot=responseData[15];
sswatek 26:ade7c813538f 436 DBGPRINT("Rect(%d, %d, %d) Tri(%d, %d, %d) Cir(%d, %d)\n\r",rectX,rectY,rectRot,triX,triY,triRot,circleX,circleY);
sswatek 26:ade7c813538f 437 break;
sswatek 26:ade7c813538f 438 } else {
sswatek 26:ade7c813538f 439 DBGPRINT("Did not pass hash check %d %d %d\n\r",responseData[15],gotAck,stepTimer.read_ms());
sswatek 26:ade7c813538f 440 }
sswatek 26:ade7c813538f 441 }
sswatek 26:ade7c813538f 442 while(1){ //put the phone back in default state
sswatek 26:ade7c813538f 443 response=BTLink.sendCmd(0x00,testdata,5);
sswatek 26:ade7c813538f 444 stepTimer.reset();
sswatek 26:ade7c813538f 445 while(1){
sswatek 26:ade7c813538f 446 gotAck=BTLink.getAck(response);
sswatek 26:ade7c813538f 447 if(gotAck || stepTimer.read_ms()>=500)
sswatek 26:ade7c813538f 448 break;
sswatek 26:ade7c813538f 449 }
sswatek 26:ade7c813538f 450 DBGPRINT("BackToNormal=%d, %d, %d [%d]\n\r",response,BTLink.bufSize(), gotAck, stepTimer.read_ms());
sswatek 26:ade7c813538f 451 if(gotAck)
sswatek 26:ade7c813538f 452 break;
sswatek 26:ade7c813538f 453 }
sswatek 26:ade7c813538f 454 stepTimer.stop();
sswatek 26:ade7c813538f 455 }