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:
- 28:8cd898ff43a2
- Parent:
- 27:a4228ea8fb8f
- Child:
- 29:9aa4d63a9bd1
diff -r a4228ea8fb8f -r 8cd898ff43a2 main.cpp --- a/main.cpp Fri Oct 13 11:59:15 2017 +0000 +++ b/main.cpp Fri Oct 13 16:24:50 2017 +0000 @@ -8,7 +8,7 @@ const float gearRatio = 131; // Controller parameters -const float k_p = 0.5; +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 @@ -33,7 +33,7 @@ InterruptIn sw2(SW2); InterruptIn sw3(SW3); InterruptIn button1(D2); -InterruptIn button2(D4); +InterruptIn button2(D3); AnalogIn pot1(A0); AnalogIn pot2(A1); @@ -118,15 +118,7 @@ // motorController sets a motorValue based on the position error. Sign indicates direction and magnitude indicates speed. Right now this is a simple feedforward float motorController(float e_pos, float e_int, float e_der){ float motorValue; - if(e_pos >= 0){ - // Positive error yields positive motor value = counterclockwise rotation - motorValue = -1*((e_pos*k_p) + k_i*e_int + k_d*e_der + 0.45); - } - else { - // Negative error yields negative motor value = clockwise rotation - motorValue = 1*((e_pos*k_p) + k_i*e_int + k_d*e_der + 0.45); - } - + motorValue = k_p*e_pos + k_d*e_der + k_i*e_int; return motorValue; } @@ -135,18 +127,32 @@ switch(currentState){ case KILLED: 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"); + } else if(motorValue < 0){ + motor1_direction = 1; + pc.printf("Bah!"); + } ledR = 0; ledG = 1; ledB = 1; break; case ACTIVE: + pc.printf("Got into ACTIVE \r\n"); // Set motor direction if (motorValue >=0){ - motor1_direction = 0; + pc.printf("Fa!"); + motor1_direction.write(0); + pc.printf("Foo!"); + } else if(motorValue < 0){ + motor1_direction.write(1); + pc.printf("Bah!"); } - else { - motor1_direction = 1; - } + // Set motor speed if (fabs(motorValue)>1){ motor1_pwm = 1; @@ -166,6 +172,10 @@ float motorValue = motorController(e_pos, e_int, e_der); float r = getReferencePosition(); 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); + pc.printf("Reference is %.2f \r\n", r); + pc.printf("motor1 direction is %d \r\n", motor1_direction.read()); setMotor1(motorValue); }