code to drive the motors to the right position
Dependencies: HIDScope QEI mbed
Fork of BMT-K9_potmeter_fade by
Diff: Point.cpp
- Revision:
- 6:d0f5da9962f5
diff -r edac3771ede4 -r d0f5da9962f5 Point.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Point.cpp Fri Sep 25 15:04:00 2015 +0000 @@ -0,0 +1,57 @@ +// Example: data structs and functions +#include <stdio.h> +#include <math.h> + +const double M_PI =3.141592653589793238463; +const float l = 10; // distance between the motors +const float armlength=15; // length of the arms from the motor + +class Point +{ + public: + + float posX; + float posY; + float rotA; + float rotB; + + bool fromCarthesian(float x, float y) + { + posX = x; + posY = y; + rotA = atan(y/x)*180/M_PI; + rotB = atan(y/(l-x))*180/M_PI; + + // function is done, return the struct type Point: + return true; + } + bool fromRotational(float a, float b) + { + rotA = a*M_PI/180; + rotB = b*M_PI/180; + posX = (tan(rotB)*l)/(tan(rotA)+tan(rotB)); + posY = tan(rotA)*posX; + + return true; + + } + bool checkbounds() + { + if (rotA <= 0 or rotB <= 0){ + return 0; + } + if (rotA > 90 or rotB > 90){ + return 0; + } + if (sqrt(pow(posX,2)+pow(posY,2)) > armlength){ // too far from left arm + return 0; + } + if (sqrt(pow(l-posX,2)+pow(posY,2)) > armlength){ // too far from right arm + return 0; + } + return true; + } +}; + + +