Milo Pan / Mbed 2 deprecated TerraBot_Drive_2D

2d drive position tracking

Dependencies:   MotorV2 QEI mbed

main.cpp@0:d4bb648b7f2c, 2016-11-14 (annotated)

Committer:
simplyellow
Date:
Mon Nov 14 21:46:17 2016 +0000
Revision:
0:d4bb648b7f2c
code with old version commented out

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 /*
simplyellow 0:d4bb648b7f2c 6 Written by Milo Pan with the help of Jacob Callahan
simplyellow 0:d4bb648b7f2c 7 */
simplyellow 0:d4bb648b7f2c 8
simplyellow 0:d4bb648b7f2c 9 /*
simplyellow 0:d4bb648b7f2c 10 Use X4 encoding.
simplyellow 0:d4bb648b7f2c 11 QEI wheel(p29, p30, NC, 624, QEI::X4_ENCODING);
simplyellow 0:d4bb648b7f2c 12 QEI constructor(CHANNEL A, CHANNEL B,PinName index (NC), int pulsesPerRev, Encoding encoding=X2_ENCODING)
simplyellow 0:d4bb648b7f2c 13 Use X2 encoding by default.
simplyellow 0:d4bb648b7f2c 14 Encoding x4 = x4_ENCODING;
simplyellow 0:d4bb648b7f2c 15 */
simplyellow 0:d4bb648b7f2c 16
simplyellow 0:d4bb648b7f2c 17 /*
simplyellow 0:d4bb648b7f2c 18 Measurement Instructions
simplyellow 0:d4bb648b7f2c 19 in order to convert the numerical value (outputted by this program) to Inches traveled,
simplyellow 0:d4bb648b7f2c 20 measurements need to be taken (i.e. have the truck travel 40" for 10 trials, average the values, and find the factor
simplyellow 0:d4bb648b7f2c 21 */
simplyellow 0:d4bb648b7f2c 22
simplyellow 0:d4bb648b7f2c 23 Serial pc(USBTX, USBRX);
simplyellow 0:d4bb648b7f2c 24
simplyellow 0:d4bb648b7f2c 25 QEI wheel (p29, p30, NC, 128, QEI::X4_ENCODING); //orange, yellow wires, VU 5 VOLTS, BOTTOM 2x2 pins on WW02
simplyellow 0:d4bb648b7f2c 26 float pulseCount; //number of pulses detected by encoder
simplyellow 0:d4bb648b7f2c 27 float encoding = 2; //X2
simplyellow 0:d4bb648b7f2c 28 float circumference = 13.25; //inches of circumference
simplyellow 0:d4bb648b7f2c 29 float distance;
simplyellow 0:d4bb648b7f2c 30
simplyellow 0:d4bb648b7f2c 31 AnalogIn pot(p15);
simplyellow 0:d4bb648b7f2c 32 float value;
simplyellow 0:d4bb648b7f2c 33 float angle;
simplyellow 0:d4bb648b7f2c 34 float zero;
simplyellow 0:d4bb648b7f2c 35
simplyellow 0:d4bb648b7f2c 36 float PI = 3.14159;
simplyellow 0:d4bb648b7f2c 37
simplyellow 0:d4bb648b7f2c 38 /* VERSION 1
simplyellow 0:d4bb648b7f2c 39 float x = 0;
simplyellow 0:d4bb648b7f2c 40 float dx;
simplyellow 0:d4bb648b7f2c 41 float y = 0;
simplyellow 0:d4bb648b7f2c 42 float dy;
simplyellow 0:d4bb648b7f2c 43 float dispAngle; // theta d
simplyellow 0:d4bb648b7f2c 44 float r; // r
simplyellow 0:d4bb648b7f2c 45 float c; // c, chord length
simplyellow 0:d4bb648b7f2c 46
simplyellow 0:d4bb648b7f2c 47 float segment; // intermediary variable for ease of calculation
simplyellow 0:d4bb648b7f2c 48
simplyellow 0:d4bb648b7f2c 49 float heading; // heading of front
simplyellow 0:d4bb648b7f2c 50 */
simplyellow 0:d4bb648b7f2c 51
simplyellow 0:d4bb648b7f2c 52
simplyellow 0:d4bb648b7f2c 53 // VERSION 2
simplyellow 0:d4bb648b7f2c 54 float df;
simplyellow 0:d4bb648b7f2c 55 float db;
simplyellow 0:d4bb648b7f2c 56 float circumference2;
simplyellow 0:d4bb648b7f2c 57 float proportion;
simplyellow 0:d4bb648b7f2c 58 float scaledDown;
simplyellow 0:d4bb648b7f2c 59 float W;
simplyellow 0:d4bb648b7f2c 60 float R;
simplyellow 0:d4bb648b7f2c 61 int dir; //1 CCW, 2 CW, 0 straight
simplyellow 0:d4bb648b7f2c 62 float heading = 0.0;
simplyellow 0:d4bb648b7f2c 63 float dispAngleRad;
simplyellow 0:d4bb648b7f2c 64 float dispAngleDeg;
simplyellow 0:d4bb648b7f2c 65 float x;
simplyellow 0:d4bb648b7f2c 66 float dx;
simplyellow 0:d4bb648b7f2c 67 float y;
simplyellow 0:d4bb648b7f2c 68 float dy;
simplyellow 0:d4bb648b7f2c 69
simplyellow 0:d4bb648b7f2c 70 float totalDistance;
simplyellow 0:d4bb648b7f2c 71 int i;
simplyellow 0:d4bb648b7f2c 72
simplyellow 0:d4bb648b7f2c 73
simplyellow 0:d4bb648b7f2c 74
simplyellow 0:d4bb648b7f2c 75 void measure() {
simplyellow 0:d4bb648b7f2c 76 // raw measurements of pot, encoder
simplyellow 0:d4bb648b7f2c 77 //(pulse count / X * N) * (1 / PPI)
simplyellow 0:d4bb648b7f2c 78 pulseCount = (float) wheel.getPulses();
simplyellow 0:d4bb648b7f2c 79 /* conversion to relevant values: distance of arc traveled, angle of turn
simplyellow 0:d4bb648b7f2c 80 | suppose the truck is a two-segment body
simplyellow 0:d4bb648b7f2c 81 / <- angle between bottom leg and the vertical = angle of turn
simplyellow 0:d4bb648b7f2c 82 the 20 and 68 were arbitrary and determined from testing, respectively
simplyellow 0:d4bb648b7f2c 83 the distance is in inches
simplyellow 0:d4bb648b7f2c 84 */
simplyellow 0:d4bb648b7f2c 85 distance = pulseCount*circumference/encoding/20/68;
simplyellow 0:d4bb648b7f2c 86 // with half inch correction after testing extensively
simplyellow 0:d4bb648b7f2c 87 /*if (distance > 0) {
simplyellow 0:d4bb648b7f2c 88 distance-=0.5;
simplyellow 0:d4bb648b7f2c 89 }
simplyellow 0:d4bb648b7f2c 90 */
simplyellow 0:d4bb648b7f2c 91 }
simplyellow 0:d4bb648b7f2c 92
simplyellow 0:d4bb648b7f2c 93 /* VERSION 1
simplyellow 0:d4bb648b7f2c 94 void calculateDisplacement() {
simplyellow 0:d4bb648b7f2c 95 /
simplyellow 0:d4bb648b7f2c 96 Premise:
simplyellow 0:d4bb648b7f2c 97 Suppose the arc s (distance), the distance traveled by the
simplyellow 0:d4bb648b7f2c 98 turning truck, is defined by two connected points on
simplyellow 0:d4bb648b7f2c 99 a circle with radius r.
simplyellow 0:d4bb648b7f2c 100 /
simplyellow 0:d4bb648b7f2c 101
simplyellow 0:d4bb648b7f2c 102 // convert angle to radians
simplyellow 0:d4bb648b7f2c 103 angle = angle*PI/180;
simplyellow 0:d4bb648b7f2c 104 // r = s/angle, for distance of arc s
simplyellow 0:d4bb648b7f2c 105 r = distance/(angle);
simplyellow 0:d4bb648b7f2c 106 // c = 2*r*sin(angle/2) for chord c
simplyellow 0:d4bb648b7f2c 107 c = 2*r*sin(angle/2);
simplyellow 0:d4bb648b7f2c 108 // intermediate value
simplyellow 0:d4bb648b7f2c 109 segment = (r*(1-cos(angle)));
simplyellow 0:d4bb648b7f2c 110 // calculation of angle of displacement
simplyellow 0:d4bb648b7f2c 111 dispAngle = acos(segment/c);
simplyellow 0:d4bb648b7f2c 112 // calculation of displacement
simplyellow 0:d4bb648b7f2c 113 dx = c*cos(dispAngle+heading);
simplyellow 0:d4bb648b7f2c 114 dy = c*sin(dispAngle+heading);
simplyellow 0:d4bb648b7f2c 115 x = x - r + dx;
simplyellow 0:d4bb648b7f2c 116 y = y + dy;
simplyellow 0:d4bb648b7f2c 117 wait(1.0);
simplyellow 0:d4bb648b7f2c 118 }
simplyellow 0:d4bb648b7f2c 119 */
simplyellow 0:d4bb648b7f2c 120
simplyellow 0:d4bb648b7f2c 121 void calculateDisplacement() {
simplyellow 0:d4bb648b7f2c 122 df = 4.5; // distance from pivot to front wheel
simplyellow 0:d4bb648b7f2c 123 db = 4.25; // distance from pivot to back wheel
simplyellow 0:d4bb648b7f2c 124 // W = 2*R*sin(0.5*angle);
simplyellow 0:d4bb648b7f2c 125 // W = shortest distance between wheels
simplyellow 0:d4bb648b7f2c 126 angle = angle*PI/180;
simplyellow 0:d4bb648b7f2c 127 W = sqrt(df*df+db*db-2*df*db*cos(PI-angle));
simplyellow 0:d4bb648b7f2c 128 R = W/(2*sin(0.5*angle));
simplyellow 0:d4bb648b7f2c 129
simplyellow 0:d4bb648b7f2c 130 circumference2 = 2*PI*R; // inches
simplyellow 0:d4bb648b7f2c 131 proportion = distance/circumference2;
simplyellow 0:d4bb648b7f2c 132 scaledDown = proportion - floor(proportion);
simplyellow 0:d4bb648b7f2c 133 dispAngleRad = scaledDown*2*PI;
simplyellow 0:d4bb648b7f2c 134 dispAngleDeg = scaledDown*360;
simplyellow 0:d4bb648b7f2c 135 dx = R*cos(dispAngleRad + heading);
simplyellow 0:d4bb648b7f2c 136 dy = R*sin(dispAngleRad + heading);
simplyellow 0:d4bb648b7f2c 137
simplyellow 0:d4bb648b7f2c 138 if(dir == 1) { // CCW
simplyellow 0:d4bb648b7f2c 139 x = 0 - R + dx;
simplyellow 0:d4bb648b7f2c 140 y = dy;
simplyellow 0:d4bb648b7f2c 141 } else if (dir == 2) { // CW
simplyellow 0:d4bb648b7f2c 142 y = dy;
simplyellow 0:d4bb648b7f2c 143 if((dispAngleDeg < 90) || (dispAngleDeg > 270)) {
simplyellow 0:d4bb648b7f2c 144 x = dx;
simplyellow 0:d4bb648b7f2c 145 } else {
simplyellow 0:d4bb648b7f2c 146 x = R - dx;
simplyellow 0:d4bb648b7f2c 147 }
simplyellow 0:d4bb648b7f2c 148 } else {
simplyellow 0:d4bb648b7f2c 149 x = 0;
simplyellow 0:d4bb648b7f2c 150 y = distance;
simplyellow 0:d4bb648b7f2c 151 }
simplyellow 0:d4bb648b7f2c 152 heading += dispAngleRad;
simplyellow 0:d4bb648b7f2c 153 wait(1.0);
simplyellow 0:d4bb648b7f2c 154 }
simplyellow 0:d4bb648b7f2c 155 /*
simplyellow 0:d4bb648b7f2c 156 dir
simplyellow 0:d4bb648b7f2c 157 and straight accuracy
simplyellow 0:d4bb648b7f2c 158 */
simplyellow 0:d4bb648b7f2c 159
simplyellow 0:d4bb648b7f2c 160 int main() {
simplyellow 0:d4bb648b7f2c 161 // required to set truck straight before start up to calibrate to zero
simplyellow 0:d4bb648b7f2c 162 zero = (float) pot;
simplyellow 0:d4bb648b7f2c 163 while(i<5) {
simplyellow 0:d4bb648b7f2c 164 printf("%d set angle \n\r",i);
simplyellow 0:d4bb648b7f2c 165 wait(1);
simplyellow 0:d4bb648b7f2c 166 i++;
simplyellow 0:d4bb648b7f2c 167 }
simplyellow 0:d4bb648b7f2c 168 if (totalDistance == 0) { // pot turns constant after the distance turns non zero
simplyellow 0:d4bb648b7f2c 169 value = (float) pot;
simplyellow 0:d4bb648b7f2c 170 // appropriate conversion from potentiometer float value to angle
simplyellow 0:d4bb648b7f2c 171 angle = (value-zero)*300;
simplyellow 0:d4bb648b7f2c 172 printf("%f",angle);
simplyellow 0:d4bb648b7f2c 173 }
simplyellow 0:d4bb648b7f2c 174 distance = 0;
simplyellow 0:d4bb648b7f2c 175 totalDistance = 0;
simplyellow 0:d4bb648b7f2c 176 heading = 0;
simplyellow 0:d4bb648b7f2c 177 while(1){
simplyellow 0:d4bb648b7f2c 178 measure();
simplyellow 0:d4bb648b7f2c 179
simplyellow 0:d4bb648b7f2c 180 calculateDisplacement();
simplyellow 0:d4bb648b7f2c 181
simplyellow 0:d4bb648b7f2c 182 totalDistance+=distance;
simplyellow 0:d4bb648b7f2c 183 if(totalDistance>0) {//@@@@@@@@@@check dir with angle vals@@@@@@@
simplyellow 0:d4bb648b7f2c 184 if (angle > 0.5) {
simplyellow 0:d4bb648b7f2c 185 dir = 1;//ccw
simplyellow 0:d4bb648b7f2c 186 } else if (angle < -0.5) {
simplyellow 0:d4bb648b7f2c 187 dir = 2;//cw
simplyellow 0:d4bb648b7f2c 188 } else {
simplyellow 0:d4bb648b7f2c 189 dir = 0;
simplyellow 0:d4bb648b7f2c 190 }
simplyellow 0:d4bb648b7f2c 191 }
simplyellow 0:d4bb648b7f2c 192 distance = 0;
simplyellow 0:d4bb648b7f2c 193
simplyellow 0:d4bb648b7f2c 194 /*
simplyellow 0:d4bb648b7f2c 195 Notes:
simplyellow 0:d4bb648b7f2c 196 At startup:
simplyellow 0:d4bb648b7f2c 197 distance = 0
simplyellow 0:d4bb648b7f2c 198 angle =! 0
simplyellow 0:d4bb648b7f2c 199 */
simplyellow 0:d4bb648b7f2c 200
simplyellow 0:d4bb648b7f2c 201 // for debugging
simplyellow 0:d4bb648b7f2c 202 pc.printf("Distance %f, Angle %f\n\r", totalDistance, angle*180/PI);
simplyellow 0:d4bb648b7f2c 203 pc.printf("(X,Y): %f %f\n\r", x, y);
simplyellow 0:d4bb648b7f2c 204 pc.printf("(DX,DY): %f %f\n\r", dx, dy);
simplyellow 0:d4bb648b7f2c 205 pc.printf("heading: %f\n\r", heading);
simplyellow 0:d4bb648b7f2c 206 pc.printf("----------------------------\n\r");
simplyellow 0:d4bb648b7f2c 207 }
simplyellow 0:d4bb648b7f2c 208 }
simplyellow 0:d4bb648b7f2c 209
simplyellow 0:d4bb648b7f2c 210