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@49:7d172c133dbf, 2014-04-03 (annotated)

Committer:
sswatek
Date:
Thu Apr 03 13:17:04 2014 +0000
Revision:
49:7d172c133dbf
Parent:
48:519deb1d4dff
Child:
50:ec6cc79132e8
fixed bluetooth comms, servo setpoints, and developing a pickup procedure

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 48:519deb1d4dff 10 pingRight(PTC7,PTA16), BTLink(PTC4,PTC3), cont(motors), wristServo(PTC9), thumbServo(PTC8),
sswatek 48:519deb1d4dff 11 stickServo(PTA5), cameraServo(PTA4) {
soonerbot 1:c28fac16a109 12
soonerbot 11:967469d7e01c 13 //bigenc.setDirections(1,-1,1,1);
soonerbot 15:b10859606504 14 //left.setReversed(1);
soonerbot 1:c28fac16a109 15 x=y=rot=0;
soonerbot 7:3b2cf7efe5d1 16
soonerbot 11:967469d7e01c 17 //set our set point in memory to match the current set point of the encoders
sswatek 35:6ef5fbc45672 18 //wait(0.5);
soonerbot 11:967469d7e01c 19 const int *referenceCounts = bigenc.getReferences();
sswatek 32:ff71f61bb9f6 20 motors.setSetPoint(referenceCounts[1],referenceCounts[0]);
sswatek 35:6ef5fbc45672 21 //DBGPRINT("__%d_%d__",referenceCounts[1],referenceCounts[0]);
soonerbot 11:967469d7e01c 22
sswatek 26:ade7c813538f 23 //storage for shape positions
sswatek 26:ade7c813538f 24 circleX=circleY=rectX=rectY=rectRot=triX=triY=triRot=0;
sswatek 27:688409727452 25 cameraMode=-1;
sswatek 27:688409727452 26 flameLocation=0;
sswatek 48:519deb1d4dff 27 grabPosition();
sswatek 48:519deb1d4dff 28 retractStick();
sswatek 48:519deb1d4dff 29 farCamera();
sswatek 49:7d172c133dbf 30 servoPos = 45;
soonerbot 1:c28fac16a109 31 }
soonerbot 1:c28fac16a109 32
soonerbot 10:926f142f16a3 33 //driveforward, but set up so that
soonerbot 10:926f142f16a3 34 int robot::absDriveForward(double desangle, int distance){
sswatek 29:1132155bc7da 35 return smoothMove(distance, 0, 40);
soonerbot 13:c2d14bf733a5 36 }
soonerbot 13:c2d14bf733a5 37
soonerbot 13:c2d14bf733a5 38 int robot::smoothMove(int distance, int rotate, int maxSpeed){
soonerbot 13:c2d14bf733a5 39 Timer tim;
soonerbot 13:c2d14bf733a5 40 int dir=1;
soonerbot 13:c2d14bf733a5 41 // if we are traveling forward a negative distance, go backwards
soonerbot 13:c2d14bf733a5 42 if(distance < 0){
soonerbot 13:c2d14bf733a5 43 dir = -1; //is multiplied by the final output
soonerbot 13:c2d14bf733a5 44 distance = -distance;
soonerbot 13:c2d14bf733a5 45 }
soonerbot 13:c2d14bf733a5 46 if(rotate)
soonerbot 13:c2d14bf733a5 47 rotate=-1;
soonerbot 13:c2d14bf733a5 48 else
soonerbot 13:c2d14bf733a5 49 rotate=1;
soonerbot 13:c2d14bf733a5 50 int i,move;
soonerbot 13:c2d14bf733a5 51 //int maxSpeed=41;
soonerbot 13:c2d14bf733a5 52 int stepsPerInc=2;
soonerbot 13:c2d14bf733a5 53 int stepsToMax=(maxSpeed-1)*stepsPerInc;
soonerbot 13:c2d14bf733a5 54 int distToMax=(maxSpeed*(maxSpeed-1)*stepsPerInc)/2;
soonerbot 13:c2d14bf733a5 55 int stopRaise,startFall;
soonerbot 13:c2d14bf733a5 56 if(distance<distToMax*2){
soonerbot 13:c2d14bf733a5 57 // can't get up to full speed before needing to slow down
soonerbot 13:c2d14bf733a5 58 //find new max speed, time to get there, and distance there
soonerbot 13:c2d14bf733a5 59 maxSpeed = ((2*stepsPerInc)+sqrt((double)(2*stepsPerInc)*((2*stepsPerInc)+8*distance)))/(4*stepsPerInc);
soonerbot 13:c2d14bf733a5 60 stepsToMax=(maxSpeed-1)*stepsPerInc;
soonerbot 13:c2d14bf733a5 61 distToMax=(maxSpeed*(maxSpeed-1)*stepsPerInc)/2;
soonerbot 13:c2d14bf733a5 62 stopRaise = stepsToMax;
soonerbot 13:c2d14bf733a5 63 startFall = stopRaise+(distance-distToMax*2)/(maxSpeed)+1;
soonerbot 13:c2d14bf733a5 64 DBGPRINT("Insufficent Ramp-Up\r\n",1);
soonerbot 13:c2d14bf733a5 65 } else {
soonerbot 13:c2d14bf733a5 66 // can get up to full speed, so we will
soonerbot 13:c2d14bf733a5 67 stopRaise=stepsToMax;
soonerbot 13:c2d14bf733a5 68 startFall=stopRaise+(distance-distToMax*2)/maxSpeed+1;
soonerbot 13:c2d14bf733a5 69 DBGPRINT("Sufficent Ramp-Up\r\n",1);
soonerbot 13:c2d14bf733a5 70 }
soonerbot 13:c2d14bf733a5 71 DBGPRINT("[%d %d %d] {%d, %d}\n\r",maxSpeed,stepsToMax,distToMax,stopRaise,startFall);
soonerbot 13:c2d14bf733a5 72 tim.start();
soonerbot 13:c2d14bf733a5 73 move = 0;
soonerbot 13:c2d14bf733a5 74 int totalMove=0;
soonerbot 13:c2d14bf733a5 75 gyro.stop();
sswatek 29:1132155bc7da 76 int xStorage[40];
sswatek 29:1132155bc7da 77 int xMaxVal=-3000;
sswatek 29:1132155bc7da 78 int xMinVal=3000;
sswatek 29:1132155bc7da 79 long xSumVal=0;
sswatek 29:1132155bc7da 80 int zStorage[40];
sswatek 29:1132155bc7da 81 int zMaxVal=-3000;
sswatek 29:1132155bc7da 82 int zMinVal=3000;
sswatek 29:1132155bc7da 83 long zSumVal=0;
soonerbot 13:c2d14bf733a5 84 for(i=0;i<startFall+stepsToMax;i++){
soonerbot 13:c2d14bf733a5 85 //start clock for this frame
soonerbot 13:c2d14bf733a5 86 tim.reset();
soonerbot 13:c2d14bf733a5 87 if(i<=stopRaise && i%stepsPerInc==0){
soonerbot 13:c2d14bf733a5 88 //increase speed every (stepsPerInc) steps up to maxSpeed
soonerbot 13:c2d14bf733a5 89 move++;
soonerbot 13:c2d14bf733a5 90 }
soonerbot 13:c2d14bf733a5 91 if(i==startFall-1){
soonerbot 13:c2d14bf733a5 92 //calibration step before we start falling to get an exact result
soonerbot 13:c2d14bf733a5 93 move = distance-totalMove-distToMax;
soonerbot 13:c2d14bf733a5 94 }
soonerbot 13:c2d14bf733a5 95 if(i>=startFall && (i-startFall)%stepsPerInc==0){
soonerbot 13:c2d14bf733a5 96 if(i==startFall) //reset move after calibration step
soonerbot 13:c2d14bf733a5 97 move=maxSpeed;
soonerbot 13:c2d14bf733a5 98
soonerbot 13:c2d14bf733a5 99 //decrement every stepsPerInc steps until stopped
soonerbot 13:c2d14bf733a5 100 move--;
soonerbot 13:c2d14bf733a5 101 }
soonerbot 13:c2d14bf733a5 102 totalMove+=move;
soonerbot 13:c2d14bf733a5 103 //motors.moveForward(move*dir);
soonerbot 13:c2d14bf733a5 104 motors.moveWheels(move*dir,move*dir*rotate);
soonerbot 13:c2d14bf733a5 105 //DBGPRINT("%d: %d\t%d\r\n",i,move,totalMove);
soonerbot 13:c2d14bf733a5 106 while(tim.read_ms()<5);
soonerbot 13:c2d14bf733a5 107 gyro.gyroUpkeep();
sswatek 32:ff71f61bb9f6 108
sswatek 32:ff71f61bb9f6 109 //compass calibration code, shouldn't be needed
sswatek 29:1132155bc7da 110 if(distance==16000 && maxSpeed == 6){
sswatek 29:1132155bc7da 111 if(gyro.xmag>xMaxVal)
sswatek 29:1132155bc7da 112 xMaxVal=gyro.xmag;
sswatek 29:1132155bc7da 113 if(gyro.xmag<xMinVal)
sswatek 29:1132155bc7da 114 xMinVal=gyro.xmag;
sswatek 29:1132155bc7da 115 if(gyro.zmag>zMaxVal)
sswatek 29:1132155bc7da 116 zMaxVal=gyro.zmag;
sswatek 29:1132155bc7da 117 if(gyro.zmag<zMinVal)
sswatek 29:1132155bc7da 118 zMinVal=gyro.zmag;
sswatek 29:1132155bc7da 119 if(i%75==0){
sswatek 29:1132155bc7da 120 xSumVal+=gyro.xmag;
sswatek 29:1132155bc7da 121 zSumVal+=gyro.zmag;
sswatek 29:1132155bc7da 122 xStorage[i/75]=gyro.xmag;
sswatek 29:1132155bc7da 123 zStorage[i/75]=gyro.zmag;
sswatek 29:1132155bc7da 124 DBGPRINT("Saved Val %d \t%d \t%d \t%f\r\n",i/75,gyro.xmag,gyro.zmag,gyro.getZDegrees());
sswatek 29:1132155bc7da 125 }
sswatek 29:1132155bc7da 126 }
soonerbot 13:c2d14bf733a5 127 addforward(double(move*dir)*0.0035362);
soonerbot 13:c2d14bf733a5 128 //waits until 10ms have passed for the 100hz timing
soonerbot 13:c2d14bf733a5 129 while(tim.read_ms()<10);
soonerbot 13:c2d14bf733a5 130 }
soonerbot 13:c2d14bf733a5 131 gyro.start();
sswatek 32:ff71f61bb9f6 132
sswatek 32:ff71f61bb9f6 133 //compass calibration code, should never run
sswatek 29:1132155bc7da 134 if(distance==16000 && maxSpeed == 6){
sswatek 29:1132155bc7da 135 double xAvg = 75.0*(double)xSumVal/(startFall+stepsToMax);
sswatek 29:1132155bc7da 136 double zAvg = 75.0*(double)zSumVal/(startFall+stepsToMax);
sswatek 29:1132155bc7da 137 double xRMSSum=0;
sswatek 29:1132155bc7da 138 double zRMSSum=0;
sswatek 29:1132155bc7da 139 for(i=0;i<(startFall+stepsToMax)/75;i++){
sswatek 29:1132155bc7da 140 xRMSSum+=(xStorage[i]-xAvg)*(xStorage[i]-xAvg);
sswatek 29:1132155bc7da 141 zRMSSum+=(zStorage[i]-zAvg)*(zStorage[i]-zAvg);
sswatek 29:1132155bc7da 142 }
sswatek 29:1132155bc7da 143 double xRMS = sqrt(10.0*xRMSSum/(startFall+stepsToMax));
sswatek 29:1132155bc7da 144 double zRMS = sqrt(10.0*zRMSSum/(startFall+stepsToMax));
sswatek 29:1132155bc7da 145 for(i=0;i<(startFall+stepsToMax)/75;i++){
sswatek 29:1132155bc7da 146 double compDir = atan2(((double)xStorage[i]-gyro.xoffs)*gyro.xamp,((double)zStorage[i]-gyro.zoffs)*gyro.zamp);
sswatek 29:1132155bc7da 147 double compDir2 = atan2(((double)xStorage[i]-xAvg)/xRMS,((double)zStorage[i]-zAvg)/zRMS);
sswatek 29:1132155bc7da 148 DBGPRINT("Saved Val %d \t%d \t%d \t%f \t%f\r\n",i,xStorage[i],zStorage[i],compDir*180.0/3.14159,compDir2*180.0/3.14159);
sswatek 29:1132155bc7da 149 }
sswatek 29:1132155bc7da 150 DBGPRINT("Calibrated to (%d,%f,%f) and (%d,%f,%f)\r\n",xMaxVal-xMinVal,xAvg,xRMS,zMaxVal-zMinVal,zAvg,zRMS);
sswatek 29:1132155bc7da 151 }
soonerbot 13:c2d14bf733a5 152 return totalMove*dir;
sswatek 27:688409727452 153 //tim.stop();//may not need
soonerbot 10:926f142f16a3 154 }
soonerbot 10:926f142f16a3 155
soonerbot 7:3b2cf7efe5d1 156 //this is the main thing that both turns and goes forward
soonerbot 8:c23cd84befa2 157 // desangle is a angle in degrees to head towards (this is relative to the direction the robot starts pointing in
soonerbot 8:c23cd84befa2 158 // distance is a distance to head in that direction in units of encoder ticks
soonerbot 15:b10859606504 159 /*int robot::driveForward(double desangle, int distance){
soonerbot 1:c28fac16a109 160 bigenc.resetAll();
soonerbot 1:c28fac16a109 161 const int* constbuf = bigenc.getVals();
soonerbot 1:c28fac16a109 162 int prev[4]={0,0,0,0};
soonerbot 1:c28fac16a109 163 int distTraveled=0, i;
soonerbot 6:62d498ee97cf 164 double maxPow=0.4;
soonerbot 4:adc885f4ab75 165 int loopcount=0;
soonerbot 4:adc885f4ab75 166 double minmain=0.05;
soonerbot 4:adc885f4ab75 167 double minalt=0.05;
soonerbot 4:adc885f4ab75 168
soonerbot 7:3b2cf7efe5d1 169 //find a point in front of where we're heading
soonerbot 4:adc885f4ab75 170 int targetang = desangle*4050000.0/360.0;//gyro.getZ();
soonerbot 4:adc885f4ab75 171 int startang = targetang;
soonerbot 4:adc885f4ab75 172 double angle=double(startang)*2*3.14159/4050000.0;
soonerbot 6:62d498ee97cf 173 double targx = x + double((distance==0)?10000:distance)*0.0035362*cos(angle)*1.5;
soonerbot 15:b10859606504 174 double targy = y + double((
soonerbot 15:b10859606504 175
soonerbot 15:b10859606504 176 ==0)?10000:distance)*0.0035362*sin(angle)*1.5;
soonerbot 4:adc885f4ab75 177 int invfactor = 0;
soonerbot 7:3b2cf7efe5d1 178
soonerbot 7:3b2cf7efe5d1 179 //if going backwards, point away from the point
soonerbot 6:62d498ee97cf 180 if(distance<0) {
soonerbot 4:adc885f4ab75 181 invfactor = 2025000;
soonerbot 4:adc885f4ab75 182 }
soonerbot 6:62d498ee97cf 183 double realfac=0.1;
soonerbot 4:adc885f4ab75 184
soonerbot 4:adc885f4ab75 185 int pmain=distance;
soonerbot 4:adc885f4ab75 186 int imain=0;
soonerbot 4:adc885f4ab75 187 int dmain=0;
soonerbot 4:adc885f4ab75 188
soonerbot 4:adc885f4ab75 189 int dterm=0;
soonerbot 4:adc885f4ab75 190 const int ptol = 75;
soonerbot 4:adc885f4ab75 191 const int dtol = 10;
soonerbot 15:b10859606504 192 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 193
soonerbot 4:adc885f4ab75 194 //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 195 for(i=0;i<4;i++)
soonerbot 1:c28fac16a109 196 prev[i]=constbuf[i];
soonerbot 1:c28fac16a109 197 //wait(0.05);
soonerbot 1:c28fac16a109 198 constbuf = bigenc.getVals();
soonerbot 4:adc885f4ab75 199
soonerbot 14:a30aa3b29a2e 200 //control system (Proportional, Integral, Derivative) = PID controller
soonerbot 5:74c8ce39334b 201 pmain = distance - (constbuf[0]+constbuf[1])/2;
soonerbot 14:a30aa3b29a2e 202 imain += pmain;
soonerbot 5:74c8ce39334b 203 dmain = ((constbuf[0]+constbuf[1]) - (prev[0]+prev[1]))/2;
soonerbot 4:adc885f4ab75 204
soonerbot 6:62d498ee97cf 205 //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 206
soonerbot 7:3b2cf7efe5d1 207 //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 208 realfac = (gyro.compZ(invfactor+atan2(targy-y,targx-x)*4050000.0/(2*3.14159)))*angfac;
soonerbot 7:3b2cf7efe5d1 209 realfac = MAX(MIN(realfac,0.3),-0.3); // limits how much the motors can turn to fix the angle
soonerbot 4:adc885f4ab75 210
soonerbot 7:3b2cf7efe5d1 211 //uses PID control for the forward/back motions and adds in the angular component
soonerbot 7:3b2cf7efe5d1 212 //the forward/back motions is limited to a certain speed
soonerbot 5:74c8ce39334b 213 double leftpow = MAX(MIN(pfac*pmain+ifac*imain+dfac*dmain,maxPow),-maxPow)-realfac;
soonerbot 5:74c8ce39334b 214 double rightpow = MAX(MIN(pfac*pmain+ifac*imain+dfac*dmain,maxPow),-maxPow)+realfac;
soonerbot 7:3b2cf7efe5d1 215
soonerbot 7:3b2cf7efe5d1 216 //if we haven't settled, but also aren't moving, then speed up until it moves
soonerbot 7:3b2cf7efe5d1 217 if((pmain <= -ptol || pmain >= ptol) || (dmain <= -dtol || dmain >= dtol) || (realfac <= -0.02 || realfac >= 0.02) ){
soonerbot 4:adc885f4ab75 218 if (leftpow>0){
soonerbot 4:adc885f4ab75 219 if (leftpow<minalt){
soonerbot 4:adc885f4ab75 220 leftpow=minalt;
soonerbot 4:adc885f4ab75 221 }
soonerbot 4:adc885f4ab75 222 }else if (leftpow>-minalt){
soonerbot 4:adc885f4ab75 223 leftpow=-minalt;
soonerbot 4:adc885f4ab75 224 }
soonerbot 4:adc885f4ab75 225 if (rightpow>0){
soonerbot 4:adc885f4ab75 226 if (rightpow<minmain){
soonerbot 4:adc885f4ab75 227 rightpow=minmain;
soonerbot 4:adc885f4ab75 228 }
soonerbot 4:adc885f4ab75 229 }else if (rightpow>-minmain){
soonerbot 4:adc885f4ab75 230 rightpow=-minmain;
soonerbot 4:adc885f4ab75 231 }
soonerbot 4:adc885f4ab75 232 }
soonerbot 4:adc885f4ab75 233 left.setPower(leftpow);
soonerbot 4:adc885f4ab75 234 right.setPower(rightpow);
soonerbot 7:3b2cf7efe5d1 235
soonerbot 7:3b2cf7efe5d1 236 //how far we've moved in the last timestep
soonerbot 4:adc885f4ab75 237 int deltaTraveled=(constbuf[0]-prev[0]+constbuf[1]-prev[1])/2;
soonerbot 4:adc885f4ab75 238 //DBGPRINT("\t %d\r\n",deltaTraveled);
soonerbot 7:3b2cf7efe5d1 239 addforward(double(deltaTraveled)*0.0035362); //update our position
soonerbot 1:c28fac16a109 240 distTraveled+=deltaTraveled;
soonerbot 4:adc885f4ab75 241 loopcount++;
soonerbot 7:3b2cf7efe5d1 242
soonerbot 7:3b2cf7efe5d1 243 //increase min speed so that it will actually move
soonerbot 4:adc885f4ab75 244 if((dmain<5&&dmain>-5)&&minmain<0.2){
soonerbot 5:74c8ce39334b 245 minmain+=0.003;
soonerbot 4:adc885f4ab75 246 } else if (minmain>=0.05) {
soonerbot 5:74c8ce39334b 247 minmain-=0.003;
soonerbot 4:adc885f4ab75 248 }
soonerbot 4:adc885f4ab75 249 if((dterm<5&&dterm>-5)&&minalt<0.2){
soonerbot 5:74c8ce39334b 250 minalt+=0.003;
soonerbot 4:adc885f4ab75 251 } else if (minalt>=0.05) {
soonerbot 5:74c8ce39334b 252 minalt-=0.003;
soonerbot 4:adc885f4ab75 253 }
soonerbot 1:c28fac16a109 254 }
soonerbot 1:c28fac16a109 255 left.brake();
soonerbot 1:c28fac16a109 256 right.brake();
soonerbot 6:62d498ee97cf 257 DBGPRINT("Loops: %d\r\n",loopcount);
soonerbot 7:3b2cf7efe5d1 258
soonerbot 7:3b2cf7efe5d1 259 //catch the slowdown movement
soonerbot 6:62d498ee97cf 260 wait(0.2);
soonerbot 5:74c8ce39334b 261 for(i=0;i<4;i++)
soonerbot 5:74c8ce39334b 262 prev[i]=constbuf[i];
soonerbot 5:74c8ce39334b 263 //wait(0.05);
soonerbot 5:74c8ce39334b 264 constbuf = bigenc.getVals();
soonerbot 6:62d498ee97cf 265 addforward(double(constbuf[0]-prev[0]+constbuf[1]-prev[1])*0.0035362/2.0);
soonerbot 5:74c8ce39334b 266 DBGPRINT("loss of %d and %d\n\r",constbuf[0]-prev[0],constbuf[1]-prev[1]);
soonerbot 1:c28fac16a109 267 return 0;
soonerbot 15:b10859606504 268 }*/
soonerbot 1:c28fac16a109 269
soonerbot 7:3b2cf7efe5d1 270 //add the motion in the direction that the robot is facing
soonerbot 4:adc885f4ab75 271 void robot::addforward(double dist){
soonerbot 4:adc885f4ab75 272 double angle=double(gyro.getZ())*2*3.14159/4050000.0;
soonerbot 1:c28fac16a109 273 x+=dist*cos(angle);
soonerbot 1:c28fac16a109 274 y+=dist*sin(angle);
soonerbot 1:c28fac16a109 275 rot=angle;
soonerbot 1:c28fac16a109 276 }
soonerbot 1:c28fac16a109 277
soonerbot 7:3b2cf7efe5d1 278 //doesn't work yet
soonerbot 15:b10859606504 279 /*int robot::moveTo(double xInches, double yInches){
soonerbot 1:c28fac16a109 280 double power=.2;
soonerbot 1:c28fac16a109 281 turntowards(xInches,yInches);
soonerbot 1:c28fac16a109 282 double distance=sqrt(pow(xInches-x,2)+pow(yInches-y,2))/0.0035362;
soonerbot 1:c28fac16a109 283 double angle=atan2(xInches-x,yInches-y)*180.0/3.14159;
soonerbot 4:adc885f4ab75 284 double currangle=double(gyro.getZ())*2*3.14159/4050000.0;
soonerbot 1:c28fac16a109 285 bigenc.resetAll();
soonerbot 1:c28fac16a109 286 const int* constbuf = bigenc.getVals();
soonerbot 1:c28fac16a109 287 int prev[4]={0,0,0,0};
soonerbot 1:c28fac16a109 288 int distTraveled=0, i;
soonerbot 1:c28fac16a109 289 double maxPow;
soonerbot 1:c28fac16a109 290 DBGPRINT("going %f at angle %f from current of %f\r\n",distance,angle,currangle);
soonerbot 1:c28fac16a109 291 while(distTraveled<distance){
soonerbot 1:c28fac16a109 292 angle=atan2(xInches-x,yInches-y)*180.0/3.14159;
soonerbot 4:adc885f4ab75 293 currangle=double(gyro.getZ())*2*3.14159/4050000.0;
soonerbot 1:c28fac16a109 294 maxPow=MAX(double(distance-distTraveled-2000)/15000.0,0.1);
soonerbot 1:c28fac16a109 295 if(currangle>angle+2.0){ //too far to the right, brake left
soonerbot 1:c28fac16a109 296 left.brake();
soonerbot 1:c28fac16a109 297 } else {
soonerbot 1:c28fac16a109 298 left.setPower(MIN(power,maxPow));
soonerbot 1:c28fac16a109 299 }
soonerbot 1:c28fac16a109 300 if(currangle<angle-2){
soonerbot 1:c28fac16a109 301 right.brake();
soonerbot 1:c28fac16a109 302 } else {
soonerbot 14:a30aa3b29a2e 303 right.setPo
soonerbot 14:a30aa3b29a2e 304
soonerbot 14:a30aa3b29a2e 305 wer(MIN(power,maxPow));
soonerbot 1:c28fac16a109 306 }
soonerbot 1:c28fac16a109 307 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 308 for(i=0;i<4;i++)
soonerbot 1:c28fac16a109 309 prev[i]=constbuf[i];
soonerbot 1:c28fac16a109 310 //wait(0.05);
soonerbot 1:c28fac16a109 311 constbuf = bigenc.getVals();
soonerbot 1:c28fac16a109 312 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 313 addforward(double(deltaTraveled)*0.0035362);
soonerbot 1:c28fac16a109 314 distTraveled+=deltaTraveled;
soonerbot 1:c28fac16a109 315 //angle=atan2(xInches-x,yInches-y)*180.0/3.14159;
soonerbot 1:c28fac16a109 316 }
soonerbot 1:c28fac16a109 317 left.brake();
soonerbot 1:c28fac16a109 318 right.brake();
soonerbot 1:c28fac16a109 319 return 0;
soonerbot 1:c28fac16a109 320
soonerbot 15:b10859606504 321 }*/
soonerbot 1:c28fac16a109 322
soonerbot 7:3b2cf7efe5d1 323 //also doesn't work
soonerbot 15:b10859606504 324 /*int robot::turntowards(double xInches, double yInches){
soonerbot 4:adc885f4ab75 325 double currangle=double(gyro.getZ())*2*3.14159/4050000.0;
soonerbot 1:c28fac16a109 326 double angle=atan2(xInches-x,yInches-y)*180.0/3.14159;
soonerbot 1:c28fac16a109 327 double finangle=angle;
sswatek 27:688409727452 328 //if(int(currangle-angle)%360>180){ //needs to turn positive degrees
sswatek 27:688409727452 329 // finangle=currangle+double(int(angle-currangle)%360);
sswatek 27:688409727452 330 //} else {//negative degrees
sswatek 27:688409727452 331 // finangle=currangle-double(int(currangle-angle)%360);
sswatek 27:688409727452 332 //}
soonerbot 1:c28fac16a109 333 double acc=turn(0.4,finangle);
soonerbot 1:c28fac16a109 334 if(acc<-0.75 && acc>0.75){
soonerbot 1:c28fac16a109 335 acc=turn(0.3,finangle);
soonerbot 1:c28fac16a109 336 }
soonerbot 1:c28fac16a109 337
soonerbot 1:c28fac16a109 338
soonerbot 1:c28fac16a109 339 return 1;
soonerbot 15:b10859606504 340 }*/
soonerbot 1:c28fac16a109 341
soonerbot 7:3b2cf7efe5d1 342 //still no
soonerbot 15:b10859606504 343 /*double robot::turn(double power, double degrees){
soonerbot 1:c28fac16a109 344 bigenc.resetAll();
soonerbot 1:c28fac16a109 345 const int* constbuf = bigenc.getVals();
soonerbot 1:c28fac16a109 346 int prev[4]={0,0,0,0};
soonerbot 1:c28fac16a109 347 int startz;
soonerbot 4:adc885f4ab75 348 startz=gyro.getZ();
soonerbot 1:c28fac16a109 349 int gyroticks=(degrees*4050000)/360;
soonerbot 1:c28fac16a109 350 double maxPow;
soonerbot 1:c28fac16a109 351 int nowz=startz;
soonerbot 1:c28fac16a109 352 int dir=0;
soonerbot 1:c28fac16a109 353 if(gyroticks>startz){
soonerbot 1:c28fac16a109 354 right.setPower(-power);
soonerbot 1:c28fac16a109 355 left.setPower(power);
soonerbot 1:c28fac16a109 356 dir=1;
soonerbot 1:c28fac16a109 357 } else {
soonerbot 1:c28fac16a109 358 right.setPower(power);
soonerbot 1:c28fac16a109 359 left.setPower(-power);
soonerbot 1:c28fac16a109 360 dir=-1;
soonerbot 1:c28fac16a109 361 }
soonerbot 1:c28fac16a109 362 while((gyroticks-nowz)*dir>0){
soonerbot 1:c28fac16a109 363 maxPow=MAX(double(abs(gyroticks-nowz))/4050000.0,0.25);
soonerbot 1:c28fac16a109 364 if(gyroticks<nowz){
soonerbot 1:c28fac16a109 365 right.setPower(-MIN(power,maxPow));
soonerbot 1:c28fac16a109 366 left.setPower(MIN(power,maxPow));
soonerbot 1:c28fac16a109 367 } else {
soonerbot 1:c28fac16a109 368 right.setPower(MIN(power,maxPow));
soonerbot 1:c28fac16a109 369 left.setPower(-MIN(power,maxPow));
soonerbot 1:c28fac16a109 370 }
soonerbot 1:c28fac16a109 371 DBGPRINT("_%d of %d {%f, %f, %f}\r\n",(gyroticks-nowz)*dir, gyroticks,x,y,rot*180.0/3.14159);
soonerbot 1:c28fac16a109 372 for(int i=0;i<4;i++)
soonerbot 1:c28fac16a109 373 prev[i]=constbuf[i];
soonerbot 1:c28fac16a109 374 constbuf = bigenc.getVals();
soonerbot 1:c28fac16a109 375 int deltaTraveled;
soonerbot 1:c28fac16a109 376 if(gyroticks<nowz){
soonerbot 1:c28fac16a109 377 deltaTraveled=(MIN(-constbuf[0]+prev[0],-constbuf[1]+prev[1])-MIN(-constbuf[2]+prev[2],-constbuf[3]+prev[3]))/2;
soonerbot 1:c28fac16a109 378 } else {
soonerbot 1:c28fac16a109 379 deltaTraveled=(-MIN(constbuf[0]-prev[0],constbuf[1]-prev[1])+MIN(constbuf[2]-prev[2],constbuf[3]-prev[3]))/2;
soonerbot 1:c28fac16a109 380 }
soonerbot 1:c28fac16a109 381 addforward(double(deltaTraveled)*0.0035362);
soonerbot 4:adc885f4ab75 382 nowz=gyro.getZ();
soonerbot 1:c28fac16a109 383 }
soonerbot 15:b10859606504 384 //right.brake();
soonerbot 15:b10859606504 385 //left.brake();
soonerbot 1:c28fac16a109 386 return (gyroticks-nowz)*dir;
sswatek 26:ade7c813538f 387 }*/
sswatek 26:ade7c813538f 388
sswatek 27:688409727452 389 int robot::switchCameraMode(int mode){
sswatek 27:688409727452 390 if(mode==cameraMode){
sswatek 27:688409727452 391 return 1;
sswatek 27:688409727452 392 }
sswatek 26:ade7c813538f 393 int response=0;
sswatek 26:ade7c813538f 394 int gotAck=0;
sswatek 26:ade7c813538f 395 int testdata[]={'H','e','l','l','o'};
sswatek 26:ade7c813538f 396 stepTimer.start();
sswatek 27:688409727452 397 while(1){
sswatek 27:688409727452 398 response=BTLink.sendCmd(mode,testdata,5);
sswatek 26:ade7c813538f 399 stepTimer.reset();
sswatek 26:ade7c813538f 400 while(1){
sswatek 26:ade7c813538f 401 gotAck=BTLink.getAck(response);
sswatek 26:ade7c813538f 402 if(gotAck || stepTimer.read_ms()>=500)
sswatek 26:ade7c813538f 403 break;
sswatek 26:ade7c813538f 404 }
sswatek 27:688409727452 405 DBGPRINT("Mode%d=%d, %d, %d [%d]\n\r",mode,response,BTLink.bufSize(), gotAck, stepTimer.read_ms());
sswatek 26:ade7c813538f 406 if(gotAck)
sswatek 26:ade7c813538f 407 break;
sswatek 26:ade7c813538f 408 }
sswatek 26:ade7c813538f 409 stepTimer.stop();
sswatek 27:688409727452 410 cameraMode=mode;
sswatek 27:688409727452 411 return 1;
sswatek 27:688409727452 412 }
sswatek 27:688409727452 413 int robot::shapeCheck(){
sswatek 27:688409727452 414 if(cameraMode!=1){
sswatek 27:688409727452 415 switchCameraMode(1);
sswatek 27:688409727452 416 }
sswatek 27:688409727452 417 int gotAck=0;
sswatek 27:688409727452 418 int responseData[16];
sswatek 27:688409727452 419 stepTimer.start();
sswatek 27:688409727452 420 stepTimer.reset();
sswatek 27:688409727452 421 gotAck=0;
sswatek 27:688409727452 422 while(1){
sswatek 27:688409727452 423 BTLink.procBuf(0x01);
sswatek 27:688409727452 424 gotAck=BTLink.getData(0x01, responseData);
sswatek 27:688409727452 425 if(gotAck || stepTimer.read_ms()>=1000)
sswatek 27:688409727452 426 break;
sswatek 27:688409727452 427 }
sswatek 27:688409727452 428 stepTimer.stop();
sswatek 27:688409727452 429 if(responseData[15]==1&&gotAck){
sswatek 27:688409727452 430 for(int i=0;i<16;i++)
sswatek 27:688409727452 431 DBGPRINT("%d,",responseData[i]);
sswatek 27:688409727452 432 DBGPRINT("\n\r",1);
sswatek 27:688409727452 433 circleX=circleY=rectX=rectY=rectRot=triX=triY=triRot=0;
sswatek 27:688409727452 434 rectX=(responseData[4]<<8)|responseData[3];
sswatek 27:688409727452 435 rectY=(responseData[6]<<8)|responseData[5];
sswatek 27:688409727452 436 rectRot=responseData[2];
sswatek 27:688409727452 437 circleX=(responseData[12]<<8)|responseData[11];
sswatek 27:688409727452 438 circleY=(responseData[14]<<8)|responseData[13];
sswatek 27:688409727452 439 triX=((responseData[8]&0xF)<<8)|responseData[7];
sswatek 27:688409727452 440 triY=((responseData[10]&0xF)<<8)|responseData[9];
sswatek 27:688409727452 441 triRot=((responseData[8]&0xF0)>>4)|(responseData[10]&0xF0);
sswatek 27:688409727452 442 DBGPRINT("Rect(%d, %d, %d) Tri(%d, %d, %d) Cir(%d, %d)\n\r",rectX,rectY,rectRot,triX,triY,triRot,circleX,circleY);
sswatek 27:688409727452 443 return 1;
sswatek 27:688409727452 444 } else {
sswatek 27:688409727452 445 DBGPRINT("Did not pass hash check %d %d %d\n\r",responseData[15],gotAck,stepTimer.read_ms());
sswatek 27:688409727452 446 return 0;
sswatek 27:688409727452 447 }
sswatek 27:688409727452 448 }
sswatek 27:688409727452 449 int robot::pollForShapes(){
sswatek 49:7d172c133dbf 450 circleX=circleY=rectX=rectY=rectRot=triX=triY=triRot=0;
sswatek 27:688409727452 451 switchCameraMode(1);
sswatek 27:688409727452 452 BTLink.clearData();
sswatek 27:688409727452 453 int found=0;
sswatek 27:688409727452 454 for(int i=0;i<5;i++){
sswatek 27:688409727452 455 found=shapeCheck();
sswatek 27:688409727452 456 if(found)
sswatek 27:688409727452 457 break;
sswatek 27:688409727452 458 }
sswatek 27:688409727452 459 switchCameraMode(0);
sswatek 27:688409727452 460 return found;
sswatek 27:688409727452 461 }
sswatek 27:688409727452 462 int robot::rigCheck(){
sswatek 27:688409727452 463 if(cameraMode!=2){
sswatek 27:688409727452 464 switchCameraMode(2);
sswatek 27:688409727452 465 }
sswatek 27:688409727452 466 int gotAck=0;
sswatek 27:688409727452 467 int responseData[16];
sswatek 27:688409727452 468 stepTimer.start();
sswatek 27:688409727452 469 stepTimer.reset();
sswatek 27:688409727452 470 while(1){
sswatek 27:688409727452 471 BTLink.procBuf(0x02);
sswatek 27:688409727452 472 gotAck=BTLink.getData(0x02, responseData);
sswatek 27:688409727452 473 if(gotAck || stepTimer.read_ms()>=1000)
sswatek 27:688409727452 474 break;
sswatek 27:688409727452 475 }
sswatek 27:688409727452 476 stepTimer.stop();
sswatek 27:688409727452 477 if(responseData[3]==1 && gotAck){
sswatek 27:688409727452 478 flameLocation = responseData[2];
sswatek 27:688409727452 479 DBGPRINT("Found fire at %d in %d\n\r",flameLocation,stepTimer.read_ms());
sswatek 34:4ad11cda1eca 480 return flameLocation;
sswatek 27:688409727452 481 } else {
sswatek 27:688409727452 482 DBGPRINT("Did not pass hash check %d %d %d\n\r",responseData[3],gotAck,stepTimer.read_ms());
sswatek 34:4ad11cda1eca 483 return -1;
sswatek 27:688409727452 484 }
sswatek 27:688409727452 485 }
sswatek 27:688409727452 486 int robot::pollForRigs(){
sswatek 27:688409727452 487 switchCameraMode(2);
sswatek 27:688409727452 488 BTLink.clearData();
sswatek 27:688409727452 489 int found=0;
sswatek 27:688409727452 490 for(int i=0;i<5;i++){
sswatek 27:688409727452 491 found=rigCheck();
sswatek 34:4ad11cda1eca 492 if(found!=-1)
sswatek 27:688409727452 493 break;
sswatek 27:688409727452 494 }
sswatek 27:688409727452 495 switchCameraMode(0);
sswatek 27:688409727452 496 return found;
sswatek 44:4b933e7409e7 497 }
sswatek 44:4b933e7409e7 498 int robot::nearRigCheck(){
sswatek 44:4b933e7409e7 499 if(cameraMode!=3){
sswatek 44:4b933e7409e7 500 switchCameraMode(3);
sswatek 44:4b933e7409e7 501 }
sswatek 44:4b933e7409e7 502 int gotAck=0;
sswatek 44:4b933e7409e7 503 int responseData[16];
sswatek 44:4b933e7409e7 504 stepTimer.start();
sswatek 44:4b933e7409e7 505 stepTimer.reset();
sswatek 44:4b933e7409e7 506 while(1){
sswatek 44:4b933e7409e7 507 BTLink.procBuf(0x03);
sswatek 44:4b933e7409e7 508 gotAck=BTLink.getData(0x03, responseData);
sswatek 44:4b933e7409e7 509 if(gotAck || stepTimer.read_ms()>=1000)
sswatek 44:4b933e7409e7 510 break;
sswatek 44:4b933e7409e7 511 }
sswatek 44:4b933e7409e7 512 stepTimer.stop();
sswatek 44:4b933e7409e7 513 if(responseData[6]==1 && gotAck){
sswatek 44:4b933e7409e7 514 int leftSide = (responseData[3]<<8)|responseData[2];
sswatek 44:4b933e7409e7 515 int rightSide = (responseData[5]<<8)|responseData[4];
sswatek 44:4b933e7409e7 516 int rigPos = ((leftSide+rightSide)/2) - 400;
sswatek 44:4b933e7409e7 517 int rigWidth = rightSide-leftSide;
sswatek 44:4b933e7409e7 518 DBGPRINT("Found rig at %d and width %d\n\r",rigPos,rigWidth);
sswatek 44:4b933e7409e7 519 return rigPos;
sswatek 44:4b933e7409e7 520 } else {
sswatek 44:4b933e7409e7 521 DBGPRINT("Did not pass hash check %d %d %d\n\r",responseData[3],gotAck,stepTimer.read_ms());
sswatek 44:4b933e7409e7 522 return -100000;
sswatek 44:4b933e7409e7 523 }
sswatek 44:4b933e7409e7 524 }
sswatek 44:4b933e7409e7 525 int robot::alignWithRig(){
sswatek 44:4b933e7409e7 526 const int cutoff = 6;
sswatek 44:4b933e7409e7 527 switchCameraMode(3);
sswatek 44:4b933e7409e7 528 BTLink.clearData();
sswatek 44:4b933e7409e7 529 cont.setSlowMode(5);
sswatek 44:4b933e7409e7 530 int found=1000;
sswatek 44:4b933e7409e7 531 while(!(found>-cutoff && found <cutoff && cont.stopped())){
sswatek 44:4b933e7409e7 532 //for(int i=0;i<5;i++){
sswatek 44:4b933e7409e7 533 found=nearRigCheck();
sswatek 44:4b933e7409e7 534 if(found!=-100000){
sswatek 44:4b933e7409e7 535 if(found>=cutoff){
sswatek 44:4b933e7409e7 536 //see the rig on the right, turn right
sswatek 44:4b933e7409e7 537 int moveFactor;
sswatek 44:4b933e7409e7 538 if(found>100){
sswatek 44:4b933e7409e7 539 moveFactor = 3;
sswatek 44:4b933e7409e7 540 } else {
sswatek 44:4b933e7409e7 541 moveFactor = found/40 + 1;
sswatek 44:4b933e7409e7 542 }
sswatek 44:4b933e7409e7 543 moveFactor = 1;
sswatek 44:4b933e7409e7 544 cont.rampSpeed(moveFactor,-moveFactor);
sswatek 44:4b933e7409e7 545 } else if(found<=-cutoff){
sswatek 44:4b933e7409e7 546 //see the rig on the left, turn left
sswatek 44:4b933e7409e7 547 int moveFactor;
sswatek 44:4b933e7409e7 548 if(found<-100){
sswatek 44:4b933e7409e7 549 moveFactor = 3;
sswatek 44:4b933e7409e7 550 } else {
sswatek 44:4b933e7409e7 551 moveFactor = -found/40 + 1;
sswatek 44:4b933e7409e7 552 }
sswatek 44:4b933e7409e7 553 moveFactor = 1;
sswatek 44:4b933e7409e7 554 cont.rampSpeed(-moveFactor,moveFactor);
sswatek 44:4b933e7409e7 555 } else {
sswatek 44:4b933e7409e7 556 cont.rampSpeed(0,0);
sswatek 44:4b933e7409e7 557 }
sswatek 44:4b933e7409e7 558 } else {
sswatek 44:4b933e7409e7 559 cont.rampSpeed(0,0);
sswatek 44:4b933e7409e7 560 }
sswatek 44:4b933e7409e7 561 }
sswatek 44:4b933e7409e7 562 cont.setSlowMode(1);
sswatek 44:4b933e7409e7 563 switchCameraMode(0);
sswatek 44:4b933e7409e7 564 return found;
sswatek 48:519deb1d4dff 565 }
sswatek 48:519deb1d4dff 566
sswatek 48:519deb1d4dff 567 int robot::grabPosition(){
sswatek 49:7d172c133dbf 568 wristServo.toPosition(-50);
sswatek 49:7d172c133dbf 569 thumbServo.toPosition(65);
sswatek 48:519deb1d4dff 570 return 1;
sswatek 48:519deb1d4dff 571 }
sswatek 48:519deb1d4dff 572 int robot::grab(){
sswatek 48:519deb1d4dff 573 thumbServo.toPosition(-10);
sswatek 48:519deb1d4dff 574 wait(0.5);
sswatek 48:519deb1d4dff 575 cont.start();
sswatek 48:519deb1d4dff 576 cont.resetTicks();
sswatek 48:519deb1d4dff 577 cont.rampSpeed(-20,-20);
sswatek 48:519deb1d4dff 578 //s.toPosition(0);
sswatek 48:519deb1d4dff 579 //wait(.5);
sswatek 48:519deb1d4dff 580 while(cont.avgTicks()>-900){
sswatek 49:7d172c133dbf 581 wristServo.toPosition((-50)+((35+45)*(-cont.avgTicks()))/900);
sswatek 48:519deb1d4dff 582 wait_ms(20);
sswatek 48:519deb1d4dff 583 }
sswatek 48:519deb1d4dff 584 cont.rampSpeed(0,0);
sswatek 49:7d172c133dbf 585 wristServo.toPosition(103);
sswatek 48:519deb1d4dff 586 while(!cont.stopped());
sswatek 48:519deb1d4dff 587 cont.stop();
sswatek 48:519deb1d4dff 588 return 1;
sswatek 48:519deb1d4dff 589 }
sswatek 48:519deb1d4dff 590 int robot::retractCamera(){
sswatek 48:519deb1d4dff 591 cameraServo.toPosition(160);
sswatek 48:519deb1d4dff 592 return 1;
sswatek 48:519deb1d4dff 593 }
sswatek 48:519deb1d4dff 594 int robot::farCamera(){
sswatek 48:519deb1d4dff 595 cameraServo.toPosition(90);
sswatek 48:519deb1d4dff 596 return 1;
sswatek 48:519deb1d4dff 597 }
sswatek 48:519deb1d4dff 598 int robot::nearCamera(){
sswatek 49:7d172c133dbf 599 cameraServo.toPosition(55);
sswatek 48:519deb1d4dff 600 return 1;
sswatek 48:519deb1d4dff 601 }
sswatek 48:519deb1d4dff 602 int robot::downCamera(){
sswatek 49:7d172c133dbf 603 cameraServo.toPosition(-80);
sswatek 48:519deb1d4dff 604 return 1;
sswatek 48:519deb1d4dff 605 }
sswatek 48:519deb1d4dff 606 int robot::extendStick(){
sswatek 48:519deb1d4dff 607 stickServo.toPosition(-40);
sswatek 48:519deb1d4dff 608 return 1;
sswatek 48:519deb1d4dff 609 }
sswatek 48:519deb1d4dff 610 int robot::retractStick(){
sswatek 48:519deb1d4dff 611 stickServo.toPosition(250);
sswatek 48:519deb1d4dff 612 return 1;
sswatek 49:7d172c133dbf 613 }
sswatek 49:7d172c133dbf 614 int robot::incServo(){
sswatek 49:7d172c133dbf 615 wristServo.toPosition(servoPos+=1);
sswatek 49:7d172c133dbf 616 DBGPRINT("inc to %d\r\n",servoPos);
sswatek 49:7d172c133dbf 617 return servoPos;
sswatek 49:7d172c133dbf 618 }
sswatek 49:7d172c133dbf 619 int robot::decServo(){
sswatek 49:7d172c133dbf 620 wristServo.toPosition(servoPos-=1);
sswatek 49:7d172c133dbf 621 DBGPRINT("inc to %d\r\n",servoPos);
sswatek 49:7d172c133dbf 622 return servoPos;
sswatek 26:ade7c813538f 623 }