Eindelijk!!!!!
Dependencies: AnglePosition2 Encoder FastPWM HIDScope MODSERIAL Movement PIDController Servo SignalNumber biquadFilter mbed
Fork of Robot_control by
main.cpp@10:614050a50c2f, 2017-11-02 (annotated)
- Committer:
- DBerendsen
- Date:
- Thu Nov 02 10:17:33 2017 +0000
- Revision:
- 10:614050a50c2f
- Parent:
- 8:24f6744d47b2
- Child:
- 11:28b5ec12a4b9
Eerste keer super mooi werkend;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
peterknoben | 3:c768a37620c9 | 1 | #include "MODSERIAL.h" |
peterknoben | 1:406519ff0f17 | 2 | #include "AnglePosition.h" |
peterknoben | 1:406519ff0f17 | 3 | #include "PIDControl.h" |
peterknoben | 3:c768a37620c9 | 4 | #include "BiQuad.h" |
peterknoben | 3:c768a37620c9 | 5 | #include "signalnumber.h" |
peterknoben | 4:e15fc329b88b | 6 | #include "Movement.h" |
DBerendsen | 0:5f4bc2d63814 | 7 | #include "mbed.h" |
DBerendsen | 0:5f4bc2d63814 | 8 | #include "encoder.h" |
DBerendsen | 0:5f4bc2d63814 | 9 | #include "Servo.h" |
peterknoben | 1:406519ff0f17 | 10 | #include "FastPWM.h" |
peterknoben | 8:24f6744d47b2 | 11 | #include "HIDScope.h" |
DBerendsen | 0:5f4bc2d63814 | 12 | |
peterknoben | 4:e15fc329b88b | 13 | //This code is for Mbed 2 |
DBerendsen | 0:5f4bc2d63814 | 14 | //------------------------------------------------------------------------------ |
DBerendsen | 0:5f4bc2d63814 | 15 | //------------------------------------------------------------------------------ |
DBerendsen | 0:5f4bc2d63814 | 16 | //------------------------------------------------------------------------------ |
peterknoben | 3:c768a37620c9 | 17 | MODSERIAL pc(USBTX, USBRX); //Establish connection |
peterknoben | 8:24f6744d47b2 | 18 | Ticker MyControllerTicker; //Declare Ticker for Motors |
peterknoben | 8:24f6744d47b2 | 19 | Ticker MyTickerMode; //Declare Ticker |
peterknoben | 3:c768a37620c9 | 20 | Ticker MyTickerMean; //Declare Ticker Signalprocessing |
peterknoben | 3:c768a37620c9 | 21 | |
peterknoben | 8:24f6744d47b2 | 22 | InterruptIn But1(PTA4); //Declare button for min calibration |
peterknoben | 8:24f6744d47b2 | 23 | InterruptIn Mode(PTC6); //Declare button for max calibration |
peterknoben | 3:c768a37620c9 | 24 | |
peterknoben | 1:406519ff0f17 | 25 | AnglePosition Angle; //Declare Angle calculater |
peterknoben | 1:406519ff0f17 | 26 | PIDControl PID; //Declare PID Controller |
peterknoben | 4:e15fc329b88b | 27 | SignalNumber SignalLeft; //Declare Signal determiner for Left arm |
peterknoben | 4:e15fc329b88b | 28 | SignalNumber SignalRight; //Declare Signal determiner for Right arm |
peterknoben | 5:b4abbd3c513c | 29 | SignalNumber SignalMode; |
peterknoben | 4:e15fc329b88b | 30 | Movement MoveLeft; //Declare Movement determiner |
peterknoben | 4:e15fc329b88b | 31 | Movement MoveRight; |
peterknoben | 3:c768a37620c9 | 32 | |
peterknoben | 4:e15fc329b88b | 33 | AnalogIn emg0( A0 ); //Set Inputpin for EMG 0 signal Left |
peterknoben | 4:e15fc329b88b | 34 | AnalogIn emg2( A2 ); //Set Inputpin for EMG 2 signal Right |
peterknoben | 4:e15fc329b88b | 35 | AnalogIn emg4( A4 ); //Set Inputpin for EMG 4 signal Mode |
peterknoben | 5:b4abbd3c513c | 36 | DigitalOut Up( D9 ); //Set digital in for mode selection |
peterknoben | 5:b4abbd3c513c | 37 | DigitalOut Down( D8 ); |
peterknoben | 4:e15fc329b88b | 38 | DigitalOut Led_red(LED_RED); |
peterknoben | 4:e15fc329b88b | 39 | DigitalOut Led_green(LED_GREEN); |
peterknoben | 4:e15fc329b88b | 40 | DigitalOut Led_blue(LED_BLUE); |
peterknoben | 1:406519ff0f17 | 41 | |
peterknoben | 1:406519ff0f17 | 42 | const float CONTROLLER_TS = 0.02; //Motor frequency |
peterknoben | 4:e15fc329b88b | 43 | const float MEAN_TS = 0.002; //Filter frequency |
peterknoben | 4:e15fc329b88b | 44 | |
peterknoben | 3:c768a37620c9 | 45 | //------------------------------------------------------------------------------ |
peterknoben | 3:c768a37620c9 | 46 | //-----------------------------EMG Signals-------------------------------------- |
peterknoben | 3:c768a37620c9 | 47 | //------------------------------------------------------------------------------ |
peterknoben | 4:e15fc329b88b | 48 | // Filtering the signal and getting the usefull information out of it. |
DBerendsen | 10:614050a50c2f | 49 | const int n = 500; //Window size for the mean value, also adjust in signalnumber.cpp |
peterknoben | 8:24f6744d47b2 | 50 | const int action =100; //Number of same mean values to change the signalnumber |
DBerendsen | 10:614050a50c2f | 51 | const int m = 500; //Number of samples for calibration |
peterknoben | 5:b4abbd3c513c | 52 | int CaseLeft, CaseRight, CaseMode; //Strength of the muscles |
peterknoben | 5:b4abbd3c513c | 53 | float emg_offsetLeft, emg_offsetmaxLeft; //Calibration offsets from zero to one for the left arm |
peterknoben | 5:b4abbd3c513c | 54 | float emg_offsetRight, emg_offsetmaxRight; //Calibration offsets from zero to one for the right arm |
peterknoben | 5:b4abbd3c513c | 55 | float emg_offsetMode, emg_offsetmaxMode; |
peterknoben | 5:b4abbd3c513c | 56 | float mean_value_left, mean_value_right, mean_value_mode; //Mean value of the filtered system |
peterknoben | 5:b4abbd3c513c | 57 | float kLeft, kRight, kMode; //Scaling factors |
peterknoben | 6:edd30e11f3c7 | 58 | float Signal_filteredLeft, Signal_filteredRight, Signal_filteredMode; |
peterknoben | 3:c768a37620c9 | 59 | |
peterknoben | 4:e15fc329b88b | 60 | //BiQuad filter variables |
peterknoben | 6:edd30e11f3c7 | 61 | BiQuad LP1L( 0.6389437261127493, 1.2778874522254986, 0.6389437261127493, 1.1429772843080919, 0.4127976201429053 ); //Lowpass filter Biquad |
peterknoben | 6:edd30e11f3c7 | 62 | BiQuad HP2L( 0.8370879899975344, -1.6741759799950688, 0.8370879899975344, -1.6474576182593796, 0.7008943417307579 ); //Highpass filter Biquad |
peterknoben | 6:edd30e11f3c7 | 63 | BiQuad NO3L( 0.7063988100714527, -1.1429772843080923, 0.7063988100714527, -1.1429772843080923, 0.41279762014290533); //Notch filter Biquad |
peterknoben | 6:edd30e11f3c7 | 64 | BiQuad LP1R( 0.6389437261127493, 1.2778874522254986, 0.6389437261127493, 1.1429772843080919, 0.4127976201429053 ); //Lowpass filter Biquad |
peterknoben | 6:edd30e11f3c7 | 65 | BiQuad HP2R( 0.8370879899975344, -1.6741759799950688, 0.8370879899975344, -1.6474576182593796, 0.7008943417307579 ); //Highpass filter Biquad |
peterknoben | 6:edd30e11f3c7 | 66 | BiQuad NO3R( 0.7063988100714527, -1.1429772843080923, 0.7063988100714527, -1.1429772843080923, 0.41279762014290533); //Notch filter Biquad |
peterknoben | 6:edd30e11f3c7 | 67 | BiQuad LP1M( 0.6389437261127493, 1.2778874522254986, 0.6389437261127493, 1.1429772843080919, 0.4127976201429053 ); //Lowpass filter Biquad |
peterknoben | 6:edd30e11f3c7 | 68 | BiQuad HP2M( 0.8370879899975344, -1.6741759799950688, 0.8370879899975344, -1.6474576182593796, 0.7008943417307579 ); //Highpass filter Biquad |
peterknoben | 6:edd30e11f3c7 | 69 | BiQuad NO3M( 0.7063988100714527, -1.1429772843080923, 0.7063988100714527, -1.1429772843080923, 0.41279762014290533); //Notch filter Biquad |
peterknoben | 6:edd30e11f3c7 | 70 | BiQuadChain BiQuad_filter_Left; |
peterknoben | 6:edd30e11f3c7 | 71 | BiQuadChain BiQuad_filter_Right; |
peterknoben | 6:edd30e11f3c7 | 72 | BiQuadChain BiQuad_filter_Mode; |
peterknoben | 3:c768a37620c9 | 73 | |
peterknoben | 7:105e3ae0fb19 | 74 | float FilterLeft(float input){ |
peterknoben | 7:105e3ae0fb19 | 75 | float Signal_filtered= BiQuad_filter_Left.step(input); |
peterknoben | 7:105e3ae0fb19 | 76 | return Signal_filtered; |
peterknoben | 7:105e3ae0fb19 | 77 | } |
peterknoben | 7:105e3ae0fb19 | 78 | float FilterRight(float input){ |
peterknoben | 7:105e3ae0fb19 | 79 | float Signal_filtered= BiQuad_filter_Right.step(input); |
peterknoben | 7:105e3ae0fb19 | 80 | return Signal_filtered; |
peterknoben | 7:105e3ae0fb19 | 81 | } |
peterknoben | 7:105e3ae0fb19 | 82 | float FilterMode(float input){ |
peterknoben | 7:105e3ae0fb19 | 83 | float Signal_filtered= BiQuad_filter_Mode.step(input); |
peterknoben | 7:105e3ae0fb19 | 84 | return Signal_filtered; |
peterknoben | 7:105e3ae0fb19 | 85 | } |
peterknoben | 5:b4abbd3c513c | 86 | |
peterknoben | 5:b4abbd3c513c | 87 | //Calibration------------------------------------------------------------------- |
peterknoben | 4:e15fc329b88b | 88 | void setled(){ |
peterknoben | 5:b4abbd3c513c | 89 | Led_red=0; Led_green=1; Led_blue=1; |
peterknoben | 5:b4abbd3c513c | 90 | } |
peterknoben | 8:24f6744d47b2 | 91 | |
peterknoben | 5:b4abbd3c513c | 92 | // One calibration |
peterknoben | 5:b4abbd3c513c | 93 | void maxcalibration(){ |
peterknoben | 5:b4abbd3c513c | 94 | pc.printf("start max calibration \r\n"); |
peterknoben | 8:24f6744d47b2 | 95 | emg_offsetmaxLeft = SignalLeft.calibrate(m, FilterLeft(emg0)); |
peterknoben | 8:24f6744d47b2 | 96 | emg_offsetmaxRight = SignalRight.calibrate(m, FilterRight(emg2)); |
peterknoben | 8:24f6744d47b2 | 97 | emg_offsetmaxMode = SignalMode.calibrate(m, FilterMode(emg4)); |
peterknoben | 5:b4abbd3c513c | 98 | kLeft = 1/emg_offsetmaxLeft; |
peterknoben | 5:b4abbd3c513c | 99 | kRight = 1/emg_offsetmaxRight; |
peterknoben | 5:b4abbd3c513c | 100 | kMode = 1/emg_offsetmaxMode; |
peterknoben | 5:b4abbd3c513c | 101 | Led_red=1; //Set led to Green |
peterknoben | 3:c768a37620c9 | 102 | } |
peterknoben | 3:c768a37620c9 | 103 | |
peterknoben | 5:b4abbd3c513c | 104 | //------------------------------------------------------------------------------ |
peterknoben | 5:b4abbd3c513c | 105 | //---------------------------------Servo---------------------------------------- |
peterknoben | 5:b4abbd3c513c | 106 | //------------------------------------------------------------------------------ |
peterknoben | 5:b4abbd3c513c | 107 | void servo(){ |
DBerendsen | 10:614050a50c2f | 108 | Signal_filteredLeft = fabs(FilterLeft(emg0)*kLeft);//*kLeft |
peterknoben | 8:24f6744d47b2 | 109 | CaseLeft = SignalLeft.getnumberLeft(n, action, Signal_filteredLeft); |
DBerendsen | 10:614050a50c2f | 110 | Signal_filteredRight = fabs(FilterRight(emg2)*kRight);//*kRight |
peterknoben | 8:24f6744d47b2 | 111 | CaseRight = SignalRight.getnumberRight(n, action, Signal_filteredRight); |
DBerendsen | 10:614050a50c2f | 112 | if (CaseLeft>=2){ |
peterknoben | 6:edd30e11f3c7 | 113 | Up = 0; |
peterknoben | 6:edd30e11f3c7 | 114 | Up = 1; |
peterknoben | 5:b4abbd3c513c | 115 | } |
DBerendsen | 10:614050a50c2f | 116 | else if (CaseRight >=2){ |
peterknoben | 6:edd30e11f3c7 | 117 | Down = 0; |
peterknoben | 5:b4abbd3c513c | 118 | Down = 1; |
peterknoben | 5:b4abbd3c513c | 119 | } |
peterknoben | 5:b4abbd3c513c | 120 | } |
DBerendsen | 10:614050a50c2f | 121 | |
peterknoben | 5:b4abbd3c513c | 122 | |
peterknoben | 5:b4abbd3c513c | 123 | //------------------------------------------------------------------------------ |
peterknoben | 5:b4abbd3c513c | 124 | //---------------------------Mode selection------------------------------------- |
peterknoben | 5:b4abbd3c513c | 125 | //------------------------------------------------------------------------------ |
peterknoben | 5:b4abbd3c513c | 126 | int mode =0; |
peterknoben | 5:b4abbd3c513c | 127 | |
peterknoben | 5:b4abbd3c513c | 128 | //Recieving mode selection from EMG mode signal |
peterknoben | 5:b4abbd3c513c | 129 | void mode_selection(){ |
peterknoben | 5:b4abbd3c513c | 130 | if(mode ==6){ |
peterknoben | 5:b4abbd3c513c | 131 | mode=1; |
peterknoben | 5:b4abbd3c513c | 132 | } |
peterknoben | 5:b4abbd3c513c | 133 | else{ |
peterknoben | 5:b4abbd3c513c | 134 | mode++; |
peterknoben | 5:b4abbd3c513c | 135 | } |
peterknoben | 5:b4abbd3c513c | 136 | if (mode==3||mode==6){ |
peterknoben | 5:b4abbd3c513c | 137 | servo(); |
peterknoben | 5:b4abbd3c513c | 138 | } |
peterknoben | 5:b4abbd3c513c | 139 | pc.printf("\r\n mode = %i \r\n", mode); |
DBerendsen | 10:614050a50c2f | 140 | |
DBerendsen | 10:614050a50c2f | 141 | |
DBerendsen | 10:614050a50c2f | 142 | /*if((mode==3) || (mode==6)) { |
DBerendsen | 10:614050a50c2f | 143 | Led_red = 0; |
DBerendsen | 10:614050a50c2f | 144 | Led_green = 0; |
DBerendsen | 10:614050a50c2f | 145 | Led_blue = 0; |
DBerendsen | 10:614050a50c2f | 146 | } |
DBerendsen | 10:614050a50c2f | 147 | else if (mode==1) { |
DBerendsen | 10:614050a50c2f | 148 | Led_red = 0; |
DBerendsen | 10:614050a50c2f | 149 | Led_green = 1; |
DBerendsen | 10:614050a50c2f | 150 | Led_blue = 1; |
DBerendsen | 10:614050a50c2f | 151 | } |
DBerendsen | 10:614050a50c2f | 152 | else if (mode==2) { |
DBerendsen | 10:614050a50c2f | 153 | Led_red = 1; |
DBerendsen | 10:614050a50c2f | 154 | Led_green = 0; |
DBerendsen | 10:614050a50c2f | 155 | Led_blue = 1; |
DBerendsen | 10:614050a50c2f | 156 | } |
DBerendsen | 10:614050a50c2f | 157 | else if (mode==4) { |
DBerendsen | 10:614050a50c2f | 158 | Led_red = 1; |
DBerendsen | 10:614050a50c2f | 159 | Led_green = 1; |
DBerendsen | 10:614050a50c2f | 160 | Led_blue = 0; |
DBerendsen | 10:614050a50c2f | 161 | } |
DBerendsen | 10:614050a50c2f | 162 | else if (mode==5) { |
DBerendsen | 10:614050a50c2f | 163 | Led_red = 0; |
DBerendsen | 10:614050a50c2f | 164 | Led_green = 0; |
DBerendsen | 10:614050a50c2f | 165 | Led_blue = 1; |
DBerendsen | 10:614050a50c2f | 166 | }*/ |
DBerendsen | 10:614050a50c2f | 167 | |
peterknoben | 5:b4abbd3c513c | 168 | } |
peterknoben | 5:b4abbd3c513c | 169 | |
peterknoben | 5:b4abbd3c513c | 170 | // Control mode selection------------------------------------------------------- |
peterknoben | 5:b4abbd3c513c | 171 | |
peterknoben | 5:b4abbd3c513c | 172 | //Determine the signalnumbers (i.e. speed) based on the strenght of the EMG signals |
peterknoben | 3:c768a37620c9 | 173 | void signalnumber(){ |
peterknoben | 4:e15fc329b88b | 174 | //Left |
peterknoben | 8:24f6744d47b2 | 175 | Signal_filteredLeft = fabs(FilterLeft(emg0)*kLeft);//*kLeft |
peterknoben | 8:24f6744d47b2 | 176 | // mean_value_left = SignalLeft.getmeanLeft(n, Signal_filteredLeft); |
peterknoben | 8:24f6744d47b2 | 177 | mean_value_left = SignalLeft.getmeanLeft(n, fabs(FilterLeft(emg0.read())*kLeft)); |
peterknoben | 8:24f6744d47b2 | 178 | CaseLeft = SignalLeft.getnumberLeft(n, action, Signal_filteredLeft); |
peterknoben | 4:e15fc329b88b | 179 | //Right |
peterknoben | 8:24f6744d47b2 | 180 | Signal_filteredRight = fabs(FilterRight(emg2)*kRight);//*kRight |
peterknoben | 8:24f6744d47b2 | 181 | // mean_value_right = SignalRight.getmeanRight(n, Signal_filteredRight); |
peterknoben | 8:24f6744d47b2 | 182 | mean_value_right = SignalRight.getmeanRight(n, fabs(FilterRight(emg2.read())*kRight)); |
peterknoben | 8:24f6744d47b2 | 183 | CaseRight = SignalRight.getnumberRight(n, action, Signal_filteredRight); |
peterknoben | 5:b4abbd3c513c | 184 | //Mode |
peterknoben | 8:24f6744d47b2 | 185 | /* |
peterknoben | 8:24f6744d47b2 | 186 | Signal_filteredMode = fabs(FilterMode(emg4));//*kMode |
peterknoben | 8:24f6744d47b2 | 187 | mean_value_mode = SignalMode.getmeanMode(n, Signal_filteredMode); |
peterknoben | 8:24f6744d47b2 | 188 | CaseMode = SignalMode.getnumberMode(n, action, Signal_filteredMode); |
peterknoben | 8:24f6744d47b2 | 189 | if(CaseMode >= 3){ |
peterknoben | 5:b4abbd3c513c | 190 | milli ++; |
peterknoben | 5:b4abbd3c513c | 191 | if(milli>=150){ |
peterknoben | 5:b4abbd3c513c | 192 | mode_selection(); |
peterknoben | 5:b4abbd3c513c | 193 | milli=0; |
peterknoben | 5:b4abbd3c513c | 194 | } |
peterknoben | 5:b4abbd3c513c | 195 | } |
peterknoben | 5:b4abbd3c513c | 196 | else{ |
peterknoben | 5:b4abbd3c513c | 197 | milli=0; |
peterknoben | 6:edd30e11f3c7 | 198 | }*/ |
peterknoben | 3:c768a37620c9 | 199 | } |
peterknoben | 3:c768a37620c9 | 200 | |
peterknoben | 1:406519ff0f17 | 201 | //------------------------------------------------------------------------------ |
peterknoben | 1:406519ff0f17 | 202 | //-------------------------Kinematic Constants---------------------------------- |
peterknoben | 1:406519ff0f17 | 203 | //------------------------------------------------------------------------------ |
peterknoben | 4:e15fc329b88b | 204 | const double RAD_PER_PULSE = 0.00074799825*2; //Number of radials per pulse from the encoder |
peterknoben | 4:e15fc329b88b | 205 | const double PI = 3.14159265358979323846; //Pi |
DBerendsen | 10:614050a50c2f | 206 | const float max_rangex = 700.0, max_rangey = 450.0; //Max range on the y axis |
peterknoben | 8:24f6744d47b2 | 207 | const float x_offset = 156.15, y_offset = -76.97; //Starting positions from the shoulder joint |
DBerendsen | 10:614050a50c2f | 208 | const float alpha_offset = -0.4114; |
DBerendsen | 10:614050a50c2f | 209 | const float beta_offset = 0.0486; |
peterknoben | 8:24f6744d47b2 | 210 | const float L1 = 450.0, L2 = 490.0; //Length of the bodies |
peterknoben | 5:b4abbd3c513c | 211 | |
DBerendsen | 0:5f4bc2d63814 | 212 | //------------------------------------------------------------------------------ |
DBerendsen | 0:5f4bc2d63814 | 213 | //--------------------------------Motor1---------------------------------------- |
DBerendsen | 0:5f4bc2d63814 | 214 | //------------------------------------------------------------------------------ |
peterknoben | 1:406519ff0f17 | 215 | FastPWM motor1(D5); |
DBerendsen | 0:5f4bc2d63814 | 216 | DigitalOut motor1DirectionPin(D4); |
DBerendsen | 0:5f4bc2d63814 | 217 | DigitalIn ENC2A(D12); |
DBerendsen | 0:5f4bc2d63814 | 218 | DigitalIn ENC2B(D13); |
DBerendsen | 0:5f4bc2d63814 | 219 | Encoder encoder1(D13,D12); |
peterknoben | 1:406519ff0f17 | 220 | const float MOTOR1_KP = 40.0; |
peterknoben | 1:406519ff0f17 | 221 | const float MOTOR1_KI = 0.0; |
peterknoben | 1:406519ff0f17 | 222 | const float MOTOR1_KD = 15.0; |
peterknoben | 5:b4abbd3c513c | 223 | double M1_v1 = 0.0, M1_v2 = 0.0; //Calculation values |
DBerendsen | 0:5f4bc2d63814 | 224 | const double motor1_gain = 2*PI; |
peterknoben | 1:406519ff0f17 | 225 | const float M1_N = 0.5; |
peterknoben | 8:24f6744d47b2 | 226 | float position_math_l =0, position_math_r=0; |
DBerendsen | 0:5f4bc2d63814 | 227 | |
DBerendsen | 10:614050a50c2f | 228 | float motor1_control(){ |
peterknoben | 8:24f6744d47b2 | 229 | float reference_alpha = Angle.getbeta(max_rangex, max_rangey, x_offset, y_offset, beta_offset, L1, L2, position_math_l, position_math_r); |
DBerendsen | 0:5f4bc2d63814 | 230 | float position_alpha = RAD_PER_PULSE * encoder1.getPosition(); |
DBerendsen | 0:5f4bc2d63814 | 231 | float error_alpha = reference_alpha-position_alpha; |
peterknoben | 1:406519ff0f17 | 232 | float magnitude1 = PID.get(error_alpha, MOTOR1_KP, MOTOR1_KI, MOTOR1_KD, CONTROLLER_TS, M1_N, M1_v1, M1_v2) / motor1_gain; |
DBerendsen | 10:614050a50c2f | 233 | |
DBerendsen | 10:614050a50c2f | 234 | //Determine Motor Magnitude |
DBerendsen | 10:614050a50c2f | 235 | if (fabs(magnitude1)>1) { |
DBerendsen | 10:614050a50c2f | 236 | motor1 = 1; |
DBerendsen | 10:614050a50c2f | 237 | } |
DBerendsen | 10:614050a50c2f | 238 | else { |
DBerendsen | 10:614050a50c2f | 239 | motor1 = fabs(magnitude1); |
DBerendsen | 10:614050a50c2f | 240 | } |
DBerendsen | 10:614050a50c2f | 241 | |
DBerendsen | 10:614050a50c2f | 242 | |
peterknoben | 1:406519ff0f17 | 243 | // Determine Motor Direction |
peterknoben | 1:406519ff0f17 | 244 | if (magnitude1 < 0){ |
DBerendsen | 0:5f4bc2d63814 | 245 | motor1DirectionPin = 1; |
DBerendsen | 0:5f4bc2d63814 | 246 | } |
DBerendsen | 0:5f4bc2d63814 | 247 | else{ |
DBerendsen | 0:5f4bc2d63814 | 248 | motor1DirectionPin = 0; |
DBerendsen | 0:5f4bc2d63814 | 249 | } |
DBerendsen | 10:614050a50c2f | 250 | return magnitude1; |
DBerendsen | 0:5f4bc2d63814 | 251 | } |
DBerendsen | 0:5f4bc2d63814 | 252 | |
DBerendsen | 0:5f4bc2d63814 | 253 | //------------------------------------------------------------------------------ |
DBerendsen | 0:5f4bc2d63814 | 254 | //--------------------------------Motor2---------------------------------------- |
DBerendsen | 0:5f4bc2d63814 | 255 | //------------------------------------------------------------------------------ |
peterknoben | 1:406519ff0f17 | 256 | FastPWM motor2(D6); |
DBerendsen | 0:5f4bc2d63814 | 257 | DigitalOut motor2DirectionPin(D7); |
DBerendsen | 0:5f4bc2d63814 | 258 | DigitalIn ENC1A(D10); |
DBerendsen | 0:5f4bc2d63814 | 259 | DigitalIn ENC1B(D11); |
DBerendsen | 0:5f4bc2d63814 | 260 | Encoder encoder2(D10,D11); |
peterknoben | 1:406519ff0f17 | 261 | const float MOTOR2_KP = 60.0; |
peterknoben | 1:406519ff0f17 | 262 | const float MOTOR2_KI = 0.0; |
peterknoben | 1:406519ff0f17 | 263 | const float MOTOR2_KD = 15.0; |
peterknoben | 5:b4abbd3c513c | 264 | double M2_v1 = 0.0, M2_v2 = 0.0; //Calculation values |
DBerendsen | 0:5f4bc2d63814 | 265 | const double motor2_gain = 2*PI; |
peterknoben | 1:406519ff0f17 | 266 | const float M2_N = 0.5; |
DBerendsen | 0:5f4bc2d63814 | 267 | |
DBerendsen | 10:614050a50c2f | 268 | float motor2_control(){ |
peterknoben | 8:24f6744d47b2 | 269 | float reference_beta = Angle.getalpha(max_rangex, max_rangey, x_offset, y_offset, alpha_offset, L1, L2, position_math_l, position_math_r); |
DBerendsen | 0:5f4bc2d63814 | 270 | float position_beta = RAD_PER_PULSE * -encoder2.getPosition(); |
DBerendsen | 0:5f4bc2d63814 | 271 | float error_beta = reference_beta-position_beta; |
peterknoben | 1:406519ff0f17 | 272 | float magnitude2 = PID.get(error_beta, MOTOR2_KP, MOTOR2_KI, MOTOR2_KD, CONTROLLER_TS, M2_N, M1_v1, M1_v2) / motor2_gain; |
DBerendsen | 10:614050a50c2f | 273 | |
DBerendsen | 10:614050a50c2f | 274 | //Determine Motor Magnitude |
DBerendsen | 10:614050a50c2f | 275 | if (fabs(magnitude2)>1) { |
DBerendsen | 10:614050a50c2f | 276 | motor2 = 1; |
DBerendsen | 10:614050a50c2f | 277 | } |
DBerendsen | 10:614050a50c2f | 278 | else { |
DBerendsen | 10:614050a50c2f | 279 | motor2 = fabs(magnitude2); |
DBerendsen | 10:614050a50c2f | 280 | } |
DBerendsen | 10:614050a50c2f | 281 | |
DBerendsen | 10:614050a50c2f | 282 | |
peterknoben | 1:406519ff0f17 | 283 | //Determine Motor Direction |
DBerendsen | 0:5f4bc2d63814 | 284 | if (magnitude2 > 0){ |
DBerendsen | 0:5f4bc2d63814 | 285 | motor2DirectionPin = 1; |
DBerendsen | 0:5f4bc2d63814 | 286 | } |
DBerendsen | 0:5f4bc2d63814 | 287 | else{ |
DBerendsen | 0:5f4bc2d63814 | 288 | motor2DirectionPin = 0; |
DBerendsen | 0:5f4bc2d63814 | 289 | } |
DBerendsen | 10:614050a50c2f | 290 | return magnitude2; |
DBerendsen | 0:5f4bc2d63814 | 291 | } |
peterknoben | 5:b4abbd3c513c | 292 | |
peterknoben | 8:24f6744d47b2 | 293 | //------------------------------------------------------------------------------ |
peterknoben | 8:24f6744d47b2 | 294 | //----------------------------Motor controller---------------------------------- |
peterknoben | 8:24f6744d47b2 | 295 | //------------------------------------------------------------------------------ |
peterknoben | 8:24f6744d47b2 | 296 | int direction_l, direction_r; |
DBerendsen | 10:614050a50c2f | 297 | float magnitude1, magnitude2; |
peterknoben | 8:24f6744d47b2 | 298 | |
peterknoben | 5:b4abbd3c513c | 299 | void motor_control(){ |
DBerendsen | 10:614050a50c2f | 300 | direction_l = MoveLeft.getdirectionLeft(mode); //x-direction |
DBerendsen | 10:614050a50c2f | 301 | direction_r = MoveRight.getdirectionRight(mode); //y-direction |
DBerendsen | 10:614050a50c2f | 302 | position_math_l= MoveLeft.getpositionLeft(CaseLeft, mode, max_rangex); //x-direction |
DBerendsen | 10:614050a50c2f | 303 | position_math_r= MoveRight.getpositionRight(CaseRight, mode, max_rangey); //y-direction |
DBerendsen | 10:614050a50c2f | 304 | magnitude1 = motor1_control(); |
DBerendsen | 10:614050a50c2f | 305 | magnitude2 = motor2_control(); |
DBerendsen | 10:614050a50c2f | 306 | |
DBerendsen | 10:614050a50c2f | 307 | |
DBerendsen | 10:614050a50c2f | 308 | |
DBerendsen | 10:614050a50c2f | 309 | |
peterknoben | 5:b4abbd3c513c | 310 | } |
DBerendsen | 10:614050a50c2f | 311 | /*//------------------------------------------------------------------------------ |
peterknoben | 8:24f6744d47b2 | 312 | //-------------------------------HID Scope-------------------------------------- |
DBerendsen | 0:5f4bc2d63814 | 313 | //------------------------------------------------------------------------------ |
peterknoben | 8:24f6744d47b2 | 314 | |
peterknoben | 8:24f6744d47b2 | 315 | HIDScope scope(4); |
peterknoben | 8:24f6744d47b2 | 316 | Ticker scopeTimer; |
peterknoben | 8:24f6744d47b2 | 317 | Ticker controllerTimer; |
peterknoben | 8:24f6744d47b2 | 318 | |
peterknoben | 8:24f6744d47b2 | 319 | void HID_Monitor(){ |
peterknoben | 8:24f6744d47b2 | 320 | scope.set(0, fabs(FilterLeft(emg0.read()))); |
peterknoben | 8:24f6744d47b2 | 321 | scope.set(1, fabs(FilterRight(emg2.read()))); |
peterknoben | 8:24f6744d47b2 | 322 | scope.set(2, SignalLeft.getmeanLeft(n, fabs(FilterLeft(emg0.read())))); |
peterknoben | 8:24f6744d47b2 | 323 | scope.set(3, SignalRight.getmeanRight(n, fabs(FilterRight(emg2.read())))); |
peterknoben | 8:24f6744d47b2 | 324 | } |
peterknoben | 8:24f6744d47b2 | 325 | |
peterknoben | 8:24f6744d47b2 | 326 | //Working |
peterknoben | 8:24f6744d47b2 | 327 | //SignalLeft.getmeanLeft(n, fabs(FilterLeft(emg0.read()))) |
DBerendsen | 10:614050a50c2f | 328 | //SignalRight.getmeanRight(n, fabs(FilterRight(emg2.read())))*/ |
peterknoben | 8:24f6744d47b2 | 329 | |
peterknoben | 8:24f6744d47b2 | 330 | //------------------------------------------------------------------------------ |
peterknoben | 8:24f6744d47b2 | 331 | //------------------------------Main-------------------------------------------- |
DBerendsen | 0:5f4bc2d63814 | 332 | //------------------------------------------------------------------------------ |
DBerendsen | 0:5f4bc2d63814 | 333 | |
DBerendsen | 0:5f4bc2d63814 | 334 | int main(){ |
DBerendsen | 0:5f4bc2d63814 | 335 | pc.baud(115200); |
peterknoben | 4:e15fc329b88b | 336 | setled(); |
peterknoben | 6:edd30e11f3c7 | 337 | BiQuad_filter_Left.add( &LP1L ).add( &HP2L ).add( &NO3L); |
peterknoben | 6:edd30e11f3c7 | 338 | BiQuad_filter_Right.add( &LP1R ).add( &HP2R ).add( &NO3R); |
peterknoben | 6:edd30e11f3c7 | 339 | BiQuad_filter_Mode.add( &LP1M ).add( &HP2M ).add( &NO3M); |
peterknoben | 4:e15fc329b88b | 340 | But1.rise(&maxcalibration); |
peterknoben | 6:edd30e11f3c7 | 341 | Mode.rise(&mode_selection); |
peterknoben | 5:b4abbd3c513c | 342 | motor1.period(0.0001f); motor2.period(0.0001f); |
peterknoben | 8:24f6744d47b2 | 343 | MyControllerTicker.attach(&motor_control, CONTROLLER_TS); //Determining motorposiitons |
peterknoben | 8:24f6744d47b2 | 344 | MyTickerMean.attach(&signalnumber, MEAN_TS); //Detemining the signalnumbers |
peterknoben | 5:b4abbd3c513c | 345 | // MyTickerMode.attach(&signalmode, MEAN_TS); |
DBerendsen | 10:614050a50c2f | 346 | /* |
peterknoben | 8:24f6744d47b2 | 347 | //Scope |
peterknoben | 8:24f6744d47b2 | 348 | scopeTimer.attach_us(&scope, &HIDScope::send, 2e4); |
DBerendsen | 10:614050a50c2f | 349 | controllerTimer.attach_us(&HID_Monitor, 1e3);*/ |
peterknoben | 8:24f6744d47b2 | 350 | |
peterknoben | 5:b4abbd3c513c | 351 | while(1) { |
DBerendsen | 10:614050a50c2f | 352 | //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); |
DBerendsen | 10:614050a50c2f | 353 | 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); |
peterknoben | 5:b4abbd3c513c | 354 | wait(0.1f); |
peterknoben | 5:b4abbd3c513c | 355 | } |
peterknoben | 1:406519ff0f17 | 356 | } |
peterknoben | 1:406519ff0f17 | 357 | |
peterknoben | 1:406519ff0f17 | 358 |