Eindelijk!!!!!
Dependencies: AnglePosition2 Encoder FastPWM HIDScope MODSERIAL Movement PIDController Servo SignalNumber biquadFilter mbed
Fork of Robot_control by
Diff: main.cpp
- Revision:
- 10:614050a50c2f
- Parent:
- 8:24f6744d47b2
- Child:
- 11:28b5ec12a4b9
--- a/main.cpp Wed Nov 01 22:47:30 2017 +0000 +++ b/main.cpp Thu Nov 02 10:17:33 2017 +0000 @@ -46,9 +46,9 @@ //-----------------------------EMG Signals-------------------------------------- //------------------------------------------------------------------------------ // Filtering the signal and getting the usefull information out of it. -const int n = 1500; //Window size for the mean value, also adjust in signalnumber.cpp +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 = 1500; //Number of samples for calibration +const int m = 500; //Number of samples for calibration 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 @@ -105,20 +105,20 @@ //---------------------------------Servo---------------------------------------- //------------------------------------------------------------------------------ void servo(){ - Signal_filteredLeft = fabs(FilterLeft(emg0));//*kLeft - Signal_filteredRight = fabs(FilterRight(emg2)); //*kRight + Signal_filteredLeft = fabs(FilterLeft(emg0)*kLeft);//*kLeft CaseLeft = SignalLeft.getnumberLeft(n, action, Signal_filteredLeft); + Signal_filteredRight = fabs(FilterRight(emg2)*kRight);//*kRight CaseRight = SignalRight.getnumberRight(n, action, Signal_filteredRight); - if (CaseLeft>=3){ + if (CaseLeft>=2){ Up = 0; Up = 1; } - else if (CaseRight >=3){ + else if (CaseRight >=2){ Down = 0; Down = 1; } } -int milli =0; + //------------------------------------------------------------------------------ //---------------------------Mode selection------------------------------------- @@ -137,6 +137,34 @@ servo(); } pc.printf("\r\n mode = %i \r\n", mode); + + + /*if((mode==3) || (mode==6)) { + Led_red = 0; + Led_green = 0; + Led_blue = 0; + } + else if (mode==1) { + Led_red = 0; + Led_green = 1; + Led_blue = 1; + } + else if (mode==2) { + Led_red = 1; + Led_green = 0; + Led_blue = 1; + } + else if (mode==4) { + Led_red = 1; + Led_green = 1; + Led_blue = 0; + } + else if (mode==5) { + Led_red = 0; + Led_green = 0; + Led_blue = 1; + }*/ + } // Control mode selection------------------------------------------------------- @@ -175,10 +203,10 @@ //------------------------------------------------------------------------------ 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_rangey = 300.0; //Max range on the y axis +const float max_rangex = 700.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 = -(21.52/180)*PI; //Begin angle Alpha at zero zero -const float beta_offset = 0.0; //Begin angle Beta at zero zero +const float alpha_offset = -0.4114; +const float beta_offset = 0.0486; const float L1 = 450.0, L2 = 490.0; //Length of the bodies //------------------------------------------------------------------------------ @@ -197,12 +225,21 @@ const float M1_N = 0.5; float position_math_l =0, position_math_r=0; -void motor1_control(){ +float motor1_control(){ float reference_alpha = Angle.getbeta(max_rangex, max_rangey, x_offset, y_offset, beta_offset, L1, L2, position_math_l, position_math_r); 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 Magnitude + if (fabs(magnitude1)>1) { + motor1 = 1; + } + else { + motor1 = fabs(magnitude1); + } + + // Determine Motor Direction if (magnitude1 < 0){ motor1DirectionPin = 1; @@ -210,6 +247,7 @@ else{ motor1DirectionPin = 0; } + return magnitude1; } //------------------------------------------------------------------------------ @@ -227,12 +265,21 @@ const double motor2_gain = 2*PI; const float M2_N = 0.5; -void motor2_control(){ +float motor2_control(){ float reference_beta = Angle.getalpha(max_rangex, max_rangey, x_offset, y_offset, alpha_offset, L1, L2, position_math_l, position_math_r); 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 Magnitude + if (fabs(magnitude2)>1) { + motor2 = 1; + } + else { + motor2 = fabs(magnitude2); + } + + //Determine Motor Direction if (magnitude2 > 0){ motor2DirectionPin = 1; @@ -240,22 +287,28 @@ else{ motor2DirectionPin = 0; } + return magnitude2; } //------------------------------------------------------------------------------ //----------------------------Motor controller---------------------------------- //------------------------------------------------------------------------------ int direction_l, direction_r; +float magnitude1, magnitude2; void motor_control(){ - direction_l = MoveLeft.getdirectionLeft(mode); - direction_r = MoveRight.getdirectionRight(mode); - position_math_l= MoveLeft.getpositionLeft(CaseLeft, mode, max_rangex); - position_math_r= MoveRight.getpositionRight(CaseRight, mode, max_rangey); -// motor1_control(); -// motor2_control(); + direction_l = MoveLeft.getdirectionLeft(mode); //x-direction + direction_r = MoveRight.getdirectionRight(mode); //y-direction + position_math_l= MoveLeft.getpositionLeft(CaseLeft, mode, max_rangex); //x-direction + position_math_r= MoveRight.getpositionRight(CaseRight, mode, max_rangey); //y-direction + magnitude1 = motor1_control(); + magnitude2 = motor2_control(); + + + + } -//------------------------------------------------------------------------------ +/*//------------------------------------------------------------------------------ //-------------------------------HID Scope-------------------------------------- //------------------------------------------------------------------------------ @@ -272,7 +325,7 @@ //Working //SignalLeft.getmeanLeft(n, fabs(FilterLeft(emg0.read()))) -//SignalRight.getmeanRight(n, fabs(FilterRight(emg2.read()))) +//SignalRight.getmeanRight(n, fabs(FilterRight(emg2.read())))*/ //------------------------------------------------------------------------------ //------------------------------Main-------------------------------------------- @@ -290,13 +343,14 @@ MyControllerTicker.attach(&motor_control, CONTROLLER_TS); //Determining motorposiitons MyTickerMean.attach(&signalnumber, MEAN_TS); //Detemining the signalnumbers // MyTickerMode.attach(&signalmode, MEAN_TS); - +/* //Scope scopeTimer.attach_us(&scope, &HIDScope::send, 2e4); - controllerTimer.attach_us(&HID_Monitor, 1e3); + controllerTimer.attach_us(&HID_Monitor, 1e3);*/ while(1) { - pc.printf(" direction %i , %i Signal numbers %i %i Position %f %f \r\n", direction_l, direction_r, CaseLeft, CaseRight, position_math_l, position_math_r); + //pc.printf(" direction %i , %i Signal numbers %i %i Position %f %f \r\n", direction_l, direction_r, CaseLeft, CaseRight, position_math_l, position_math_r); + pc.printf("direction %i , %i Signal numbers %i %i Position %f %f magnitude1 =%f magnitude2=%f \r\n",direction_l, direction_r, CaseLeft, CaseRight, position_math_l, position_math_r, magnitude1, magnitude2); wait(0.1f); } }