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:
- 29:9aa4d63a9bd1
- Parent:
- 28:8cd898ff43a2
- Child:
- 30:65f0c9ecf810
--- a/main.cpp Fri Oct 13 16:24:50 2017 +0000 +++ b/main.cpp Mon Oct 16 08:45:03 2017 +0000 @@ -3,6 +3,7 @@ #include "HIDScope.h" #include "QEI.h" #include "FastPWM.h" +#include "refGen.h" // Defining relevant constant parameters const float gearRatio = 131; @@ -34,7 +35,6 @@ InterruptIn sw3(SW3); InterruptIn button1(D2); InterruptIn button2(D3); -AnalogIn pot1(A0); AnalogIn pot2(A1); // PWM settings @@ -77,18 +77,9 @@ const float Ts = 0.1; // Function getReferencePosition returns reference angle based on potmeter 1 -float getReferencePosition(){ - float r; - if(r_direction == false){ - // Clockwise rotation yields positive reference - r = maxAngle*pot1.read(); - } - if(r_direction == true){ - // Counterclockwise rotation yields negative reference - r = -1*maxAngle*pot1.read(); - } - return r; - } +refGen ref(A0); + +//ref.getReferencePosition(maxAngle); // Initializing position and integral errors to zero float e_pos = 0; @@ -103,7 +94,7 @@ double motor1Position = 2*3.14*(counts/(gearRatio*64.0f)); // Computing position error - e_pos = getReferencePosition() - motor1Position; + e_pos = ref.getReferencePosition(maxAngle, r_direction) - motor1Position; // Limiting the integral error to prevent integrator saturation if(fabs(e_int) <= 5){ @@ -129,11 +120,10 @@ motor1_pwm.write(0.0); // Set motor direction if (motorValue >=0){ - pc.printf("Fa! \r\n"); - motor1_direction = 0; //This doesn't seem to set motor 1 direction to 0 - pc.printf("M1D = %d \r\n", motor1_direction.read()); - pc.printf("Foo! \r\n"); + // corresponds to CW rotation of motor axle + motor1_direction = 0; } else if(motorValue < 0){ + // corresponds to CCW rotation of motor axle motor1_direction = 1; pc.printf("Bah!"); } @@ -145,12 +135,11 @@ pc.printf("Got into ACTIVE \r\n"); // Set motor direction if (motorValue >=0){ - pc.printf("Fa!"); + // corresponds to CW rotation of motor axle motor1_direction.write(0); - pc.printf("Foo!"); } else if(motorValue < 0){ + // corresponds to CCW rotation of motor axle motor1_direction.write(1); - pc.printf("Bah!"); } // Set motor speed @@ -158,7 +147,7 @@ motor1_pwm = 1; } else { - motor1_pwm.write(fabs(motorValue)); + motor1_pwm.write(fabs(motorValue) + 0.4); } ledR = 1; ledG = 1; @@ -170,7 +159,7 @@ void measureAndControl(){ getError(e_pos, e_int, e_der); float motorValue = motorController(e_pos, e_int, e_der); - float r = getReferencePosition(); + float r = ref.getReferencePosition(maxAngle, r_direction); sendDataToPc(r, e_pos, e_int, e_der, motorValue); pc.printf("Motorvalue is %.2f \r\n", motorValue); pc.printf("Error is %.2f \r\n", e_pos);