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@18:44905b008f44, 2015-10-12 (annotated)

Committer:
Vigilance88
Date:
Mon Oct 12 13:44:29 2015 +0000
Revision:
18:44905b008f44
Parent:
16:9f7797ffd0fb
Child:
19:0a3ee31dcdb4
first draft

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 18:44905b008f44 74 void calibrate(void)
Vigilance88 18:44905b008f44 75 {
Vigilance88 18:44905b008f44 76 }
Vigilance88 18:44905b008f44 77
Vigilance88 18:44905b008f44 78 //use biquadFilter library, output filtered EMG (combine with sample?)
Vigilance88 18:44905b008f44 79 double filter(double u)
Vigilance88 18:44905b008f44 80 {
Vigilance88 18:44905b008f44 81 double test;
Vigilance88 18:44905b008f44 82 return test;
Vigilance88 18:44905b008f44 83 }
Vigilance88 18:44905b008f44 84
Vigilance88 18:44905b008f44 85 //Input error and Kp, Kd, Ki, output control signal
Vigilance88 18:44905b008f44 86 void pid()
vsluiter 2:e314bb3b2d99 87 {
Vigilance88 18:44905b008f44 88 }
Vigilance88 18:44905b008f44 89
Vigilance88 18:44905b008f44 90 //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 91 void control()
Vigilance88 18:44905b008f44 92 {
Vigilance88 18:44905b008f44 93 //analyse emg (= velocity, averages?)
Vigilance88 18:44905b008f44 94
Vigilance88 18:44905b008f44 95 //calculate reference position based on the average emg (integrate)
Vigilance88 18:44905b008f44 96
Vigilance88 18:44905b008f44 97 //calculate error (refpos-currentpos) currentpos = forward kinematics
Vigilance88 18:44905b008f44 98
Vigilance88 18:44905b008f44 99 //inverse kinematics (pos error to angle error)
Vigilance88 18:44905b008f44 100
Vigilance88 18:44905b008f44 101 //PID controller
Vigilance88 18:44905b008f44 102 pid();
Vigilance88 18:44905b008f44 103 //send PWM and DIR to motors
Vigilance88 18:44905b008f44 104 }
Vigilance88 18:44905b008f44 105
Vigilance88 18:44905b008f44 106 //Start control
Vigilance88 18:44905b008f44 107 void start_control(void)
Vigilance88 18:44905b008f44 108 {
Vigilance88 18:44905b008f44 109 control_timer.attach(&control,SAMPLE_RATE);
Vigilance88 18:44905b008f44 110 }
Vigilance88 18:44905b008f44 111
Vigilance88 18:44905b008f44 112 //stop control
Vigilance88 18:44905b008f44 113 void stop_control(void)
Vigilance88 18:44905b008f44 114 {
Vigilance88 18:44905b008f44 115 control_timer.detach();
vsluiter 2:e314bb3b2d99 116 }
vsluiter 0:32bb76391d89 117
vsluiter 0:32bb76391d89 118 int main()
vsluiter 0:32bb76391d89 119 {
Vigilance88 18:44905b008f44 120 // make a menu, user has to initiated next step
Vigilance88 18:44905b008f44 121
Vigilance88 18:44905b008f44 122 calibrate(); //Start Calibration
Vigilance88 18:44905b008f44 123 start_sampling(); //500 Hz EMG
Vigilance88 18:44905b008f44 124 start_control(); //100 Hz control
Vigilance88 18:44905b008f44 125
Vigilance88 18:44905b008f44 126 //maybe some stop commands when a button is pressed: detach from timers.
Vigilance88 18:44905b008f44 127 //stop_control();
Vigilance88 18:44905b008f44 128 //stop_sampling();
Vigilance88 18:44905b008f44 129
Vigilance88 18:44905b008f44 130 while(1) {
Vigilance88 18:44905b008f44 131
Vigilance88 18:44905b008f44 132
Vigilance88 18:44905b008f44 133 }//end of while loop
Vigilance88 18:44905b008f44 134 }//end of main
tomlankhorst 15:0da764eea774 135
Vigilance88 18:44905b008f44 136 void keep_in_range(float * in, float min, float max)
Vigilance88 18:44905b008f44 137 {
Vigilance88 18:44905b008f44 138 *in > min ? *in < max? : *in = max: *in = min;
vsluiter 0:32bb76391d89 139 }