Numero Uno / Mbed 2 deprecated MotorDriver

code to drive the motors to the right position

Dependencies:   HIDScope QEI mbed

Fork of BMT-K9_potmeter_fade by First Last

Point.cpp@6:d0f5da9962f5, 2015-09-25 (annotated)

Committer:
ewoud
Date:
Fri Sep 25 15:04:00 2015 +0000
Revision:
6:d0f5da9962f5
can now calculate the desired velocities from user input and position of the encoder

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ewoud 6:d0f5da9962f5 1 // Example: data structs and functions
ewoud 6:d0f5da9962f5 2 #include <stdio.h>
ewoud 6:d0f5da9962f5 3 #include <math.h>
ewoud 6:d0f5da9962f5 4
ewoud 6:d0f5da9962f5 5 const double M_PI =3.141592653589793238463;
ewoud 6:d0f5da9962f5 6 const float l = 10; // distance between the motors
ewoud 6:d0f5da9962f5 7 const float armlength=15; // length of the arms from the motor
ewoud 6:d0f5da9962f5 8
ewoud 6:d0f5da9962f5 9 class Point
ewoud 6:d0f5da9962f5 10 {
ewoud 6:d0f5da9962f5 11 public:
ewoud 6:d0f5da9962f5 12
ewoud 6:d0f5da9962f5 13 float posX;
ewoud 6:d0f5da9962f5 14 float posY;
ewoud 6:d0f5da9962f5 15 float rotA;
ewoud 6:d0f5da9962f5 16 float rotB;
ewoud 6:d0f5da9962f5 17
ewoud 6:d0f5da9962f5 18 bool fromCarthesian(float x, float y)
ewoud 6:d0f5da9962f5 19 {
ewoud 6:d0f5da9962f5 20 posX = x;
ewoud 6:d0f5da9962f5 21 posY = y;
ewoud 6:d0f5da9962f5 22 rotA = atan(y/x)*180/M_PI;
ewoud 6:d0f5da9962f5 23 rotB = atan(y/(l-x))*180/M_PI;
ewoud 6:d0f5da9962f5 24
ewoud 6:d0f5da9962f5 25 // function is done, return the struct type Point:
ewoud 6:d0f5da9962f5 26 return true;
ewoud 6:d0f5da9962f5 27 }
ewoud 6:d0f5da9962f5 28 bool fromRotational(float a, float b)
ewoud 6:d0f5da9962f5 29 {
ewoud 6:d0f5da9962f5 30 rotA = a*M_PI/180;
ewoud 6:d0f5da9962f5 31 rotB = b*M_PI/180;
ewoud 6:d0f5da9962f5 32 posX = (tan(rotB)*l)/(tan(rotA)+tan(rotB));
ewoud 6:d0f5da9962f5 33 posY = tan(rotA)*posX;
ewoud 6:d0f5da9962f5 34
ewoud 6:d0f5da9962f5 35 return true;
ewoud 6:d0f5da9962f5 36
ewoud 6:d0f5da9962f5 37 }
ewoud 6:d0f5da9962f5 38 bool checkbounds()
ewoud 6:d0f5da9962f5 39 {
ewoud 6:d0f5da9962f5 40 if (rotA <= 0 or rotB <= 0){
ewoud 6:d0f5da9962f5 41 return 0;
ewoud 6:d0f5da9962f5 42 }
ewoud 6:d0f5da9962f5 43 if (rotA > 90 or rotB > 90){
ewoud 6:d0f5da9962f5 44 return 0;
ewoud 6:d0f5da9962f5 45 }
ewoud 6:d0f5da9962f5 46 if (sqrt(pow(posX,2)+pow(posY,2)) > armlength){ // too far from left arm
ewoud 6:d0f5da9962f5 47 return 0;
ewoud 6:d0f5da9962f5 48 }
ewoud 6:d0f5da9962f5 49 if (sqrt(pow(l-posX,2)+pow(posY,2)) > armlength){ // too far from right arm
ewoud 6:d0f5da9962f5 50 return 0;
ewoud 6:d0f5da9962f5 51 }
ewoud 6:d0f5da9962f5 52 return true;
ewoud 6:d0f5da9962f5 53 }
ewoud 6:d0f5da9962f5 54 };
ewoud 6:d0f5da9962f5 55
ewoud 6:d0f5da9962f5 56
ewoud 6:d0f5da9962f5 57