code to drive the motors to the right position
Dependencies: HIDScope QEI mbed
Fork of BMT-K9_potmeter_fade by
Point.cpp
- Committer:
- ewoud
- Date:
- 2015-09-25
- Revision:
- 6:d0f5da9962f5
File content as of revision 6:d0f5da9962f5:
// 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; } };