![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
sdfa
Dependencies: AnglePosition2 Encoder FastPWM MODSERIAL Movement PIDController Servo SignalNumber2 biquadFilter mbed
Fork of kinematics_controlv2 by
Diff: main.cpp
- Revision:
- 3:c768a37620c9
- Parent:
- 1:406519ff0f17
- Child:
- 4:e15fc329b88b
--- a/main.cpp Thu Oct 26 10:54:56 2017 +0000 +++ b/main.cpp Mon Oct 30 10:43:26 2017 +0000 @@ -1,23 +1,91 @@ +#include "MODSERIAL.h" #include "AnglePosition.h" #include "PIDControl.h" +#include "BiQuad.h" +#include "signalnumber.h" #include "mbed.h" #include "encoder.h" #include "Servo.h" -#include "MODSERIAL.h" #include "FastPWM.h" //------------------------------------------------------------------------------ //------------------------------------------------------------------------------ //------------------------------------------------------------------------------ -MODSERIAL pc(USBTX, USBRX); // +MODSERIAL pc(USBTX, USBRX); //Establish connection Ticker MyControllerTicker1; //Declare Ticker Motor 1 Ticker MyControllerTicker2; //Declare Ticker Motor 2 +Ticker MySampleTicker; //Declare Ticker HIDscope +Ticker MyTickerMean; //Declare Ticker Signalprocessing + +InterruptIn But2(PTC6); //Declare button for calibration + AnglePosition Angle; //Declare Angle calculater PIDControl PID; //Declare PID Controller -AnalogIn potmeter1(A3); //Set Inputpin +SignalNumber Signal; //Declare Signal determiner + +AnalogIn potmeter1(A5); +AnalogIn potmeter5(A3); //Set Inputpin AnalogIn potmeter2(A4); //Set Inputpin +AnalogIn emg0( A0 ); //Set Inputpin +AnalogIn emg1( A1 ); //Set Inputpin + const float CONTROLLER_TS = 0.02; //Motor frequency +const float MEAN_TS = 0.1; //Mean value frequency +const float SAMPLE_TS = 0.02; //BiQuad frequency + + +//------------------------------------------------------------------------------ +//-----------------------------EMG Signals-------------------------------------- +//------------------------------------------------------------------------------ +const int n = 10; //Window size for the mean value, also adjust in signalnumber.cpp +const int action =50; //Number of same mean values to change the signalnumber +const int m = 10; //Number of samples for calibration +int mode; +float emg_offset; +float meanx; + +//BiQuad filter +BiQuad LP1( 0.6389437261127493, 1.2778874522254986, 0.6389437261127493, 1.1429772843080919, 0.4127976201429053 ); //Lowpass filter Biquad +BiQuad HP2( 0.8370879899975344, -1.6741759799950688, 0.8370879899975344, -1.6474576182593796, 0.7008943417307579 ); //Highpass filter Biquad +BiQuad NO3( 0.7063988100714527, -1.1429772843080923, 0.7063988100714527, -1.1429772843080923, 0.41279762014290533); //Notch filter Biquad +BiQuadChain BiQuad_filter; + +// Calibration function +void calibration(){ + emg_offset = Signal.calibrate(m, potmeter5); + pc.printf("offset = %f \r\n", emg_offset); +} + +/* +//Sample function +void sample() +{ + float Signal=((emg0+emg1)/2)-emg_offset; + float Signal_filtered= BiQuad_filter.step(Signal); + /* scope + scope.set(0, emg0.read() ); + scope.set(1, emg1.read() ); + scope.set(2, Signal_filtered); + scope.send(); + led = !led; + / +} +*/ + +float Filter(float input0, float input1, float offset){ + float Signal=((input0+input1)/2)-offset; + float Signal_filtered= BiQuad_filter.step(Signal); + return Signal_filtered; +} + +void signalnumber(){ + float Signal_filtered = Filter(emg0, emg1, emg_offset); + meanx = Signal.getmean(n, Signal_filtered); + mode = Signal.getnumber(n, action, Signal_filtered); + pc.printf("pot = %d after filtering = %f mean = %f Signalnumber = %i \r\n" , potmeter5, meanx, mode); +} + //------------------------------------------------------------------------------ //-------------------------Kinematic Constants---------------------------------- @@ -37,6 +105,7 @@ //------------------------------------------------------------------------------ //--------------------------------Servo----------------------------------------- //------------------------------------------------------------------------------ +// This will be programmed on a different Mbed Servo MyServo(D9); //Declare button InterruptIn But1(D8); //Declare used button int k=0; //Position flag @@ -76,8 +145,8 @@ float error_alpha = reference_alpha-position_alpha; float magnitude1 = PID.get(error_alpha, MOTOR1_KP, MOTOR1_KI, MOTOR1_KD, CONTROLLER_TS, M1_N, M1_v1, M1_v2) / motor1_gain; motor1 = fabs(magnitude1); - pc.printf("err_a = %f alpha = %f mag1 = %f\r\n", error_alpha, reference_alpha, magnitude1); - pc.printf("\r\n"); +// pc.printf("err_a = %f alpha = %f mag1 = %f\r\n", error_alpha, reference_alpha, magnitude1); +// pc.printf("\r\n"); // Determine Motor Direction if (magnitude1 < 0){ motor1DirectionPin = 1; @@ -111,8 +180,8 @@ float error_beta = reference_beta-position_beta; float magnitude2 = PID.get(error_beta, MOTOR2_KP, MOTOR2_KI, MOTOR2_KD, CONTROLLER_TS, M2_N, M1_v1, M1_v2) / motor2_gain; motor2 = fabs(magnitude2); - pc.printf("err2 = %f beta = %f mag2 = %f\r\n", error_beta, reference_beta, magnitude2); - pc.printf("\r\n"); +// pc.printf("err2 = %f beta = %f mag2 = %f\r\n", error_beta, reference_beta, magnitude2); +// pc.printf("\r\n"); //Determine Motor Direction if (magnitude2 > 0){ motor2DirectionPin = 1; @@ -127,11 +196,15 @@ int main(){ pc.baud(115200); + BiQuad_filter.add( &LP1 ).add( &HP2 ).add( &NO3); motor1.period(0.0001f); motor2.period(0.0001f); MyControllerTicker1.attach(&motor1_control, CONTROLLER_TS); MyControllerTicker2.attach(&motor2_control, CONTROLLER_TS); + MyTickerMean.attach(&signalnumber, MEAN_TS); +// MySampleTicker.attach(&sample, SAMPLE_TS); But1.rise(&servo_control); + But2.rise(&calibration); while(1) {} }