Final Version
Dependencies: AnglePosition2 Encoder FastPWM MovementClean PIDController Servo SignalNumberClean biquadFilter mbed
Fork of ShowItv2 by
main.cpp@14:a8a69fee3fad, 2017-11-02 (annotated)
- Committer:
- peterknoben
- Date:
- Thu Nov 02 15:11:19 2017 +0000
- Revision:
- 14:a8a69fee3fad
- Parent:
- 12:0765ea2c4c85
- Child:
- 15:a73b5abd0696
Working, Clean
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
peterknoben | 1:406519ff0f17 | 1 | #include "AnglePosition.h" |
peterknoben | 1:406519ff0f17 | 2 | #include "PIDControl.h" |
peterknoben | 3:c768a37620c9 | 3 | #include "BiQuad.h" |
peterknoben | 3:c768a37620c9 | 4 | #include "signalnumber.h" |
peterknoben | 4:e15fc329b88b | 5 | #include "Movement.h" |
DBerendsen | 0:5f4bc2d63814 | 6 | #include "mbed.h" |
DBerendsen | 0:5f4bc2d63814 | 7 | #include "encoder.h" |
DBerendsen | 0:5f4bc2d63814 | 8 | #include "Servo.h" |
peterknoben | 1:406519ff0f17 | 9 | #include "FastPWM.h" |
DBerendsen | 0:5f4bc2d63814 | 10 | |
peterknoben | 14:a8a69fee3fad | 11 | //This code is for the main Mbed |
DBerendsen | 0:5f4bc2d63814 | 12 | //------------------------------------------------------------------------------ |
DBerendsen | 0:5f4bc2d63814 | 13 | //------------------------------------------------------------------------------ |
DBerendsen | 0:5f4bc2d63814 | 14 | //------------------------------------------------------------------------------ |
peterknoben | 14:a8a69fee3fad | 15 | Ticker MyTickerController; //Declare Ticker for Motors |
DBerendsen | 11:28b5ec12a4b9 | 16 | Ticker MyTickerServo; //Declare Ticker Servo control |
peterknoben | 3:c768a37620c9 | 17 | Ticker MyTickerMean; //Declare Ticker Signalprocessing |
peterknoben | 14:a8a69fee3fad | 18 | Ticker MyTickerFilter; //Declare Ticker Filtering |
peterknoben | 3:c768a37620c9 | 19 | |
peterknoben | 14:a8a69fee3fad | 20 | InterruptIn But1(PTA4); //Declare button for emg calibration |
peterknoben | 14:a8a69fee3fad | 21 | InterruptIn Mode(PTC6); //Declare button for mode selection |
peterknoben | 3:c768a37620c9 | 22 | |
peterknoben | 1:406519ff0f17 | 23 | AnglePosition Angle; //Declare Angle calculater |
peterknoben | 1:406519ff0f17 | 24 | PIDControl PID; //Declare PID Controller |
peterknoben | 4:e15fc329b88b | 25 | SignalNumber SignalLeft; //Declare Signal determiner for Left arm |
peterknoben | 4:e15fc329b88b | 26 | SignalNumber SignalRight; //Declare Signal determiner for Right arm |
peterknoben | 14:a8a69fee3fad | 27 | Movement MoveLeft; //Declare Left Movement determiner |
peterknoben | 14:a8a69fee3fad | 28 | Movement MoveRight; //Declare Right movement determiner |
peterknoben | 3:c768a37620c9 | 29 | |
peterknoben | 4:e15fc329b88b | 30 | AnalogIn emg0( A0 ); //Set Inputpin for EMG 0 signal Left |
peterknoben | 4:e15fc329b88b | 31 | AnalogIn emg2( A2 ); //Set Inputpin for EMG 2 signal Right |
peterknoben | 14:a8a69fee3fad | 32 | DigitalOut Up( D9 ); //Set digital out for raising the pen off the board |
peterknoben | 14:a8a69fee3fad | 33 | DigitalOut Down( D8 ); //Set digital out for lowering the pen off the board |
peterknoben | 14:a8a69fee3fad | 34 | |
peterknoben | 14:a8a69fee3fad | 35 | //Setting LEDs |
peterknoben | 14:a8a69fee3fad | 36 | DigitalOut Led_red(LED_RED); |
peterknoben | 4:e15fc329b88b | 37 | DigitalOut Led_green(LED_GREEN); |
peterknoben | 4:e15fc329b88b | 38 | DigitalOut Led_blue(LED_BLUE); |
DBerendsen | 12:0765ea2c4c85 | 39 | DigitalOut Led_calibration(D2); |
peterknoben | 1:406519ff0f17 | 40 | |
peterknoben | 1:406519ff0f17 | 41 | const float CONTROLLER_TS = 0.02; //Motor frequency |
peterknoben | 14:a8a69fee3fad | 42 | const float MEAN_TS = 0.002; //Moving average frequency |
peterknoben | 14:a8a69fee3fad | 43 | const float FILTER_TS = 1e3; //Filter frequency |
peterknoben | 14:a8a69fee3fad | 44 | const float SERVO_TS = 0.01; //Servo frequency |
peterknoben | 4:e15fc329b88b | 45 | |
peterknoben | 14:a8a69fee3fad | 46 | void setled(){ |
peterknoben | 14:a8a69fee3fad | 47 | Led_red=1; Led_green=1; Led_blue=1; Led_calibration=0; |
peterknoben | 14:a8a69fee3fad | 48 | } |
peterknoben | 3:c768a37620c9 | 49 | //------------------------------------------------------------------------------ |
peterknoben | 3:c768a37620c9 | 50 | //-----------------------------EMG Signals-------------------------------------- |
peterknoben | 3:c768a37620c9 | 51 | //------------------------------------------------------------------------------ |
peterknoben | 14:a8a69fee3fad | 52 | |
peterknoben | 4:e15fc329b88b | 53 | // Filtering the signal and getting the usefull information out of it. |
peterknoben | 14:a8a69fee3fad | 54 | const int n = 500; //Window size for the mean value, also adjust in signalnumber.cpp |
peterknoben | 14:a8a69fee3fad | 55 | const int action =100; //Number of same mean values to change the signalnumber |
peterknoben | 14:a8a69fee3fad | 56 | const int m = 500; //Number of samples for calibration |
peterknoben | 14:a8a69fee3fad | 57 | int SpeedLeft, SpeedRight; //Strength of the muscles |
peterknoben | 14:a8a69fee3fad | 58 | float emg_offsetLeft, emg_offsetRight; //Calibration offsets from zero to one for the right arm |
peterknoben | 14:a8a69fee3fad | 59 | float kLeft, kRight; //Scaling factors |
peterknoben | 14:a8a69fee3fad | 60 | float Signal_filteredLeft, Signal_filteredRight; //Variables to store the filterd signals |
peterknoben | 3:c768a37620c9 | 61 | |
peterknoben | 4:e15fc329b88b | 62 | //BiQuad filter variables |
peterknoben | 6:edd30e11f3c7 | 63 | BiQuad LP1L( 0.6389437261127493, 1.2778874522254986, 0.6389437261127493, 1.1429772843080919, 0.4127976201429053 ); //Lowpass filter Biquad |
peterknoben | 6:edd30e11f3c7 | 64 | BiQuad HP2L( 0.8370879899975344, -1.6741759799950688, 0.8370879899975344, -1.6474576182593796, 0.7008943417307579 ); //Highpass filter Biquad |
peterknoben | 6:edd30e11f3c7 | 65 | BiQuad NO3L( 0.7063988100714527, -1.1429772843080923, 0.7063988100714527, -1.1429772843080923, 0.41279762014290533); //Notch filter Biquad |
peterknoben | 6:edd30e11f3c7 | 66 | BiQuad LP1R( 0.6389437261127493, 1.2778874522254986, 0.6389437261127493, 1.1429772843080919, 0.4127976201429053 ); //Lowpass filter Biquad |
peterknoben | 6:edd30e11f3c7 | 67 | BiQuad HP2R( 0.8370879899975344, -1.6741759799950688, 0.8370879899975344, -1.6474576182593796, 0.7008943417307579 ); //Highpass filter Biquad |
peterknoben | 6:edd30e11f3c7 | 68 | BiQuad NO3R( 0.7063988100714527, -1.1429772843080923, 0.7063988100714527, -1.1429772843080923, 0.41279762014290533); //Notch filter Biquad |
peterknoben | 6:edd30e11f3c7 | 69 | BiQuadChain BiQuad_filter_Left; |
peterknoben | 6:edd30e11f3c7 | 70 | BiQuadChain BiQuad_filter_Right; |
peterknoben | 3:c768a37620c9 | 71 | |
peterknoben | 14:a8a69fee3fad | 72 | //Filters----------------------------------------------------------------------- |
peterknoben | 14:a8a69fee3fad | 73 | |
peterknoben | 14:a8a69fee3fad | 74 | // Filter for the left signal |
peterknoben | 7:105e3ae0fb19 | 75 | float FilterLeft(float input){ |
peterknoben | 7:105e3ae0fb19 | 76 | float Signal_filtered= BiQuad_filter_Left.step(input); |
peterknoben | 7:105e3ae0fb19 | 77 | return Signal_filtered; |
peterknoben | 7:105e3ae0fb19 | 78 | } |
peterknoben | 14:a8a69fee3fad | 79 | // Filter for the right signal |
peterknoben | 7:105e3ae0fb19 | 80 | float FilterRight(float input){ |
peterknoben | 7:105e3ae0fb19 | 81 | float Signal_filtered= BiQuad_filter_Right.step(input); |
peterknoben | 7:105e3ae0fb19 | 82 | return Signal_filtered; |
peterknoben | 7:105e3ae0fb19 | 83 | } |
peterknoben | 5:b4abbd3c513c | 84 | |
peterknoben | 5:b4abbd3c513c | 85 | //Calibration------------------------------------------------------------------- |
peterknoben | 14:a8a69fee3fad | 86 | |
peterknoben | 14:a8a69fee3fad | 87 | // Calibrating the EMG signals. This function needs to be ativated when the |
peterknoben | 14:a8a69fee3fad | 88 | // muscles are flexed for over a second |
peterknoben | 14:a8a69fee3fad | 89 | void calibration(){ |
peterknoben | 14:a8a69fee3fad | 90 | emg_offsetLeft = SignalLeft.calibrate(m, FilterLeft(emg0)); |
peterknoben | 14:a8a69fee3fad | 91 | emg_offsetRight = SignalRight.calibrate(m, FilterRight(emg2)); |
peterknoben | 14:a8a69fee3fad | 92 | //Setting the emg scaling constants to reed between zero and one. |
peterknoben | 14:a8a69fee3fad | 93 | kLeft = 1/emg_offsetLeft; |
peterknoben | 14:a8a69fee3fad | 94 | kRight = 1/emg_offsetRight; |
peterknoben | 14:a8a69fee3fad | 95 | //Set led to Green to indicate if calibration is done |
peterknoben | 14:a8a69fee3fad | 96 | Led_calibration = 1; |
peterknoben | 5:b4abbd3c513c | 97 | } |
peterknoben | 8:24f6744d47b2 | 98 | |
peterknoben | 14:a8a69fee3fad | 99 | //Speedcontrol------------------------------------------------------------------ |
peterknoben | 14:a8a69fee3fad | 100 | //Determine the signalnumbers (i.e. speed) based on the strenght of the EMG signals |
peterknoben | 14:a8a69fee3fad | 101 | void Speedcontrol(){ |
peterknoben | 14:a8a69fee3fad | 102 | //Left Signals |
peterknoben | 14:a8a69fee3fad | 103 | Signal_filteredLeft = fabs(FilterLeft(emg0)*kLeft);//*kLeft |
peterknoben | 14:a8a69fee3fad | 104 | SpeedLeft = SignalLeft.getspeedLeft(n, action, Signal_filteredLeft); |
peterknoben | 14:a8a69fee3fad | 105 | //Right Signals |
peterknoben | 14:a8a69fee3fad | 106 | Signal_filteredRight = fabs(FilterRight(emg2)*kRight);//*kRight |
peterknoben | 14:a8a69fee3fad | 107 | SpeedRight = SignalRight.getspeedRight(n, action, Signal_filteredRight); |
peterknoben | 3:c768a37620c9 | 108 | } |
peterknoben | 3:c768a37620c9 | 109 | |
peterknoben | 5:b4abbd3c513c | 110 | //------------------------------------------------------------------------------ |
peterknoben | 5:b4abbd3c513c | 111 | //---------------------------------Servo---------------------------------------- |
peterknoben | 5:b4abbd3c513c | 112 | //------------------------------------------------------------------------------ |
peterknoben | 14:a8a69fee3fad | 113 | //Sending a pulse to the other Mbed to raise the pen |
DBerendsen | 11:28b5ec12a4b9 | 114 | void up(){ |
peterknoben | 14:a8a69fee3fad | 115 | Up = 1; Up = 0; Up = 1; |
DBerendsen | 11:28b5ec12a4b9 | 116 | } |
peterknoben | 14:a8a69fee3fad | 117 | //Sending a pulse to the other Mbed to lower the pen |
DBerendsen | 11:28b5ec12a4b9 | 118 | void down(){ |
peterknoben | 14:a8a69fee3fad | 119 | Down = 1; Down = 0; Down = 1; |
DBerendsen | 11:28b5ec12a4b9 | 120 | } |
DBerendsen | 11:28b5ec12a4b9 | 121 | |
peterknoben | 14:a8a69fee3fad | 122 | //Setting the begin mode to zero |
DBerendsen | 11:28b5ec12a4b9 | 123 | int mode =0; |
peterknoben | 14:a8a69fee3fad | 124 | // Enable the servo function when mode 3 or mode 6 is activated. |
peterknoben | 14:a8a69fee3fad | 125 | // If the mean value for the Left arm stays long enough high the servo will raise |
peterknoben | 14:a8a69fee3fad | 126 | // If the mean value for the Right arm stays long enough high the servo will lower |
peterknoben | 5:b4abbd3c513c | 127 | void servo(){ |
DBerendsen | 11:28b5ec12a4b9 | 128 | if(mode==3||mode==6){ |
peterknoben | 14:a8a69fee3fad | 129 | Signal_filteredLeft = fabs(FilterLeft(emg0)*kLeft); |
peterknoben | 14:a8a69fee3fad | 130 | Signal_filteredRight = fabs(FilterRight(emg2)*kRight); |
peterknoben | 14:a8a69fee3fad | 131 | SpeedLeft = SignalLeft.getspeedLeft(n, action, Signal_filteredLeft); |
peterknoben | 14:a8a69fee3fad | 132 | SpeedRight = SignalRight.getspeedRight(n, action, Signal_filteredRight); |
peterknoben | 14:a8a69fee3fad | 133 | if (SpeedLeft == 2){ |
DBerendsen | 11:28b5ec12a4b9 | 134 | up(); |
DBerendsen | 11:28b5ec12a4b9 | 135 | } |
peterknoben | 14:a8a69fee3fad | 136 | if (SpeedRight == 2){ |
DBerendsen | 11:28b5ec12a4b9 | 137 | down(); |
DBerendsen | 11:28b5ec12a4b9 | 138 | } |
peterknoben | 5:b4abbd3c513c | 139 | } |
peterknoben | 5:b4abbd3c513c | 140 | } |
peterknoben | 5:b4abbd3c513c | 141 | //------------------------------------------------------------------------------ |
peterknoben | 5:b4abbd3c513c | 142 | //---------------------------Mode selection------------------------------------- |
peterknoben | 5:b4abbd3c513c | 143 | //------------------------------------------------------------------------------ |
peterknoben | 5:b4abbd3c513c | 144 | |
peterknoben | 14:a8a69fee3fad | 145 | //Function is called when the mode button is pressed and switches modes and led indication |
peterknoben | 5:b4abbd3c513c | 146 | void mode_selection(){ |
peterknoben | 14:a8a69fee3fad | 147 | //Switching to the next mode. (Total of 6 modes) |
peterknoben | 14:a8a69fee3fad | 148 | if(mode == 6){ |
peterknoben | 5:b4abbd3c513c | 149 | mode=1; |
peterknoben | 5:b4abbd3c513c | 150 | } |
peterknoben | 5:b4abbd3c513c | 151 | else{ |
peterknoben | 5:b4abbd3c513c | 152 | mode++; |
peterknoben | 14:a8a69fee3fad | 153 | } |
peterknoben | 14:a8a69fee3fad | 154 | //Setting the LED colour to indicate the operating mode |
DBerendsen | 12:0765ea2c4c85 | 155 | if((mode==3) || (mode==6)) { |
peterknoben | 14:a8a69fee3fad | 156 | Led_red = 0, Led_green = 0, Led_blue = 0; //Set Led Mode to White |
DBerendsen | 10:614050a50c2f | 157 | } |
DBerendsen | 10:614050a50c2f | 158 | else if (mode==1) { |
peterknoben | 14:a8a69fee3fad | 159 | Led_red = 0, Led_green = 1, Led_blue = 1; //Set Led Mode to Red |
DBerendsen | 10:614050a50c2f | 160 | } |
DBerendsen | 10:614050a50c2f | 161 | else if (mode==2) { |
peterknoben | 14:a8a69fee3fad | 162 | Led_red = 1, Led_green = 0, Led_blue = 1; //Set Led Mode to Green |
DBerendsen | 10:614050a50c2f | 163 | } |
DBerendsen | 10:614050a50c2f | 164 | else if (mode==4) { |
peterknoben | 14:a8a69fee3fad | 165 | Led_red = 1, Led_green = 1, Led_blue = 0; //Set Led Mode to Blue |
DBerendsen | 10:614050a50c2f | 166 | } |
DBerendsen | 10:614050a50c2f | 167 | else if (mode==5) { |
peterknoben | 14:a8a69fee3fad | 168 | Led_red = 0, Led_green = 0, Led_blue = 1; //Set Led Mode to Yellow |
DBerendsen | 12:0765ea2c4c85 | 169 | } |
peterknoben | 5:b4abbd3c513c | 170 | } |
peterknoben | 1:406519ff0f17 | 171 | //------------------------------------------------------------------------------ |
peterknoben | 1:406519ff0f17 | 172 | //-------------------------Kinematic Constants---------------------------------- |
peterknoben | 1:406519ff0f17 | 173 | //------------------------------------------------------------------------------ |
peterknoben | 4:e15fc329b88b | 174 | const double RAD_PER_PULSE = 0.00074799825*2; //Number of radials per pulse from the encoder |
peterknoben | 4:e15fc329b88b | 175 | const double PI = 3.14159265358979323846; //Pi |
DBerendsen | 11:28b5ec12a4b9 | 176 | const float max_rangex = 650.0, max_rangey = 450.0; //Max range on the y axis |
peterknoben | 8:24f6744d47b2 | 177 | const float x_offset = 156.15, y_offset = -76.97; //Starting positions from the shoulder joint |
DBerendsen | 10:614050a50c2f | 178 | const float alpha_offset = -0.4114; |
DBerendsen | 10:614050a50c2f | 179 | const float beta_offset = 0.0486; |
peterknoben | 8:24f6744d47b2 | 180 | const float L1 = 450.0, L2 = 490.0; //Length of the bodies |
peterknoben | 5:b4abbd3c513c | 181 | |
DBerendsen | 0:5f4bc2d63814 | 182 | //------------------------------------------------------------------------------ |
DBerendsen | 0:5f4bc2d63814 | 183 | //--------------------------------Motor1---------------------------------------- |
DBerendsen | 0:5f4bc2d63814 | 184 | //------------------------------------------------------------------------------ |
peterknoben | 1:406519ff0f17 | 185 | FastPWM motor1(D5); |
DBerendsen | 0:5f4bc2d63814 | 186 | DigitalOut motor1DirectionPin(D4); |
DBerendsen | 0:5f4bc2d63814 | 187 | DigitalIn ENC2A(D12); |
DBerendsen | 0:5f4bc2d63814 | 188 | DigitalIn ENC2B(D13); |
DBerendsen | 0:5f4bc2d63814 | 189 | Encoder encoder1(D13,D12); |
peterknoben | 1:406519ff0f17 | 190 | const float MOTOR1_KP = 40.0; |
peterknoben | 1:406519ff0f17 | 191 | const float MOTOR1_KI = 0.0; |
peterknoben | 1:406519ff0f17 | 192 | const float MOTOR1_KD = 15.0; |
peterknoben | 5:b4abbd3c513c | 193 | double M1_v1 = 0.0, M1_v2 = 0.0; //Calculation values |
DBerendsen | 0:5f4bc2d63814 | 194 | const double motor1_gain = 2*PI; |
peterknoben | 1:406519ff0f17 | 195 | const float M1_N = 0.5; |
peterknoben | 8:24f6744d47b2 | 196 | float position_math_l =0, position_math_r=0; |
DBerendsen | 0:5f4bc2d63814 | 197 | |
DBerendsen | 12:0765ea2c4c85 | 198 | void motor1_control(){ |
peterknoben | 8:24f6744d47b2 | 199 | 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 | 200 | float position_alpha = RAD_PER_PULSE * encoder1.getPosition(); |
DBerendsen | 0:5f4bc2d63814 | 201 | float error_alpha = reference_alpha-position_alpha; |
peterknoben | 1:406519ff0f17 | 202 | 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 | 203 | //Determine Motor Magnitude |
DBerendsen | 10:614050a50c2f | 204 | if (fabs(magnitude1)>1) { |
DBerendsen | 10:614050a50c2f | 205 | motor1 = 1; |
DBerendsen | 10:614050a50c2f | 206 | } |
DBerendsen | 10:614050a50c2f | 207 | else { |
DBerendsen | 10:614050a50c2f | 208 | motor1 = fabs(magnitude1); |
DBerendsen | 10:614050a50c2f | 209 | } |
peterknoben | 1:406519ff0f17 | 210 | // Determine Motor Direction |
peterknoben | 1:406519ff0f17 | 211 | if (magnitude1 < 0){ |
DBerendsen | 0:5f4bc2d63814 | 212 | motor1DirectionPin = 1; |
DBerendsen | 0:5f4bc2d63814 | 213 | } |
DBerendsen | 0:5f4bc2d63814 | 214 | else{ |
DBerendsen | 0:5f4bc2d63814 | 215 | motor1DirectionPin = 0; |
DBerendsen | 0:5f4bc2d63814 | 216 | } |
DBerendsen | 0:5f4bc2d63814 | 217 | } |
DBerendsen | 0:5f4bc2d63814 | 218 | //------------------------------------------------------------------------------ |
DBerendsen | 0:5f4bc2d63814 | 219 | //--------------------------------Motor2---------------------------------------- |
DBerendsen | 0:5f4bc2d63814 | 220 | //------------------------------------------------------------------------------ |
peterknoben | 1:406519ff0f17 | 221 | FastPWM motor2(D6); |
DBerendsen | 0:5f4bc2d63814 | 222 | DigitalOut motor2DirectionPin(D7); |
DBerendsen | 0:5f4bc2d63814 | 223 | DigitalIn ENC1A(D10); |
DBerendsen | 0:5f4bc2d63814 | 224 | DigitalIn ENC1B(D11); |
DBerendsen | 0:5f4bc2d63814 | 225 | Encoder encoder2(D10,D11); |
peterknoben | 1:406519ff0f17 | 226 | const float MOTOR2_KP = 60.0; |
peterknoben | 1:406519ff0f17 | 227 | const float MOTOR2_KI = 0.0; |
peterknoben | 1:406519ff0f17 | 228 | const float MOTOR2_KD = 15.0; |
peterknoben | 5:b4abbd3c513c | 229 | double M2_v1 = 0.0, M2_v2 = 0.0; //Calculation values |
DBerendsen | 0:5f4bc2d63814 | 230 | const double motor2_gain = 2*PI; |
peterknoben | 1:406519ff0f17 | 231 | const float M2_N = 0.5; |
DBerendsen | 0:5f4bc2d63814 | 232 | |
DBerendsen | 12:0765ea2c4c85 | 233 | void motor2_control(){ |
peterknoben | 8:24f6744d47b2 | 234 | 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 | 235 | float position_beta = RAD_PER_PULSE * -encoder2.getPosition(); |
DBerendsen | 0:5f4bc2d63814 | 236 | float error_beta = reference_beta-position_beta; |
peterknoben | 1:406519ff0f17 | 237 | 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 | 238 | //Determine Motor Magnitude |
DBerendsen | 10:614050a50c2f | 239 | if (fabs(magnitude2)>1) { |
DBerendsen | 10:614050a50c2f | 240 | motor2 = 1; |
DBerendsen | 10:614050a50c2f | 241 | } |
DBerendsen | 10:614050a50c2f | 242 | else { |
DBerendsen | 10:614050a50c2f | 243 | motor2 = fabs(magnitude2); |
DBerendsen | 10:614050a50c2f | 244 | } |
peterknoben | 1:406519ff0f17 | 245 | //Determine Motor Direction |
DBerendsen | 0:5f4bc2d63814 | 246 | if (magnitude2 > 0){ |
DBerendsen | 0:5f4bc2d63814 | 247 | motor2DirectionPin = 1; |
DBerendsen | 0:5f4bc2d63814 | 248 | } |
DBerendsen | 0:5f4bc2d63814 | 249 | else{ |
DBerendsen | 0:5f4bc2d63814 | 250 | motor2DirectionPin = 0; |
DBerendsen | 0:5f4bc2d63814 | 251 | } |
DBerendsen | 0:5f4bc2d63814 | 252 | } |
peterknoben | 8:24f6744d47b2 | 253 | //------------------------------------------------------------------------------ |
peterknoben | 8:24f6744d47b2 | 254 | //----------------------------Motor controller---------------------------------- |
peterknoben | 8:24f6744d47b2 | 255 | //------------------------------------------------------------------------------ |
peterknoben | 14:a8a69fee3fad | 256 | int direction_l, direction_r; //Variables to store the directions |
peterknoben | 14:a8a69fee3fad | 257 | float magnitude1, magnitude2; //Variables to store the magnitude signal |
peterknoben | 8:24f6744d47b2 | 258 | |
peterknoben | 5:b4abbd3c513c | 259 | void motor_control(){ |
DBerendsen | 10:614050a50c2f | 260 | direction_l = MoveLeft.getdirectionLeft(mode); //x-direction |
DBerendsen | 10:614050a50c2f | 261 | direction_r = MoveRight.getdirectionRight(mode); //y-direction |
peterknoben | 14:a8a69fee3fad | 262 | position_math_l= MoveLeft.getpositionLeft(SpeedLeft, mode, max_rangex); //x-direction |
peterknoben | 14:a8a69fee3fad | 263 | position_math_r= MoveRight.getpositionRight(SpeedRight, mode, max_rangey); //y-direction |
peterknoben | 14:a8a69fee3fad | 264 | motor1_control(); |
peterknoben | 14:a8a69fee3fad | 265 | motor2_control(); |
peterknoben | 5:b4abbd3c513c | 266 | } |
DBerendsen | 11:28b5ec12a4b9 | 267 | //------------------------------------------------------------------------------ |
peterknoben | 14:a8a69fee3fad | 268 | //-----------------------------Filter Update------------------------------------ |
DBerendsen | 0:5f4bc2d63814 | 269 | //------------------------------------------------------------------------------ |
peterknoben | 8:24f6744d47b2 | 270 | |
peterknoben | 14:a8a69fee3fad | 271 | //Updating Filter values |
DBerendsen | 12:0765ea2c4c85 | 272 | void FilterUpdate(){ |
DBerendsen | 12:0765ea2c4c85 | 273 | FilterLeft(emg0.read()); |
DBerendsen | 12:0765ea2c4c85 | 274 | FilterRight(emg2.read()); |
DBerendsen | 12:0765ea2c4c85 | 275 | SignalLeft.getmeanLeft(n, fabs(FilterLeft(emg0.read()))); |
DBerendsen | 12:0765ea2c4c85 | 276 | SignalRight.getmeanRight(n, fabs(FilterRight(emg2.read()))); |
peterknoben | 8:24f6744d47b2 | 277 | } |
peterknoben | 8:24f6744d47b2 | 278 | //------------------------------------------------------------------------------ |
peterknoben | 8:24f6744d47b2 | 279 | //------------------------------Main-------------------------------------------- |
DBerendsen | 0:5f4bc2d63814 | 280 | //------------------------------------------------------------------------------ |
DBerendsen | 0:5f4bc2d63814 | 281 | |
DBerendsen | 0:5f4bc2d63814 | 282 | int main(){ |
peterknoben | 4:e15fc329b88b | 283 | setled(); |
peterknoben | 14:a8a69fee3fad | 284 | |
peterknoben | 14:a8a69fee3fad | 285 | //Creating Filters |
peterknoben | 6:edd30e11f3c7 | 286 | BiQuad_filter_Left.add( &LP1L ).add( &HP2L ).add( &NO3L); |
peterknoben | 6:edd30e11f3c7 | 287 | BiQuad_filter_Right.add( &LP1R ).add( &HP2R ).add( &NO3R); |
peterknoben | 14:a8a69fee3fad | 288 | |
peterknoben | 14:a8a69fee3fad | 289 | //Button detection |
peterknoben | 14:a8a69fee3fad | 290 | But1.rise(&calibration); |
peterknoben | 6:edd30e11f3c7 | 291 | Mode.rise(&mode_selection); |
peterknoben | 14:a8a69fee3fad | 292 | |
peterknoben | 14:a8a69fee3fad | 293 | //Setting motor periods |
peterknoben | 5:b4abbd3c513c | 294 | motor1.period(0.0001f); motor2.period(0.0001f); |
peterknoben | 14:a8a69fee3fad | 295 | |
peterknoben | 14:a8a69fee3fad | 296 | //Tickers |
peterknoben | 14:a8a69fee3fad | 297 | MyTickerController.attach(&motor_control, CONTROLLER_TS); |
peterknoben | 14:a8a69fee3fad | 298 | MyTickerMean.attach(&Speedcontrol, MEAN_TS); |
peterknoben | 14:a8a69fee3fad | 299 | MyTickerServo.attach(&servo, SERVO_TS); |
peterknoben | 14:a8a69fee3fad | 300 | MyTickerFilter.attach_us(&FilterUpdate, FILTER_TS); |
peterknoben | 8:24f6744d47b2 | 301 | |
peterknoben | 5:b4abbd3c513c | 302 | while(1) { |
peterknoben | 5:b4abbd3c513c | 303 | } |
peterknoben | 14:a8a69fee3fad | 304 | } |