![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
sdfa
Dependencies: AnglePosition2 Encoder FastPWM MODSERIAL Movement PIDController Servo SignalNumber2 biquadFilter mbed
Fork of kinematics_controlv2 by
main.cpp@3:c768a37620c9, 2017-10-30 (annotated)
- Committer:
- peterknoben
- Date:
- Mon Oct 30 10:43:26 2017 +0000
- Revision:
- 3:c768a37620c9
- Parent:
- 1:406519ff0f17
- Child:
- 4:e15fc329b88b
kaldf
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" |
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 | |
DBerendsen | 0:5f4bc2d63814 | 11 | //------------------------------------------------------------------------------ |
DBerendsen | 0:5f4bc2d63814 | 12 | //------------------------------------------------------------------------------ |
DBerendsen | 0:5f4bc2d63814 | 13 | //------------------------------------------------------------------------------ |
peterknoben | 3:c768a37620c9 | 14 | MODSERIAL pc(USBTX, USBRX); //Establish connection |
peterknoben | 1:406519ff0f17 | 15 | Ticker MyControllerTicker1; //Declare Ticker Motor 1 |
peterknoben | 1:406519ff0f17 | 16 | Ticker MyControllerTicker2; //Declare Ticker Motor 2 |
peterknoben | 3:c768a37620c9 | 17 | Ticker MySampleTicker; //Declare Ticker HIDscope |
peterknoben | 3:c768a37620c9 | 18 | Ticker MyTickerMean; //Declare Ticker Signalprocessing |
peterknoben | 3:c768a37620c9 | 19 | |
peterknoben | 3:c768a37620c9 | 20 | InterruptIn But2(PTC6); //Declare button for calibration |
peterknoben | 3:c768a37620c9 | 21 | |
peterknoben | 1:406519ff0f17 | 22 | AnglePosition Angle; //Declare Angle calculater |
peterknoben | 1:406519ff0f17 | 23 | PIDControl PID; //Declare PID Controller |
peterknoben | 3:c768a37620c9 | 24 | SignalNumber Signal; //Declare Signal determiner |
peterknoben | 3:c768a37620c9 | 25 | |
peterknoben | 3:c768a37620c9 | 26 | AnalogIn potmeter1(A5); |
peterknoben | 3:c768a37620c9 | 27 | AnalogIn potmeter5(A3); //Set Inputpin |
peterknoben | 1:406519ff0f17 | 28 | AnalogIn potmeter2(A4); //Set Inputpin |
peterknoben | 3:c768a37620c9 | 29 | AnalogIn emg0( A0 ); //Set Inputpin |
peterknoben | 3:c768a37620c9 | 30 | AnalogIn emg1( A1 ); //Set Inputpin |
peterknoben | 3:c768a37620c9 | 31 | |
peterknoben | 1:406519ff0f17 | 32 | |
peterknoben | 1:406519ff0f17 | 33 | const float CONTROLLER_TS = 0.02; //Motor frequency |
peterknoben | 3:c768a37620c9 | 34 | const float MEAN_TS = 0.1; //Mean value frequency |
peterknoben | 3:c768a37620c9 | 35 | const float SAMPLE_TS = 0.02; //BiQuad frequency |
peterknoben | 3:c768a37620c9 | 36 | |
peterknoben | 3:c768a37620c9 | 37 | |
peterknoben | 3:c768a37620c9 | 38 | //------------------------------------------------------------------------------ |
peterknoben | 3:c768a37620c9 | 39 | //-----------------------------EMG Signals-------------------------------------- |
peterknoben | 3:c768a37620c9 | 40 | //------------------------------------------------------------------------------ |
peterknoben | 3:c768a37620c9 | 41 | const int n = 10; //Window size for the mean value, also adjust in signalnumber.cpp |
peterknoben | 3:c768a37620c9 | 42 | const int action =50; //Number of same mean values to change the signalnumber |
peterknoben | 3:c768a37620c9 | 43 | const int m = 10; //Number of samples for calibration |
peterknoben | 3:c768a37620c9 | 44 | int mode; |
peterknoben | 3:c768a37620c9 | 45 | float emg_offset; |
peterknoben | 3:c768a37620c9 | 46 | float meanx; |
peterknoben | 3:c768a37620c9 | 47 | |
peterknoben | 3:c768a37620c9 | 48 | //BiQuad filter |
peterknoben | 3:c768a37620c9 | 49 | BiQuad LP1( 0.6389437261127493, 1.2778874522254986, 0.6389437261127493, 1.1429772843080919, 0.4127976201429053 ); //Lowpass filter Biquad |
peterknoben | 3:c768a37620c9 | 50 | BiQuad HP2( 0.8370879899975344, -1.6741759799950688, 0.8370879899975344, -1.6474576182593796, 0.7008943417307579 ); //Highpass filter Biquad |
peterknoben | 3:c768a37620c9 | 51 | BiQuad NO3( 0.7063988100714527, -1.1429772843080923, 0.7063988100714527, -1.1429772843080923, 0.41279762014290533); //Notch filter Biquad |
peterknoben | 3:c768a37620c9 | 52 | BiQuadChain BiQuad_filter; |
peterknoben | 3:c768a37620c9 | 53 | |
peterknoben | 3:c768a37620c9 | 54 | // Calibration function |
peterknoben | 3:c768a37620c9 | 55 | void calibration(){ |
peterknoben | 3:c768a37620c9 | 56 | emg_offset = Signal.calibrate(m, potmeter5); |
peterknoben | 3:c768a37620c9 | 57 | pc.printf("offset = %f \r\n", emg_offset); |
peterknoben | 3:c768a37620c9 | 58 | } |
peterknoben | 3:c768a37620c9 | 59 | |
peterknoben | 3:c768a37620c9 | 60 | /* |
peterknoben | 3:c768a37620c9 | 61 | //Sample function |
peterknoben | 3:c768a37620c9 | 62 | void sample() |
peterknoben | 3:c768a37620c9 | 63 | { |
peterknoben | 3:c768a37620c9 | 64 | float Signal=((emg0+emg1)/2)-emg_offset; |
peterknoben | 3:c768a37620c9 | 65 | float Signal_filtered= BiQuad_filter.step(Signal); |
peterknoben | 3:c768a37620c9 | 66 | /* scope |
peterknoben | 3:c768a37620c9 | 67 | scope.set(0, emg0.read() ); |
peterknoben | 3:c768a37620c9 | 68 | scope.set(1, emg1.read() ); |
peterknoben | 3:c768a37620c9 | 69 | scope.set(2, Signal_filtered); |
peterknoben | 3:c768a37620c9 | 70 | scope.send(); |
peterknoben | 3:c768a37620c9 | 71 | led = !led; |
peterknoben | 3:c768a37620c9 | 72 | / |
peterknoben | 3:c768a37620c9 | 73 | } |
peterknoben | 3:c768a37620c9 | 74 | */ |
peterknoben | 3:c768a37620c9 | 75 | |
peterknoben | 3:c768a37620c9 | 76 | float Filter(float input0, float input1, float offset){ |
peterknoben | 3:c768a37620c9 | 77 | float Signal=((input0+input1)/2)-offset; |
peterknoben | 3:c768a37620c9 | 78 | float Signal_filtered= BiQuad_filter.step(Signal); |
peterknoben | 3:c768a37620c9 | 79 | return Signal_filtered; |
peterknoben | 3:c768a37620c9 | 80 | } |
peterknoben | 3:c768a37620c9 | 81 | |
peterknoben | 3:c768a37620c9 | 82 | void signalnumber(){ |
peterknoben | 3:c768a37620c9 | 83 | float Signal_filtered = Filter(emg0, emg1, emg_offset); |
peterknoben | 3:c768a37620c9 | 84 | meanx = Signal.getmean(n, Signal_filtered); |
peterknoben | 3:c768a37620c9 | 85 | mode = Signal.getnumber(n, action, Signal_filtered); |
peterknoben | 3:c768a37620c9 | 86 | pc.printf("pot = %d after filtering = %f mean = %f Signalnumber = %i \r\n" , potmeter5, meanx, mode); |
peterknoben | 3:c768a37620c9 | 87 | } |
peterknoben | 3:c768a37620c9 | 88 | |
peterknoben | 1:406519ff0f17 | 89 | |
peterknoben | 1:406519ff0f17 | 90 | //------------------------------------------------------------------------------ |
peterknoben | 1:406519ff0f17 | 91 | //-------------------------Kinematic Constants---------------------------------- |
peterknoben | 1:406519ff0f17 | 92 | //------------------------------------------------------------------------------ |
DBerendsen | 0:5f4bc2d63814 | 93 | const double RAD_PER_PULSE = 0.00074799825*2; |
DBerendsen | 0:5f4bc2d63814 | 94 | const double PI = 3.14159265358979323846; |
peterknoben | 1:406519ff0f17 | 95 | const float max_rangex = 500.0; |
peterknoben | 1:406519ff0f17 | 96 | const float max_rangey = 300.0; |
peterknoben | 1:406519ff0f17 | 97 | const float x_offset = 156.15; |
peterknoben | 1:406519ff0f17 | 98 | const float y_offset = -76.97; |
peterknoben | 1:406519ff0f17 | 99 | const float alpha_offset = -(21.52/180)*PI; |
peterknoben | 1:406519ff0f17 | 100 | const float beta_offset = 0.0; |
peterknoben | 1:406519ff0f17 | 101 | const float L1 = 450.0; |
peterknoben | 1:406519ff0f17 | 102 | const float L2 = 490.0; |
DBerendsen | 0:5f4bc2d63814 | 103 | |
DBerendsen | 0:5f4bc2d63814 | 104 | |
DBerendsen | 0:5f4bc2d63814 | 105 | //------------------------------------------------------------------------------ |
DBerendsen | 0:5f4bc2d63814 | 106 | //--------------------------------Servo----------------------------------------- |
DBerendsen | 0:5f4bc2d63814 | 107 | //------------------------------------------------------------------------------ |
peterknoben | 3:c768a37620c9 | 108 | // This will be programmed on a different Mbed |
peterknoben | 1:406519ff0f17 | 109 | Servo MyServo(D9); //Declare button |
peterknoben | 1:406519ff0f17 | 110 | InterruptIn But1(D8); //Declare used button |
peterknoben | 1:406519ff0f17 | 111 | int k=0; //Position flag |
DBerendsen | 0:5f4bc2d63814 | 112 | |
DBerendsen | 0:5f4bc2d63814 | 113 | void servo_control (){ |
DBerendsen | 0:5f4bc2d63814 | 114 | if (k==0){ |
DBerendsen | 0:5f4bc2d63814 | 115 | MyServo = 0; |
DBerendsen | 0:5f4bc2d63814 | 116 | k=1; |
DBerendsen | 0:5f4bc2d63814 | 117 | } |
DBerendsen | 0:5f4bc2d63814 | 118 | else{ |
DBerendsen | 0:5f4bc2d63814 | 119 | MyServo = 2; |
DBerendsen | 0:5f4bc2d63814 | 120 | k=0; |
peterknoben | 1:406519ff0f17 | 121 | } |
DBerendsen | 0:5f4bc2d63814 | 122 | } |
DBerendsen | 0:5f4bc2d63814 | 123 | |
DBerendsen | 0:5f4bc2d63814 | 124 | |
DBerendsen | 0:5f4bc2d63814 | 125 | //------------------------------------------------------------------------------ |
DBerendsen | 0:5f4bc2d63814 | 126 | //--------------------------------Motor1---------------------------------------- |
DBerendsen | 0:5f4bc2d63814 | 127 | //------------------------------------------------------------------------------ |
peterknoben | 1:406519ff0f17 | 128 | FastPWM motor1(D5); |
DBerendsen | 0:5f4bc2d63814 | 129 | DigitalOut motor1DirectionPin(D4); |
DBerendsen | 0:5f4bc2d63814 | 130 | DigitalIn ENC2A(D12); |
DBerendsen | 0:5f4bc2d63814 | 131 | DigitalIn ENC2B(D13); |
DBerendsen | 0:5f4bc2d63814 | 132 | Encoder encoder1(D13,D12); |
peterknoben | 1:406519ff0f17 | 133 | const float MOTOR1_KP = 40.0; |
peterknoben | 1:406519ff0f17 | 134 | const float MOTOR1_KI = 0.0; |
peterknoben | 1:406519ff0f17 | 135 | const float MOTOR1_KD = 15.0; |
peterknoben | 1:406519ff0f17 | 136 | double M1_v1 = 0.0; |
peterknoben | 1:406519ff0f17 | 137 | double M1_v2 = 0.0; |
DBerendsen | 0:5f4bc2d63814 | 138 | const double motor1_gain = 2*PI; |
peterknoben | 1:406519ff0f17 | 139 | const float M1_N = 0.5; |
DBerendsen | 0:5f4bc2d63814 | 140 | |
DBerendsen | 0:5f4bc2d63814 | 141 | |
DBerendsen | 0:5f4bc2d63814 | 142 | void motor1_control(){ |
peterknoben | 1:406519ff0f17 | 143 | float reference_alpha = Angle.getbeta(max_rangex, max_rangey, x_offset, y_offset, beta_offset, L1, L2, potmeter1, potmeter2); |
DBerendsen | 0:5f4bc2d63814 | 144 | float position_alpha = RAD_PER_PULSE * encoder1.getPosition(); |
DBerendsen | 0:5f4bc2d63814 | 145 | float error_alpha = reference_alpha-position_alpha; |
peterknoben | 1:406519ff0f17 | 146 | float magnitude1 = PID.get(error_alpha, MOTOR1_KP, MOTOR1_KI, MOTOR1_KD, CONTROLLER_TS, M1_N, M1_v1, M1_v2) / motor1_gain; |
DBerendsen | 0:5f4bc2d63814 | 147 | motor1 = fabs(magnitude1); |
peterknoben | 3:c768a37620c9 | 148 | // pc.printf("err_a = %f alpha = %f mag1 = %f\r\n", error_alpha, reference_alpha, magnitude1); |
peterknoben | 3:c768a37620c9 | 149 | // pc.printf("\r\n"); |
peterknoben | 1:406519ff0f17 | 150 | // Determine Motor Direction |
peterknoben | 1:406519ff0f17 | 151 | if (magnitude1 < 0){ |
DBerendsen | 0:5f4bc2d63814 | 152 | motor1DirectionPin = 1; |
DBerendsen | 0:5f4bc2d63814 | 153 | } |
DBerendsen | 0:5f4bc2d63814 | 154 | else{ |
DBerendsen | 0:5f4bc2d63814 | 155 | motor1DirectionPin = 0; |
DBerendsen | 0:5f4bc2d63814 | 156 | } |
DBerendsen | 0:5f4bc2d63814 | 157 | } |
DBerendsen | 0:5f4bc2d63814 | 158 | |
DBerendsen | 0:5f4bc2d63814 | 159 | //------------------------------------------------------------------------------ |
DBerendsen | 0:5f4bc2d63814 | 160 | //--------------------------------Motor2---------------------------------------- |
DBerendsen | 0:5f4bc2d63814 | 161 | //------------------------------------------------------------------------------ |
peterknoben | 1:406519ff0f17 | 162 | FastPWM motor2(D6); |
DBerendsen | 0:5f4bc2d63814 | 163 | DigitalOut motor2DirectionPin(D7); |
DBerendsen | 0:5f4bc2d63814 | 164 | DigitalIn ENC1A(D10); |
DBerendsen | 0:5f4bc2d63814 | 165 | DigitalIn ENC1B(D11); |
DBerendsen | 0:5f4bc2d63814 | 166 | Encoder encoder2(D10,D11); |
peterknoben | 1:406519ff0f17 | 167 | const float MOTOR2_KP = 60.0; |
peterknoben | 1:406519ff0f17 | 168 | const float MOTOR2_KI = 0.0; |
peterknoben | 1:406519ff0f17 | 169 | const float MOTOR2_KD = 15.0; |
DBerendsen | 0:5f4bc2d63814 | 170 | double m2_err_int = 0; |
DBerendsen | 0:5f4bc2d63814 | 171 | const double motor2_gain = 2*PI; |
peterknoben | 1:406519ff0f17 | 172 | const float M2_N = 0.5; |
peterknoben | 1:406519ff0f17 | 173 | double M2_v1 = 0.0; |
peterknoben | 1:406519ff0f17 | 174 | double M2_v2 = 0.0; |
DBerendsen | 0:5f4bc2d63814 | 175 | |
DBerendsen | 0:5f4bc2d63814 | 176 | |
DBerendsen | 0:5f4bc2d63814 | 177 | void motor2_control(){ |
peterknoben | 1:406519ff0f17 | 178 | float reference_beta = Angle.getalpha(max_rangex, max_rangey, x_offset, y_offset, alpha_offset, L1, L2, potmeter1, potmeter2); |
DBerendsen | 0:5f4bc2d63814 | 179 | float position_beta = RAD_PER_PULSE * -encoder2.getPosition(); |
DBerendsen | 0:5f4bc2d63814 | 180 | float error_beta = reference_beta-position_beta; |
peterknoben | 1:406519ff0f17 | 181 | float magnitude2 = PID.get(error_beta, MOTOR2_KP, MOTOR2_KI, MOTOR2_KD, CONTROLLER_TS, M2_N, M1_v1, M1_v2) / motor2_gain; |
DBerendsen | 0:5f4bc2d63814 | 182 | motor2 = fabs(magnitude2); |
peterknoben | 3:c768a37620c9 | 183 | // pc.printf("err2 = %f beta = %f mag2 = %f\r\n", error_beta, reference_beta, magnitude2); |
peterknoben | 3:c768a37620c9 | 184 | // pc.printf("\r\n"); |
peterknoben | 1:406519ff0f17 | 185 | //Determine Motor Direction |
DBerendsen | 0:5f4bc2d63814 | 186 | if (magnitude2 > 0){ |
DBerendsen | 0:5f4bc2d63814 | 187 | motor2DirectionPin = 1; |
DBerendsen | 0:5f4bc2d63814 | 188 | } |
DBerendsen | 0:5f4bc2d63814 | 189 | else{ |
DBerendsen | 0:5f4bc2d63814 | 190 | motor2DirectionPin = 0; |
DBerendsen | 0:5f4bc2d63814 | 191 | } |
DBerendsen | 0:5f4bc2d63814 | 192 | } |
DBerendsen | 0:5f4bc2d63814 | 193 | //------------------------------------------------------------------------------ |
DBerendsen | 0:5f4bc2d63814 | 194 | //------------------------------------------------------------------------------ |
DBerendsen | 0:5f4bc2d63814 | 195 | //------------------------------------------------------------------------------ |
DBerendsen | 0:5f4bc2d63814 | 196 | |
DBerendsen | 0:5f4bc2d63814 | 197 | int main(){ |
DBerendsen | 0:5f4bc2d63814 | 198 | pc.baud(115200); |
peterknoben | 3:c768a37620c9 | 199 | BiQuad_filter.add( &LP1 ).add( &HP2 ).add( &NO3); |
peterknoben | 1:406519ff0f17 | 200 | motor1.period(0.0001f); |
peterknoben | 1:406519ff0f17 | 201 | motor2.period(0.0001f); |
DBerendsen | 0:5f4bc2d63814 | 202 | MyControllerTicker1.attach(&motor1_control, CONTROLLER_TS); |
DBerendsen | 0:5f4bc2d63814 | 203 | MyControllerTicker2.attach(&motor2_control, CONTROLLER_TS); |
peterknoben | 3:c768a37620c9 | 204 | MyTickerMean.attach(&signalnumber, MEAN_TS); |
peterknoben | 3:c768a37620c9 | 205 | // MySampleTicker.attach(&sample, SAMPLE_TS); |
peterknoben | 1:406519ff0f17 | 206 | But1.rise(&servo_control); |
peterknoben | 3:c768a37620c9 | 207 | But2.rise(&calibration); |
DBerendsen | 0:5f4bc2d63814 | 208 | while(1) {} |
peterknoben | 1:406519ff0f17 | 209 | } |
peterknoben | 1:406519ff0f17 | 210 | |
peterknoben | 1:406519ff0f17 | 211 |