2nd draft
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed Servo
Fork of robot_mockup by
main.cpp@19:0a3ee31dcdb4, 2015-10-12 (annotated)
- Committer:
- Vigilance88
- Date:
- Mon Oct 12 13:47:21 2015 +0000
- Revision:
- 19:0a3ee31dcdb4
- Parent:
- 18:44905b008f44
- Child:
- 20:0ede3818e08e
calibrate split to arm and emg
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
vsluiter | 0:32bb76391d89 | 1 | #include "mbed.h" |
vsluiter | 11:ce72ec658a95 | 2 | #include "HIDScope.h" |
Vigilance88 | 18:44905b008f44 | 3 | #include "MODSERIAL.h" |
Vigilance88 | 18:44905b008f44 | 4 | #include "biquadFilter.h" |
Vigilance88 | 18:44905b008f44 | 5 | #include "QEI.h" |
vsluiter | 0:32bb76391d89 | 6 | |
Vigilance88 | 18:44905b008f44 | 7 | //Define important constants in memory |
Vigilance88 | 18:44905b008f44 | 8 | #define SAMPLE_RATE 0.002 //500 Hz EMG sample rate |
Vigilance88 | 18:44905b008f44 | 9 | #define CONTROL_RATE 0.01 //100 Hz Control rate |
Vigilance88 | 18:44905b008f44 | 10 | #define ENCODER1_CPR 4200 //encoders have 4200 counts per revolution of the output shaft (X2) |
Vigilance88 | 18:44905b008f44 | 11 | #define ENCODER2_CPR 4200 //64 X4, 32 X4 counts per revolution of motor shaft, gearbox 1:131.25 |
Vigilance88 | 18:44905b008f44 | 12 | |
Vigilance88 | 18:44905b008f44 | 13 | //Objects |
Vigilance88 | 18:44905b008f44 | 14 | MODSERIAL pc(USBTX,USBRX); //serial communication |
Vigilance88 | 18:44905b008f44 | 15 | DigitalIn button(PTA1); //buttons |
Vigilance88 | 18:44905b008f44 | 16 | DigitalIn button1(PTB9); // |
Vigilance88 | 18:44905b008f44 | 17 | |
Vigilance88 | 18:44905b008f44 | 18 | AnalogIn emg1(A0); //Analog input - Biceps EMG |
Vigilance88 | 18:44905b008f44 | 19 | AnalogIn emg2(A1); //Analog input - Triceps EMG |
Vigilance88 | 18:44905b008f44 | 20 | AnalogIn emg3(A2); //Analog input - Flexor EMG |
Vigilance88 | 18:44905b008f44 | 21 | AnalogIn emg4(A3); //Analog input - Extensor EMG |
Vigilance88 | 18:44905b008f44 | 22 | |
Vigilance88 | 18:44905b008f44 | 23 | Ticker sample_timer; //Ticker for EMG sampling |
Vigilance88 | 18:44905b008f44 | 24 | Ticker control_timer; //Ticker for control loop |
Vigilance88 | 18:44905b008f44 | 25 | HIDScope scope(4); //Scope 4 channels |
Vigilance88 | 18:44905b008f44 | 26 | |
Vigilance88 | 18:44905b008f44 | 27 | // AnalogIn potmeter(A4); //potmeters |
Vigilance88 | 18:44905b008f44 | 28 | // AnalogIn potmeter2(A5); // |
Vigilance88 | 18:44905b008f44 | 29 | |
Vigilance88 | 18:44905b008f44 | 30 | QEI Encoder1(D13,D12,NC,32); //channel A and B from encoder, counts = Encoder.getPulses(); |
Vigilance88 | 18:44905b008f44 | 31 | QEI Encoder2(D10,D9,NC,32); //channel A and B from encoder, |
Vigilance88 | 18:44905b008f44 | 32 | PwmOut pwm_motor1(D6); //D4 and D5 = motor 2, D7 and D6 = motor 1 |
Vigilance88 | 18:44905b008f44 | 33 | PwmOut pwm_motor2(D5); //D4 and D5 = motor 2, D7 and D6 = motor 1 |
Vigilance88 | 18:44905b008f44 | 34 | DigitalOut dir_motor1(D7); //Direction motor 1 |
Vigilance88 | 18:44905b008f44 | 35 | DigitalOut dir_motor2(D4); //Direction motor 2 |
Vigilance88 | 18:44905b008f44 | 36 | |
Vigilance88 | 18:44905b008f44 | 37 | //Filters |
Vigilance88 | 18:44905b008f44 | 38 | //biquadFilter filter(a1, a2, b0, b1, b2); coefficients |
Vigilance88 | 18:44905b008f44 | 39 | biquadFilter filter1( 0.0009446914586925257 , 0.0018893829173850514 , 0.0009446914586925257 , -1.911196288237583 , 0.914975054072353 ); //eg. notch / lowpass / highpass |
Vigilance88 | 18:44905b008f44 | 40 | biquadFilter filter2( 0.0009446914586925257 , 0.0018893829173850514 , 0.0009446914586925257 , -1.911196288237583 , 0.914975054072353 ); //eg. notch / lowpass / highpass |
Vigilance88 | 18:44905b008f44 | 41 | biquadFilter filter3( 0.0009446914586925257 , 0.0018893829173850514 , 0.0009446914586925257 , -1.911196288237583 , 0.914975054072353 ); //eg. notch / lowpass / highpass |
Vigilance88 | 18:44905b008f44 | 42 | biquadFilter filter4( 0.0009446914586925257 , 0.0018893829173850514 , 0.0009446914586925257 , -1.911196288237583 , 0.914975054072353 ); //eg. notch / lowpass / highpass |
Vigilance88 | 18:44905b008f44 | 43 | |
Vigilance88 | 18:44905b008f44 | 44 | void keep_in_range(float * in, float min, float max); |
Vigilance88 | 18:44905b008f44 | 45 | void sample(void); |
Vigilance88 | 18:44905b008f44 | 46 | |
Vigilance88 | 18:44905b008f44 | 47 | //Start sampling |
Vigilance88 | 18:44905b008f44 | 48 | void start_sampling(void) |
Vigilance88 | 18:44905b008f44 | 49 | { |
Vigilance88 | 18:44905b008f44 | 50 | sample_timer.attach(&sample,SAMPLE_RATE); |
Vigilance88 | 18:44905b008f44 | 51 | } |
Vigilance88 | 18:44905b008f44 | 52 | |
Vigilance88 | 18:44905b008f44 | 53 | //stop sampling |
Vigilance88 | 18:44905b008f44 | 54 | void stop_sampling(void) |
Vigilance88 | 18:44905b008f44 | 55 | { |
Vigilance88 | 18:44905b008f44 | 56 | sample_timer.detach(); |
Vigilance88 | 18:44905b008f44 | 57 | } |
vsluiter | 2:e314bb3b2d99 | 58 | |
Vigilance88 | 18:44905b008f44 | 59 | //Sample function |
Vigilance88 | 18:44905b008f44 | 60 | void sample(void) |
Vigilance88 | 18:44905b008f44 | 61 | { |
Vigilance88 | 18:44905b008f44 | 62 | double emg_value1 = emg1.read(); |
Vigilance88 | 18:44905b008f44 | 63 | double emg_value2 = emg2.read(); |
Vigilance88 | 18:44905b008f44 | 64 | double emg_value3 = emg3.read(); |
Vigilance88 | 18:44905b008f44 | 65 | double emg_value4 = emg4.read(); |
Vigilance88 | 18:44905b008f44 | 66 | scope.set(0,emg_value1); |
Vigilance88 | 18:44905b008f44 | 67 | scope.set(1,emg_value2); |
Vigilance88 | 18:44905b008f44 | 68 | scope.set(2,emg_value3); |
Vigilance88 | 18:44905b008f44 | 69 | scope.set(3,emg_value4); |
Vigilance88 | 18:44905b008f44 | 70 | scope.send(); |
Vigilance88 | 18:44905b008f44 | 71 | } |
Vigilance88 | 18:44905b008f44 | 72 | |
Vigilance88 | 18:44905b008f44 | 73 | //Send arm to mechanical limits, and set encoder to 0. Then send arm to starting position. |
Vigilance88 | 19:0a3ee31dcdb4 | 74 | void calibrate_arm(void) |
Vigilance88 | 19:0a3ee31dcdb4 | 75 | { |
Vigilance88 | 19:0a3ee31dcdb4 | 76 | } |
Vigilance88 | 19:0a3ee31dcdb4 | 77 | |
Vigilance88 | 19:0a3ee31dcdb4 | 78 | //EMG MVC measurement |
Vigilance88 | 19:0a3ee31dcdb4 | 79 | void calibrate_emg(void) |
Vigilance88 | 18:44905b008f44 | 80 | { |
Vigilance88 | 18:44905b008f44 | 81 | } |
Vigilance88 | 18:44905b008f44 | 82 | |
Vigilance88 | 18:44905b008f44 | 83 | //use biquadFilter library, output filtered EMG (combine with sample?) |
Vigilance88 | 18:44905b008f44 | 84 | double filter(double u) |
Vigilance88 | 18:44905b008f44 | 85 | { |
Vigilance88 | 18:44905b008f44 | 86 | double test; |
Vigilance88 | 18:44905b008f44 | 87 | return test; |
Vigilance88 | 18:44905b008f44 | 88 | } |
Vigilance88 | 18:44905b008f44 | 89 | |
Vigilance88 | 18:44905b008f44 | 90 | //Input error and Kp, Kd, Ki, output control signal |
Vigilance88 | 18:44905b008f44 | 91 | void pid() |
vsluiter | 2:e314bb3b2d99 | 92 | { |
Vigilance88 | 18:44905b008f44 | 93 | } |
Vigilance88 | 18:44905b008f44 | 94 | |
Vigilance88 | 18:44905b008f44 | 95 | //Analyse filtered EMG, calculate reference position from EMG, compare reference position with current position,convert to angles, send error through pid(), send PWM and DIR to motors |
Vigilance88 | 18:44905b008f44 | 96 | void control() |
Vigilance88 | 18:44905b008f44 | 97 | { |
Vigilance88 | 18:44905b008f44 | 98 | //analyse emg (= velocity, averages?) |
Vigilance88 | 18:44905b008f44 | 99 | |
Vigilance88 | 18:44905b008f44 | 100 | //calculate reference position based on the average emg (integrate) |
Vigilance88 | 18:44905b008f44 | 101 | |
Vigilance88 | 18:44905b008f44 | 102 | //calculate error (refpos-currentpos) currentpos = forward kinematics |
Vigilance88 | 18:44905b008f44 | 103 | |
Vigilance88 | 18:44905b008f44 | 104 | //inverse kinematics (pos error to angle error) |
Vigilance88 | 18:44905b008f44 | 105 | |
Vigilance88 | 18:44905b008f44 | 106 | //PID controller |
Vigilance88 | 18:44905b008f44 | 107 | pid(); |
Vigilance88 | 18:44905b008f44 | 108 | //send PWM and DIR to motors |
Vigilance88 | 18:44905b008f44 | 109 | } |
Vigilance88 | 18:44905b008f44 | 110 | |
Vigilance88 | 18:44905b008f44 | 111 | //Start control |
Vigilance88 | 18:44905b008f44 | 112 | void start_control(void) |
Vigilance88 | 18:44905b008f44 | 113 | { |
Vigilance88 | 18:44905b008f44 | 114 | control_timer.attach(&control,SAMPLE_RATE); |
Vigilance88 | 18:44905b008f44 | 115 | } |
Vigilance88 | 18:44905b008f44 | 116 | |
Vigilance88 | 18:44905b008f44 | 117 | //stop control |
Vigilance88 | 18:44905b008f44 | 118 | void stop_control(void) |
Vigilance88 | 18:44905b008f44 | 119 | { |
Vigilance88 | 18:44905b008f44 | 120 | control_timer.detach(); |
vsluiter | 2:e314bb3b2d99 | 121 | } |
vsluiter | 0:32bb76391d89 | 122 | |
vsluiter | 0:32bb76391d89 | 123 | int main() |
vsluiter | 0:32bb76391d89 | 124 | { |
Vigilance88 | 18:44905b008f44 | 125 | // make a menu, user has to initiated next step |
Vigilance88 | 18:44905b008f44 | 126 | |
Vigilance88 | 19:0a3ee31dcdb4 | 127 | calibrate_arm(); //Start Calibration |
Vigilance88 | 19:0a3ee31dcdb4 | 128 | calibrate_emg(); |
Vigilance88 | 18:44905b008f44 | 129 | start_sampling(); //500 Hz EMG |
Vigilance88 | 18:44905b008f44 | 130 | start_control(); //100 Hz control |
Vigilance88 | 18:44905b008f44 | 131 | |
Vigilance88 | 18:44905b008f44 | 132 | //maybe some stop commands when a button is pressed: detach from timers. |
Vigilance88 | 18:44905b008f44 | 133 | //stop_control(); |
Vigilance88 | 18:44905b008f44 | 134 | //stop_sampling(); |
Vigilance88 | 18:44905b008f44 | 135 | |
Vigilance88 | 18:44905b008f44 | 136 | while(1) { |
Vigilance88 | 18:44905b008f44 | 137 | |
Vigilance88 | 18:44905b008f44 | 138 | |
Vigilance88 | 18:44905b008f44 | 139 | }//end of while loop |
Vigilance88 | 18:44905b008f44 | 140 | }//end of main |
tomlankhorst | 15:0da764eea774 | 141 | |
Vigilance88 | 18:44905b008f44 | 142 | void keep_in_range(float * in, float min, float max) |
Vigilance88 | 18:44905b008f44 | 143 | { |
Vigilance88 | 18:44905b008f44 | 144 | *in > min ? *in < max? : *in = max: *in = min; |
vsluiter | 0:32bb76391d89 | 145 | } |