Terrabots / Mbed 2 deprecated TerraBot_Drive_2D_FINAL

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:   MotorV2 QEI mbed

Fork of TerraBot_Drive_2D by Terrabots

main.cpp@1:17db90f61f6c, 2016-11-29 (annotated)

Committer:
simplyellow
Date:
Tue Nov 29 21:16:58 2016 +0000
Revision:
1:17db90f61f6c
Parent:
0:d4bb648b7f2c
Child:
2:e68fd2af041d
final 2d tracking

Who changed what in which revision?

UserRevisionLine numberNew contents of line
simplyellow 0:d4bb648b7f2c 1 #include "QEI.h"
simplyellow 0:d4bb648b7f2c 2 #include "mbed.h"
simplyellow 0:d4bb648b7f2c 3 #include "Motor.h"
simplyellow 0:d4bb648b7f2c 4
simplyellow 0:d4bb648b7f2c 5 Serial pc(USBTX, USBRX);
simplyellow 0:d4bb648b7f2c 6
simplyellow 1:17db90f61f6c 7 QEI wheel (p29, p30, NC, 128, QEI::X4_ENCODING);
simplyellow 1:17db90f61f6c 8 //orange, yellow wires, VU 5 VOLTS, BOTTOM 2x2 pins on WW02
simplyellow 1:17db90f61f6c 9
simplyellow 1:17db90f61f6c 10 float pulseCount;
simplyellow 1:17db90f61f6c 11 float encoding = 2;
simplyellow 1:17db90f61f6c 12 float circumference = 13.25;
simplyellow 1:17db90f61f6c 13 float constant = 1360; // constant = 20*68
simplyellow 1:17db90f61f6c 14
simplyellow 0:d4bb648b7f2c 15 float distance;
simplyellow 0:d4bb648b7f2c 16
simplyellow 1:17db90f61f6c 17 float x;
simplyellow 1:17db90f61f6c 18 float y;
simplyellow 1:17db90f61f6c 19
simplyellow 0:d4bb648b7f2c 20 AnalogIn pot(p15);
simplyellow 0:d4bb648b7f2c 21 float value;
simplyellow 1:17db90f61f6c 22 float turnAngle;
simplyellow 1:17db90f61f6c 23 float zeroAngle;
simplyellow 1:17db90f61f6c 24
simplyellow 1:17db90f61f6c 25 float PI = 3.14159265;
simplyellow 1:17db90f61f6c 26
simplyellow 1:17db90f61f6c 27 #define STRAIGHT 0
simplyellow 1:17db90f61f6c 28 #define CCW 1
simplyellow 1:17db90f61f6c 29 #define CW 2
simplyellow 1:17db90f61f6c 30
simplyellow 1:17db90f61f6c 31 int dir = 3;
simplyellow 1:17db90f61f6c 32
simplyellow 1:17db90f61f6c 33 float df = 4.50; // distance from pivot to front wheel
simplyellow 1:17db90f61f6c 34 float db = 4.25; // distance from pivot to back wheel
simplyellow 1:17db90f61f6c 35 float W;
simplyellow 1:17db90f61f6c 36 float R;
simplyellow 1:17db90f61f6c 37 float dispAngle;
simplyellow 1:17db90f61f6c 38
simplyellow 1:17db90f61f6c 39 #define Q1 1
simplyellow 1:17db90f61f6c 40 #define Q2 2
simplyellow 1:17db90f61f6c 41 #define Q3 3
simplyellow 1:17db90f61f6c 42 #define Q4 4
simplyellow 1:17db90f61f6c 43
simplyellow 1:17db90f61f6c 44 int quadrant = 0;
simplyellow 0:d4bb648b7f2c 45
simplyellow 1:17db90f61f6c 46 void clear() {
simplyellow 1:17db90f61f6c 47 distance = 0;
simplyellow 1:17db90f61f6c 48 x = 0;
simplyellow 1:17db90f61f6c 49 y = 0;
simplyellow 1:17db90f61f6c 50 }
simplyellow 1:17db90f61f6c 51
simplyellow 1:17db90f61f6c 52 void potSetup() {
simplyellow 1:17db90f61f6c 53 zeroAngle = (float) pot;
simplyellow 1:17db90f61f6c 54 for(int i = 5; i > 0; i--) {
simplyellow 1:17db90f61f6c 55 pc.printf("%d seconds to set turning angle.\n\r", i);
simplyellow 1:17db90f61f6c 56 wait(1);
simplyellow 1:17db90f61f6c 57 }
simplyellow 1:17db90f61f6c 58 turnAngle = ((float) pot - zeroAngle) * 300;
simplyellow 1:17db90f61f6c 59 pc.printf("---------------------------------\n\r");
simplyellow 1:17db90f61f6c 60 //debug
simplyellow 1:17db90f61f6c 61 /*
simplyellow 1:17db90f61f6c 62 for(int i = 5; i > 0; i--) {
simplyellow 1:17db90f61f6c 63 pc.printf("%f is the turning Angle.\n\r", turnAngle);
simplyellow 1:17db90f61f6c 64 pc.printf("%f is the zero value.\n\r", zeroAngle);
simplyellow 1:17db90f61f6c 65 pc.printf("%f is the pot value.\n\r", (float) pot);
simplyellow 1:17db90f61f6c 66 wait(1);
simplyellow 1:17db90f61f6c 67 }*/
simplyellow 1:17db90f61f6c 68 }
simplyellow 0:d4bb648b7f2c 69
simplyellow 1:17db90f61f6c 70 void setDirection() {
simplyellow 1:17db90f61f6c 71 if (turnAngle > 0.5) { // +/- 0.5 leeway
simplyellow 1:17db90f61f6c 72 pc.printf("DIRECTION SET TO CCW with turnAngle %f\n\r", turnAngle);
simplyellow 1:17db90f61f6c 73 dir = CCW;
simplyellow 1:17db90f61f6c 74 pc.printf("%d is direction\n\r", dir);
simplyellow 1:17db90f61f6c 75 } else if (turnAngle < -0.5) {
simplyellow 1:17db90f61f6c 76 pc.printf("DIRECTION SET TO CW with turnAngle %f\n\r", turnAngle);
simplyellow 1:17db90f61f6c 77 dir = CW;
simplyellow 1:17db90f61f6c 78 } else {
simplyellow 1:17db90f61f6c 79 pc.printf("DIRECTION SET TO STRAIGHT with turnAngle %f\n\r", turnAngle);
simplyellow 1:17db90f61f6c 80 dir = STRAIGHT;
simplyellow 1:17db90f61f6c 81 }
simplyellow 1:17db90f61f6c 82 turnAngle = abs(turnAngle);
simplyellow 1:17db90f61f6c 83 }
simplyellow 0:d4bb648b7f2c 84
simplyellow 1:17db90f61f6c 85 void setQuadrant() {
simplyellow 1:17db90f61f6c 86 if ((turnAngle > 0) && (turnAngle < 90)) {
simplyellow 1:17db90f61f6c 87 quadrant = Q1;
simplyellow 1:17db90f61f6c 88 } else if ((turnAngle > 90) && (turnAngle < 180)) {
simplyellow 1:17db90f61f6c 89 quadrant = Q2;
simplyellow 1:17db90f61f6c 90 } else if ((turnAngle > 180) && (turnAngle < 270)) {
simplyellow 1:17db90f61f6c 91 quadrant = Q3;
simplyellow 1:17db90f61f6c 92 } else if ((turnAngle > 270) && (turnAngle < 360)) {
simplyellow 1:17db90f61f6c 93 quadrant = Q4;
simplyellow 1:17db90f61f6c 94 } else if (turnAngle == 0) {
simplyellow 1:17db90f61f6c 95 //don't worry
simplyellow 1:17db90f61f6c 96 } else {
simplyellow 1:17db90f61f6c 97 pc.printf("turnAngle not valid. TERMINATED. \n\r");
simplyellow 1:17db90f61f6c 98 while(true) {}
simplyellow 1:17db90f61f6c 99 }
simplyellow 1:17db90f61f6c 100 }
simplyellow 0:d4bb648b7f2c 101
simplyellow 1:17db90f61f6c 102 void checkToStart() { // start tracking when distance > 0
simplyellow 1:17db90f61f6c 103 while(true) {
simplyellow 1:17db90f61f6c 104 pulseCount = (float) wheel.getPulses();
simplyellow 1:17db90f61f6c 105 distance = pulseCount*circumference/encoding/constant;
simplyellow 1:17db90f61f6c 106 if (distance > 0) {
simplyellow 1:17db90f61f6c 107 return;
simplyellow 1:17db90f61f6c 108 } else {
simplyellow 1:17db90f61f6c 109 wait(0.0001);
simplyellow 1:17db90f61f6c 110 }
simplyellow 1:17db90f61f6c 111 }
simplyellow 1:17db90f61f6c 112 }
simplyellow 1:17db90f61f6c 113 /*
simplyellow 1:17db90f61f6c 114 AXES
simplyellow 1:17db90f61f6c 115 when robot is facing forward
simplyellow 1:17db90f61f6c 116 forward is +x
simplyellow 1:17db90f61f6c 117 left is +y
simplyellow 0:d4bb648b7f2c 118 */
simplyellow 0:d4bb648b7f2c 119
simplyellow 0:d4bb648b7f2c 120
simplyellow 1:17db90f61f6c 121 void calcDisplacement() {
simplyellow 1:17db90f61f6c 122 pulseCount = (float) wheel.getPulses();
simplyellow 1:17db90f61f6c 123 distance = pulseCount*circumference/encoding/constant;
simplyellow 1:17db90f61f6c 124
simplyellow 1:17db90f61f6c 125 W = sqrt(df*df+db*db-2*df*db*cos(PI-turnAngle*PI/180));
simplyellow 1:17db90f61f6c 126 R = W/(2*sin(0.5*turnAngle*PI/180));
simplyellow 1:17db90f61f6c 127 dispAngle = distance/R;
simplyellow 0:d4bb648b7f2c 128
simplyellow 1:17db90f61f6c 129 if (dir == STRAIGHT) {
simplyellow 1:17db90f61f6c 130 x = 0;
simplyellow 1:17db90f61f6c 131 y = distance;
simplyellow 1:17db90f61f6c 132 } else if (dir == CCW) {
simplyellow 1:17db90f61f6c 133 x = -1*(R*(1-cos(dispAngle)));
simplyellow 1:17db90f61f6c 134 y = R*sin(dispAngle);
simplyellow 1:17db90f61f6c 135 /*
simplyellow 1:17db90f61f6c 136 switch(quadrant) {
simplyellow 1:17db90f61f6c 137 case Q1:
simplyellow 1:17db90f61f6c 138 x = R*(cos(dispAngle)-1);
simplyellow 1:17db90f61f6c 139 y = R*sin(dispAngle);
simplyellow 1:17db90f61f6c 140 break;
simplyellow 1:17db90f61f6c 141 case Q2:
simplyellow 1:17db90f61f6c 142 x = -1*R*sin(dispAngle);
simplyellow 1:17db90f61f6c 143 y = R*(cos(dispAngle)-1);
simplyellow 1:17db90f61f6c 144 break;
simplyellow 1:17db90f61f6c 145 case Q3:
simplyellow 1:17db90f61f6c 146 x = R*(cos(dispAngle)-1);
simplyellow 1:17db90f61f6c 147 y = -1*R*sin(dispAngle);
simplyellow 1:17db90f61f6c 148 break;
simplyellow 1:17db90f61f6c 149 case Q4:
simplyellow 1:17db90f61f6c 150 x = R*sin(dispAngle);
simplyellow 1:17db90f61f6c 151 y = R*(1-cos(dispAngle));
simplyellow 1:17db90f61f6c 152 break;
simplyellow 1:17db90f61f6c 153 default:
simplyellow 1:17db90f61f6c 154 pc.printf("default \n\r");
simplyellow 1:17db90f61f6c 155 }*/
simplyellow 1:17db90f61f6c 156 } else if (dir == CW) {
simplyellow 1:17db90f61f6c 157 x = R*(1-cos(dispAngle));
simplyellow 1:17db90f61f6c 158 y = R*sin(dispAngle);
simplyellow 1:17db90f61f6c 159 /*
simplyellow 1:17db90f61f6c 160 switch(quadrant) {
simplyellow 1:17db90f61f6c 161 case Q1:
simplyellow 1:17db90f61f6c 162 x = R*sin(dispAngle);
simplyellow 1:17db90f61f6c 163 y = R*(cos(dispAngle)-1);
simplyellow 1:17db90f61f6c 164 break;
simplyellow 1:17db90f61f6c 165 case Q2:
simplyellow 1:17db90f61f6c 166 x = R*(cos(dispAngle)-1);
simplyellow 1:17db90f61f6c 167 y = R*sin(dispAngle);
simplyellow 1:17db90f61f6c 168 break;
simplyellow 1:17db90f61f6c 169 case Q3:
simplyellow 1:17db90f61f6c 170 x = -1*R*sin(dispAngle);
simplyellow 1:17db90f61f6c 171 y = R*(1-cos(dispAngle));
simplyellow 1:17db90f61f6c 172 break;
simplyellow 1:17db90f61f6c 173 case Q4:
simplyellow 1:17db90f61f6c 174 x = R*(cos(dispAngle)-1);
simplyellow 1:17db90f61f6c 175 y = -1*R*sin(dispAngle);
simplyellow 1:17db90f61f6c 176 break;
simplyellow 1:17db90f61f6c 177 default:
simplyellow 1:17db90f61f6c 178 pc.printf("default \n\r");
simplyellow 1:17db90f61f6c 179 }*/
simplyellow 1:17db90f61f6c 180 } else {
simplyellow 1:17db90f61f6c 181 pc.printf("Direction not set. TERMINATED. \n\r");
simplyellow 1:17db90f61f6c 182 while(true) {}
simplyellow 1:17db90f61f6c 183 }
simplyellow 0:d4bb648b7f2c 184
simplyellow 1:17db90f61f6c 185 wait(.0001);
simplyellow 0:d4bb648b7f2c 186 }
simplyellow 0:d4bb648b7f2c 187
simplyellow 1:17db90f61f6c 188 int main() {
simplyellow 1:17db90f61f6c 189 clear();
simplyellow 1:17db90f61f6c 190 potSetup();
simplyellow 1:17db90f61f6c 191 setDirection();
simplyellow 1:17db90f61f6c 192 setQuadrant();
simplyellow 1:17db90f61f6c 193 checkToStart();
simplyellow 1:17db90f61f6c 194 while(true) {
simplyellow 1:17db90f61f6c 195 calcDisplacement();
simplyellow 1:17db90f61f6c 196 pc.printf("----------------------\n\r");
simplyellow 1:17db90f61f6c 197 pc.printf("x: %f\n\r", x);
simplyellow 1:17db90f61f6c 198 pc.printf("y: %f\n\r", y);
simplyellow 1:17db90f61f6c 199 pc.printf("distance: %f\n\r", distance);
simplyellow 1:17db90f61f6c 200 pc.printf("turnAngle: %f\n\r", turnAngle);
simplyellow 1:17db90f61f6c 201 pc.printf("dispAngle: %f\n\r", dispAngle);
simplyellow 1:17db90f61f6c 202 pc.printf("direction: %d\n\r", dir);
simplyellow 1:17db90f61f6c 203 pc.printf("quadrant: %d\n\r", quadrant);
simplyellow 1:17db90f61f6c 204 pc.printf("W: %f\n\r", W);
simplyellow 1:17db90f61f6c 205 pc.printf("R: %f\n\r", R);
simplyellow 0:d4bb648b7f2c 206 }
simplyellow 1:17db90f61f6c 207 }