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

main.cpp@43:f7ef0f5f980c, 2014-03-31 (annotated)

Committer:
sswatek
Date:
Mon Mar 31 21:54:34 2014 +0000
Revision:
43:f7ef0f5f980c
Parent:
42:28d73a7c624e
Child:
44:4b933e7409e7
testing and filling out absMotorControl. Also motors work correctly for current electrical setup

Who changed what in which revision?

UserRevisionLine numberNew contents of line
soonerbot 0:3a3dd78038a6 1 #include "mbed.h"
soonerbot 1:c28fac16a109 2 #include "dbgprint.h"
soonerbot 1:c28fac16a109 3 #include "robot.h"
soonerbot 14:a30aa3b29a2e 4 #include "hcsr04.h"
sswatek 33:03b0b66038e1 5 #include "game.h"
soonerbot 0:3a3dd78038a6 6
soonerbot 1:c28fac16a109 7 BusOut leds(LED_RED,LED_GREEN,LED_BLUE);
soonerbot 1:c28fac16a109 8 Serial pc(USBTX, USBRX);
soonerbot 0:3a3dd78038a6 9
sswatek 35:6ef5fbc45672 10
soonerbot 14:a30aa3b29a2e 11
soonerbot 14:a30aa3b29a2e 12 /*
soonerbot 14:a30aa3b29a2e 13 //actual competition code, kinda
soonerbot 14:a30aa3b29a2e 14 int firstSection(){ // drive forward and measure distance
soonerbot 14:a30aa3b29a2e 15 tmpOut = bot.absDriveForward(targetAngle,3000);
soonerbot 14:a30aa3b29a2e 16 // ping sensor
soonerbot 14:a30aa3b29a2e 17 tmpOut = bot.absDriveForward(targetAngle,3000);
soonerbot 14:a30aa3b29a2e 18 // ping sensor
soonerbot 14:a30aa3b29a2e 19 }
soonerbot 14:a30aa3b29a2e 20 int secondsection(){
soonerbot 14:a30aa3b29a2e 21 // second step
soonerbot 14:a30aa3b29a2e 22 // etc
soonerbot 14:a30aa3b29a2e 23 }
soonerbot 14:a30aa3b29a2e 24 */
soonerbot 14:a30aa3b29a2e 25
soonerbot 0:3a3dd78038a6 26 int main() {
sswatek 35:6ef5fbc45672 27 wait(0.1);
sswatek 35:6ef5fbc45672 28 robot bot;
sswatek 35:6ef5fbc45672 29 gamePlayer game(bot);
sswatek 35:6ef5fbc45672 30 servo s(PTC9);
sswatek 37:38a4ad49343e 31 servo s2(PTC8);
sswatek 37:38a4ad49343e 32 servo s3(PTA5);
sswatek 37:38a4ad49343e 33 servo s4(PTA4);
soonerbot 14:a30aa3b29a2e 34 // Initialize variables
soonerbot 14:a30aa3b29a2e 35 leds = 0x2;
soonerbot 1:c28fac16a109 36 char tmpchar = 0;
soonerbot 14:a30aa3b29a2e 37 const int* constbuf; // displays values
soonerbot 10:926f142f16a3 38 int tmpOut;
soonerbot 4:adc885f4ab75 39 double targetAngle=0.0;
soonerbot 14:a30aa3b29a2e 40 DBGPRINT("AA\n\r",1); // to see if robot powers up at least
soonerbot 14:a30aa3b29a2e 41
soonerbot 15:b10859606504 42 //test servo
soonerbot 15:b10859606504 43 s.toPosition(45);
sswatek 41:fa498d6e7538 44 s2.toPosition(45);
sswatek 41:fa498d6e7538 45 s3.toPosition(45);
sswatek 41:fa498d6e7538 46 s4.toPosition(45);
soonerbot 15:b10859606504 47
sswatek 42:28d73a7c624e 48 int s1pos = 45;
sswatek 42:28d73a7c624e 49 int s2pos = 45;
soonerbot 15:b10859606504 50
soonerbot 15:b10859606504 51 //timer issue navigation test
soonerbot 15:b10859606504 52 bot.absDriveForward(0,50);//assuming the bot stops in this function
soonerbot 15:b10859606504 53 /*
soonerbot 15:b10859606504 54 DBGPRINT("%f",bot.gyro.getZDegrees());
soonerbot 15:b10859606504 55 bot.gyro.stop();//stopping the gyro timer
soonerbot 15:b10859606504 56 //bot.gyro.gyroUpkeepTicker.detach();//stopping the gyro timer
soonerbot 15:b10859606504 57 //use sensor to find distance
soonerbot 15:b10859606504 58 bot.pingLeft.trigger();
soonerbot 15:b10859606504 59 wait(0.1);
soonerbot 15:b10859606504 60 float pingresult = bot.pingLeft.inches();
soonerbot 15:b10859606504 61 DBGPRINT("Distance = %f\r\n",pingresult);
soonerbot 15:b10859606504 62 bot.gyro.start();//stopping the gyro timer
soonerbot 15:b10859606504 63 DBGPRINT("%f",bot.gyro.getZDegrees());
soonerbot 15:b10859606504 64 */
sswatek 23:53cafcd67828 65 Timer dataTimer;
sswatek 23:53cafcd67828 66 int testdata[]={'H','e','l','l','o'};
sswatek 30:4158120cf801 67 //int responseData[17];
sswatek 23:53cafcd67828 68 int response;
sswatek 23:53cafcd67828 69 int gotAck=0;
soonerbot 14:a30aa3b29a2e 70 // Loop
soonerbot 0:3a3dd78038a6 71 while(1) {
soonerbot 1:c28fac16a109 72 DBGPRINT("BB\n\r",1);
soonerbot 7:3b2cf7efe5d1 73 leds = leds^0x7; // toggle the LEDs for each loop
soonerbot 1:c28fac16a109 74 tmpchar = pc.getc();
sswatek 23:53cafcd67828 75 bot.BTLink.procBuf(0x02);
soonerbot 7:3b2cf7efe5d1 76 // all of these movement commands are blocking, so they can't be easily stopped short of resetting the microcontroller
soonerbot 1:c28fac16a109 77 switch(tmpchar){
soonerbot 15:b10859606504 78 /*case 'e': //drive in a smallish square
soonerbot 7:3b2cf7efe5d1 79 bot.driveForward(0,3000);
soonerbot 7:3b2cf7efe5d1 80 bot.driveForward(-90,0);
soonerbot 7:3b2cf7efe5d1 81 bot.driveForward(-90,3000);
soonerbot 7:3b2cf7efe5d1 82 bot.driveForward(-180,0);
soonerbot 7:3b2cf7efe5d1 83 bot.driveForward(-180,3000);
soonerbot 7:3b2cf7efe5d1 84 bot.driveForward(-270,0);
soonerbot 7:3b2cf7efe5d1 85 bot.driveForward(-270,3000);
soonerbot 7:3b2cf7efe5d1 86 bot.driveForward(0,0);
soonerbot 7:3b2cf7efe5d1 87 DBGPRINT("={%f,\t%f,\t%f}\n\r",bot.x,bot.y,bot.rot*180.0/3.14159);
soonerbot 15:b10859606504 88 break;*/
soonerbot 7:3b2cf7efe5d1 89 case 'q': //poll the encoders
soonerbot 1:c28fac16a109 90 constbuf = bot.bigenc.getVals();
soonerbot 1:c28fac16a109 91 DBGPRINT("\n\r%d\t%d\t%d\t%d\n\r",constbuf[0],constbuf[1],constbuf[2],constbuf[3]);
soonerbot 1:c28fac16a109 92 break;
soonerbot 7:3b2cf7efe5d1 93 case 'z': //set the current direction to "forward" for the following "go forward/reverse" commands
soonerbot 4:adc885f4ab75 94 targetAngle = bot.gyro.getZDegrees();
soonerbot 4:adc885f4ab75 95 break;
soonerbot 7:3b2cf7efe5d1 96 case 'w': // turn 90 degrees counter clockwise from the starting rotation
soonerbot 15:b10859606504 97
sswatek 42:28d73a7c624e 98 s.toPosition(-40);
soonerbot 15:b10859606504 99 //bot.driveForward(90,0);
soonerbot 7:3b2cf7efe5d1 100 DBGPRINT("={%f,\t%f,\t%f}\n\r",bot.x,bot.y,bot.rot*180.0/3.14159);
soonerbot 7:3b2cf7efe5d1 101 break;
soonerbot 7:3b2cf7efe5d1 102 case 'x': // turn 90 degrees clockwise from the starting rotation
sswatek 42:28d73a7c624e 103 s.toPosition(220);
soonerbot 15:b10859606504 104 //bot.driveForward(-90,0);
soonerbot 6:62d498ee97cf 105 DBGPRINT("={%f,\t%f,\t%f}\n\r",bot.x,bot.y,bot.rot*180.0/3.14159);
soonerbot 6:62d498ee97cf 106 break;
soonerbot 7:3b2cf7efe5d1 107 case 'a': // turn the opposite direction from the starting one
soonerbot 15:b10859606504 108 s.toPosition(90);
soonerbot 15:b10859606504 109 //bot.driveForward(180,0);
soonerbot 6:62d498ee97cf 110 DBGPRINT("={%f,\t%f,\t%f}\n\r",bot.x,bot.y,bot.rot*180.0/3.14159);
soonerbot 1:c28fac16a109 111 break;
soonerbot 7:3b2cf7efe5d1 112 case 'd': // turn back to starting rotation
soonerbot 15:b10859606504 113 //bot.driveForward(0,0);
sswatek 18:c834bd26869e 114 response=bot.BTLink.sendCmd(0x02,testdata,5);
sswatek 17:e247d58d9f42 115 DBGPRINT("=%d\n\r",response);
soonerbot 7:3b2cf7efe5d1 116 break;
soonerbot 7:3b2cf7efe5d1 117 case 'W': // drive forward a small bit
soonerbot 9:aff48e331147 118 //bot.driveForward(targetAngle,1000);
soonerbot 10:926f142f16a3 119 //bot.motors.moveForward(100);
soonerbot 11:967469d7e01c 120 tmpOut = bot.absDriveForward(targetAngle,3000);
soonerbot 10:926f142f16a3 121 DBGPRINT("W -> %d\n\r",tmpOut);
soonerbot 12:925f52da3ba9 122 DBGPRINT("={%f,\t%f,\t%f}\n\r",bot.x,bot.y,bot.rot*180.0/3.14159);
soonerbot 4:adc885f4ab75 123 break;
soonerbot 7:3b2cf7efe5d1 124 case 'E': //drive forward five times as much
soonerbot 15:b10859606504 125 //bot.driveForward(targetAngle,5000);
sswatek 31:4ef53fbd6759 126 bot.absDriveForward(0,290*16);
soonerbot 4:adc885f4ab75 127 DBGPRINT("={%f,\t%f,\t%f}\n\r",bot.x,bot.y,bot.rot*180.0/3.14159);
soonerbot 4:adc885f4ab75 128 break;
soonerbot 7:3b2cf7efe5d1 129 case 'X': // small reverse
soonerbot 10:926f142f16a3 130 //bot.driveForward(targetAngle,-1000);
soonerbot 10:926f142f16a3 131
soonerbot 11:967469d7e01c 132 tmpOut = bot.absDriveForward(targetAngle,-3000);
soonerbot 10:926f142f16a3 133 DBGPRINT("X -> %d\n\r",tmpOut);
soonerbot 12:925f52da3ba9 134 DBGPRINT("={%f,\t%f,\t%f}\n\r",bot.x,bot.y,bot.rot*180.0/3.14159);
soonerbot 4:adc885f4ab75 135 break;
soonerbot 7:3b2cf7efe5d1 136 case 'C': // big reverse
soonerbot 15:b10859606504 137 //bot.driveForward(targetAngle,-5000);
sswatek 31:4ef53fbd6759 138 bot.absDriveForward(0,-290*16);
soonerbot 4:adc885f4ab75 139 DBGPRINT("={%f,\t%f,\t%f}\n\r",bot.x,bot.y,bot.rot*180.0/3.14159);
soonerbot 4:adc885f4ab75 140 break;
soonerbot 7:3b2cf7efe5d1 141 case 'p': // poll the gyro
soonerbot 3:a223b0bf8256 142 DBGPRINT("%d\r\n",bot.gyro.getZ());
soonerbot 3:a223b0bf8256 143 break;
soonerbot 7:3b2cf7efe5d1 144 case 'l': //calibrate the gyro ( caution, only use after resetting and before moving, otherwise will break any calibration)
soonerbot 3:a223b0bf8256 145 bot.gyro.calibrate();
soonerbot 3:a223b0bf8256 146 break;
soonerbot 7:3b2cf7efe5d1 147
soonerbot 7:3b2cf7efe5d1 148 // these are all control system modifications which should be fairly well locked down at this point
soonerbot 4:adc885f4ab75 149 case 't':
sswatek 18:c834bd26869e 150 response=bot.BTLink.sendCmd(0x00,testdata,5);
sswatek 23:53cafcd67828 151 //bot.gyro.stop();
sswatek 23:53cafcd67828 152 dataTimer.start();
sswatek 23:53cafcd67828 153 dataTimer.reset();
sswatek 30:4158120cf801 154 while(1){
sswatek 30:4158120cf801 155 gotAck=bot.BTLink.getAck(response);
sswatek 30:4158120cf801 156 if(gotAck || dataTimer.read_ms()>=500)
sswatek 30:4158120cf801 157 break;
sswatek 30:4158120cf801 158 }
sswatek 23:53cafcd67828 159 dataTimer.stop();
sswatek 23:53cafcd67828 160 //bot.gyro.start();
sswatek 18:c834bd26869e 161 //bot.pfac*=2;
sswatek 18:c834bd26869e 162 //DBGPRINT("pfac = %f\r\n",bot.pfac);
sswatek 23:53cafcd67828 163 DBGPRINT("Mode0=%d, %d, %d [%d]\n\r",response,bot.BTLink.bufSize(), gotAck, dataTimer.read_ms());
soonerbot 4:adc885f4ab75 164 break;
soonerbot 4:adc885f4ab75 165 case 'g':
sswatek 29:1132155bc7da 166 DBGPRINT("full rotation %d,%f -> ",bot.gyro.getZ(),bot.gyro.getZDegrees());
sswatek 33:03b0b66038e1 167 for(int i=0;i<36;i++){
sswatek 33:03b0b66038e1 168 bot.smoothMove((8000.0/360.0)*(10.0), 1, 40);
sswatek 33:03b0b66038e1 169 }
sswatek 29:1132155bc7da 170 DBGPRINT(" %d,%f\r\n",bot.gyro.getZ(),bot.gyro.getZDegrees());
soonerbot 4:adc885f4ab75 171 break;
soonerbot 4:adc885f4ab75 172 case 'y':
sswatek 26:ade7c813538f 173 bot.pollForShapes();
soonerbot 4:adc885f4ab75 174 break;
soonerbot 4:adc885f4ab75 175 case 'h':
sswatek 29:1132155bc7da 176 DBGPRINT("full rotation %d,%f -> ",bot.gyro.getZ(),bot.gyro.getZDegrees());
sswatek 29:1132155bc7da 177 bot.smoothMove(-8000, 1, 40);
sswatek 29:1132155bc7da 178 DBGPRINT(" %d,%f\r\n",bot.gyro.getZ(),bot.gyro.getZDegrees());
soonerbot 4:adc885f4ab75 179 break;
soonerbot 4:adc885f4ab75 180 case 'u':
sswatek 27:688409727452 181 bot.pollForRigs();
soonerbot 4:adc885f4ab75 182 break;
soonerbot 4:adc885f4ab75 183 case 'j':
sswatek 31:4ef53fbd6759 184 DBGPRINT("LeftStable (15,8) = %f\r\n",bot.pingLeft.getStablePoll());
soonerbot 4:adc885f4ab75 185 break;
soonerbot 4:adc885f4ab75 186 case 'i':
soonerbot 14:a30aa3b29a2e 187 //put distance ping here
soonerbot 14:a30aa3b29a2e 188 bot.pingLeft.trigger();
sswatek 30:4158120cf801 189 dataTimer.start();
sswatek 30:4158120cf801 190 dataTimer.reset();
sswatek 30:4158120cf801 191 while(bot.pingLeft.measuring && dataTimer.read_ms()<300);
sswatek 30:4158120cf801 192 dataTimer.stop();
sswatek 30:4158120cf801 193 DBGPRINT("Distance = %f\r\n",bot.pingLeft.inches());
soonerbot 4:adc885f4ab75 194 break;
soonerbot 4:adc885f4ab75 195 case 'k':
sswatek 30:4158120cf801 196 dataTimer.start();
sswatek 30:4158120cf801 197 for(int i=0;i<100;i++){
sswatek 30:4158120cf801 198 bot.pingLeft.trigger();
sswatek 30:4158120cf801 199 dataTimer.reset();
sswatek 30:4158120cf801 200 while(bot.pingLeft.measuring && dataTimer.read_ms()<300);
sswatek 30:4158120cf801 201 DBGPRINT("Distance %d = %f\r\n",i,bot.pingLeft.inches());
sswatek 30:4158120cf801 202 }
sswatek 30:4158120cf801 203 dataTimer.stop();
sswatek 26:ade7c813538f 204 //bot.angfac/=2;
sswatek 26:ade7c813538f 205 //DBGPRINT("angfac = %f\r\n",bot.angfac);
soonerbot 4:adc885f4ab75 206 break;
soonerbot 4:adc885f4ab75 207 case 'T':
sswatek 33:03b0b66038e1 208 DBGPRINT("Finding Gap\r\n",1);
sswatek 33:03b0b66038e1 209 game.checkWaves();
soonerbot 4:adc885f4ab75 210 break;
soonerbot 4:adc885f4ab75 211 case 'G':
soonerbot 38:3062838df0ce 212 DBGPRINT("Running First and Second Row\r\n",1);
sswatek 34:4ad11cda1eca 213 game.runFirstRow();
soonerbot 38:3062838df0ce 214 game.runSecondRow();
sswatek 26:ade7c813538f 215 //bot.pfac/=1.05;
sswatek 26:ade7c813538f 216 //DBGPRINT("pfac = %f\r\n",bot.pfac);
soonerbot 4:adc885f4ab75 217 break;
soonerbot 4:adc885f4ab75 218 case 'Y':
sswatek 43:f7ef0f5f980c 219 bot.cont.start();
sswatek 43:f7ef0f5f980c 220 bot.cont.rampSpeed(40,40);
sswatek 43:f7ef0f5f980c 221 wait(2);
sswatek 43:f7ef0f5f980c 222 bot.cont.rampSpeed(0,0);
sswatek 43:f7ef0f5f980c 223 wait(2);
sswatek 43:f7ef0f5f980c 224 bot.cont.stop();
sswatek 43:f7ef0f5f980c 225 //s.toPosition(s1pos+=5);
sswatek 42:28d73a7c624e 226 DBGPRINT("1 pos = %d\r\n",s1pos);
soonerbot 4:adc885f4ab75 227 break;
soonerbot 4:adc885f4ab75 228 case 'H':
sswatek 42:28d73a7c624e 229 s.toPosition(s1pos-=5);
sswatek 42:28d73a7c624e 230 DBGPRINT("1 pos = %d\r\n",s1pos);
soonerbot 4:adc885f4ab75 231 break;
soonerbot 4:adc885f4ab75 232 case 'U':
sswatek 42:28d73a7c624e 233 s2.toPosition(s2pos+=5);
sswatek 42:28d73a7c624e 234 DBGPRINT("2 pos = %d\r\n",s2pos);
soonerbot 4:adc885f4ab75 235 break;
soonerbot 4:adc885f4ab75 236 case 'J':
sswatek 42:28d73a7c624e 237 s2.toPosition(s2pos-=5);
sswatek 42:28d73a7c624e 238 DBGPRINT("2 pos = %d\r\n",s2pos);
soonerbot 4:adc885f4ab75 239 break;
soonerbot 7:3b2cf7efe5d1 240
soonerbot 7:3b2cf7efe5d1 241 // poll the compass ( currently gives bad values )
soonerbot 4:adc885f4ab75 242 case 'Q':
sswatek 28:c7e8d2db03f5 243 //bot.gyro.checkCompass();
sswatek 28:c7e8d2db03f5 244 DBGPRINT("Compass vector = %d\t%d\t%d\t%f\r\n",bot.gyro.xmag,bot.gyro.ymag,bot.gyro.zmag,bot.gyro.compDir*180.0/3.14159);
soonerbot 4:adc885f4ab75 245 break;
soonerbot 7:3b2cf7efe5d1 246
soonerbot 7:3b2cf7efe5d1 247 // brake if we get an unknown command
soonerbot 1:c28fac16a109 248 default:
soonerbot 15:b10859606504 249 //bot.left.brake();
soonerbot 15:b10859606504 250 //bot.right.brake();
soonerbot 1:c28fac16a109 251 break;
soonerbot 1:c28fac16a109 252
soonerbot 1:c28fac16a109 253 }
soonerbot 0:3a3dd78038a6 254 }
soonerbot 0:3a3dd78038a6 255 }