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
main.cpp
- Committer:
- tvlogman
- Date:
- 2017-10-26
- Revision:
- 41:9678fd827d25
- Parent:
- 40:7418f46a1ac0
- Child:
- 42:ae78ff03d9d6
File content as of revision 41:9678fd827d25:
#include <vector> #include <numeric> #include "mbed.h" #include "Matrix.h" #include "MODSERIAL.h" #include "HIDScope.h" #include "QEI.h" #include "FastPWM.h" #include "refGen.h" #include "controller.h" #include "motorConfig.h" #include "errorFetch.h" #include "BiQuad.h" // ADJUSTABLE PARAMETERS // controller ticker time interval const float Ts = 0.01; // EMG filter parameters // calibration time const int calSamples = 100; // Initialize average and max EMG value for calibration to 0 and 1 respectively volatile float avgEMGvalue = 0; volatile double maxEMGvalue = 1; // high pass BiQuadChain HPbqc; BiQuad HPbq1(0.9837,-1.9674, 0.9837,1.0000,-1.9769,0.9770); BiQuad HPbq2(1.0000, -2.0000, 1.0000, 1.0000, -1.9903, 0.9904); // low pass BiQuadChain LPbqc; BiQuad LPbq1(1.0e-5*1.3294, 1.0e-5*2.6587, 1.0e-5*1.3294, 1.0000, -1.7783, 0.7924); BiQuad LPbq2(1.0000, 2.0000, 1.0000, 1.0000, -1.8934, 0.9085); // Controller parameters const float k_p = 10; 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; // 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; 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); HIDScope scope(5); // Defining inputs InterruptIn sw2(SW2); InterruptIn sw3(SW3); InterruptIn button1(D2); InterruptIn button2(D3); //AnalogIn pot2(A2); //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; volatile float z; volatile float q; volatile float k; volatile float w; void sendDataToPc(float data1, float data2, float data3, float data4, float data5, float data6){ // Capture data x = data1; y = data2; z = data3; q = data4; k = data5; w = data6; scope.set(0, x); scope.set(1, y); scope.set(2, z); scope.set(3, q); scope.set(4, z); scope.set(5, w); scope.send(); // send what's in scope memory to PC } // REFERENCE PARAMETERS int posRevRange = 5; // 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 // Function getReferencePosition returns reference angle based on potmeter 1 refGen ref1(A1, maxAngle); refGen ref2(A1, maxAngle); // readEncoder reads counts and revs and logs results to serial window errorFetch e1(gearRatio, Ts); errorFetch e2(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); controller motorController2(k_p, k_d, k_i); 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(); double m1position = e1.fetchMotorPosition(m1counts); double m2position = e2.fetchMotorPosition(m2counts); // measuring and normalizing EMG signals to use as basis for reference float emg1 = ref1.getReference()/maxEMGvalue; float emg2 = ref2.getReference()/maxEMGvalue; // Filtering the EMG signals double thet1 = HPbqc.step(emg1); thet1 = fabs(thet1); thet1 = LPbqc.step(thet1); double thet2 = HPbqc.step(emg2); thet2 = fabs(thet2); thet2 = LPbqc.step(thet2); // Something worth trying: set a position setpoint that constantly changes but will never be reached until EMG value is 0 as it is computed from the robot's current position double thet1 = m1position + thet1; double thet2 = m1position + thet2; // Other possibility: use thet1 and thet2 directly as reference angles. That'll require the user to hold muscle tension to stay in a certain position though // 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 // 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 e1.fetchError(m1position, thet1); e2.fetchError(m2position, thet1); sendDataToPc(thet1, thet2, e1.e_pos, e2.e_pos, 0.0,0.0); // just send the EMG signal value to HIDscope break; } case ACTIVE: { if(stateChanged){ pc.printf("Active state \r\n"); } // Compute error e1.fetchError(m1position, thet1); e2.fetchError(m2position, thet2); // Compute motor value using controller and set motor float motorValue1 = motorController1.control(e1.e_pos, e1.e_int, e1.e_der); float motorValue2 = motorController2.control(e2.e_pos, e2.e_int, e2.e_der); motor1.setMotor(motorValue1); motor2.setMotor(motorValue2); // Send data to HIDscope sendDataToPc(thet1, thet2, e1.e_pos, e2.e_pos, motorValue1, motorValue2); // 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){ EMGsamples.push_back(emg1); 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 double maxEMGvalue = std::*max_element(EMGsamples.begin(), EMGsamples.end()); double max = *max_element(vector.begin(), vector.end()); 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(thet1, thet2, 0.0, 0.0, 0.0, 0.0); break; } } } void r1SwitchDirection(){ ref1.r_direction = !ref1.r_direction; pc.printf("Switched reference direction! \r\n"); } void r2SwitchDirection(){ ref2.r_direction = !ref2.r_direction; 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"); HPbqc.add(&HPbq1).add(&HPbq2); LPbqc.add(&LPbq1).add(&LPbq2); // Attaching state change functions to buttons; sw2.fall(&killSwitch); sw3.fall(&activateRobot); button1.rise(&r1SwitchDirection); button2.rise(&calibrateRobot); controllerTicker.attach(measureAndControl, Ts); pc.printf("Encoder ticker attached and baudrate set"); }