Terrabots / Mbed 2 deprecated TerraBot_Drive_2D_FINAL

2d drive position tracking (final draft) (CLEANED UP)

Dependencies:   MotorV2 QEI mbed

Fork of TerraBot_Drive_2D by Terrabots

main.cpp@2:e68fd2af041d, 2016-12-05 (annotated)

Committer:
simplyellow
Date:
Mon Dec 05 22:56:32 2016 +0000
Revision:
2:e68fd2af041d
Parent:
1:17db90f61f6c 
cleaned up final draft

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 void clear() {
simplyellow 1:17db90f61f6c 40 distance = 0;
simplyellow 1:17db90f61f6c 41 x = 0;
simplyellow 1:17db90f61f6c 42 y = 0;
simplyellow 1:17db90f61f6c 43 }
simplyellow 1:17db90f61f6c 44
simplyellow 1:17db90f61f6c 45 void potSetup() {
simplyellow 1:17db90f61f6c 46 zeroAngle = (float) pot;
simplyellow 1:17db90f61f6c 47 for(int i = 5; i > 0; i--) {
simplyellow 1:17db90f61f6c 48 pc.printf("%d seconds to set turning angle.\n\r", i);
simplyellow 1:17db90f61f6c 49 wait(1);
simplyellow 1:17db90f61f6c 50 }
simplyellow 1:17db90f61f6c 51 turnAngle = ((float) pot - zeroAngle) * 300;
simplyellow 1:17db90f61f6c 52 pc.printf("---------------------------------\n\r");
simplyellow 1:17db90f61f6c 53 //debug
simplyellow 1:17db90f61f6c 54 /*
simplyellow 1:17db90f61f6c 55 for(int i = 5; i > 0; i--) {
simplyellow 1:17db90f61f6c 56 pc.printf("%f is the turning Angle.\n\r", turnAngle);
simplyellow 1:17db90f61f6c 57 pc.printf("%f is the zero value.\n\r", zeroAngle);
simplyellow 1:17db90f61f6c 58 pc.printf("%f is the pot value.\n\r", (float) pot);
simplyellow 1:17db90f61f6c 59 wait(1);
simplyellow 1:17db90f61f6c 60 }*/
simplyellow 1:17db90f61f6c 61 }
simplyellow 0:d4bb648b7f2c 62
simplyellow 1:17db90f61f6c 63 void setDirection() {
simplyellow 1:17db90f61f6c 64 if (turnAngle > 0.5) { // +/- 0.5 leeway
simplyellow 1:17db90f61f6c 65 pc.printf("DIRECTION SET TO CCW with turnAngle %f\n\r", turnAngle);
simplyellow 1:17db90f61f6c 66 dir = CCW;
simplyellow 1:17db90f61f6c 67 pc.printf("%d is direction\n\r", dir);
simplyellow 1:17db90f61f6c 68 } else if (turnAngle < -0.5) {
simplyellow 1:17db90f61f6c 69 pc.printf("DIRECTION SET TO CW with turnAngle %f\n\r", turnAngle);
simplyellow 1:17db90f61f6c 70 dir = CW;
simplyellow 1:17db90f61f6c 71 } else {
simplyellow 1:17db90f61f6c 72 pc.printf("DIRECTION SET TO STRAIGHT with turnAngle %f\n\r", turnAngle);
simplyellow 1:17db90f61f6c 73 dir = STRAIGHT;
simplyellow 1:17db90f61f6c 74 }
simplyellow 1:17db90f61f6c 75 turnAngle = abs(turnAngle);
simplyellow 1:17db90f61f6c 76 }
simplyellow 0:d4bb648b7f2c 77
simplyellow 1:17db90f61f6c 78 void checkToStart() { // start tracking when distance > 0
simplyellow 1:17db90f61f6c 79 while(true) {
simplyellow 1:17db90f61f6c 80 pulseCount = (float) wheel.getPulses();
simplyellow 1:17db90f61f6c 81 distance = pulseCount*circumference/encoding/constant;
simplyellow 1:17db90f61f6c 82 if (distance > 0) {
simplyellow 1:17db90f61f6c 83 return;
simplyellow 1:17db90f61f6c 84 } else {
simplyellow 1:17db90f61f6c 85 wait(0.0001);
simplyellow 1:17db90f61f6c 86 }
simplyellow 1:17db90f61f6c 87 }
simplyellow 1:17db90f61f6c 88 }
simplyellow 0:d4bb648b7f2c 89
simplyellow 1:17db90f61f6c 90 void calcDisplacement() {
simplyellow 1:17db90f61f6c 91 pulseCount = (float) wheel.getPulses();
simplyellow 1:17db90f61f6c 92 distance = pulseCount*circumference/encoding/constant;
simplyellow 1:17db90f61f6c 93
simplyellow 1:17db90f61f6c 94 W = sqrt(df*df+db*db-2*df*db*cos(PI-turnAngle*PI/180));
simplyellow 1:17db90f61f6c 95 R = W/(2*sin(0.5*turnAngle*PI/180));
simplyellow 1:17db90f61f6c 96 dispAngle = distance/R;
simplyellow 0:d4bb648b7f2c 97
simplyellow 1:17db90f61f6c 98 if (dir == STRAIGHT) {
simplyellow 1:17db90f61f6c 99 x = 0;
simplyellow 1:17db90f61f6c 100 y = distance;
simplyellow 1:17db90f61f6c 101 } else if (dir == CCW) {
simplyellow 1:17db90f61f6c 102 x = -1*(R*(1-cos(dispAngle)));
simplyellow 1:17db90f61f6c 103 y = R*sin(dispAngle);
simplyellow 1:17db90f61f6c 104 } else if (dir == CW) {
simplyellow 1:17db90f61f6c 105 x = R*(1-cos(dispAngle));
simplyellow 1:17db90f61f6c 106 y = R*sin(dispAngle);
simplyellow 1:17db90f61f6c 107 } else {
simplyellow 1:17db90f61f6c 108 pc.printf("Direction not set. TERMINATED. \n\r");
simplyellow 1:17db90f61f6c 109 while(true) {}
simplyellow 1:17db90f61f6c 110 }
simplyellow 0:d4bb648b7f2c 111
simplyellow 1:17db90f61f6c 112 wait(.0001);
simplyellow 0:d4bb648b7f2c 113 }
simplyellow 0:d4bb648b7f2c 114
simplyellow 1:17db90f61f6c 115 int main() {
simplyellow 1:17db90f61f6c 116 clear();
simplyellow 1:17db90f61f6c 117 potSetup();
simplyellow 1:17db90f61f6c 118 setDirection();
simplyellow 1:17db90f61f6c 119 setQuadrant();
simplyellow 1:17db90f61f6c 120 checkToStart();
simplyellow 1:17db90f61f6c 121 while(true) {
simplyellow 1:17db90f61f6c 122 calcDisplacement();
simplyellow 1:17db90f61f6c 123 pc.printf("----------------------\n\r");
simplyellow 1:17db90f61f6c 124 pc.printf("x: %f\n\r", x);
simplyellow 1:17db90f61f6c 125 pc.printf("y: %f\n\r", y);
simplyellow 1:17db90f61f6c 126 pc.printf("distance: %f\n\r", distance);
simplyellow 1:17db90f61f6c 127 pc.printf("turnAngle: %f\n\r", turnAngle);
simplyellow 1:17db90f61f6c 128 pc.printf("dispAngle: %f\n\r", dispAngle);
simplyellow 1:17db90f61f6c 129 pc.printf("direction: %d\n\r", dir);
simplyellow 1:17db90f61f6c 130 pc.printf("quadrant: %d\n\r", quadrant);
simplyellow 1:17db90f61f6c 131 pc.printf("W: %f\n\r", W);
simplyellow 1:17db90f61f6c 132 pc.printf("R: %f\n\r", R);
simplyellow 0:d4bb648b7f2c 133 }
simplyellow 1:17db90f61f6c 134 }