Sam Leighton / Navigate_Pololu

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

Navigate_Pololu.cpp@1:b6c1de65e591, 2016-01-31 (annotated)

Committer:
sleighton
Date:
Sun Jan 31 16:08:34 2016 +0000
Revision:
1:b6c1de65e591
Parent:
0:33c364521d16
Child:
2:1255feea1c59
Sends location above proximity threshold

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 1:b6c1de65e591 6 Navigate_Pololu::Navigate_Pololu(PinName leftIn, PinName rightIn, int countsPerRevolution, float wheelDiameter, float trackWidth,PinName robotTx, PinName robotRx):leftOpto(leftIn),rightOpto(rightIn){
sleighton 1:b6c1de65e591 7 calibratePos(robotTx, robotRx); //call routine to calibrate wheels
sleighton 0:33c364521d16 8 X = 0.0f;
sleighton 0:33c364521d16 9 Y = 0.0f;
sleighton 0:33c364521d16 10 heading = 0.0f;
sleighton 0:33c364521d16 11 leftCounts = 0;
sleighton 0:33c364521d16 12 rightCounts = 0;
sleighton 0:33c364521d16 13 previousLeftCounts = 0;
sleighton 0:33c364521d16 14 previousRightCounts = 0;
sleighton 0:33c364521d16 15 distancePerCount = (PI * wheelDiameter) / (float)countsPerRevolution;
sleighton 0:33c364521d16 16 radiansPerCount = PI * (wheelDiameter/trackWidth)/countsPerRevolution;
sleighton 0:33c364521d16 17 leftOpto.rise(this, &Navigate_Pololu::leftWheelInterrupt);
sleighton 0:33c364521d16 18 rightOpto.rise(this, &Navigate_Pololu::rightWheelInterrupt);
sleighton 0:33c364521d16 19 printf("DistancePerCount: %f, RadiansPerCounts: %f\n",distancePerCount,radiansPerCount);
sleighton 0:33c364521d16 20 }
sleighton 0:33c364521d16 21
sleighton 0:33c364521d16 22 void Navigate_Pololu::leftWheelInterrupt(){
sleighton 0:33c364521d16 23 leftCounts++;
sleighton 0:33c364521d16 24 UpdatePosition();
sleighton 0:33c364521d16 25 previousLeftCounts = leftCounts;
sleighton 0:33c364521d16 26 }
sleighton 0:33c364521d16 27
sleighton 0:33c364521d16 28 void Navigate_Pololu::rightWheelInterrupt(){
sleighton 0:33c364521d16 29 rightCounts++;
sleighton 0:33c364521d16 30 UpdatePosition();
sleighton 0:33c364521d16 31 previousRightCounts = rightCounts;
sleighton 0:33c364521d16 32 }
sleighton 0:33c364521d16 33
sleighton 0:33c364521d16 34 void Navigate_Pololu::UpdatePosition(){
sleighton 0:33c364521d16 35 deltaLeft = leftCounts - previousLeftCounts;
sleighton 0:33c364521d16 36 deltaRight = rightCounts - previousRightCounts;
sleighton 0:33c364521d16 37 deltaDistance = 0.5f * (float)(deltaLeft + deltaRight) * distancePerCount;
sleighton 0:33c364521d16 38 deltaHeading = (float) (deltaRight - deltaLeft) * radiansPerCount;
sleighton 0:33c364521d16 39 heading += deltaHeading;
sleighton 0:33c364521d16 40 deltaX = deltaDistance * (float)cos(heading);
sleighton 0:33c364521d16 41 deltaY = deltaDistance * (float)sin(heading);
sleighton 0:33c364521d16 42 X += deltaX;
sleighton 0:33c364521d16 43 Y += deltaY;
sleighton 0:33c364521d16 44 if(heading>PI)
sleighton 0:33c364521d16 45 heading -= TWO_PI;
sleighton 0:33c364521d16 46 else if (heading <= -PI)
sleighton 0:33c364521d16 47 heading += TWO_PI;
sleighton 0:33c364521d16 48 printf("X = %f, Y = %f, Heading = %f\n",X,Y,heading);
sleighton 0:33c364521d16 49 }
sleighton 0:33c364521d16 50
sleighton 1:b6c1de65e591 51 int Navigate_Pololu::getX(){
sleighton 1:b6c1de65e591 52 return (int)X;
sleighton 1:b6c1de65e591 53 }
sleighton 1:b6c1de65e591 54
sleighton 1:b6c1de65e591 55 int Navigate_Pololu::getY(){
sleighton 1:b6c1de65e591 56 return (int)Y;
sleighton 1:b6c1de65e591 57 }
sleighton 1:b6c1de65e591 58
sleighton 1:b6c1de65e591 59 int Navigate_Pololu::getHeading(){
sleighton 1:b6c1de65e591 60 return (int)((heading/PI)*180);
sleighton 0:33c364521d16 61 }
sleighton 0:33c364521d16 62
sleighton 1:b6c1de65e591 63 void Navigate_Pololu::calibratePos(PinName robotTx,PinName robotRx){
sleighton 1:b6c1de65e591 64 m3pi robot(D7, robotTx, robotRx);
sleighton 1:b6c1de65e591 65 wait(1);
sleighton 1:b6c1de65e591 66 while(leftOpto){
sleighton 1:b6c1de65e591 67 robot.left_motor(0.2);
sleighton 1:b6c1de65e591 68 wait(0.01);
sleighton 1:b6c1de65e591 69 }
sleighton 1:b6c1de65e591 70 robot.stop();
sleighton 1:b6c1de65e591 71 wait(0.1);
sleighton 1:b6c1de65e591 72 while(rightOpto){
sleighton 1:b6c1de65e591 73 robot.right_motor(0.2);
sleighton 1:b6c1de65e591 74 wait(0.01);
sleighton 1:b6c1de65e591 75 }
sleighton 1:b6c1de65e591 76 robot.stop();
sleighton 1:b6c1de65e591 77 }