Eindelijk!!!!!
Dependencies: AnglePosition2 Encoder FastPWM HIDScope MODSERIAL Movement PIDController Servo SignalNumber biquadFilter mbed
Fork of Robot_control by
main.cpp
- Committer:
- peterknoben
- Date:
- 2017-10-31
- Revision:
- 4:e15fc329b88b
- Parent:
- 3:c768a37620c9
- Child:
- 5:b4abbd3c513c
File content as of revision 4:e15fc329b88b:
#include "MODSERIAL.h" #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 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 MyTickerMean; //Declare Ticker Signalprocessing 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 SignalLeft; //Declare Signal determiner for Left arm SignalNumber SignalRight; //Declare Signal determiner for Right arm 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 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.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-------------------------------------- //------------------------------------------------------------------------------ // 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 = 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 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; void setled(){ Led_red=0; Led_green=1; Led_blue=1; } // 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-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(){ //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; //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 //------------------------------------------------------------------------------ //--------------------------------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; double M1_v2 = 0.0; const double motor1_gain = 2*PI; const float M1_N = 0.5; 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; 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); // 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_err_int = 0; 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; 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); //Determine Motor Direction if (magnitude2 > 0){ motor2DirectionPin = 1; } else{ motor2DirectionPin = 0; } } //------------------------------------------------------------------------------ //------------------------------------------------------------------------------ //------------------------------------------------------------------------------ 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); while(1) {} }