motor controller with P velocity control
Dependencies: HIDScope PID QEI mbed EMG
Fork of PID_VelocityExample by
Point.cpp
- Committer:
- ewoud
- Date:
- 2015-10-12
- Revision:
- 22:ae8012b12890
- Parent:
- 9:07189a75e979
- Child:
- 14:102a2b4f5c86
File content as of revision 22:ae8012b12890:
// 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; } };