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:
- 19:f08b5cd2b7ce
- Parent:
- 17:616ce7bc1f96
- Child:
- 20:4ce3fb543a45
diff -r 616ce7bc1f96 -r f08b5cd2b7ce main.cpp --- a/main.cpp Thu Sep 21 11:53:09 2017 +0000 +++ b/main.cpp Fri Oct 06 05:29:52 2017 +0000 @@ -9,7 +9,6 @@ QEI Encoder(D12,D13,NC,64, QEI::X4_ENCODING); MODSERIAL pc(USBTX, USBRX); -HIDScope scope(2); // Defining outputs DigitalOut motor1_direction(D4); @@ -31,24 +30,67 @@ // Initializing encoder Ticker encoderTicker; +Ticker controllerTicker; volatile int counts = 0; volatile float revs = 0.00; -void readEncoder(){ - counts = Encoder.getPulses(); - revs = counts/(131*64.0f); - pc.printf("%0.2f revolutions \r\n", revs); - - // Displaying revs in HIDscope - x = revs; // Capture data - scope.set(0, x); // store data in first element of scope memory - y = (x_prev + x)/2.0; // averaging filter - scope.set(1, y); // store data in second element of scope memory - x_prev = x; // Prepare for next round - - scope.send(); // send what's in scope memory to PC +// MOTOR CONTROL PART + +// Function getReferenceVelocity returns reference r between -8.4 and 8.4 rad/s +float getReferenceVelocity(){ + const float maxVelocity = 8.4; // max velocity in rad/s + double r; + if(motor1_direction = false){ + // Clockwise rotation yields positive reference + r = maxVelocity*pot.read(); + } + if(motor1_direction = true){ + // Counterclockwise rotation yields negative reference + r = -1*maxVelocity*pot.read(); + } + return r; + } + +// motorController sets a motorValue based on the reference. Sign indicates direction and magnitude indicates speed. Right now this is a simple feedforward +float motorController(float reference){ + const float motorGain = 8.4; // max power of the motor should normalize to a motorValue magnitude of 1 + double motorValue = reference/motorGain; // motorValue is the normalized voltage applied to motor magnitude pin between -1 and 1 + return motorValue; } +// setMotor1 sets the direction and magnitude pins of motor1 depending on the given motorValue. Negative motorValue means clockwise rotation +void setMotor1(float motorValue){ + switch(currentState){ + case KILLED: + motor1_pwm.write(0.0); + break; + case ACTIVE: + // Set motor direction + if (motorValue >=0){ + motor1_direction = 0; + } + else { + motor1_direction = 1; + } + // Set motor speed + if (fabs(motorValue)>1){ + motor1_pwm = 1; + } + else { + motor1_pwm.write(fabs(motorValue)); + } + break; + } + } + +void measureAndControl(){ + float referenceVelocity = getReferenceVelocity(); + float motorValue = motorController(referenceVelocity); + pc.printf("Motorvalue is %.2f \r \n", motorValue); + pc.printf("Motor direction is %d \r \n", motor1_direction); + setMotor1(motorValue); + } + void killSwitch(){ pc.printf("Motors turned off"); currentState = KILLED; @@ -64,28 +106,27 @@ motor1_direction = !motor1_direction; } +// readEncoder reads counts and revs and logs results to serial window +void readEncoder(){ + counts = Encoder.getPulses(); + revs = counts/(131*64.0f); + pc.printf("%0.2f revolutions \r\n", revs); + float reference = getReferenceVelocity(); + pc.printf("Reference velocity %0.2f \r\n", reference); + } + int main() { - motor1_direction = false; + pc.printf("Main function"); + motor1_direction = false; // False = clockwise rotation motor1_pwm.period(pwmPeriod);//T=1/f sw2.fall(&killSwitch); sw3.fall(&turnMotorsOn); button1.rise(&M1switchDirection); pc.baud(115200); - encoderTicker.attach(readEncoder, 0.1); + encoderTicker.attach(readEncoder, 2); + controllerTicker.attach(measureAndControl, 0.1); - pc.printf("Encoder ticker attached and baudrate set"); - - while(true){ - switch(currentState){ - case KILLED: - motor1_pwm.write(0.0); - break; - case ACTIVE: - motor1_pwm.write(pot.read()); // Motor speed proportional to pot meter - break; - } - } - + pc.printf("Encoder ticker attached and baudrate set"); }