![](/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:
- 4:e15fc329b88b
- Parent:
- 3:c768a37620c9
- Child:
- 5:b4abbd3c513c
--- a/main.cpp Mon Oct 30 10:43:26 2017 +0000 +++ b/main.cpp Tue Oct 31 14:35:52 2017 +0000 @@ -3,123 +3,157 @@ #include "PIDControl.h" #include "BiQuad.h" #include "signalnumber.h" +#include "Movement.h" #include "mbed.h" #include "encoder.h" #include "Servo.h" #include "FastPWM.h" +//This code is for Mbed 2 //------------------------------------------------------------------------------ //------------------------------------------------------------------------------ //------------------------------------------------------------------------------ MODSERIAL pc(USBTX, USBRX); //Establish connection Ticker MyControllerTicker1; //Declare Ticker Motor 1 Ticker MyControllerTicker2; //Declare Ticker Motor 2 -Ticker MySampleTicker; //Declare Ticker HIDscope +Ticker MySampleTicker; //Declare Ticker HIDscope Ticker MyTickerMean; //Declare Ticker Signalprocessing -InterruptIn But2(PTC6); //Declare button for calibration +InterruptIn But2(PTA4); //Declare button for min calibration +InterruptIn But1(PTC6); //Declare button for max calibration AnglePosition Angle; //Declare Angle calculater PIDControl PID; //Declare PID Controller -SignalNumber Signal; //Declare Signal determiner +SignalNumber SignalLeft; //Declare Signal determiner for Left arm +SignalNumber SignalRight; //Declare Signal determiner for Right arm +Movement MoveLeft; //Declare Movement determiner +Movement MoveRight; -AnalogIn potmeter1(A5); -AnalogIn potmeter5(A3); //Set Inputpin -AnalogIn potmeter2(A4); //Set Inputpin -AnalogIn emg0( A0 ); //Set Inputpin -AnalogIn emg1( A1 ); //Set Inputpin - +AnalogIn emg0( A0 ); //Set Inputpin for EMG 0 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 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 +DigitalOut Led_red(LED_RED); +DigitalOut Led_green(LED_GREEN); +DigitalOut Led_blue(LED_BLUE); 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 +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-------------------------------------- //------------------------------------------------------------------------------ -const int n = 10; //Window size for the mean value, also adjust in signalnumber.cpp +// Filtering the signal and getting the usefull information out of it. +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 = 10; //Number of samples for calibration -int mode; -float emg_offset; -float meanx; +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 -//BiQuad filter +//BiQuad filter variables 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); +void setled(){ + Led_red=0; + Led_green=1; + Led_blue=1; } -/* -//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; - / +// 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; +} + +//Filter de EMG signals with a BiQuad filter float Filter(float input0, float input1, float offset){ - float Signal=((input0+input1)/2)-offset; + float Signal=input0-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 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); + //Left + float Signal_filteredLeft = fabs(Filter(emg0, emg1, emg_offsetLeft)); + meanxL = SignalLeft.getmean(n, Signal_filteredLeft)*kLeft; //Testing variable + 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 + CaseRight = SignalRight.getnumber(n, action, Signal_filteredRight, kRight); + pc.printf("m %f C %i \r\n",meanxR, CaseRight); //Testing print } - //------------------------------------------------------------------------------ //-------------------------Kinematic Constants---------------------------------- //------------------------------------------------------------------------------ -const double RAD_PER_PULSE = 0.00074799825*2; -const double PI = 3.14159265358979323846; -const float max_rangex = 500.0; -const float max_rangey = 300.0; -const float x_offset = 156.15; -const float y_offset = -76.97; -const float alpha_offset = -(21.52/180)*PI; -const float beta_offset = 0.0; -const float L1 = 450.0; -const float L2 = 490.0; - - -//------------------------------------------------------------------------------ -//--------------------------------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 - -void servo_control (){ - if (k==0){ - MyServo = 0; - k=1; - } - else{ - MyServo = 2; - k=0; - } -} +const double RAD_PER_PULSE = 0.00074799825*2; //Number of radials per pulse from the encoder +const double PI = 3.14159265358979323846; //Pi +const float max_rangex = 500.0; //Max range on the x axis +const float max_rangey = 300.0; //Max range on the y axis +const float x_offset = 156.15; //Start x position from the shoulder joint +const float y_offset = -76.97; //Start y position from the shoulder joint +const float alpha_offset = -(21.52/180)*PI; //Begin angle Alpha at zero zero +const float beta_offset = 0.0; //Begin angle Beta at zero zero +const float L1 = 450.0; //Length of the first body +const float L2 = 490.0; //Length of the second body //------------------------------------------------------------------------------ @@ -140,13 +174,14 @@ void motor1_control(){ - float reference_alpha = Angle.getbeta(max_rangex, max_rangey, x_offset, y_offset, beta_offset, L1, L2, potmeter1, potmeter2); + 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; 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"); // Determine Motor Direction if (magnitude1 < 0){ motor1DirectionPin = 1; @@ -175,13 +210,14 @@ void motor2_control(){ - float reference_beta = Angle.getalpha(max_rangex, max_rangey, x_offset, y_offset, alpha_offset, L1, L2, potmeter1, potmeter2); + 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; 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"); //Determine Motor Direction if (magnitude2 > 0){ motor2DirectionPin = 1; @@ -196,15 +232,17 @@ int main(){ pc.baud(115200); + setled(); 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); MyTickerMean.attach(&signalnumber, MEAN_TS); -// MySampleTicker.attach(&sample, SAMPLE_TS); - But1.rise(&servo_control); - But2.rise(&calibration); + while(1) {} }