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@4:adc885f4ab75, 2013-11-17 (annotated)

Committer:
soonerbot
Date:
Sun Nov 17 21:04:47 2013 +0000
Revision:
4:adc885f4ab75
Parent:
3:a223b0bf8256
Child:
5:74c8ce39334b
Gyro working, compass giving values, and fiddling with control systems for driving forward/back

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 3:a223b0bf8256 6 robot::robot() : spi(PTD2, PTD3, PTD1), bigenc(spi,PTD0), gyro(PTE0, PTE1),
soonerbot 2:7c6b494f9005 7 right(bigenc,0,1,PTA5), left(bigenc,2,3,PTA4){
soonerbot 1:c28fac16a109 8
soonerbot 1:c28fac16a109 9 bigenc.setDirections(1,-1,1,1);
soonerbot 1:c28fac16a109 10 left.setReversed(1);
soonerbot 1:c28fac16a109 11 x=y=rot=0;
soonerbot 4:adc885f4ab75 12 pfac=0.00035;
soonerbot 4:adc885f4ab75 13 ifac=0.00000001;
soonerbot 4:adc885f4ab75 14 dfac=0.000001;
soonerbot 4:adc885f4ab75 15
soonerbot 4:adc885f4ab75 16 angfac=0.0000001;
soonerbot 1:c28fac16a109 17 //blah
soonerbot 1:c28fac16a109 18 }
soonerbot 1:c28fac16a109 19
soonerbot 1:c28fac16a109 20
soonerbot 4:adc885f4ab75 21 int robot::driveForward(double desangle, int distance){
soonerbot 1:c28fac16a109 22 bigenc.resetAll();
soonerbot 1:c28fac16a109 23 const int* constbuf = bigenc.getVals();
soonerbot 1:c28fac16a109 24 int prev[4]={0,0,0,0};
soonerbot 1:c28fac16a109 25 int distTraveled=0, i;
soonerbot 1:c28fac16a109 26 double maxPow;
soonerbot 4:adc885f4ab75 27 int loopcount=0;
soonerbot 4:adc885f4ab75 28 double minmain=0.05;
soonerbot 4:adc885f4ab75 29 double minalt=0.05;
soonerbot 4:adc885f4ab75 30
soonerbot 4:adc885f4ab75 31 int targetang = desangle*4050000.0/360.0;//gyro.getZ();
soonerbot 4:adc885f4ab75 32 int startang = targetang;
soonerbot 4:adc885f4ab75 33 double angle=double(startang)*2*3.14159/4050000.0;
soonerbot 4:adc885f4ab75 34 double targx = x + double(distance)*0.0035362*cos(angle)*1.5;
soonerbot 4:adc885f4ab75 35 double targy = y + double(distance)*0.0035362*sin(angle)*1.5;
soonerbot 4:adc885f4ab75 36 int invfactor = 0;
soonerbot 4:adc885f4ab75 37 if(distance<=0) {
soonerbot 4:adc885f4ab75 38 invfactor = 2025000;
soonerbot 4:adc885f4ab75 39 }
soonerbot 4:adc885f4ab75 40
soonerbot 4:adc885f4ab75 41 int pmain=distance;
soonerbot 4:adc885f4ab75 42 int imain=0;
soonerbot 4:adc885f4ab75 43 int dmain=0;
soonerbot 4:adc885f4ab75 44 int plast=distance;
soonerbot 4:adc885f4ab75 45
soonerbot 4:adc885f4ab75 46 int pterm=distance;
soonerbot 4:adc885f4ab75 47 int iterm=0;
soonerbot 4:adc885f4ab75 48 int dterm=0;
soonerbot 4:adc885f4ab75 49 const int ptol = 75;
soonerbot 4:adc885f4ab75 50 const int dtol = 10;
soonerbot 4:adc885f4ab75 51 while((pterm+pmain <= -ptol || pterm >= ptol) || (dterm <= -dtol || dterm >= dtol) || (pmain <= -ptol || pmain >= ptol) || (dmain <= -dtol || dmain >= dtol)){
soonerbot 4:adc885f4ab75 52 //maxPow=MAX(double(distance-distTraveled-2000)/15000.0,minspeed);
soonerbot 4:adc885f4ab75 53 //left.setPower(MIN(power,maxPow));
soonerbot 4:adc885f4ab75 54 //right.setPower(MIN(power,maxPow));
soonerbot 4:adc885f4ab75 55 //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 56 for(i=0;i<4;i++)
soonerbot 1:c28fac16a109 57 prev[i]=constbuf[i];
soonerbot 1:c28fac16a109 58 //wait(0.05);
soonerbot 1:c28fac16a109 59 constbuf = bigenc.getVals();
soonerbot 4:adc885f4ab75 60
soonerbot 4:adc885f4ab75 61 pmain = distance - constbuf[0];
soonerbot 4:adc885f4ab75 62 dmain = constbuf[0] - prev[0];
soonerbot 4:adc885f4ab75 63 imain += pmain;
soonerbot 4:adc885f4ab75 64
soonerbot 4:adc885f4ab75 65 pterm = distance - constbuf[1];
soonerbot 4:adc885f4ab75 66 dterm = constbuf[1] - prev[1];
soonerbot 4:adc885f4ab75 67 iterm += pterm;
soonerbot 4:adc885f4ab75 68
soonerbot 4:adc885f4ab75 69 //pterm = constbuf[0] - constbuf[1];
soonerbot 4:adc885f4ab75 70 //dterm = pterm - (prev[0] - prev[1]);
soonerbot 4:adc885f4ab75 71 //iterm += pterm;
soonerbot 4:adc885f4ab75 72
soonerbot 4:adc885f4ab75 73 DBGPRINT("%d like %f\n\r", gyro.compZ(invfactor+atan2(targy-y,targx-x)*4050000.0/(2*3.14159)),angle*180/3.14159);
soonerbot 4:adc885f4ab75 74
soonerbot 4:adc885f4ab75 75 double realfac = (gyro.compZ(invfactor+atan2(targy-y,targx-x)*4050000.0/(2*3.14159)))*angfac;
soonerbot 4:adc885f4ab75 76
soonerbot 4:adc885f4ab75 77 double leftpow = MAX(MIN(pfac*pterm+ifac*iterm+dfac*dterm-realfac,0.6),-0.6);
soonerbot 4:adc885f4ab75 78 if((pterm <= -ptol || pterm >= ptol) || (dterm <= -dtol || dterm >= dtol)){
soonerbot 4:adc885f4ab75 79 if (leftpow>0){
soonerbot 4:adc885f4ab75 80 if (leftpow<minalt){
soonerbot 4:adc885f4ab75 81 leftpow=minalt;
soonerbot 4:adc885f4ab75 82 }
soonerbot 4:adc885f4ab75 83 }else if (leftpow>-minalt){
soonerbot 4:adc885f4ab75 84 leftpow=-minalt;
soonerbot 4:adc885f4ab75 85 }
soonerbot 4:adc885f4ab75 86 }
soonerbot 4:adc885f4ab75 87 double rightpow = MAX(MIN(pfac*pmain+ifac*imain+dfac*dmain+realfac,0.6),-0.6);
soonerbot 4:adc885f4ab75 88 if((pmain <= -ptol || pmain >= ptol) || (dmain <= -dtol || dmain >= dtol)){
soonerbot 4:adc885f4ab75 89 if (rightpow>0){
soonerbot 4:adc885f4ab75 90 if (rightpow<minmain){
soonerbot 4:adc885f4ab75 91 rightpow=minmain;
soonerbot 4:adc885f4ab75 92 }
soonerbot 4:adc885f4ab75 93 }else if (rightpow>-minmain){
soonerbot 4:adc885f4ab75 94 rightpow=-minmain;
soonerbot 4:adc885f4ab75 95 }
soonerbot 4:adc885f4ab75 96 }
soonerbot 4:adc885f4ab75 97 left.setPower(leftpow);
soonerbot 4:adc885f4ab75 98 right.setPower(rightpow);
soonerbot 1:c28fac16a109 99 //int deltaTraveled=MAX(MIN(MIN(-constbuf[0]+prev[0],-constbuf[1]+prev[1]),MIN(constbuf[2]-prev[2],constbuf[3]-prev[3])),0);
soonerbot 4:adc885f4ab75 100 int deltaTraveled=(constbuf[0]-prev[0]+constbuf[1]-prev[1])/2;
soonerbot 4:adc885f4ab75 101 //DBGPRINT("\t %d\r\n",deltaTraveled);
soonerbot 1:c28fac16a109 102 addforward(double(deltaTraveled)*0.0035362);
soonerbot 1:c28fac16a109 103 distTraveled+=deltaTraveled;
soonerbot 4:adc885f4ab75 104 loopcount++;
soonerbot 4:adc885f4ab75 105 if((dmain<5&&dmain>-5)&&minmain<0.2){
soonerbot 4:adc885f4ab75 106 minmain+=0.001;
soonerbot 4:adc885f4ab75 107 } else if (minmain>=0.05) {
soonerbot 4:adc885f4ab75 108 minmain-=0.001;
soonerbot 4:adc885f4ab75 109 }
soonerbot 4:adc885f4ab75 110 if((dterm<5&&dterm>-5)&&minalt<0.2){
soonerbot 4:adc885f4ab75 111 minalt+=0.001;
soonerbot 4:adc885f4ab75 112 } else if (minalt>=0.05) {
soonerbot 4:adc885f4ab75 113 minalt-=0.001;
soonerbot 4:adc885f4ab75 114 }
soonerbot 1:c28fac16a109 115 }
soonerbot 4:adc885f4ab75 116 DBGPRINT("Loops: %d\r\n",loopcount);
soonerbot 1:c28fac16a109 117 left.brake();
soonerbot 1:c28fac16a109 118 right.brake();
soonerbot 1:c28fac16a109 119 return 0;
soonerbot 1:c28fac16a109 120 }
soonerbot 1:c28fac16a109 121
soonerbot 4:adc885f4ab75 122 void robot::addforward(double dist){
soonerbot 4:adc885f4ab75 123 double angle=double(gyro.getZ())*2*3.14159/4050000.0;
soonerbot 1:c28fac16a109 124 x+=dist*cos(angle);
soonerbot 1:c28fac16a109 125 y+=dist*sin(angle);
soonerbot 1:c28fac16a109 126 rot=angle;
soonerbot 1:c28fac16a109 127 }
soonerbot 1:c28fac16a109 128
soonerbot 1:c28fac16a109 129 int robot::moveTo(double xInches, double yInches){
soonerbot 1:c28fac16a109 130 double power=.2;
soonerbot 1:c28fac16a109 131 turntowards(xInches,yInches);
soonerbot 1:c28fac16a109 132 double distance=sqrt(pow(xInches-x,2)+pow(yInches-y,2))/0.0035362;
soonerbot 1:c28fac16a109 133 double angle=atan2(xInches-x,yInches-y)*180.0/3.14159;
soonerbot 4:adc885f4ab75 134 double currangle=double(gyro.getZ())*2*3.14159/4050000.0;
soonerbot 1:c28fac16a109 135 bigenc.resetAll();
soonerbot 1:c28fac16a109 136 const int* constbuf = bigenc.getVals();
soonerbot 1:c28fac16a109 137 int prev[4]={0,0,0,0};
soonerbot 1:c28fac16a109 138 int distTraveled=0, i;
soonerbot 1:c28fac16a109 139 double maxPow;
soonerbot 1:c28fac16a109 140 DBGPRINT("going %f at angle %f from current of %f\r\n",distance,angle,currangle);
soonerbot 1:c28fac16a109 141 while(distTraveled<distance){
soonerbot 1:c28fac16a109 142 angle=atan2(xInches-x,yInches-y)*180.0/3.14159;
soonerbot 4:adc885f4ab75 143 currangle=double(gyro.getZ())*2*3.14159/4050000.0;
soonerbot 1:c28fac16a109 144 maxPow=MAX(double(distance-distTraveled-2000)/15000.0,0.1);
soonerbot 1:c28fac16a109 145 if(currangle>angle+2.0){ //too far to the right, brake left
soonerbot 1:c28fac16a109 146 left.brake();
soonerbot 1:c28fac16a109 147 } else {
soonerbot 1:c28fac16a109 148 left.setPower(MIN(power,maxPow));
soonerbot 1:c28fac16a109 149 }
soonerbot 1:c28fac16a109 150 if(currangle<angle-2){
soonerbot 1:c28fac16a109 151 right.brake();
soonerbot 1:c28fac16a109 152 } else {
soonerbot 1:c28fac16a109 153 right.setPower(MIN(power,maxPow));
soonerbot 1:c28fac16a109 154 }
soonerbot 1:c28fac16a109 155 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 156 for(i=0;i<4;i++)
soonerbot 1:c28fac16a109 157 prev[i]=constbuf[i];
soonerbot 1:c28fac16a109 158 //wait(0.05);
soonerbot 1:c28fac16a109 159 constbuf = bigenc.getVals();
soonerbot 1:c28fac16a109 160 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 161 addforward(double(deltaTraveled)*0.0035362);
soonerbot 1:c28fac16a109 162 distTraveled+=deltaTraveled;
soonerbot 1:c28fac16a109 163 //angle=atan2(xInches-x,yInches-y)*180.0/3.14159;
soonerbot 1:c28fac16a109 164 }
soonerbot 1:c28fac16a109 165 left.brake();
soonerbot 1:c28fac16a109 166 right.brake();
soonerbot 1:c28fac16a109 167 return 0;
soonerbot 1:c28fac16a109 168
soonerbot 1:c28fac16a109 169 }
soonerbot 1:c28fac16a109 170
soonerbot 1:c28fac16a109 171 int robot::turntowards(double xInches, double yInches){
soonerbot 4:adc885f4ab75 172 double currangle=double(gyro.getZ())*2*3.14159/4050000.0;
soonerbot 1:c28fac16a109 173 double angle=atan2(xInches-x,yInches-y)*180.0/3.14159;
soonerbot 1:c28fac16a109 174 double finangle=angle;
soonerbot 1:c28fac16a109 175 /*if(int(currangle-angle)%360>180){ //needs to turn positive degrees
soonerbot 1:c28fac16a109 176 finangle=currangle+double(int(angle-currangle)%360);
soonerbot 1:c28fac16a109 177 } else {//negative degrees
soonerbot 1:c28fac16a109 178 finangle=currangle-double(int(currangle-angle)%360);
soonerbot 1:c28fac16a109 179 }*/
soonerbot 1:c28fac16a109 180 double acc=turn(0.4,finangle);
soonerbot 1:c28fac16a109 181 if(acc<-0.75 && acc>0.75){
soonerbot 1:c28fac16a109 182 acc=turn(0.3,finangle);
soonerbot 1:c28fac16a109 183 }
soonerbot 1:c28fac16a109 184
soonerbot 1:c28fac16a109 185
soonerbot 1:c28fac16a109 186 return 1;
soonerbot 1:c28fac16a109 187 }
soonerbot 1:c28fac16a109 188
soonerbot 1:c28fac16a109 189
soonerbot 1:c28fac16a109 190 double robot::turn(double power, double degrees){
soonerbot 1:c28fac16a109 191 bigenc.resetAll();
soonerbot 1:c28fac16a109 192 const int* constbuf = bigenc.getVals();
soonerbot 1:c28fac16a109 193 int prev[4]={0,0,0,0};
soonerbot 1:c28fac16a109 194 int startz;
soonerbot 4:adc885f4ab75 195 startz=gyro.getZ();
soonerbot 1:c28fac16a109 196 int gyroticks=(degrees*4050000)/360;
soonerbot 1:c28fac16a109 197 double maxPow;
soonerbot 1:c28fac16a109 198 int nowz=startz;
soonerbot 1:c28fac16a109 199 int dir=0;
soonerbot 1:c28fac16a109 200 if(gyroticks>startz){
soonerbot 1:c28fac16a109 201 right.setPower(-power);
soonerbot 1:c28fac16a109 202 left.setPower(power);
soonerbot 1:c28fac16a109 203 dir=1;
soonerbot 1:c28fac16a109 204 } else {
soonerbot 1:c28fac16a109 205 right.setPower(power);
soonerbot 1:c28fac16a109 206 left.setPower(-power);
soonerbot 1:c28fac16a109 207 dir=-1;
soonerbot 1:c28fac16a109 208 }
soonerbot 1:c28fac16a109 209 while((gyroticks-nowz)*dir>0){
soonerbot 1:c28fac16a109 210 maxPow=MAX(double(abs(gyroticks-nowz))/4050000.0,0.25);
soonerbot 1:c28fac16a109 211 if(gyroticks<nowz){
soonerbot 1:c28fac16a109 212 right.setPower(-MIN(power,maxPow));
soonerbot 1:c28fac16a109 213 left.setPower(MIN(power,maxPow));
soonerbot 1:c28fac16a109 214 } else {
soonerbot 1:c28fac16a109 215 right.setPower(MIN(power,maxPow));
soonerbot 1:c28fac16a109 216 left.setPower(-MIN(power,maxPow));
soonerbot 1:c28fac16a109 217 }
soonerbot 1:c28fac16a109 218 DBGPRINT("_%d of %d {%f, %f, %f}\r\n",(gyroticks-nowz)*dir, gyroticks,x,y,rot*180.0/3.14159);
soonerbot 1:c28fac16a109 219 for(int i=0;i<4;i++)
soonerbot 1:c28fac16a109 220 prev[i]=constbuf[i];
soonerbot 1:c28fac16a109 221 constbuf = bigenc.getVals();
soonerbot 1:c28fac16a109 222 int deltaTraveled;
soonerbot 1:c28fac16a109 223 if(gyroticks<nowz){
soonerbot 1:c28fac16a109 224 deltaTraveled=(MIN(-constbuf[0]+prev[0],-constbuf[1]+prev[1])-MIN(-constbuf[2]+prev[2],-constbuf[3]+prev[3]))/2;
soonerbot 1:c28fac16a109 225 } else {
soonerbot 1:c28fac16a109 226 deltaTraveled=(-MIN(constbuf[0]-prev[0],constbuf[1]-prev[1])+MIN(constbuf[2]-prev[2],constbuf[3]-prev[3]))/2;
soonerbot 1:c28fac16a109 227 }
soonerbot 1:c28fac16a109 228 addforward(double(deltaTraveled)*0.0035362);
soonerbot 4:adc885f4ab75 229 nowz=gyro.getZ();
soonerbot 1:c28fac16a109 230 }
soonerbot 1:c28fac16a109 231 right.brake();
soonerbot 1:c28fac16a109 232 left.brake();
soonerbot 1:c28fac16a109 233 return (gyroticks-nowz)*dir;
soonerbot 1:c28fac16a109 234 }