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-22
- Revision:
- 34:1a70aa045c8f
- Parent:
- 33:6f4858b98fe5
- Child:
- 35:99bf23b34ee3
File content as of revision 34:1a70aa045c8f:
#include <vector> #include "mbed.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 "biquadFilter.h" #include "biquadChain.h" // ADJUSTABLE PARAMETERS // EMG filter parameters // high pass biquadFilter HPbq1(0.9837,-1.9674, 0.9837,1.0000,-1.9769,0.9770); biquadFilter HPbq2(1.0000, -2.0000, 1.0000, 1.0000, -1.9903, 0.9904); biquadChain HPbqc(HPbq1, HPbq2); // low pass biquadFilter LPbq1(1.0e-6*0.1202, 1.0e-6*0.2405, 1.0e-6*0.1202, 1.0000, -1.9313, 0.9327); biquadFilter LPbq2(1.0000, 2.0000, 1.0000, 1.0000, -1.9702, 0.9716); biquadChain LPbqc(LPbq1, LPbq2); // Controller parameters 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 // Defining motor gear ratio - for BOTH motors as this is the same in the current configuration const float gearRatio = 131; // 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(A3); //AnalogIn emg0( A0 ); //AnalogIn emg1( A1 ); // Setting up HIDscope volatile float x; volatile float y; volatile float z; volatile float q; volatile float k; void sendDataToPc(float data1, float data2, float data3, float data4, float data5){ // Capture data x = data1; y = data2; z = data3; q = data4; k = data5; scope.set(0, x); scope.set(1, y); scope.set(2, z); scope.set(3, q); scope.set(4, z); scope.send(); // send what's in scope memory to PC } // 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); // 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); // 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); void measureAndControl(){ m1counts = Encoder1.getPulses(); m2counts = Encoder2.getPulses(); float r1 = ref1.getFilteredReference; 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); } void rSwitchDirection(){ ref1.r_direction = !ref1.r_direction; pc.printf("Switched reference direction! \r\n"); } int main() { pc.printf("Main function"); sw2.fall(&motor1,&motorConfig::killSwitch); sw3.fall(&motor1, &motorConfig::turnMotorOn); button2.rise(&rSwitchDirection); pc.baud(115200); controllerTicker.attach(measureAndControl, Ts); pc.printf("Encoder ticker attached and baudrate set"); }