Working, Clean
Dependencies: AnglePosition2 Encoder FastPWM MovementClean PIDController Servo SignalNumberClean biquadFilter mbed
Fork of ShowIt by
main.cpp
- Committer:
- peterknoben
- Date:
- 2017-11-07
- Revision:
- 15:a73b5abd0696
- Parent:
- 14:a8a69fee3fad
File content as of revision 15:a73b5abd0696:
#include "AnglePosition.h" #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 the main Mbed //------------------------------------------------------------------------------ //------------------------------------------------------------------------------ //------------------------------------------------------------------------------ Ticker MyTickerController; //Declare Ticker for Motors Ticker MyTickerServo; //Declare Ticker Servo control Ticker MyTickerMean; //Declare Ticker Signalprocessing Ticker MyTickerFilter; //Declare Ticker Filtering InterruptIn But1(PTA4); //Declare button for emg calibration InterruptIn Mode(PTC6); //Declare button for mode selection AnglePosition Angle; //Declare Angle calculater PIDControl PID; //Declare PID Controller SignalNumber SignalLeft; //Declare Signal determiner for Left arm SignalNumber SignalRight; //Declare Signal determiner for Right arm Movement MoveLeft; //Declare Left Movement determiner Movement MoveRight; //Declare Right movement determiner AnalogIn emg0( A0 ); //Set Inputpin for EMG 0 signal Left AnalogIn emg2( A2 ); //Set Inputpin for EMG 2 signal Right DigitalOut Up( D9 ); //Set digital out for raising the pen off the board DigitalOut Down( D8 ); //Set digital out for lowering the pen off the board //Setting LEDs DigitalOut Led_red(LED_RED); DigitalOut Led_green(LED_GREEN); DigitalOut Led_blue(LED_BLUE); DigitalOut Led_calibration(D2); const float CONTROLLER_TS = 0.02; //Motor frequency const float MEAN_TS = 0.002; //Moving average frequency const float FILTER_TS = 1e3; //Filter frequency const float SERVO_TS = 0.01; //Servo frequency void setled(){ Led_red=1; Led_green=1; Led_blue=1; Led_calibration=0; } //------------------------------------------------------------------------------ //-----------------------------EMG Signals-------------------------------------- //------------------------------------------------------------------------------ // Filtering the signal and getting the usefull information out of it. const int n = 500; //Window size for the mean value, also adjust in signalnumber.cpp const int action =100; //Number of same mean values to change the signalnumber const int m = 500; //Number of samples for calibration int SpeedLeft, SpeedRight; //Strength of the muscles float emg_offsetLeft, emg_offsetRight; //Calibration offsets from zero to one for the right arm float kLeft, kRight; //Scaling factors float Signal_filteredLeft, Signal_filteredRight; //Variables to store the filterd signals //BiQuad filter variables BiQuad LP1L( 0.6389437261127493, 1.2778874522254986, 0.6389437261127493, 1.1429772843080919, 0.4127976201429053 ); //Lowpass filter Biquad BiQuad HP2L( 0.8370879899975344, -1.6741759799950688, 0.8370879899975344, -1.6474576182593796, 0.7008943417307579 ); //Highpass filter Biquad BiQuad NO3L( 0.7063988100714527, -1.1429772843080923, 0.7063988100714527, -1.1429772843080923, 0.41279762014290533); //Notch filter Biquad BiQuad LP1R( 0.6389437261127493, 1.2778874522254986, 0.6389437261127493, 1.1429772843080919, 0.4127976201429053 ); //Lowpass filter Biquad BiQuad HP2R( 0.8370879899975344, -1.6741759799950688, 0.8370879899975344, -1.6474576182593796, 0.7008943417307579 ); //Highpass filter Biquad BiQuad NO3R( 0.7063988100714527, -1.1429772843080923, 0.7063988100714527, -1.1429772843080923, 0.41279762014290533); //Notch filter Biquad BiQuadChain BiQuad_filter_Left; BiQuadChain BiQuad_filter_Right; //Filters----------------------------------------------------------------------- // Filter for the left signal float FilterLeft(float input){ float Signal_filtered= BiQuad_filter_Left.step(input); return Signal_filtered; } // Filter for the right signal float FilterRight(float input){ float Signal_filtered= BiQuad_filter_Right.step(input); return Signal_filtered; } //Calibration------------------------------------------------------------------- // Calibrating the EMG signals. This function needs to be ativated when the // muscles are flexed for over a second void calibration(){ emg_offsetLeft = SignalLeft.calibrate(m, FilterLeft(emg0)); emg_offsetRight = SignalRight.calibrate(m, FilterRight(emg2)); //Setting the emg scaling constants to reed between zero and one. kLeft = 1/emg_offsetLeft; kRight = 1/emg_offsetRight; //Set led to Green to indicate if calibration is done Led_calibration = 1; } //Speedcontrol------------------------------------------------------------------ //Determine the signalnumbers (i.e. speed) based on the strenght of the EMG signals void Speedcontrol(){ //Left Signals Signal_filteredLeft = fabs(FilterLeft(emg0)*kLeft);//*kLeft SpeedLeft = SignalLeft.getspeedLeft(n, action, Signal_filteredLeft); //Right Signals Signal_filteredRight = fabs(FilterRight(emg2)*kRight);//*kRight SpeedRight = SignalRight.getspeedRight(n, action, Signal_filteredRight); } //------------------------------------------------------------------------------ //---------------------------------Servo---------------------------------------- //------------------------------------------------------------------------------ //Sending a pulse to the other Mbed to raise the pen void up(){ Up = 1; Up = 0; Up = 1; } //Sending a pulse to the other Mbed to lower the pen void down(){ Down = 1; Down = 0; Down = 1; } //Setting the begin mode to zero int mode =0; // Enable the servo function when mode 3 or mode 6 is activated. // If the mean value for the Left arm stays long enough high the servo will raise // If the mean value for the Right arm stays long enough high the servo will lower void servo(){ if(mode==3||mode==6){ Signal_filteredLeft = fabs(FilterLeft(emg0)*kLeft); Signal_filteredRight = fabs(FilterRight(emg2)*kRight); SpeedLeft = SignalLeft.getspeedLeft(n, action, Signal_filteredLeft); SpeedRight = SignalRight.getspeedRight(n, action, Signal_filteredRight); if (SpeedLeft == 2){ up(); } if (SpeedRight == 2){ down(); } } } //------------------------------------------------------------------------------ //---------------------------Mode selection------------------------------------- //------------------------------------------------------------------------------ //Function is called when the mode button is pressed and switches modes and led indication void mode_selection(){ //Switching to the next mode. (Total of 6 modes) if(mode == 6){ mode=1; } else{ mode++; } //Setting the LED colour to indicate the operating mode if((mode==3) || (mode==6)) { Led_red = 0, Led_green = 0, Led_blue = 0; //Set Led Mode to White } else if (mode==1) { Led_red = 0, Led_green = 1, Led_blue = 1; //Set Led Mode to Red } else if (mode==2) { Led_red = 1, Led_green = 0, Led_blue = 1; //Set Led Mode to Green } else if (mode==4) { Led_red = 1, Led_green = 1, Led_blue = 0; //Set Led Mode to Blue } else if (mode==5) { Led_red = 0, Led_green = 0, Led_blue = 1; //Set Led Mode to Yellow } } //------------------------------------------------------------------------------ //-------------------------Kinematic Constants---------------------------------- //------------------------------------------------------------------------------ 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 = 650.0, max_rangey = 450.0; //Max range on the y axis const float x_offset = 156.15, y_offset = -76.97; //Starting positions from the shoulder joint const float alpha_offset = -0.4114; const float beta_offset = 0.0486; const float L1 = 450.0, L2 = 490.0; //Length of the bodies //------------------------------------------------------------------------------ //--------------------------------Motor1---------------------------------------- //------------------------------------------------------------------------------ FastPWM motor1(D5); DigitalOut motor1DirectionPin(D4); DigitalIn ENC2A(D12); DigitalIn ENC2B(D13); Encoder encoder1(D13,D12); const float MOTOR1_KP = 40.0; const float MOTOR1_KI = 0.0; const float MOTOR1_KD = 15.0; double M1_v1 = 0.0, M1_v2 = 0.0; //Calculation values const double motor1_gain = 2*PI; const float M1_N = 0.5; float position_math_l =0, position_math_r=0; void motor1_control(){ float reference_beta = Angle.getbeta(max_rangex, max_rangey, x_offset, y_offset, beta_offset, L1, L2, position_math_l, position_math_r); float position_beta = RAD_PER_PULSE * encoder1.getPosition(); float error_beta = reference_beta-position_beta; float magnitude1 = PID.get(error_beta, MOTOR1_KP, MOTOR1_KI, MOTOR1_KD, CONTROLLER_TS, M1_N, M1_v1, M1_v2) / motor1_gain; //Determine Motor Magnitude if (fabs(magnitude1)>1) { motor1 = 1; } else { motor1 = fabs(magnitude1); } // Determine Motor Direction if (magnitude1 < 0){ motor1DirectionPin = 1; } else{ motor1DirectionPin = 0; } } //------------------------------------------------------------------------------ //--------------------------------Motor2---------------------------------------- //------------------------------------------------------------------------------ FastPWM motor2(D6); DigitalOut motor2DirectionPin(D7); DigitalIn ENC1A(D10); DigitalIn ENC1B(D11); Encoder encoder2(D10,D11); const float MOTOR2_KP = 60.0; const float MOTOR2_KI = 0.0; const float MOTOR2_KD = 15.0; double M2_v1 = 0.0, M2_v2 = 0.0; //Calculation values const double motor2_gain = 2*PI; const float M2_N = 0.5; void motor2_control(){ float reference_alpha = Angle.getalpha(max_rangex, max_rangey, x_offset, y_offset, alpha_offset, L1, L2, position_math_l, position_math_r); float position_alpha = RAD_PER_PULSE * -encoder2.getPosition(); float error_alpha = reference_alpha-position_alpha; float magnitude2 = PID.get(error_alpha, MOTOR2_KP, MOTOR2_KI, MOTOR2_KD, CONTROLLER_TS, M2_N, M1_v1, M1_v2) / motor2_gain; //Determine Motor Magnitude if (fabs(magnitude2)>1) { motor2 = 1; } else { motor2 = fabs(magnitude2); } //Determine Motor Direction if (magnitude2 > 0){ motor2DirectionPin = 1; } else{ motor2DirectionPin = 0; } } //------------------------------------------------------------------------------ //----------------------------Motor controller---------------------------------- //------------------------------------------------------------------------------ int direction_l, direction_r; //Variables to store the directions float magnitude1, magnitude2; //Variables to store the magnitude signal void motor_control(){ direction_l = MoveLeft.getdirectionLeft(mode); //x-direction direction_r = MoveRight.getdirectionRight(mode); //y-direction position_math_l= MoveLeft.getpositionLeft(SpeedLeft, mode, max_rangex); //x-direction position_math_r= MoveRight.getpositionRight(SpeedRight, mode, max_rangey); //y-direction motor1_control(); motor2_control(); } //------------------------------------------------------------------------------ //-----------------------------Filter Update------------------------------------ //------------------------------------------------------------------------------ //Updating Filter values void FilterUpdate(){ FilterLeft(emg0.read()); FilterRight(emg2.read()); SignalLeft.getmeanLeft(n, fabs(FilterLeft(emg0.read()))); SignalRight.getmeanRight(n, fabs(FilterRight(emg2.read()))); } //------------------------------------------------------------------------------ //------------------------------Main-------------------------------------------- //------------------------------------------------------------------------------ int main(){ setled(); //Creating Filters BiQuad_filter_Left.add( &LP1L ).add( &HP2L ).add( &NO3L); BiQuad_filter_Right.add( &LP1R ).add( &HP2R ).add( &NO3R); //Button detection But1.rise(&calibration); Mode.rise(&mode_selection); //Setting motor periods motor1.period(0.0001f); motor2.period(0.0001f); //Tickers MyTickerController.attach(&motor_control, CONTROLLER_TS); MyTickerMean.attach(&Speedcontrol, MEAN_TS); MyTickerServo.attach(&servo, SERVO_TS); MyTickerFilter.attach_us(&FilterUpdate, FILTER_TS); while(1) { } }