2d drive position tracking (final draft)
Dependencies: MotorV2 QEI mbed
Fork of TerraBot_Drive_2D by
main.cpp
- Committer:
- simplyellow
- Date:
- 2016-11-29
- Revision:
- 1:17db90f61f6c
- Parent:
- 0:d4bb648b7f2c
File content as of revision 1:17db90f61f6c:
#include "QEI.h" #include "mbed.h" #include "Motor.h" Serial pc(USBTX, USBRX); QEI wheel (p29, p30, NC, 128, QEI::X4_ENCODING); //orange, yellow wires, VU 5 VOLTS, BOTTOM 2x2 pins on WW02 float pulseCount; float encoding = 2; float circumference = 13.25; float constant = 1360; // constant = 20*68 float distance; float x; float y; AnalogIn pot(p15); float value; float turnAngle; float zeroAngle; float PI = 3.14159265; #define STRAIGHT 0 #define CCW 1 #define CW 2 int dir = 3; float df = 4.50; // distance from pivot to front wheel float db = 4.25; // distance from pivot to back wheel float W; float R; float dispAngle; #define Q1 1 #define Q2 2 #define Q3 3 #define Q4 4 int quadrant = 0; void clear() { distance = 0; x = 0; y = 0; } void potSetup() { zeroAngle = (float) pot; for(int i = 5; i > 0; i--) { pc.printf("%d seconds to set turning angle.\n\r", i); wait(1); } turnAngle = ((float) pot - zeroAngle) * 300; pc.printf("---------------------------------\n\r"); //debug /* for(int i = 5; i > 0; i--) { pc.printf("%f is the turning Angle.\n\r", turnAngle); pc.printf("%f is the zero value.\n\r", zeroAngle); pc.printf("%f is the pot value.\n\r", (float) pot); wait(1); }*/ } void setDirection() { if (turnAngle > 0.5) { // +/- 0.5 leeway pc.printf("DIRECTION SET TO CCW with turnAngle %f\n\r", turnAngle); dir = CCW; pc.printf("%d is direction\n\r", dir); } else if (turnAngle < -0.5) { pc.printf("DIRECTION SET TO CW with turnAngle %f\n\r", turnAngle); dir = CW; } else { pc.printf("DIRECTION SET TO STRAIGHT with turnAngle %f\n\r", turnAngle); dir = STRAIGHT; } turnAngle = abs(turnAngle); } void setQuadrant() { if ((turnAngle > 0) && (turnAngle < 90)) { quadrant = Q1; } else if ((turnAngle > 90) && (turnAngle < 180)) { quadrant = Q2; } else if ((turnAngle > 180) && (turnAngle < 270)) { quadrant = Q3; } else if ((turnAngle > 270) && (turnAngle < 360)) { quadrant = Q4; } else if (turnAngle == 0) { //don't worry } else { pc.printf("turnAngle not valid. TERMINATED. \n\r"); while(true) {} } } void checkToStart() { // start tracking when distance > 0 while(true) { pulseCount = (float) wheel.getPulses(); distance = pulseCount*circumference/encoding/constant; if (distance > 0) { return; } else { wait(0.0001); } } } /* AXES when robot is facing forward forward is +x left is +y */ void calcDisplacement() { pulseCount = (float) wheel.getPulses(); distance = pulseCount*circumference/encoding/constant; W = sqrt(df*df+db*db-2*df*db*cos(PI-turnAngle*PI/180)); R = W/(2*sin(0.5*turnAngle*PI/180)); dispAngle = distance/R; if (dir == STRAIGHT) { x = 0; y = distance; } else if (dir == CCW) { x = -1*(R*(1-cos(dispAngle))); y = R*sin(dispAngle); /* switch(quadrant) { case Q1: x = R*(cos(dispAngle)-1); y = R*sin(dispAngle); break; case Q2: x = -1*R*sin(dispAngle); y = R*(cos(dispAngle)-1); break; case Q3: x = R*(cos(dispAngle)-1); y = -1*R*sin(dispAngle); break; case Q4: x = R*sin(dispAngle); y = R*(1-cos(dispAngle)); break; default: pc.printf("default \n\r"); }*/ } else if (dir == CW) { x = R*(1-cos(dispAngle)); y = R*sin(dispAngle); /* switch(quadrant) { case Q1: x = R*sin(dispAngle); y = R*(cos(dispAngle)-1); break; case Q2: x = R*(cos(dispAngle)-1); y = R*sin(dispAngle); break; case Q3: x = -1*R*sin(dispAngle); y = R*(1-cos(dispAngle)); break; case Q4: x = R*(cos(dispAngle)-1); y = -1*R*sin(dispAngle); break; default: pc.printf("default \n\r"); }*/ } else { pc.printf("Direction not set. TERMINATED. \n\r"); while(true) {} } wait(.0001); } int main() { clear(); potSetup(); setDirection(); setQuadrant(); checkToStart(); while(true) { calcDisplacement(); pc.printf("----------------------\n\r"); pc.printf("x: %f\n\r", x); pc.printf("y: %f\n\r", y); pc.printf("distance: %f\n\r", distance); pc.printf("turnAngle: %f\n\r", turnAngle); pc.printf("dispAngle: %f\n\r", dispAngle); pc.printf("direction: %d\n\r", dir); pc.printf("quadrant: %d\n\r", quadrant); pc.printf("W: %f\n\r", W); pc.printf("R: %f\n\r", R); } }