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@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?

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 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 }