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

Committer:
soonerbot
Date:
Sun Nov 17 03:17:09 2013 +0000
Revision:
2:7c6b494f9005
Parent:
1:c28fac16a109
Child:
3:a223b0bf8256
Properly set up motors

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 1:c28fac16a109 6 robot::robot() : spi(PTD2, PTD3, PTD1), bigenc(spi,PTD0), /*gyro(p9, p10),*/
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 1:c28fac16a109 12 //blah
soonerbot 1:c28fac16a109 13 }
soonerbot 1:c28fac16a109 14
soonerbot 1:c28fac16a109 15
soonerbot 1:c28fac16a109 16 int robot::driveForward(double power, int distance){
soonerbot 1:c28fac16a109 17 bigenc.resetAll();
soonerbot 1:c28fac16a109 18 const int* constbuf = bigenc.getVals();
soonerbot 1:c28fac16a109 19 int prev[4]={0,0,0,0};
soonerbot 1:c28fac16a109 20 int distTraveled=0, i;
soonerbot 1:c28fac16a109 21 double maxPow;
soonerbot 1:c28fac16a109 22 while(distTraveled<distance){
soonerbot 1:c28fac16a109 23 maxPow=MAX(double(distance-distTraveled-2000)/15000.0,0.1);
soonerbot 1:c28fac16a109 24 left.setPower(MIN(power,maxPow));
soonerbot 1:c28fac16a109 25 right.setPower(MIN(power,maxPow));
soonerbot 1:c28fac16a109 26 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 27 for(i=0;i<4;i++)
soonerbot 1:c28fac16a109 28 prev[i]=constbuf[i];
soonerbot 1:c28fac16a109 29 //wait(0.05);
soonerbot 1:c28fac16a109 30 constbuf = bigenc.getVals();
soonerbot 1:c28fac16a109 31 //int deltaTraveled=MAX(MIN(MIN(-constbuf[0]+prev[0],-constbuf[1]+prev[1]),MIN(constbuf[2]-prev[2],constbuf[3]-prev[3])),0);
soonerbot 1:c28fac16a109 32 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 33 addforward(double(deltaTraveled)*0.0035362);
soonerbot 1:c28fac16a109 34 distTraveled+=deltaTraveled;
soonerbot 1:c28fac16a109 35 }
soonerbot 1:c28fac16a109 36 left.brake();
soonerbot 1:c28fac16a109 37 right.brake();
soonerbot 1:c28fac16a109 38 return 0;
soonerbot 1:c28fac16a109 39 }
soonerbot 1:c28fac16a109 40
soonerbot 1:c28fac16a109 41 int robot::addforward(double dist){
soonerbot 1:c28fac16a109 42 double angle=0;//double(gyro.getZ())*2*3.14159/4050000.0;
soonerbot 1:c28fac16a109 43 x+=dist*cos(angle);
soonerbot 1:c28fac16a109 44 y+=dist*sin(angle);
soonerbot 1:c28fac16a109 45 rot=angle;
soonerbot 1:c28fac16a109 46 }
soonerbot 1:c28fac16a109 47
soonerbot 1:c28fac16a109 48 int robot::moveTo(double xInches, double yInches){
soonerbot 1:c28fac16a109 49 double power=.2;
soonerbot 1:c28fac16a109 50 turntowards(xInches,yInches);
soonerbot 1:c28fac16a109 51 double distance=sqrt(pow(xInches-x,2)+pow(yInches-y,2))/0.0035362;
soonerbot 1:c28fac16a109 52 double angle=atan2(xInches-x,yInches-y)*180.0/3.14159;
soonerbot 1:c28fac16a109 53 double currangle=0;//double(gyro.getZ())*2*3.14159/4050000.0;
soonerbot 1:c28fac16a109 54 bigenc.resetAll();
soonerbot 1:c28fac16a109 55 const int* constbuf = bigenc.getVals();
soonerbot 1:c28fac16a109 56 int prev[4]={0,0,0,0};
soonerbot 1:c28fac16a109 57 int distTraveled=0, i;
soonerbot 1:c28fac16a109 58 double maxPow;
soonerbot 1:c28fac16a109 59 DBGPRINT("going %f at angle %f from current of %f\r\n",distance,angle,currangle);
soonerbot 1:c28fac16a109 60 while(distTraveled<distance){
soonerbot 1:c28fac16a109 61 angle=atan2(xInches-x,yInches-y)*180.0/3.14159;
soonerbot 1:c28fac16a109 62 currangle=0;//double(gyro.getZ())*2*3.14159/4050000.0;
soonerbot 1:c28fac16a109 63 maxPow=MAX(double(distance-distTraveled-2000)/15000.0,0.1);
soonerbot 1:c28fac16a109 64 if(currangle>angle+2.0){ //too far to the right, brake left
soonerbot 1:c28fac16a109 65 left.brake();
soonerbot 1:c28fac16a109 66 } else {
soonerbot 1:c28fac16a109 67 left.setPower(MIN(power,maxPow));
soonerbot 1:c28fac16a109 68 }
soonerbot 1:c28fac16a109 69 if(currangle<angle-2){
soonerbot 1:c28fac16a109 70 right.brake();
soonerbot 1:c28fac16a109 71 } else {
soonerbot 1:c28fac16a109 72 right.setPower(MIN(power,maxPow));
soonerbot 1:c28fac16a109 73 }
soonerbot 1:c28fac16a109 74 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 75 for(i=0;i<4;i++)
soonerbot 1:c28fac16a109 76 prev[i]=constbuf[i];
soonerbot 1:c28fac16a109 77 //wait(0.05);
soonerbot 1:c28fac16a109 78 constbuf = bigenc.getVals();
soonerbot 1:c28fac16a109 79 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 80 addforward(double(deltaTraveled)*0.0035362);
soonerbot 1:c28fac16a109 81 distTraveled+=deltaTraveled;
soonerbot 1:c28fac16a109 82 //angle=atan2(xInches-x,yInches-y)*180.0/3.14159;
soonerbot 1:c28fac16a109 83 }
soonerbot 1:c28fac16a109 84 left.brake();
soonerbot 1:c28fac16a109 85 right.brake();
soonerbot 1:c28fac16a109 86 return 0;
soonerbot 1:c28fac16a109 87
soonerbot 1:c28fac16a109 88 }
soonerbot 1:c28fac16a109 89
soonerbot 1:c28fac16a109 90 int robot::turntowards(double xInches, double yInches){
soonerbot 1:c28fac16a109 91 double currangle=0;//double(gyro.getZ())*2*3.14159/4050000.0;
soonerbot 1:c28fac16a109 92 double angle=atan2(xInches-x,yInches-y)*180.0/3.14159;
soonerbot 1:c28fac16a109 93 double finangle=angle;
soonerbot 1:c28fac16a109 94 /*if(int(currangle-angle)%360>180){ //needs to turn positive degrees
soonerbot 1:c28fac16a109 95 finangle=currangle+double(int(angle-currangle)%360);
soonerbot 1:c28fac16a109 96 } else {//negative degrees
soonerbot 1:c28fac16a109 97 finangle=currangle-double(int(currangle-angle)%360);
soonerbot 1:c28fac16a109 98 }*/
soonerbot 1:c28fac16a109 99 double acc=turn(0.4,finangle);
soonerbot 1:c28fac16a109 100 if(acc<-0.75 && acc>0.75){
soonerbot 1:c28fac16a109 101 acc=turn(0.3,finangle);
soonerbot 1:c28fac16a109 102 }
soonerbot 1:c28fac16a109 103
soonerbot 1:c28fac16a109 104
soonerbot 1:c28fac16a109 105 return 1;
soonerbot 1:c28fac16a109 106 }
soonerbot 1:c28fac16a109 107
soonerbot 1:c28fac16a109 108
soonerbot 1:c28fac16a109 109 double robot::turn(double power, double degrees){
soonerbot 1:c28fac16a109 110 bigenc.resetAll();
soonerbot 1:c28fac16a109 111 const int* constbuf = bigenc.getVals();
soonerbot 1:c28fac16a109 112 int prev[4]={0,0,0,0};
soonerbot 1:c28fac16a109 113 int startz;
soonerbot 1:c28fac16a109 114 startz=0;//gyro.getZ();
soonerbot 1:c28fac16a109 115 int gyroticks=(degrees*4050000)/360;
soonerbot 1:c28fac16a109 116 double maxPow;
soonerbot 1:c28fac16a109 117 int nowz=startz;
soonerbot 1:c28fac16a109 118 int dir=0;
soonerbot 1:c28fac16a109 119 if(gyroticks>startz){
soonerbot 1:c28fac16a109 120 right.setPower(-power);
soonerbot 1:c28fac16a109 121 left.setPower(power);
soonerbot 1:c28fac16a109 122 dir=1;
soonerbot 1:c28fac16a109 123 } else {
soonerbot 1:c28fac16a109 124 right.setPower(power);
soonerbot 1:c28fac16a109 125 left.setPower(-power);
soonerbot 1:c28fac16a109 126 dir=-1;
soonerbot 1:c28fac16a109 127 }
soonerbot 1:c28fac16a109 128 while((gyroticks-nowz)*dir>0){
soonerbot 1:c28fac16a109 129 maxPow=MAX(double(abs(gyroticks-nowz))/4050000.0,0.25);
soonerbot 1:c28fac16a109 130 if(gyroticks<nowz){
soonerbot 1:c28fac16a109 131 right.setPower(-MIN(power,maxPow));
soonerbot 1:c28fac16a109 132 left.setPower(MIN(power,maxPow));
soonerbot 1:c28fac16a109 133 } else {
soonerbot 1:c28fac16a109 134 right.setPower(MIN(power,maxPow));
soonerbot 1:c28fac16a109 135 left.setPower(-MIN(power,maxPow));
soonerbot 1:c28fac16a109 136 }
soonerbot 1:c28fac16a109 137 DBGPRINT("_%d of %d {%f, %f, %f}\r\n",(gyroticks-nowz)*dir, gyroticks,x,y,rot*180.0/3.14159);
soonerbot 1:c28fac16a109 138 for(int i=0;i<4;i++)
soonerbot 1:c28fac16a109 139 prev[i]=constbuf[i];
soonerbot 1:c28fac16a109 140 constbuf = bigenc.getVals();
soonerbot 1:c28fac16a109 141 int deltaTraveled;
soonerbot 1:c28fac16a109 142 if(gyroticks<nowz){
soonerbot 1:c28fac16a109 143 deltaTraveled=(MIN(-constbuf[0]+prev[0],-constbuf[1]+prev[1])-MIN(-constbuf[2]+prev[2],-constbuf[3]+prev[3]))/2;
soonerbot 1:c28fac16a109 144 } else {
soonerbot 1:c28fac16a109 145 deltaTraveled=(-MIN(constbuf[0]-prev[0],constbuf[1]-prev[1])+MIN(constbuf[2]-prev[2],constbuf[3]-prev[3]))/2;
soonerbot 1:c28fac16a109 146 }
soonerbot 1:c28fac16a109 147 addforward(double(deltaTraveled)*0.0035362);
soonerbot 1:c28fac16a109 148 nowz=0;//gyro.getZ();
soonerbot 1:c28fac16a109 149 }
soonerbot 1:c28fac16a109 150 right.brake();
soonerbot 1:c28fac16a109 151 left.brake();
soonerbot 1:c28fac16a109 152 return (gyroticks-nowz)*dir;
soonerbot 1:c28fac16a109 153 }