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@10:926f142f16a3, 2014-02-12 (annotated)

Committer:
soonerbot
Date:
Wed Feb 12 05:38:52 2014 +0000
Revision:
10:926f142f16a3
Parent:
9:aff48e331147
Child:
11:967469d7e01c
Can move a set distance forward smoothly.

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),
soonerbot 9:aff48e331147 9 right(bigenc,0,1,PTA5), left(bigenc,2,3,PTA4), motors(spi,PTD0) {
soonerbot 1:c28fac16a109 10
soonerbot 1:c28fac16a109 11 bigenc.setDirections(1,-1,1,1);
soonerbot 1:c28fac16a109 12 left.setReversed(1);
soonerbot 1:c28fac16a109 13 x=y=rot=0;
soonerbot 7:3b2cf7efe5d1 14
soonerbot 7:3b2cf7efe5d1 15 //control system constants
soonerbot 4:adc885f4ab75 16 pfac=0.00035;
soonerbot 4:adc885f4ab75 17 ifac=0.00000001;
soonerbot 4:adc885f4ab75 18 dfac=0.000001;
soonerbot 4:adc885f4ab75 19
soonerbot 5:74c8ce39334b 20 angfac=0.0000016;
soonerbot 1:c28fac16a109 21 }
soonerbot 1:c28fac16a109 22
soonerbot 10:926f142f16a3 23 //driveforward, but set up so that
soonerbot 10:926f142f16a3 24 int robot::absDriveForward(double desangle, int distance){
soonerbot 10:926f142f16a3 25 Timer tim;
soonerbot 10:926f142f16a3 26 int i,move;
soonerbot 10:926f142f16a3 27 int maxSpeed=41;
soonerbot 10:926f142f16a3 28 int stepsPerInc=3;
soonerbot 10:926f142f16a3 29 int stepsToMax=(maxSpeed-1)*stepsPerInc;
soonerbot 10:926f142f16a3 30 int distToMax=(maxSpeed*(maxSpeed-1)*stepsPerInc)/2;
soonerbot 10:926f142f16a3 31 int stopRaise,startFall;
soonerbot 10:926f142f16a3 32 if(distance<distToMax*2){
soonerbot 10:926f142f16a3 33 maxSpeed = ((2*stepsPerInc)+sqrt((double)(2*stepsPerInc)*((2*stepsPerInc)+8*distance)))/(4*stepsPerInc);
soonerbot 10:926f142f16a3 34 stepsToMax=(maxSpeed-1)*stepsPerInc;
soonerbot 10:926f142f16a3 35 distToMax=(maxSpeed*(maxSpeed-1)*stepsPerInc)/2;
soonerbot 10:926f142f16a3 36 stopRaise = stepsToMax;
soonerbot 10:926f142f16a3 37 startFall = stopRaise+(distance-distToMax*2)/(maxSpeed)+1;
soonerbot 10:926f142f16a3 38 DBGPRINT("Insufficent Ramp-Up\r\n",1);
soonerbot 10:926f142f16a3 39 } else {
soonerbot 10:926f142f16a3 40 stopRaise=stepsToMax;
soonerbot 10:926f142f16a3 41 startFall=stopRaise+(distance-distToMax*2)/maxSpeed;
soonerbot 10:926f142f16a3 42 DBGPRINT("Sufficent Ramp-Up\r\n",1);
soonerbot 10:926f142f16a3 43 }
soonerbot 10:926f142f16a3 44 DBGPRINT("[%d %d %d] {%d, %d}\n\r",maxSpeed,stepsToMax,distToMax,stopRaise,startFall);
soonerbot 10:926f142f16a3 45 tim.start();
soonerbot 10:926f142f16a3 46 move = 0;
soonerbot 10:926f142f16a3 47 int totalMove=0;
soonerbot 10:926f142f16a3 48 for(i=0;i<startFall+stepsToMax;i++){
soonerbot 10:926f142f16a3 49 tim.reset();
soonerbot 10:926f142f16a3 50 if(i<=stopRaise && i%stepsPerInc==0){
soonerbot 10:926f142f16a3 51 move++;
soonerbot 10:926f142f16a3 52 }
soonerbot 10:926f142f16a3 53 if(i==startFall-1){
soonerbot 10:926f142f16a3 54 move = distance-totalMove-distToMax;
soonerbot 10:926f142f16a3 55 }
soonerbot 10:926f142f16a3 56 if(i>=startFall && (i-startFall)%stepsPerInc==0){
soonerbot 10:926f142f16a3 57 if(i==startFall)
soonerbot 10:926f142f16a3 58 move=maxSpeed;
soonerbot 10:926f142f16a3 59 move--;
soonerbot 10:926f142f16a3 60 }
soonerbot 10:926f142f16a3 61 totalMove+=move;
soonerbot 10:926f142f16a3 62 motors.moveForward(move);
soonerbot 10:926f142f16a3 63 DBGPRINT("%d: %d\t%d\r\n",i,move,totalMove);
soonerbot 10:926f142f16a3 64 while(tim.read_ms()<10);
soonerbot 10:926f142f16a3 65 }
soonerbot 10:926f142f16a3 66 return totalMove;
soonerbot 10:926f142f16a3 67 }
soonerbot 10:926f142f16a3 68
soonerbot 7:3b2cf7efe5d1 69 //this is the main thing that both turns and goes forward
soonerbot 8:c23cd84befa2 70 // desangle is a angle in degrees to head towards (this is relative to the direction the robot starts pointing in
soonerbot 8:c23cd84befa2 71 // distance is a distance to head in that direction in units of encoder ticks
soonerbot 4:adc885f4ab75 72 int robot::driveForward(double desangle, int distance){
soonerbot 1:c28fac16a109 73 bigenc.resetAll();
soonerbot 1:c28fac16a109 74 const int* constbuf = bigenc.getVals();
soonerbot 1:c28fac16a109 75 int prev[4]={0,0,0,0};
soonerbot 1:c28fac16a109 76 int distTraveled=0, i;
soonerbot 6:62d498ee97cf 77 double maxPow=0.4;
soonerbot 4:adc885f4ab75 78 int loopcount=0;
soonerbot 4:adc885f4ab75 79 double minmain=0.05;
soonerbot 4:adc885f4ab75 80 double minalt=0.05;
soonerbot 4:adc885f4ab75 81
soonerbot 7:3b2cf7efe5d1 82 //find a point in front of where we're heading
soonerbot 4:adc885f4ab75 83 int targetang = desangle*4050000.0/360.0;//gyro.getZ();
soonerbot 4:adc885f4ab75 84 int startang = targetang;
soonerbot 4:adc885f4ab75 85 double angle=double(startang)*2*3.14159/4050000.0;
soonerbot 6:62d498ee97cf 86 double targx = x + double((distance==0)?10000:distance)*0.0035362*cos(angle)*1.5;
soonerbot 6:62d498ee97cf 87 double targy = y + double((distance==0)?10000:distance)*0.0035362*sin(angle)*1.5;
soonerbot 4:adc885f4ab75 88 int invfactor = 0;
soonerbot 7:3b2cf7efe5d1 89
soonerbot 7:3b2cf7efe5d1 90 //if going backwards, point away from the point
soonerbot 6:62d498ee97cf 91 if(distance<0) {
soonerbot 4:adc885f4ab75 92 invfactor = 2025000;
soonerbot 4:adc885f4ab75 93 }
soonerbot 6:62d498ee97cf 94 double realfac=0.1;
soonerbot 4:adc885f4ab75 95
soonerbot 4:adc885f4ab75 96 int pmain=distance;
soonerbot 4:adc885f4ab75 97 int imain=0;
soonerbot 4:adc885f4ab75 98 int dmain=0;
soonerbot 4:adc885f4ab75 99
soonerbot 4:adc885f4ab75 100 int dterm=0;
soonerbot 4:adc885f4ab75 101 const int ptol = 75;
soonerbot 4:adc885f4ab75 102 const int dtol = 10;
soonerbot 8:c23cd84befa2 103 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 104
soonerbot 4:adc885f4ab75 105 //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 106 for(i=0;i<4;i++)
soonerbot 1:c28fac16a109 107 prev[i]=constbuf[i];
soonerbot 1:c28fac16a109 108 //wait(0.05);
soonerbot 1:c28fac16a109 109 constbuf = bigenc.getVals();
soonerbot 4:adc885f4ab75 110
soonerbot 7:3b2cf7efe5d1 111 //control system (Proportional, Derivative, Integral)
soonerbot 5:74c8ce39334b 112 pmain = distance - (constbuf[0]+constbuf[1])/2;
soonerbot 5:74c8ce39334b 113 dmain = ((constbuf[0]+constbuf[1]) - (prev[0]+prev[1]))/2;
soonerbot 4:adc885f4ab75 114 imain += pmain;
soonerbot 4:adc885f4ab75 115
soonerbot 6:62d498ee97cf 116 //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 117
soonerbot 7:3b2cf7efe5d1 118 //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 119 realfac = (gyro.compZ(invfactor+atan2(targy-y,targx-x)*4050000.0/(2*3.14159)))*angfac;
soonerbot 7:3b2cf7efe5d1 120 realfac = MAX(MIN(realfac,0.3),-0.3); // limits how much the motors can turn to fix the angle
soonerbot 4:adc885f4ab75 121
soonerbot 7:3b2cf7efe5d1 122 //uses PID control for the forward/back motions and adds in the angular component
soonerbot 7:3b2cf7efe5d1 123 //the forward/back motions is limited to a certain speed
soonerbot 5:74c8ce39334b 124 double leftpow = MAX(MIN(pfac*pmain+ifac*imain+dfac*dmain,maxPow),-maxPow)-realfac;
soonerbot 5:74c8ce39334b 125 double rightpow = MAX(MIN(pfac*pmain+ifac*imain+dfac*dmain,maxPow),-maxPow)+realfac;
soonerbot 7:3b2cf7efe5d1 126
soonerbot 7:3b2cf7efe5d1 127 //if we haven't settled, but also aren't moving, then speed up until it moves
soonerbot 7:3b2cf7efe5d1 128 if((pmain <= -ptol || pmain >= ptol) || (dmain <= -dtol || dmain >= dtol) || (realfac <= -0.02 || realfac >= 0.02) ){
soonerbot 4:adc885f4ab75 129 if (leftpow>0){
soonerbot 4:adc885f4ab75 130 if (leftpow<minalt){
soonerbot 4:adc885f4ab75 131 leftpow=minalt;
soonerbot 4:adc885f4ab75 132 }
soonerbot 4:adc885f4ab75 133 }else if (leftpow>-minalt){
soonerbot 4:adc885f4ab75 134 leftpow=-minalt;
soonerbot 4:adc885f4ab75 135 }
soonerbot 4:adc885f4ab75 136 if (rightpow>0){
soonerbot 4:adc885f4ab75 137 if (rightpow<minmain){
soonerbot 4:adc885f4ab75 138 rightpow=minmain;
soonerbot 4:adc885f4ab75 139 }
soonerbot 4:adc885f4ab75 140 }else if (rightpow>-minmain){
soonerbot 4:adc885f4ab75 141 rightpow=-minmain;
soonerbot 4:adc885f4ab75 142 }
soonerbot 4:adc885f4ab75 143 }
soonerbot 4:adc885f4ab75 144 left.setPower(leftpow);
soonerbot 4:adc885f4ab75 145 right.setPower(rightpow);
soonerbot 7:3b2cf7efe5d1 146
soonerbot 7:3b2cf7efe5d1 147 //how far we've moved in the last timestep
soonerbot 4:adc885f4ab75 148 int deltaTraveled=(constbuf[0]-prev[0]+constbuf[1]-prev[1])/2;
soonerbot 4:adc885f4ab75 149 //DBGPRINT("\t %d\r\n",deltaTraveled);
soonerbot 7:3b2cf7efe5d1 150 addforward(double(deltaTraveled)*0.0035362); //update our position
soonerbot 1:c28fac16a109 151 distTraveled+=deltaTraveled;
soonerbot 4:adc885f4ab75 152 loopcount++;
soonerbot 7:3b2cf7efe5d1 153
soonerbot 7:3b2cf7efe5d1 154 //increase min speed so that it will actually move
soonerbot 4:adc885f4ab75 155 if((dmain<5&&dmain>-5)&&minmain<0.2){
soonerbot 5:74c8ce39334b 156 minmain+=0.003;
soonerbot 4:adc885f4ab75 157 } else if (minmain>=0.05) {
soonerbot 5:74c8ce39334b 158 minmain-=0.003;
soonerbot 4:adc885f4ab75 159 }
soonerbot 4:adc885f4ab75 160 if((dterm<5&&dterm>-5)&&minalt<0.2){
soonerbot 5:74c8ce39334b 161 minalt+=0.003;
soonerbot 4:adc885f4ab75 162 } else if (minalt>=0.05) {
soonerbot 5:74c8ce39334b 163 minalt-=0.003;
soonerbot 4:adc885f4ab75 164 }
soonerbot 1:c28fac16a109 165 }
soonerbot 1:c28fac16a109 166 left.brake();
soonerbot 1:c28fac16a109 167 right.brake();
soonerbot 6:62d498ee97cf 168 DBGPRINT("Loops: %d\r\n",loopcount);
soonerbot 7:3b2cf7efe5d1 169
soonerbot 7:3b2cf7efe5d1 170 //catch the slowdown movement
soonerbot 6:62d498ee97cf 171 wait(0.2);
soonerbot 5:74c8ce39334b 172 for(i=0;i<4;i++)
soonerbot 5:74c8ce39334b 173 prev[i]=constbuf[i];
soonerbot 5:74c8ce39334b 174 //wait(0.05);
soonerbot 5:74c8ce39334b 175 constbuf = bigenc.getVals();
soonerbot 6:62d498ee97cf 176 addforward(double(constbuf[0]-prev[0]+constbuf[1]-prev[1])*0.0035362/2.0);
soonerbot 5:74c8ce39334b 177 DBGPRINT("loss of %d and %d\n\r",constbuf[0]-prev[0],constbuf[1]-prev[1]);
soonerbot 1:c28fac16a109 178 return 0;
soonerbot 1:c28fac16a109 179 }
soonerbot 1:c28fac16a109 180
soonerbot 7:3b2cf7efe5d1 181 //add the motion in the direction that the robot is facing
soonerbot 4:adc885f4ab75 182 void robot::addforward(double dist){
soonerbot 4:adc885f4ab75 183 double angle=double(gyro.getZ())*2*3.14159/4050000.0;
soonerbot 1:c28fac16a109 184 x+=dist*cos(angle);
soonerbot 1:c28fac16a109 185 y+=dist*sin(angle);
soonerbot 1:c28fac16a109 186 rot=angle;
soonerbot 1:c28fac16a109 187 }
soonerbot 1:c28fac16a109 188
soonerbot 7:3b2cf7efe5d1 189 //doesn't work yet
soonerbot 1:c28fac16a109 190 int robot::moveTo(double xInches, double yInches){
soonerbot 1:c28fac16a109 191 double power=.2;
soonerbot 1:c28fac16a109 192 turntowards(xInches,yInches);
soonerbot 1:c28fac16a109 193 double distance=sqrt(pow(xInches-x,2)+pow(yInches-y,2))/0.0035362;
soonerbot 1:c28fac16a109 194 double angle=atan2(xInches-x,yInches-y)*180.0/3.14159;
soonerbot 4:adc885f4ab75 195 double currangle=double(gyro.getZ())*2*3.14159/4050000.0;
soonerbot 1:c28fac16a109 196 bigenc.resetAll();
soonerbot 1:c28fac16a109 197 const int* constbuf = bigenc.getVals();
soonerbot 1:c28fac16a109 198 int prev[4]={0,0,0,0};
soonerbot 1:c28fac16a109 199 int distTraveled=0, i;
soonerbot 1:c28fac16a109 200 double maxPow;
soonerbot 1:c28fac16a109 201 DBGPRINT("going %f at angle %f from current of %f\r\n",distance,angle,currangle);
soonerbot 1:c28fac16a109 202 while(distTraveled<distance){
soonerbot 1:c28fac16a109 203 angle=atan2(xInches-x,yInches-y)*180.0/3.14159;
soonerbot 4:adc885f4ab75 204 currangle=double(gyro.getZ())*2*3.14159/4050000.0;
soonerbot 1:c28fac16a109 205 maxPow=MAX(double(distance-distTraveled-2000)/15000.0,0.1);
soonerbot 1:c28fac16a109 206 if(currangle>angle+2.0){ //too far to the right, brake left
soonerbot 1:c28fac16a109 207 left.brake();
soonerbot 1:c28fac16a109 208 } else {
soonerbot 1:c28fac16a109 209 left.setPower(MIN(power,maxPow));
soonerbot 1:c28fac16a109 210 }
soonerbot 1:c28fac16a109 211 if(currangle<angle-2){
soonerbot 1:c28fac16a109 212 right.brake();
soonerbot 1:c28fac16a109 213 } else {
soonerbot 1:c28fac16a109 214 right.setPower(MIN(power,maxPow));
soonerbot 1:c28fac16a109 215 }
soonerbot 1:c28fac16a109 216 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 217 for(i=0;i<4;i++)
soonerbot 1:c28fac16a109 218 prev[i]=constbuf[i];
soonerbot 1:c28fac16a109 219 //wait(0.05);
soonerbot 1:c28fac16a109 220 constbuf = bigenc.getVals();
soonerbot 1:c28fac16a109 221 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 222 addforward(double(deltaTraveled)*0.0035362);
soonerbot 1:c28fac16a109 223 distTraveled+=deltaTraveled;
soonerbot 1:c28fac16a109 224 //angle=atan2(xInches-x,yInches-y)*180.0/3.14159;
soonerbot 1:c28fac16a109 225 }
soonerbot 1:c28fac16a109 226 left.brake();
soonerbot 1:c28fac16a109 227 right.brake();
soonerbot 1:c28fac16a109 228 return 0;
soonerbot 1:c28fac16a109 229
soonerbot 1:c28fac16a109 230 }
soonerbot 1:c28fac16a109 231
soonerbot 7:3b2cf7efe5d1 232 //also doesn't work
soonerbot 1:c28fac16a109 233 int robot::turntowards(double xInches, double yInches){
soonerbot 4:adc885f4ab75 234 double currangle=double(gyro.getZ())*2*3.14159/4050000.0;
soonerbot 1:c28fac16a109 235 double angle=atan2(xInches-x,yInches-y)*180.0/3.14159;
soonerbot 1:c28fac16a109 236 double finangle=angle;
soonerbot 1:c28fac16a109 237 /*if(int(currangle-angle)%360>180){ //needs to turn positive degrees
soonerbot 1:c28fac16a109 238 finangle=currangle+double(int(angle-currangle)%360);
soonerbot 1:c28fac16a109 239 } else {//negative degrees
soonerbot 1:c28fac16a109 240 finangle=currangle-double(int(currangle-angle)%360);
soonerbot 1:c28fac16a109 241 }*/
soonerbot 1:c28fac16a109 242 double acc=turn(0.4,finangle);
soonerbot 1:c28fac16a109 243 if(acc<-0.75 && acc>0.75){
soonerbot 1:c28fac16a109 244 acc=turn(0.3,finangle);
soonerbot 1:c28fac16a109 245 }
soonerbot 1:c28fac16a109 246
soonerbot 1:c28fac16a109 247
soonerbot 1:c28fac16a109 248 return 1;
soonerbot 1:c28fac16a109 249 }
soonerbot 1:c28fac16a109 250
soonerbot 7:3b2cf7efe5d1 251 //still no
soonerbot 1:c28fac16a109 252 double robot::turn(double power, double degrees){
soonerbot 1:c28fac16a109 253 bigenc.resetAll();
soonerbot 1:c28fac16a109 254 const int* constbuf = bigenc.getVals();
soonerbot 1:c28fac16a109 255 int prev[4]={0,0,0,0};
soonerbot 1:c28fac16a109 256 int startz;
soonerbot 4:adc885f4ab75 257 startz=gyro.getZ();
soonerbot 1:c28fac16a109 258 int gyroticks=(degrees*4050000)/360;
soonerbot 1:c28fac16a109 259 double maxPow;
soonerbot 1:c28fac16a109 260 int nowz=startz;
soonerbot 1:c28fac16a109 261 int dir=0;
soonerbot 1:c28fac16a109 262 if(gyroticks>startz){
soonerbot 1:c28fac16a109 263 right.setPower(-power);
soonerbot 1:c28fac16a109 264 left.setPower(power);
soonerbot 1:c28fac16a109 265 dir=1;
soonerbot 1:c28fac16a109 266 } else {
soonerbot 1:c28fac16a109 267 right.setPower(power);
soonerbot 1:c28fac16a109 268 left.setPower(-power);
soonerbot 1:c28fac16a109 269 dir=-1;
soonerbot 1:c28fac16a109 270 }
soonerbot 1:c28fac16a109 271 while((gyroticks-nowz)*dir>0){
soonerbot 1:c28fac16a109 272 maxPow=MAX(double(abs(gyroticks-nowz))/4050000.0,0.25);
soonerbot 1:c28fac16a109 273 if(gyroticks<nowz){
soonerbot 1:c28fac16a109 274 right.setPower(-MIN(power,maxPow));
soonerbot 1:c28fac16a109 275 left.setPower(MIN(power,maxPow));
soonerbot 1:c28fac16a109 276 } else {
soonerbot 1:c28fac16a109 277 right.setPower(MIN(power,maxPow));
soonerbot 1:c28fac16a109 278 left.setPower(-MIN(power,maxPow));
soonerbot 1:c28fac16a109 279 }
soonerbot 1:c28fac16a109 280 DBGPRINT("_%d of %d {%f, %f, %f}\r\n",(gyroticks-nowz)*dir, gyroticks,x,y,rot*180.0/3.14159);
soonerbot 1:c28fac16a109 281 for(int i=0;i<4;i++)
soonerbot 1:c28fac16a109 282 prev[i]=constbuf[i];
soonerbot 1:c28fac16a109 283 constbuf = bigenc.getVals();
soonerbot 1:c28fac16a109 284 int deltaTraveled;
soonerbot 1:c28fac16a109 285 if(gyroticks<nowz){
soonerbot 1:c28fac16a109 286 deltaTraveled=(MIN(-constbuf[0]+prev[0],-constbuf[1]+prev[1])-MIN(-constbuf[2]+prev[2],-constbuf[3]+prev[3]))/2;
soonerbot 1:c28fac16a109 287 } else {
soonerbot 1:c28fac16a109 288 deltaTraveled=(-MIN(constbuf[0]-prev[0],constbuf[1]-prev[1])+MIN(constbuf[2]-prev[2],constbuf[3]-prev[3]))/2;
soonerbot 1:c28fac16a109 289 }
soonerbot 1:c28fac16a109 290 addforward(double(deltaTraveled)*0.0035362);
soonerbot 4:adc885f4ab75 291 nowz=gyro.getZ();
soonerbot 1:c28fac16a109 292 }
soonerbot 1:c28fac16a109 293 right.brake();
soonerbot 1:c28fac16a109 294 left.brake();
soonerbot 1:c28fac16a109 295 return (gyroticks-nowz)*dir;
soonerbot 1:c28fac16a109 296 }