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@12:925f52da3ba9, 2014-02-12 (annotated)

Committer:
soonerbot
Date:
Wed Feb 12 06:41:29 2014 +0000
Revision:
12:925f52da3ba9
Parent:
11:967469d7e01c
Child:
14:a30aa3b29a2e
Added gyro heading tracking during absController movements.

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 0:3a3dd78038a6 4
soonerbot 1:c28fac16a109 5 BusOut leds(LED_RED,LED_GREEN,LED_BLUE);
soonerbot 1:c28fac16a109 6 Serial pc(USBTX, USBRX);
soonerbot 0:3a3dd78038a6 7
soonerbot 1:c28fac16a109 8 robot bot;
soonerbot 0:3a3dd78038a6 9 int main() {
soonerbot 1:c28fac16a109 10 DBGPRINT("AA\n\r",1);
soonerbot 1:c28fac16a109 11 char tmpchar = 0;
soonerbot 1:c28fac16a109 12 leds = 0x2;
soonerbot 1:c28fac16a109 13 const int* constbuf;
soonerbot 10:926f142f16a3 14 int tmpOut;
soonerbot 4:adc885f4ab75 15 double targetAngle=0.0;
soonerbot 0:3a3dd78038a6 16 while(1) {
soonerbot 1:c28fac16a109 17 DBGPRINT("BB\n\r",1);
soonerbot 7:3b2cf7efe5d1 18 leds = leds^0x7; // toggle the LEDs for each loop
soonerbot 1:c28fac16a109 19 tmpchar = pc.getc();
soonerbot 1:c28fac16a109 20
soonerbot 7:3b2cf7efe5d1 21 // all of these movement commands are blocking, so they can't be easily stopped short of resetting the microcontroller
soonerbot 1:c28fac16a109 22 switch(tmpchar){
soonerbot 7:3b2cf7efe5d1 23 case 'e': //drive in a smallish square
soonerbot 7:3b2cf7efe5d1 24 bot.driveForward(0,3000);
soonerbot 7:3b2cf7efe5d1 25 bot.driveForward(-90,0);
soonerbot 7:3b2cf7efe5d1 26 bot.driveForward(-90,3000);
soonerbot 7:3b2cf7efe5d1 27 bot.driveForward(-180,0);
soonerbot 7:3b2cf7efe5d1 28 bot.driveForward(-180,3000);
soonerbot 7:3b2cf7efe5d1 29 bot.driveForward(-270,0);
soonerbot 7:3b2cf7efe5d1 30 bot.driveForward(-270,3000);
soonerbot 7:3b2cf7efe5d1 31 bot.driveForward(0,0);
soonerbot 7:3b2cf7efe5d1 32 DBGPRINT("={%f,\t%f,\t%f}\n\r",bot.x,bot.y,bot.rot*180.0/3.14159);
soonerbot 7:3b2cf7efe5d1 33 break;
soonerbot 7:3b2cf7efe5d1 34 case 'q': //poll the encoders
soonerbot 1:c28fac16a109 35 constbuf = bot.bigenc.getVals();
soonerbot 1:c28fac16a109 36 DBGPRINT("\n\r%d\t%d\t%d\t%d\n\r",constbuf[0],constbuf[1],constbuf[2],constbuf[3]);
soonerbot 1:c28fac16a109 37 break;
soonerbot 7:3b2cf7efe5d1 38 case 'z': //set the current direction to "forward" for the following "go forward/reverse" commands
soonerbot 4:adc885f4ab75 39 targetAngle = bot.gyro.getZDegrees();
soonerbot 4:adc885f4ab75 40 break;
soonerbot 7:3b2cf7efe5d1 41 case 'w': // turn 90 degrees counter clockwise from the starting rotation
soonerbot 7:3b2cf7efe5d1 42 bot.driveForward(90,0);
soonerbot 7:3b2cf7efe5d1 43 DBGPRINT("={%f,\t%f,\t%f}\n\r",bot.x,bot.y,bot.rot*180.0/3.14159);
soonerbot 7:3b2cf7efe5d1 44 break;
soonerbot 7:3b2cf7efe5d1 45 case 'x': // turn 90 degrees clockwise from the starting rotation
soonerbot 6:62d498ee97cf 46 bot.driveForward(-90,0);
soonerbot 6:62d498ee97cf 47 DBGPRINT("={%f,\t%f,\t%f}\n\r",bot.x,bot.y,bot.rot*180.0/3.14159);
soonerbot 6:62d498ee97cf 48 break;
soonerbot 7:3b2cf7efe5d1 49 case 'a': // turn the opposite direction from the starting one
soonerbot 7:3b2cf7efe5d1 50 bot.driveForward(180,0);
soonerbot 6:62d498ee97cf 51 DBGPRINT("={%f,\t%f,\t%f}\n\r",bot.x,bot.y,bot.rot*180.0/3.14159);
soonerbot 1:c28fac16a109 52 break;
soonerbot 7:3b2cf7efe5d1 53 case 'd': // turn back to starting rotation
soonerbot 7:3b2cf7efe5d1 54 bot.driveForward(0,0);
soonerbot 7:3b2cf7efe5d1 55 DBGPRINT("={%f,\t%f,\t%f}\n\r",bot.x,bot.y,bot.rot*180.0/3.14159);
soonerbot 7:3b2cf7efe5d1 56 break;
soonerbot 7:3b2cf7efe5d1 57 case 'W': // drive forward a small bit
soonerbot 9:aff48e331147 58 //bot.driveForward(targetAngle,1000);
soonerbot 10:926f142f16a3 59 //bot.motors.moveForward(100);
soonerbot 11:967469d7e01c 60 tmpOut = bot.absDriveForward(targetAngle,3000);
soonerbot 10:926f142f16a3 61 DBGPRINT("W -> %d\n\r",tmpOut);
soonerbot 12:925f52da3ba9 62 DBGPRINT("={%f,\t%f,\t%f}\n\r",bot.x,bot.y,bot.rot*180.0/3.14159);
soonerbot 4:adc885f4ab75 63 break;
soonerbot 7:3b2cf7efe5d1 64 case 'E': //drive forward five times as much
soonerbot 4:adc885f4ab75 65 bot.driveForward(targetAngle,5000);
soonerbot 4:adc885f4ab75 66 DBGPRINT("={%f,\t%f,\t%f}\n\r",bot.x,bot.y,bot.rot*180.0/3.14159);
soonerbot 4:adc885f4ab75 67 break;
soonerbot 7:3b2cf7efe5d1 68 case 'X': // small reverse
soonerbot 10:926f142f16a3 69 //bot.driveForward(targetAngle,-1000);
soonerbot 10:926f142f16a3 70
soonerbot 11:967469d7e01c 71 tmpOut = bot.absDriveForward(targetAngle,-3000);
soonerbot 10:926f142f16a3 72 DBGPRINT("X -> %d\n\r",tmpOut);
soonerbot 12:925f52da3ba9 73 DBGPRINT("={%f,\t%f,\t%f}\n\r",bot.x,bot.y,bot.rot*180.0/3.14159);
soonerbot 4:adc885f4ab75 74 break;
soonerbot 7:3b2cf7efe5d1 75 case 'C': // big reverse
soonerbot 4:adc885f4ab75 76 bot.driveForward(targetAngle,-5000);
soonerbot 4:adc885f4ab75 77 DBGPRINT("={%f,\t%f,\t%f}\n\r",bot.x,bot.y,bot.rot*180.0/3.14159);
soonerbot 4:adc885f4ab75 78 break;
soonerbot 7:3b2cf7efe5d1 79 case 'p': // poll the gyro
soonerbot 3:a223b0bf8256 80 DBGPRINT("%d\r\n",bot.gyro.getZ());
soonerbot 3:a223b0bf8256 81 break;
soonerbot 7:3b2cf7efe5d1 82 case 'l': //calibrate the gyro ( caution, only use after resetting and before moving, otherwise will break any calibration)
soonerbot 3:a223b0bf8256 83 bot.gyro.calibrate();
soonerbot 3:a223b0bf8256 84 break;
soonerbot 7:3b2cf7efe5d1 85
soonerbot 7:3b2cf7efe5d1 86 // these are all control system modifications which should be fairly well locked down at this point
soonerbot 4:adc885f4ab75 87 case 't':
soonerbot 4:adc885f4ab75 88 bot.pfac*=2;
soonerbot 4:adc885f4ab75 89 DBGPRINT("pfac = %f\r\n",bot.pfac);
soonerbot 4:adc885f4ab75 90 break;
soonerbot 4:adc885f4ab75 91 case 'g':
soonerbot 4:adc885f4ab75 92 bot.pfac/=2;
soonerbot 4:adc885f4ab75 93 DBGPRINT("pfac = %f\r\n",bot.pfac);
soonerbot 4:adc885f4ab75 94 break;
soonerbot 4:adc885f4ab75 95 case 'y':
soonerbot 4:adc885f4ab75 96 bot.ifac*=2;
soonerbot 4:adc885f4ab75 97 DBGPRINT("ifac = %f\r\n",bot.ifac);
soonerbot 4:adc885f4ab75 98 break;
soonerbot 4:adc885f4ab75 99 case 'h':
soonerbot 4:adc885f4ab75 100 bot.ifac/=2;
soonerbot 4:adc885f4ab75 101 DBGPRINT("ifac = %f\r\n",bot.ifac);
soonerbot 4:adc885f4ab75 102 break;
soonerbot 4:adc885f4ab75 103 case 'u':
soonerbot 4:adc885f4ab75 104 bot.dfac*=2;
soonerbot 4:adc885f4ab75 105 DBGPRINT("dfac = %f\r\n",bot.dfac);
soonerbot 4:adc885f4ab75 106 break;
soonerbot 4:adc885f4ab75 107 case 'j':
soonerbot 4:adc885f4ab75 108 bot.dfac/=2;
soonerbot 4:adc885f4ab75 109 DBGPRINT("dfac = %f\r\n",bot.dfac);
soonerbot 4:adc885f4ab75 110 break;
soonerbot 4:adc885f4ab75 111 case 'i':
soonerbot 4:adc885f4ab75 112 bot.angfac*=2;
soonerbot 4:adc885f4ab75 113 DBGPRINT("angfac = %f\r\n",bot.angfac);
soonerbot 4:adc885f4ab75 114 break;
soonerbot 4:adc885f4ab75 115 case 'k':
soonerbot 4:adc885f4ab75 116 bot.angfac/=2;
soonerbot 4:adc885f4ab75 117 DBGPRINT("angfac = %f\r\n",bot.angfac);
soonerbot 4:adc885f4ab75 118 break;
soonerbot 4:adc885f4ab75 119 case 'T':
soonerbot 4:adc885f4ab75 120 bot.pfac*=1.05;
soonerbot 4:adc885f4ab75 121 DBGPRINT("pfac = %f\r\n",bot.pfac);
soonerbot 4:adc885f4ab75 122 break;
soonerbot 4:adc885f4ab75 123 case 'G':
soonerbot 4:adc885f4ab75 124 bot.pfac/=1.05;
soonerbot 4:adc885f4ab75 125 DBGPRINT("pfac = %f\r\n",bot.pfac);
soonerbot 4:adc885f4ab75 126 break;
soonerbot 4:adc885f4ab75 127 case 'Y':
soonerbot 4:adc885f4ab75 128 bot.ifac*=1.05;
soonerbot 4:adc885f4ab75 129 DBGPRINT("ifac = %f\r\n",bot.ifac);
soonerbot 4:adc885f4ab75 130 break;
soonerbot 4:adc885f4ab75 131 case 'H':
soonerbot 4:adc885f4ab75 132 bot.ifac/=1.05;
soonerbot 4:adc885f4ab75 133 DBGPRINT("ifac = %f\r\n",bot.ifac);
soonerbot 4:adc885f4ab75 134 break;
soonerbot 4:adc885f4ab75 135 case 'U':
soonerbot 4:adc885f4ab75 136 bot.dfac*=1.05;
soonerbot 4:adc885f4ab75 137 DBGPRINT("dfac = %f\r\n",bot.dfac);
soonerbot 4:adc885f4ab75 138 break;
soonerbot 4:adc885f4ab75 139 case 'J':
soonerbot 4:adc885f4ab75 140 bot.dfac/=1.05;
soonerbot 4:adc885f4ab75 141 DBGPRINT("dfac = %f\r\n",bot.dfac);
soonerbot 4:adc885f4ab75 142 break;
soonerbot 7:3b2cf7efe5d1 143
soonerbot 7:3b2cf7efe5d1 144 // poll the compass ( currently gives bad values )
soonerbot 4:adc885f4ab75 145 case 'Q':
soonerbot 4:adc885f4ab75 146 DBGPRINT("Compass vector = %d\t%d\t%d\r\n",bot.gyro.xmag,bot.gyro.ymag,bot.gyro.zmag);
soonerbot 4:adc885f4ab75 147 break;
soonerbot 7:3b2cf7efe5d1 148
soonerbot 7:3b2cf7efe5d1 149 // brake if we get an unknown command
soonerbot 1:c28fac16a109 150 default:
soonerbot 1:c28fac16a109 151 bot.left.brake();
soonerbot 1:c28fac16a109 152 bot.right.brake();
soonerbot 1:c28fac16a109 153 break;
soonerbot 1:c28fac16a109 154
soonerbot 1:c28fac16a109 155 }
soonerbot 0:3a3dd78038a6 156 }
soonerbot 0:3a3dd78038a6 157 }