Sam Leighton / Navigate_Pololu

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

Navigate_Pololu.cpp@3:c5dff598fea4, 2016-03-04 (annotated)

Committer:
sleighton
Date:
Fri Mar 04 18:46:26 2016 +0000
Revision:
3:c5dff598fea4
Parent:
2:1255feea1c59 
Before Tidy up

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 3:c5dff598fea4 15 //take out pi on distance per count and change to two_pi on raidanspercount
sleighton 2:1255feea1c59 16 distancePerCount = (PI * wheelDiameter) / (float)countsPerRevolution; //set variable
sleighton 2:1255feea1c59 17 radiansPerCount = PI * (wheelDiameter/trackWidth)/countsPerRevolution; //set variable
sleighton 2:1255feea1c59 18 leftOpto.rise(this, &Navigate_Pololu::leftWheelInterrupt); //create interrupt for left opto switch
sleighton 2:1255feea1c59 19 rightOpto.rise(this, &Navigate_Pololu::rightWheelInterrupt); //create interrupt for right opto switch
sleighton 2:1255feea1c59 20 printf("DistancePerCount: %f, RadiansPerCounts: %f\n",distancePerCount,radiansPerCount); //print out initial setup variables
sleighton 0:33c364521d16 21 }
sleighton 0:33c364521d16 22
sleighton 0:33c364521d16 23 void Navigate_Pololu::leftWheelInterrupt(){
sleighton 3:c5dff598fea4 24 if(robot.leftDir == 1)//check direction of left motor and increment or decrement count accordingly
sleighton 2:1255feea1c59 25 leftCounts++;
sleighton 2:1255feea1c59 26 else
sleighton 2:1255feea1c59 27 leftCounts--;
sleighton 2:1255feea1c59 28 UpdatePosition(); //recalculate position
sleighton 3:c5dff598fea4 29 //printf("Left: %d\n",leftCounts); //testing use
sleighton 0:33c364521d16 30 previousLeftCounts = leftCounts;
sleighton 0:33c364521d16 31 }
sleighton 0:33c364521d16 32
sleighton 0:33c364521d16 33 void Navigate_Pololu::rightWheelInterrupt(){
sleighton 3:c5dff598fea4 34 if(robot.rightDir == 1)//check direction of right motor and increment or decrement count accordingly
sleighton 2:1255feea1c59 35 rightCounts++;
sleighton 2:1255feea1c59 36 else
sleighton 2:1255feea1c59 37 rightCounts--;
sleighton 2:1255feea1c59 38 UpdatePosition(); //recalculate position
sleighton 3:c5dff598fea4 39 //printf("Right: %d\n",rightCounts); //testing use
sleighton 0:33c364521d16 40 previousRightCounts = rightCounts;
sleighton 0:33c364521d16 41 }
sleighton 0:33c364521d16 42
sleighton 0:33c364521d16 43 void Navigate_Pololu::UpdatePosition(){
sleighton 0:33c364521d16 44 deltaLeft = leftCounts - previousLeftCounts;
sleighton 0:33c364521d16 45 deltaRight = rightCounts - previousRightCounts;
sleighton 0:33c364521d16 46 deltaDistance = 0.5f * (float)(deltaLeft + deltaRight) * distancePerCount;
sleighton 0:33c364521d16 47 deltaHeading = (float) (deltaRight - deltaLeft) * radiansPerCount;
sleighton 2:1255feea1c59 48 heading += deltaHeading; //add deltaHeading to running total
sleighton 2:1255feea1c59 49 deltaX = deltaDistance * (float)cos(heading); //convert from polar to cartesian coordinates
sleighton 0:33c364521d16 50 deltaY = deltaDistance * (float)sin(heading);
sleighton 2:1255feea1c59 51 X += deltaX; //add to running coordinate totals
sleighton 0:33c364521d16 52 Y += deltaY;
sleighton 2:1255feea1c59 53 if(heading>PI) //keep heading between pi and -pi
sleighton 0:33c364521d16 54 heading -= TWO_PI;
sleighton 0:33c364521d16 55 else if (heading <= -PI)
sleighton 0:33c364521d16 56 heading += TWO_PI;
sleighton 3:c5dff598fea4 57 //printf("X = %f, Y = %f, Heading = %f\n",X,Y,heading); //print coorindates after update
sleighton 0:33c364521d16 58 }
sleighton 0:33c364521d16 59
sleighton 1:b6c1de65e591 60 int Navigate_Pololu::getX(){
sleighton 1:b6c1de65e591 61 return (int)X;
sleighton 1:b6c1de65e591 62 }
sleighton 1:b6c1de65e591 63
sleighton 1:b6c1de65e591 64 int Navigate_Pololu::getY(){
sleighton 1:b6c1de65e591 65 return (int)Y;
sleighton 1:b6c1de65e591 66 }
sleighton 1:b6c1de65e591 67
sleighton 1:b6c1de65e591 68 int Navigate_Pololu::getHeading(){
sleighton 2:1255feea1c59 69 return (int)((heading/PI)*180); //returns heading in degrees
sleighton 0:33c364521d16 70 }
sleighton 0:33c364521d16 71
sleighton 2:1255feea1c59 72 void Navigate_Pololu::calibratePos(){
sleighton 2:1255feea1c59 73 //m3pi robot(D7, robotTx, robotRx); //create new m3pi object with D7 unused pin for reset
sleighton 1:b6c1de65e591 74 wait(1);
sleighton 2:1255feea1c59 75 //align wheels
sleighton 1:b6c1de65e591 76 while(leftOpto){
sleighton 1:b6c1de65e591 77 robot.left_motor(0.2);
sleighton 1:b6c1de65e591 78 wait(0.01);
sleighton 1:b6c1de65e591 79 }
sleighton 2:1255feea1c59 80 robot.stop();
sleighton 2:1255feea1c59 81 wait(0.1);
sleighton 1:b6c1de65e591 82 while(rightOpto){
sleighton 2:1255feea1c59 83 robot.right_motor(0.2);
sleighton 2:1255feea1c59 84 wait(0.01);
sleighton 2:1255feea1c59 85 }
sleighton 2:1255feea1c59 86 robot.stop();
sleighton 3:c5dff598fea4 87 }
sleighton 3:c5dff598fea4 88
sleighton 3:c5dff598fea4 89 void Navigate_Pololu::resetCoordinates(){
sleighton 3:c5dff598fea4 90 X = 0.0f; //reset x coordinate
sleighton 3:c5dff598fea4 91 Y = 0.0f; //reset y coordinate
sleighton 1:b6c1de65e591 92 }