Control up to two motors using filtered EMG signals and a PID controller
Dependencies: FastPWM HIDScope MODSERIAL QEI Matrix biquadFilter controller errorFetch mbed motorConfig refGen MatrixMath inverseKinematics
Fork of Minor_test_serial by
Diff: main.cpp
- Revision:
- 33:6f4858b98fe5
- Parent:
- 32:1bb406d2b3c3
- Child:
- 34:1a70aa045c8f
diff -r 1bb406d2b3c3 -r 6f4858b98fe5 main.cpp --- a/main.cpp Fri Oct 20 12:24:37 2017 +0000 +++ b/main.cpp Fri Oct 20 16:30:26 2017 +0000 @@ -1,3 +1,4 @@ +#include <vector> #include "mbed.h" #include "MODSERIAL.h" #include "HIDScope.h" @@ -8,14 +9,30 @@ #include "motorConfig.h" #include "errorFetch.h" -// Defining relevant constant parameters -const float gearRatio = 131; +// ADJUSTABLE PARAMETERS // Controller parameters const float k_p = 1; const float k_i = 0; // Still needs a reasonable value const float k_d = 0; // Again, still need to pick a reasonable value +// Defining motor gear ratio - for BOTH motors as this is the same in the current configuration +const float gearRatio = 131; + + + +// Declaring a controller ticker and volatile variables to store encoder counts and revs +Ticker controllerTicker; +volatile int m1counts = 0; +volatile int m2counts = 0; +volatile float m1revs = 0.00; +volatile float m2revs = 0.00; + +// PWM settings +float pwmPeriod = 1.0/5000.0; +int frequency_pwm = 10000; //10kHz PWM + +// Initializing encoder QEI Encoder1(D12,D13,NC,64, QEI::X4_ENCODING); QEI Encoder2(D11,D10,NC,64, QEI::X4_ENCODING); MODSERIAL pc(USBTX, USBRX); @@ -30,11 +47,6 @@ AnalogIn emg0( A0 ); //AnalogIn emg1( A1 ); -// PWM settings -float pwmPeriod = 1.0/5000.0; -int frequency_pwm = 10000; //10kHz PWM - - // Setting up HIDscope volatile float x; volatile float y; @@ -57,25 +69,19 @@ scope.send(); // send what's in scope memory to PC } -// Initializing encoder -Ticker encoderTicker; -Ticker controllerTicker; -volatile int counts = 0; -volatile float revs = 0.00; -// MOTOR CONTROL PART -bool r_direction = false; +// REFERENCE PARAMETERS int posRevRange = 2; // describes the ends of the position range in complete motor output shaft revolutions in both directions const float maxAngle = 2*3.14*posRevRange; // max angle in radians const float Ts = 0.1; // Function getReferencePosition returns reference angle based on potmeter 1 -refGen ref1(A1); - -//ref.getReferencePosition(maxAngle); +refGen ref1(A1, maxAngle); +refGen ref2(A1, maxAngle); // readEncoder reads counts and revs and logs results to serial window -errorFetch err1(Encoder1, gearRatio, Ts, ref1); +errorFetch e1(ref1, gearRatio, Ts); +errorFetch e2(ref2, gearRatio, Ts); // Generate a PID controller with the specified values of k_p, k_d and k_i controller motorController1(k_p, k_d, k_i); @@ -84,19 +90,17 @@ motorConfig motor1(LED_GREEN,LED_RED,LED_BLUE,D5,D4); void measureAndControl(){ - err1.fetchError(); - float motorValue = motorController1.control(err1.e_pos, err1.e_int, err1.e_der); - float r = ref1.getReferencePosition(ref.maxAngle, ref1.r_direction); - sendDataToPc(r, err1.e_pos, emg0.read(), err1.e_der, motorValue); - pc.printf("Motorvalue is %.2f \r\n", motorValue); - pc.printf("Error is %.2f \r\n", e_pos); - pc.printf("Reference is %.2f \r\n", r); - //pc.printf("motor1 direction is %d \r\n", motor1_direction.read()); + m1counts = Encoder1.getPulses(); + m2counts = Encoder2.getPulses(); + e1.fetchError(m1counts); + float motorValue = motorController1.control(e1.e_pos, e1.e_int, e1.e_der); + float r = ref1.getReferencePosition(); + sendDataToPc(r, e1.e_pos, emg0.read(), e1.e_der, motorValue); motor1.setMotor(motorValue); } void rSwitchDirection(){ - r_direction = !r_direction; + ref1.r_direction = !ref1.r_direction; pc.printf("Switched reference direction! \r\n"); } @@ -109,7 +113,6 @@ button2.rise(&rSwitchDirection); pc.baud(115200); controllerTicker.attach(measureAndControl, Ts); - pc.printf("Encoder ticker attached and baudrate set"); }