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:
- 37:633dd1901681
- Parent:
- 36:f3f3327d1d5a
- Child:
- 38:f1d2d42a4bdc
diff -r f3f3327d1d5a -r 633dd1901681 main.cpp --- a/main.cpp Sun Oct 22 08:39:28 2017 +0000 +++ b/main.cpp Mon Oct 23 13:50:49 2017 +0000 @@ -1,4 +1,5 @@ #include <vector> +#include <numeric> #include "mbed.h" #include "MODSERIAL.h" #include "HIDScope.h" @@ -12,8 +13,15 @@ #include "biquadChain.h" // ADJUSTABLE PARAMETERS +// controller ticker time interval +const float Ts = 0.1; // EMG filter parameters +// calibration time +const int calSamples = 100; + +volatile float avgEMGvalue = 0; + // high pass biquadFilter HPbq1(0.9837,-1.9674, 0.9837,1.0000,-1.9769,0.9770); @@ -32,7 +40,11 @@ // Defining motor gear ratio - for BOTH motors as this is the same in the current configuration const float gearRatio = 131; - +// LOGISTICS +// Declaring finite-state-machine states +enum robotStates {KILLED, ACTIVE, CALIBRATING}; +volatile robotStates currentState = KILLED; +volatile bool stateChanged = true; // Declaring a controller ticker and volatile variables to store encoder counts and revs Ticker controllerTicker; @@ -56,10 +68,15 @@ InterruptIn sw3(SW3); InterruptIn button1(D2); InterruptIn button2(D3); -AnalogIn pot2(A3); +//AnalogIn pot2(A3); //AnalogIn emg0( A0 ); //AnalogIn emg1( A1 ); +// Defining LED outputs to indicate robot state-us +DigitalOut ledG(LED_GREEN); +DigitalOut ledR(LED_RED); +DigitalOut ledB(LED_BLUE); + // Setting up HIDscope volatile float x; volatile float y; @@ -86,11 +103,11 @@ // 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, maxAngle); -//refGen ref2(A1, maxAngle); +//refGen ref2(A2, maxAngle); // readEncoder reads counts and revs and logs results to serial window errorFetch e1(gearRatio, Ts); @@ -100,20 +117,126 @@ controller motorController1(k_p, k_d, k_i); // setMotor1 sets the direction and magnitude pins of motor1 depending on the given motorValue. Negative motorValue means clockwise rotation -motorConfig motor1(LED_GREEN,LED_RED,LED_BLUE,D5,D4); - +motorConfig motor1(D4,D5); +motorConfig motor2(D7,D6); + +// PROBLEM: if I'm processing the state machine in the endless while loop, how can I adjust robot behavior in the ticker (as it'll keep running)? Do I need to also implement it there? If so, why bother with the while(1) in the main function in the first place? void measureAndControl(){ + // Read encoders and EMG signal (unnfiltered reference) m1counts = Encoder1.getPulses(); m2counts = Encoder2.getPulses(); - float r1 = ref1.getReference(); - r1 = HPbqc.applyFilter(r1); - r1 = fabs(r1); - r1 = LPbqc.applyFilter(r1); - e1.fetchError(m1counts, r1); - float motorValue = motorController1.control(e1.e_pos, e1.e_int, e1.e_der); - float r1_unfiltered = ref1.getReference(); - sendDataToPc(r1_unfiltered, r1, e1.e_pos, e1.e_der, motorValue); - motor1.setMotor(motorValue); + float r1_uf = ref1.getReference(); + //float r2_uf = ref2.getReference(); + pc.printf("In controller ticker \r\n"); + // Finite state machine + switch(currentState){ + case KILLED: + { + // Initialization of KILLED state: cut power to both motors + if(stateChanged){ + motor1.kill(); + //motor2.kill(); + pc.printf("Killed state \r\n"); + stateChanged = false; + } + + // Send reference data to pc + sendDataToPc(r1_uf, r1_uf, r1_uf, r1_uf, r1_uf); // just send the EMG signal value to HIDscope + + // Set LED to red + ledR = 0; + ledG = 1; + ledB = 1; + // need something here to check if "killswitch" has been pressed (?) + // NOTE: state transition is handled using buttons triggering functions motorConfig::kill() and motorConfig::turnMotorOn + break; + } + case ACTIVE: + { + if(stateChanged){ + pc.printf("Active state \r\n"); + } + // Filter reference + float r1 = HPbqc.applyFilter(r1_uf); + r1 = fabs(r1); + r1 = LPbqc.applyFilter(r1); + + //float r2 = HPbqc.applyFilter(r2_uf); + //r2 = fabs(r2); + //r2 = LPbqc.applyFilter(r2); + + + // Compute error + e1.fetchError(m1counts, r1); + //e2.fetchError(m2counts, r2); + + // Compute motor value using controller and set motor + float motorValue = motorController1.control(e1.e_pos, e1.e_int, e1.e_der); + motor1.setMotor(motorValue); + + // Send data to HIDscope + sendDataToPc(r1_uf, r1, e1.e_pos, e1.e_der, motorValue); + + // Set LED to blue + ledR = 1; + ledG = 1; + ledB = 0; + // NOTE: state transition is handled using buttons triggering functions motorConfig::kill() and motorConfig::turnMotorOn + break; + } + case CALIBRATING: + { + // NOTE: maybe wrap this whole calibrating thing in a library? + + // Do I need to kill motors here? + + + // Initialization of CALIBRATE state + static int Ncal = 0; + std::vector<float> EMGsamples; + + if(stateChanged){ + // Kill motors + pc.printf("Calibrate state \r\n"); + motor1.kill(); + //motor2.kill(); + + // Clear sample value vector and reset counter variable + EMGsamples.clear(); + Ncal = 0; + stateChanged = false; + } + + // fill the array with sample data if it is not yet filled + if(Ncal < calSamples){ + pc.printf("%.2f \r\n", r1_uf); + EMGsamples.push_back(r1_uf); + pc.printf("%.2f \r\n", EMGsamples.end()); + Ncal++; + } + // if array is filled compute the mean value and switch to active state + else { + // Set new avgEMGvalue + avgEMGvalue = std::accumulate(EMGsamples.begin(), EMGsamples.end(), 0)/calSamples; // still needs to return this value + + pc.printf("Avg emg value is %.2f changed state to active", avgEMGvalue); + // State transition logic + currentState = ACTIVE; + stateChanged = true; + Ncal = 0; + } + + // Set LED to green + ledR = 1; + ledG = 0; + ledB = 1; + sendDataToPc(r1_uf, r1_uf, r1_uf, r1_uf, r1_uf); + break; + } + } + + + } void rSwitchDirection(){ @@ -121,14 +244,31 @@ pc.printf("Switched reference direction! \r\n"); } +void killSwitch(){ + currentState = KILLED; + stateChanged = true; + } + +void activateRobot(){ + currentState = ACTIVE; + stateChanged = true; + } + +void calibrateRobot(){ + currentState = CALIBRATING; + stateChanged = true; + } int main() { + pc.baud(115200); pc.printf("Main function"); - sw2.fall(&motor1,&motorConfig::killSwitch); - sw3.fall(&motor1, &motorConfig::turnMotorOn); + // Attaching state change functions to buttons; + sw2.fall(&killSwitch); + sw3.fall(&activateRobot); + button1.rise(&calibrateRobot); button2.rise(&rSwitchDirection); - pc.baud(115200); + controllerTicker.attach(measureAndControl, Ts); pc.printf("Encoder ticker attached and baudrate set"); }