merged EMG and PID codes
Dependencies: FastPWM HIDScope MODSERIAL QEI biquadFilter mbed
Diff: main.cpp
- Revision:
- 1:ffa6f4d78c8e
- Parent:
- 0:d02f4c7e8906
- Child:
- 2:625837aa7a56
--- a/main.cpp Wed Oct 26 08:45:24 2016 +0000 +++ b/main.cpp Wed Oct 26 10:46:50 2016 +0000 @@ -3,7 +3,7 @@ #include "MODSERIAL.h" #include "QEI.h" #include "HIDScope.h" //make sure hidscope cable is also attached -#include "BiQuad.h" + // in gebruik: D(0,1,4,5,6,7,10,11,12,13) @@ -32,8 +32,8 @@ const double pi=3.14159265359; const double res = 64/(1/131.25*2*pi); // resolution on gearbox shaft per pulse const double V_max=9.0; // maximum voltage supplied by trafo -const double minRadius=0.3; // minimum radius of arm -const double maxRadius=0.6; // maximum radius of arm +const double minRadius=0.0; // minimum radius of arm +const double maxRadius=0.3; // maximum radius of arm const double pulleyDiameter=0.0398; // pulley diameter const double minAngle=-1.25; // minimum angle for limiting controller @@ -61,30 +61,10 @@ } - void controller(){ //function for executing controller action - // convert ref to gearbox angle - double theta=atan(refy/refx); - double radius=sqrt(pow(refx,2)+pow(refy,2)); - - //limit theta - if (theta =< minAngle){ - theta=minAngle - } - else if (theta => 0){ - theta=0; - } - - //limit radius - if (radius =< minRadius){ - radius=minRadius - } - else if (radius > maxRadius){ - radius=maxRadius; - } - + void controller(){ //function for executing controller action //converting radius and theta to gearbox angle ref_angle1=16*theta; - ref_angle2=(-radius+minRadius)/pi/pulleyDiameter; + ref_angle2=-radius/pi/pulleyDiameter; angle1 = Encoder1.getPulses()/res; //get number of pulses (counterclockwise is positive) @@ -130,7 +110,7 @@ motor2.period(0.02f); // period of pwm signal for motor 2 motor1dir=0; // setting direction to ccw motor2dir=0; // setting direction to ccw - control.attach(&controller,m1_Ts); //Ticker for processing encoder input + control.attach(&activate_controller,m1_Ts); //Ticker for processing encoder input pc.printf("RESET\n\r"); while (true) { if(controller_go){