Sam Leighton / Navigate_Pololu

Used to track the robot's position and initialise an m3pi object to drive the robot

Navigate_Pololu.cpp@2:1255feea1c59, 2016-01-31 (annotated)

Committer:
sleighton
Date:
Sun Jan 31 21:02:13 2016 +0000
Revision:
2:1255feea1c59
Parent:
1:b6c1de65e591
Child:
3:c5dff598fea4
Includes m3pi object and decrements wheel count if direction is negative

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sleighton 0:33c364521d16 1 #include "Navigate_Pololu.h"
sleighton 1:b6c1de65e591 2 #include "mbed.h"
sleighton 0:33c364521d16 3 #include <cstring>
sleighton 1:b6c1de65e591 4 #include "m3pi.h"
sleighton 0:33c364521d16 5
sleighton 2:1255feea1c59 6 Navigate_Pololu::Navigate_Pololu(PinName leftIn, PinName rightIn, int countsPerRevolution, float wheelDiameter, float trackWidth,PinName robotTx, PinName robotRx):leftOpto(leftIn),rightOpto(rightIn),robot(D7,robotTx,robotRx){
sleighton 2:1255feea1c59 7 calibratePos(); //call routine to calibrate wheels
sleighton 2:1255feea1c59 8 X = 0.0f; //set x coordinate to zero
sleighton 2:1255feea1c59 9 Y = 0.0f; //set y coordinate to zero
sleighton 2:1255feea1c59 10 heading = 0.0f; //set heading to zero
sleighton 2:1255feea1c59 11 leftCounts = 0; //reset counter for left wheel
sleighton 2:1255feea1c59 12 rightCounts = 0; //reset counter for right wheel
sleighton 0:33c364521d16 13 previousLeftCounts = 0;
sleighton 0:33c364521d16 14 previousRightCounts = 0;
sleighton 2:1255feea1c59 15 distancePerCount = (PI * wheelDiameter) / (float)countsPerRevolution; //set variable
sleighton 2:1255feea1c59 16 radiansPerCount = PI * (wheelDiameter/trackWidth)/countsPerRevolution; //set variable
sleighton 2:1255feea1c59 17 leftOpto.rise(this, &Navigate_Pololu::leftWheelInterrupt); //create interrupt for left opto switch
sleighton 2:1255feea1c59 18 rightOpto.rise(this, &Navigate_Pololu::rightWheelInterrupt); //create interrupt for right opto switch
sleighton 2:1255feea1c59 19 printf("DistancePerCount: %f, RadiansPerCounts: %f\n",distancePerCount,radiansPerCount); //print out initial setup variables
sleighton 0:33c364521d16 20 }
sleighton 0:33c364521d16 21
sleighton 0:33c364521d16 22 void Navigate_Pololu::leftWheelInterrupt(){
sleighton 2:1255feea1c59 23 if(robot.leftDir == 1)
sleighton 2:1255feea1c59 24 leftCounts++;
sleighton 2:1255feea1c59 25 else
sleighton 2:1255feea1c59 26 leftCounts--;
sleighton 2:1255feea1c59 27 UpdatePosition(); //recalculate position
sleighton 0:33c364521d16 28 previousLeftCounts = leftCounts;
sleighton 0:33c364521d16 29 }
sleighton 0:33c364521d16 30
sleighton 0:33c364521d16 31 void Navigate_Pololu::rightWheelInterrupt(){
sleighton 2:1255feea1c59 32 if(robot.rightDir == 1)
sleighton 2:1255feea1c59 33 rightCounts++;
sleighton 2:1255feea1c59 34 else
sleighton 2:1255feea1c59 35 rightCounts--;
sleighton 2:1255feea1c59 36 UpdatePosition(); //recalculate position
sleighton 0:33c364521d16 37 previousRightCounts = rightCounts;
sleighton 0:33c364521d16 38 }
sleighton 0:33c364521d16 39
sleighton 0:33c364521d16 40 void Navigate_Pololu::UpdatePosition(){
sleighton 0:33c364521d16 41 deltaLeft = leftCounts - previousLeftCounts;
sleighton 0:33c364521d16 42 deltaRight = rightCounts - previousRightCounts;
sleighton 0:33c364521d16 43 deltaDistance = 0.5f * (float)(deltaLeft + deltaRight) * distancePerCount;
sleighton 0:33c364521d16 44 deltaHeading = (float) (deltaRight - deltaLeft) * radiansPerCount;
sleighton 2:1255feea1c59 45 heading += deltaHeading; //add deltaHeading to running total
sleighton 2:1255feea1c59 46 deltaX = deltaDistance * (float)cos(heading); //convert from polar to cartesian coordinates
sleighton 0:33c364521d16 47 deltaY = deltaDistance * (float)sin(heading);
sleighton 2:1255feea1c59 48 X += deltaX; //add to running coordinate totals
sleighton 0:33c364521d16 49 Y += deltaY;
sleighton 2:1255feea1c59 50 if(heading>PI) //keep heading between pi and -pi
sleighton 0:33c364521d16 51 heading -= TWO_PI;
sleighton 0:33c364521d16 52 else if (heading <= -PI)
sleighton 0:33c364521d16 53 heading += TWO_PI;
sleighton 2:1255feea1c59 54 printf("X = %f, Y = %f, Heading = %f\n",X,Y,heading); //print coorindates after update
sleighton 0:33c364521d16 55 }
sleighton 0:33c364521d16 56
sleighton 1:b6c1de65e591 57 int Navigate_Pololu::getX(){
sleighton 1:b6c1de65e591 58 return (int)X;
sleighton 1:b6c1de65e591 59 }
sleighton 1:b6c1de65e591 60
sleighton 1:b6c1de65e591 61 int Navigate_Pololu::getY(){
sleighton 1:b6c1de65e591 62 return (int)Y;
sleighton 1:b6c1de65e591 63 }
sleighton 1:b6c1de65e591 64
sleighton 1:b6c1de65e591 65 int Navigate_Pololu::getHeading(){
sleighton 2:1255feea1c59 66 return (int)((heading/PI)*180); //returns heading in degrees
sleighton 0:33c364521d16 67 }
sleighton 0:33c364521d16 68
sleighton 2:1255feea1c59 69 void Navigate_Pololu::calibratePos(){
sleighton 2:1255feea1c59 70 //m3pi robot(D7, robotTx, robotRx); //create new m3pi object with D7 unused pin for reset
sleighton 1:b6c1de65e591 71 wait(1);
sleighton 2:1255feea1c59 72 //align wheels
sleighton 1:b6c1de65e591 73 while(leftOpto){
sleighton 1:b6c1de65e591 74 robot.left_motor(0.2);
sleighton 1:b6c1de65e591 75 wait(0.01);
sleighton 1:b6c1de65e591 76 }
sleighton 2:1255feea1c59 77 robot.stop();
sleighton 2:1255feea1c59 78 wait(0.1);
sleighton 1:b6c1de65e591 79 while(rightOpto){
sleighton 2:1255feea1c59 80 robot.right_motor(0.2);
sleighton 2:1255feea1c59 81 wait(0.01);
sleighton 2:1255feea1c59 82 }
sleighton 2:1255feea1c59 83 robot.stop();
sleighton 1:b6c1de65e591 84 }