![](/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
Revision 5:b4abbd3c513c, committed 2017-11-01
- Comitter:
- peterknoben
- Date:
- Wed Nov 01 11:47:03 2017 +0000
- Parent:
- 4:e15fc329b88b
- Commit message:
- fsd
Changed in this revision
Movement.lib | Show annotated file Show diff for this revision Revisions of this file |
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/Movement.lib Tue Oct 31 14:35:52 2017 +0000 +++ b/Movement.lib Wed Nov 01 11:47:03 2017 +0000 @@ -1,1 +1,1 @@ -https://os.mbed.com/users/peterknoben/code/Movement/#9c61094ffaac +https://os.mbed.com/users/peterknoben/code/Movement/#03e2651b4a24
--- a/main.cpp Tue Oct 31 14:35:52 2017 +0000 +++ b/main.cpp Wed Nov 01 11:47:03 2017 +0000 @@ -14,9 +14,8 @@ //------------------------------------------------------------------------------ //------------------------------------------------------------------------------ MODSERIAL pc(USBTX, USBRX); //Establish connection -Ticker MyControllerTicker1; //Declare Ticker Motor 1 -Ticker MyControllerTicker2; //Declare Ticker Motor 2 -Ticker MySampleTicker; //Declare Ticker HIDscope +Ticker MyControllerTicker; //Declare Ticker Motor 1 +Ticker MyTickerMode; //Declare Ticker Motor 2 Ticker MyTickerMean; //Declare Ticker Signalprocessing InterruptIn But2(PTA4); //Declare button for min calibration @@ -26,16 +25,18 @@ PIDControl PID; //Declare PID Controller SignalNumber SignalLeft; //Declare Signal determiner for Left arm SignalNumber SignalRight; //Declare Signal determiner for Right arm +SignalNumber SignalMode; Movement MoveLeft; //Declare Movement determiner Movement MoveRight; AnalogIn emg0( A0 ); //Set Inputpin for EMG 0 signal Left -AnalogIn emg1( A1 ); //Set Inputpin for EMG 1 signal Left +//AnalogIn emg1( A1 ); //Set Inputpin for EMG 1 signal Left AnalogIn emg2( A2 ); //Set Inputpin for EMG 2 signal Right -AnalogIn emg3( A3 ); //Set Inputpin for EMG 3 signal Right +//AnalogIn emg3( A3 ); //Set Inputpin for EMG 3 signal Right AnalogIn emg4( A4 ); //Set Inputpin for EMG 4 signal Mode -AnalogIn emg5( A5 ); //Set Inputpin for EMG 5 signal Mode -DigitalOut M( D9 ); //Set digital in for mode selection +//AnalogIn emg5( A5 ); //Set Inputpin for EMG 5 signal Mode +DigitalOut Up( D9 ); //Set digital in for mode selection +DigitalOut Down( D8 ); DigitalOut Led_red(LED_RED); DigitalOut Led_green(LED_GREEN); DigitalOut Led_blue(LED_BLUE); @@ -43,31 +44,6 @@ const float CONTROLLER_TS = 0.02; //Motor frequency const float MEAN_TS = 0.002; //Filter frequency -//Testing methods -/* -AnalogIn potmeter1(A5); -AnalogIn potmeter5(A3); //Set Inputpin for x axis -AnalogIn potmeter2(A4); //Set Inputpin for y axis -*/ - -//------------------------------------------------------------------------------ -//---------------------------Mode selection------------------------------------- -//------------------------------------------------------------------------------ -// From the other Mbed there will be send a signal to determine in which mode the system is in -int mode =0; - -//Recieving mode selection from Mbed 1 -void mode_selection(){ - if(mode ==6){ - mode=1; - } - else{ - mode++; - } - pc.printf("mode = %i\r\n", mode); -} - - //------------------------------------------------------------------------------ //-----------------------------EMG Signals-------------------------------------- @@ -76,16 +52,12 @@ const int n = 400; //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 = 300; //Number of samples for calibration -int CaseLeft; //Strength of the muscles Left -int CaseRight; //Strength of the muscles Right -float emg_offsetLeft; //Calibtarion value to get zero -float emg_offsetmaxLeft; //Calibration value to scale to 1 -float emg_offsetRight; //Calibtarion value to get zero -float emg_offsetmaxRight; //Calibration value to scale to 1 -float meanxL; //Temporary variable for mean value -float meanxR; -float kLeft; //Scaling factor mean value -float kRight; //Scaling factor mean value +int CaseLeft, CaseRight, CaseMode; //Strength of the muscles +float emg_offsetLeft, emg_offsetmaxLeft; //Calibration offsets from zero to one for the left arm +float emg_offsetRight, emg_offsetmaxRight; //Calibration offsets from zero to one for the right arm +float emg_offsetMode, emg_offsetmaxMode; +float mean_value_left, mean_value_right, mean_value_mode; //Mean value of the filtered system +float kLeft, kRight, kMode; //Scaling factors //BiQuad filter variables BiQuad LP1( 0.6389437261127493, 1.2778874522254986, 0.6389437261127493, 1.1429772843080919, 0.4127976201429053 ); //Lowpass filter Biquad @@ -93,54 +65,110 @@ BiQuad NO3( 0.7063988100714527, -1.1429772843080923, 0.7063988100714527, -1.1429772843080923, 0.41279762014290533); //Notch filter Biquad BiQuadChain BiQuad_filter; + +//Calibration------------------------------------------------------------------- void setled(){ - Led_red=0; - Led_green=1; - Led_blue=1; + Led_red=0; Led_green=1; Led_blue=1; +} +// Zero calibration +void mincalibration(){ + pc.printf("start min calibration \r\n"); + emg_offsetLeft = SignalLeft.calibrate(m,emg0); + emg_offsetRight = SignalRight.calibrate(m,emg2); + emg_offsetMode = SignalMode.calibrate(m, emg4); + pc.printf("offsets: %f %f \r\n", emg_offsetLeft, emg_offsetRight); + Led_green=0; Led_red=0; //Set led to Yellow +} +// One calibration +void maxcalibration(){ + pc.printf("start max calibration \r\n"); + emg_offsetmaxLeft = SignalLeft.calibrate(m,(emg0))-emg_offsetLeft; + emg_offsetmaxRight = SignalRight.calibrate(m,(emg2))-emg_offsetRight; + emg_offsetmaxMode = SignalMode.calibrate(m, (emg4))-emg_offsetMode; + kLeft = 1/emg_offsetmaxLeft; + kRight = 1/emg_offsetmaxRight; + kMode = 1/emg_offsetmaxMode; + pc.printf("offsets: %f %f, scale %f %f \r\n", emg_offsetmaxLeft, emg_offsetmaxRight, kLeft, kRight); + Led_red=1; //Set led to Green } -// Calibration function -void mincalibration(){ - pc.printf("start cali \r\n"); - emg_offsetLeft = SignalLeft.calibrate(m,((emg0+emg1)/2)); - emg_offsetRight = SignalRight.calibrate(m,((emg2+emg3)/2)); -// pc.printf("calibrated, offset = %f \r\n", emg_offset); - Led_green=0; - Led_red=0; -} - -void maxcalibration(){ - pc.printf("start cali max\r\n"); - emg_offsetmaxLeft = SignalLeft.calibrate(m,((emg0+emg1)/2))-emg_offsetLeft; - emg_offsetmaxRight = SignalRight.calibrate(m,((emg2+emg3)/2))-emg_offsetRight; - kLeft = 1/emg_offsetmaxLeft; - kRight = 1/emg_offsetmaxRight; -// pc.printf("calibrated, offset = %f scale = %f \r\n",emg_offsetmax, k); - Led_red=1; -} - +//Filtering the signals--------------------------------------------------------- //Filter de EMG signals with a BiQuad filter -float Filter(float input0, float input1, float offset){ - float Signal=input0-offset; //((input0+input1)/2) +float Filter(float input, float offset){ + float Signal=input-offset; //((input0+input1)/2) float Signal_filtered= BiQuad_filter.step(Signal); return Signal_filtered; } -//Determine the signalnumbers (i.e. speed) based on the EMG signals +//------------------------------------------------------------------------------ +//---------------------------------Servo---------------------------------------- +//------------------------------------------------------------------------------ +void servo(){ + float Signal_filteredLeft = fabs(Filter(emg0, emg_offsetLeft)); + float Signal_filteredRight = fabs(Filter(emg2, emg_offsetRight)); + CaseLeft = SignalLeft.getnumber(n, action, Signal_filteredLeft, kLeft); + CaseRight = SignalRight.getnumber(n, action, Signal_filteredRight, kRight); + if (CaseLeft>=3){ + Up = 1; + Up = 0; + } + else if (CaseRight >=3){ + Down = 1; + Down = 0; + } +} +int milli =0; + +//------------------------------------------------------------------------------ +//---------------------------Mode selection------------------------------------- +//------------------------------------------------------------------------------ +int mode =0; + +//Recieving mode selection from EMG mode signal +void mode_selection(){ + if(mode ==6){ + mode=1; + } + else{ + mode++; + } + if (mode==3||mode==6){ + servo(); + } + pc.printf("\r\n mode = %i \r\n", mode); +} + +// Control mode selection------------------------------------------------------- + +//Determine the signalnumbers (i.e. speed) based on the strenght of the EMG signals void signalnumber(){ //Left - float Signal_filteredLeft = fabs(Filter(emg0, emg1, emg_offsetLeft)); - meanxL = SignalLeft.getmean(n, Signal_filteredLeft)*kLeft; //Testing variable + float Signal_filteredLeft = fabs(Filter(emg0, emg_offsetLeft)); + mean_value_left = SignalLeft.getmean(n, Signal_filteredLeft)*kLeft; CaseLeft = SignalLeft.getnumber(n, action, Signal_filteredLeft, kLeft); - pc.printf("m %f C %i \r\n",meanxL, CaseLeft); //Testing print //Right - float Signal_filteredRight = fabs(Filter(emg2, emg3, emg_offsetRight)); - meanxR = SignalRight.getmean(n, Signal_filteredRight)*kRight; //Testing variable + float Signal_filteredRight = fabs(Filter(emg2, emg_offsetRight)); + mean_value_right = SignalRight.getmean(n, Signal_filteredRight)*kRight; CaseRight = SignalRight.getnumber(n, action, Signal_filteredRight, kRight); - pc.printf("m %f C %i \r\n",meanxR, CaseRight); //Testing print + //Mode + float Signal_filteredMode = fabs(Filter(emg4, emg_offsetMode)); + mean_value_mode = SignalMode.getmean(n, Signal_filteredMode)*kMode; + CaseMode = SignalMode.getnumber(n, action, Signal_filteredMode, kMode); + if(CaseMode >= 3){ + milli ++; + if(milli>=150){ + mode_selection(); + milli=0; + } + } + else{ + milli=0; + } } + + //------------------------------------------------------------------------------ //-------------------------Kinematic Constants---------------------------------- //------------------------------------------------------------------------------ @@ -156,6 +184,7 @@ const float L2 = 490.0; //Length of the second body + //------------------------------------------------------------------------------ //--------------------------------Motor1---------------------------------------- //------------------------------------------------------------------------------ @@ -167,16 +196,12 @@ const float MOTOR1_KP = 40.0; const float MOTOR1_KI = 0.0; const float MOTOR1_KD = 15.0; -double M1_v1 = 0.0; -double M1_v2 = 0.0; +double M1_v1 = 0.0, M1_v2 = 0.0; //Calculation values const double motor1_gain = 2*PI; const float M1_N = 0.5; - +static float position_math[2]={}; void motor1_control(){ - float *position_math; - position_math[0]= MoveLeft.getposition(CaseLeft, mode, 0, max_rangex); - position_math[1]= MoveRight.getposition(CaseRight, mode, 1, max_rangey); float reference_alpha = Angle.getbeta(max_rangex, max_rangey, x_offset, y_offset, beta_offset, L1, L2, position_math[0], position_math[1]); float position_alpha = RAD_PER_PULSE * encoder1.getPosition(); float error_alpha = reference_alpha-position_alpha; @@ -202,17 +227,11 @@ const float MOTOR2_KP = 60.0; const float MOTOR2_KI = 0.0; const float MOTOR2_KD = 15.0; -double m2_err_int = 0; +double M2_v1 = 0.0, M2_v2 = 0.0; //Calculation values const double motor2_gain = 2*PI; const float M2_N = 0.5; -double M2_v1 = 0.0; -double M2_v2 = 0.0; - void motor2_control(){ - float *position_math; - position_math[0]= MoveLeft.getposition(CaseLeft, mode, 0, max_rangex); - position_math[1]= MoveRight.getposition(CaseRight, mode, 1, max_rangey); float reference_beta = Angle.getalpha(max_rangex, max_rangey, x_offset, y_offset, alpha_offset, L1, L2, position_math[0], position_math[1]); float position_beta = RAD_PER_PULSE * -encoder2.getPosition(); float error_beta = reference_beta-position_beta; @@ -226,6 +245,13 @@ motor2DirectionPin = 0; } } + +void motor_control(){ + position_math[0]= MoveLeft.getposition(CaseLeft, mode, 0, max_rangex); + position_math[1]= MoveRight.getposition(CaseRight, mode, 1, max_rangey); + motor1_control(); + motor2_control(); +} //------------------------------------------------------------------------------ //------------------------------------------------------------------------------ //------------------------------------------------------------------------------ @@ -236,14 +262,18 @@ BiQuad_filter.add( &LP1 ).add( &HP2 ).add( &NO3); But2.rise(&mincalibration); But1.rise(&maxcalibration); -// M.rise(&mode_selection); - motor1.period(0.0001f); - motor2.period(0.0001f); - MyControllerTicker1.attach(&motor1_control, CONTROLLER_TS); - MyControllerTicker2.attach(&motor2_control, CONTROLLER_TS); + motor1.period(0.0001f); motor2.period(0.0001f); + MyControllerTicker.attach(&motor_control, CONTROLLER_TS); MyTickerMean.attach(&signalnumber, MEAN_TS); +// MyTickerMode.attach(&signalmode, MEAN_TS); +// MyTickerMean.attach(&signalnumberright, MEAN_TS); +// MyTickerMean.attach(&signalmode,MEAN_TS); - while(1) {} + while(1) { + pc.printf("Mean %f %f %f \r\n", mean_value_left, mean_value_right, mean_value_mode); +// pc.printf("Case %i %i %i, mode = %i \r\n", CaseLeft, CaseRight, CaseMode, mode); + wait(0.1f); + } }