Sam Leighton / Navigate_Pololu

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

Navigate_Pololu.cpp@0:33c364521d16, 2016-01-30 (annotated)

Committer:
sleighton
Date:
Sat Jan 30 20:26:14 2016 +0000
Revision:
0:33c364521d16
Child:
1:b6c1de65e591
Returns array

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sleighton 0:33c364521d16 1 #include "Navigate_Pololu.h"
sleighton 0:33c364521d16 2 #include <cstring>
sleighton 0:33c364521d16 3
sleighton 0:33c364521d16 4 Navigate_Pololu::Navigate_Pololu(PinName leftIn, PinName rightIn, int countsPerRevolution, float wheelDiameter, float trackWidth):leftOpto(leftIn),rightOpto(rightIn){
sleighton 0:33c364521d16 5 X = 0.0f;
sleighton 0:33c364521d16 6 Y = 0.0f;
sleighton 0:33c364521d16 7 heading = 0.0f;
sleighton 0:33c364521d16 8 leftCounts = 0;
sleighton 0:33c364521d16 9 rightCounts = 0;
sleighton 0:33c364521d16 10 previousLeftCounts = 0;
sleighton 0:33c364521d16 11 previousRightCounts = 0;
sleighton 0:33c364521d16 12 distancePerCount = (PI * wheelDiameter) / (float)countsPerRevolution;
sleighton 0:33c364521d16 13 radiansPerCount = PI * (wheelDiameter/trackWidth)/countsPerRevolution;
sleighton 0:33c364521d16 14 leftOpto.rise(this, &Navigate_Pololu::leftWheelInterrupt);
sleighton 0:33c364521d16 15 rightOpto.rise(this, &Navigate_Pololu::rightWheelInterrupt);
sleighton 0:33c364521d16 16 printf("DistancePerCount: %f, RadiansPerCounts: %f\n",distancePerCount,radiansPerCount);
sleighton 0:33c364521d16 17 }
sleighton 0:33c364521d16 18
sleighton 0:33c364521d16 19 void Navigate_Pololu::leftWheelInterrupt(){
sleighton 0:33c364521d16 20 leftCounts++;
sleighton 0:33c364521d16 21 UpdatePosition();
sleighton 0:33c364521d16 22 previousLeftCounts = leftCounts;
sleighton 0:33c364521d16 23 }
sleighton 0:33c364521d16 24
sleighton 0:33c364521d16 25 void Navigate_Pololu::rightWheelInterrupt(){
sleighton 0:33c364521d16 26 rightCounts++;
sleighton 0:33c364521d16 27 UpdatePosition();
sleighton 0:33c364521d16 28 previousRightCounts = rightCounts;
sleighton 0:33c364521d16 29 }
sleighton 0:33c364521d16 30
sleighton 0:33c364521d16 31 void Navigate_Pololu::UpdatePosition(){
sleighton 0:33c364521d16 32 deltaLeft = leftCounts - previousLeftCounts;
sleighton 0:33c364521d16 33 deltaRight = rightCounts - previousRightCounts;
sleighton 0:33c364521d16 34 deltaDistance = 0.5f * (float)(deltaLeft + deltaRight) * distancePerCount;
sleighton 0:33c364521d16 35 deltaHeading = (float) (deltaRight - deltaLeft) * radiansPerCount;
sleighton 0:33c364521d16 36 heading += deltaHeading;
sleighton 0:33c364521d16 37 deltaX = deltaDistance * (float)cos(heading);
sleighton 0:33c364521d16 38 deltaY = deltaDistance * (float)sin(heading);
sleighton 0:33c364521d16 39 X += deltaX;
sleighton 0:33c364521d16 40 Y += deltaY;
sleighton 0:33c364521d16 41 if(heading>PI)
sleighton 0:33c364521d16 42 heading -= TWO_PI;
sleighton 0:33c364521d16 43 else if (heading <= -PI)
sleighton 0:33c364521d16 44 heading += TWO_PI;
sleighton 0:33c364521d16 45 printf("X = %f, Y = %f, Heading = %f\n",X,Y,heading);
sleighton 0:33c364521d16 46 }
sleighton 0:33c364521d16 47
sleighton 0:33c364521d16 48 uint8_t* Navigate_Pololu::getX(){
sleighton 0:33c364521d16 49 //static_assert(sizeof(float) == 2);
sleighton 0:33c364521d16 50 int f = 0; // whatever value
sleighton 0:33c364521d16 51 uint8_t bytes[4];
sleighton 0:33c364521d16 52 std::memcpy(bytes, &X,4);
sleighton 0:33c364521d16 53 return bytes;
sleighton 0:33c364521d16 54 }
sleighton 0:33c364521d16 55
sleighton 0:33c364521d16 56 uint8_t* Navigate_Pololu::getY(){
sleighton 0:33c364521d16 57 //static_assert(sizeof(float) == 2);
sleighton 0:33c364521d16 58 float f = 0; // whatever value
sleighton 0:33c364521d16 59 uint8_t bytes[4];
sleighton 0:33c364521d16 60 std::memcpy(bytes, &Y,4);
sleighton 0:33c364521d16 61 return bytes;
sleighton 0:33c364521d16 62 }
sleighton 0:33c364521d16 63
sleighton 0:33c364521d16 64 uint8_t* Navigate_Pololu::getHeading(){
sleighton 0:33c364521d16 65 //static_assert(sizeof(float) == 2);
sleighton 0:33c364521d16 66 float f = 0; // whatever value
sleighton 0:33c364521d16 67 uint8_t bytes[4];
sleighton 0:33c364521d16 68 std::memcpy(bytes, &heading,4);
sleighton 0:33c364521d16 69 return bytes;
sleighton 0:33c364521d16 70 }
sleighton 0:33c364521d16 71