group14 - k9 / Mbed 2 deprecated robot_2

2nd draft

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed Servo

Fork of robot_mockup by Martijn Kern

main.cpp@20:0ede3818e08e, 2015-10-12 (annotated)

Committer:
Vigilance88
Date:
Mon Oct 12 14:02:20 2015 +0000
Revision:
20:0ede3818e08e
Parent:
19:0a3ee31dcdb4
Child:
21:d6a46315d5f5
reusable pid

Who changed what in which revision?

UserRevisionLine numberNew 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 20:0ede3818e08e 91 double pid(double error, double kp, double ki, double kd,double &e_int, double &e_prev)
vsluiter 2:e314bb3b2d99 92 {
Vigilance88 20:0ede3818e08e 93
Vigilance88 20:0ede3818e08e 94 // Derivative
Vigilance88 20:0ede3818e08e 95 double e_der = (error − e_prev)/CONTROL_RATE;
Vigilance88 20:0ede3818e08e 96 e_der = biquad( e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2 );
Vigilance88 20:0ede3818e08e 97 e_prev = e;
Vigilance88 20:0ede3818e08e 98 // Integral
Vigilance88 20:0ede3818e08e 99 e_int = e_int + CONTROL_RATE ∗ error;
Vigilance88 20:0ede3818e08e 100 // PID
Vigilance88 20:0ede3818e08e 101 return Kp ∗ e + Ki ∗ e_int + Kd ∗ e_der;
Vigilance88 20:0ede3818e08e 102
Vigilance88 18:44905b008f44 103 }
Vigilance88 18:44905b008f44 104
Vigilance88 20:0ede3818e08e 105 //Analyze 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 106 void control()
Vigilance88 18:44905b008f44 107 {
Vigilance88 20:0ede3818e08e 108 //analyze emg (= velocity, averages?)
Vigilance88 18:44905b008f44 109
Vigilance88 18:44905b008f44 110 //calculate reference position based on the average emg (integrate)
Vigilance88 18:44905b008f44 111
Vigilance88 18:44905b008f44 112 //calculate error (refpos-currentpos) currentpos = forward kinematics
Vigilance88 18:44905b008f44 113
Vigilance88 18:44905b008f44 114 //inverse kinematics (pos error to angle error)
Vigilance88 18:44905b008f44 115
Vigilance88 18:44905b008f44 116 //PID controller
Vigilance88 18:44905b008f44 117 pid();
Vigilance88 18:44905b008f44 118 //send PWM and DIR to motors
Vigilance88 18:44905b008f44 119 }
Vigilance88 18:44905b008f44 120
Vigilance88 18:44905b008f44 121 //Start control
Vigilance88 18:44905b008f44 122 void start_control(void)
Vigilance88 18:44905b008f44 123 {
Vigilance88 18:44905b008f44 124 control_timer.attach(&control,SAMPLE_RATE);
Vigilance88 18:44905b008f44 125 }
Vigilance88 18:44905b008f44 126
Vigilance88 18:44905b008f44 127 //stop control
Vigilance88 18:44905b008f44 128 void stop_control(void)
Vigilance88 18:44905b008f44 129 {
Vigilance88 18:44905b008f44 130 control_timer.detach();
vsluiter 2:e314bb3b2d99 131 }
vsluiter 0:32bb76391d89 132
vsluiter 0:32bb76391d89 133 int main()
vsluiter 0:32bb76391d89 134 {
Vigilance88 18:44905b008f44 135 // make a menu, user has to initiated next step
Vigilance88 18:44905b008f44 136
Vigilance88 19:0a3ee31dcdb4 137 calibrate_arm(); //Start Calibration
Vigilance88 19:0a3ee31dcdb4 138 calibrate_emg();
Vigilance88 18:44905b008f44 139 start_sampling(); //500 Hz EMG
Vigilance88 18:44905b008f44 140 start_control(); //100 Hz control
Vigilance88 18:44905b008f44 141
Vigilance88 18:44905b008f44 142 //maybe some stop commands when a button is pressed: detach from timers.
Vigilance88 18:44905b008f44 143 //stop_control();
Vigilance88 18:44905b008f44 144 //stop_sampling();
Vigilance88 18:44905b008f44 145
Vigilance88 18:44905b008f44 146 while(1) {
Vigilance88 18:44905b008f44 147
Vigilance88 18:44905b008f44 148
Vigilance88 18:44905b008f44 149 }//end of while loop
Vigilance88 18:44905b008f44 150 }//end of main
tomlankhorst 15:0da764eea774 151
Vigilance88 18:44905b008f44 152 void keep_in_range(float * in, float min, float max)
Vigilance88 18:44905b008f44 153 {
Vigilance88 18:44905b008f44 154 *in > min ? *in < max? : *in = max: *in = min;
vsluiter 0:32bb76391d89 155 }