2nd draft
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed Servo
Fork of robot_mockup by
main.cpp
- Committer:
- Vigilance88
- Date:
- 2015-10-12
- Revision:
- 18:44905b008f44
- Parent:
- 16:9f7797ffd0fb
- Child:
- 19:0a3ee31dcdb4
File content as of revision 18:44905b008f44:
#include "mbed.h" #include "HIDScope.h" #include "MODSERIAL.h" #include "biquadFilter.h" #include "QEI.h" //Define important constants in memory #define SAMPLE_RATE 0.002 //500 Hz EMG sample rate #define CONTROL_RATE 0.01 //100 Hz Control rate #define ENCODER1_CPR 4200 //encoders have 4200 counts per revolution of the output shaft (X2) #define ENCODER2_CPR 4200 //64 X4, 32 X4 counts per revolution of motor shaft, gearbox 1:131.25 //Objects MODSERIAL pc(USBTX,USBRX); //serial communication DigitalIn button(PTA1); //buttons DigitalIn button1(PTB9); // AnalogIn emg1(A0); //Analog input - Biceps EMG AnalogIn emg2(A1); //Analog input - Triceps EMG AnalogIn emg3(A2); //Analog input - Flexor EMG AnalogIn emg4(A3); //Analog input - Extensor EMG Ticker sample_timer; //Ticker for EMG sampling Ticker control_timer; //Ticker for control loop HIDScope scope(4); //Scope 4 channels // AnalogIn potmeter(A4); //potmeters // AnalogIn potmeter2(A5); // QEI Encoder1(D13,D12,NC,32); //channel A and B from encoder, counts = Encoder.getPulses(); QEI Encoder2(D10,D9,NC,32); //channel A and B from encoder, PwmOut pwm_motor1(D6); //D4 and D5 = motor 2, D7 and D6 = motor 1 PwmOut pwm_motor2(D5); //D4 and D5 = motor 2, D7 and D6 = motor 1 DigitalOut dir_motor1(D7); //Direction motor 1 DigitalOut dir_motor2(D4); //Direction motor 2 //Filters //biquadFilter filter(a1, a2, b0, b1, b2); coefficients biquadFilter filter1( 0.0009446914586925257 , 0.0018893829173850514 , 0.0009446914586925257 , -1.911196288237583 , 0.914975054072353 ); //eg. notch / lowpass / highpass biquadFilter filter2( 0.0009446914586925257 , 0.0018893829173850514 , 0.0009446914586925257 , -1.911196288237583 , 0.914975054072353 ); //eg. notch / lowpass / highpass biquadFilter filter3( 0.0009446914586925257 , 0.0018893829173850514 , 0.0009446914586925257 , -1.911196288237583 , 0.914975054072353 ); //eg. notch / lowpass / highpass biquadFilter filter4( 0.0009446914586925257 , 0.0018893829173850514 , 0.0009446914586925257 , -1.911196288237583 , 0.914975054072353 ); //eg. notch / lowpass / highpass void keep_in_range(float * in, float min, float max); void sample(void); //Start sampling void start_sampling(void) { sample_timer.attach(&sample,SAMPLE_RATE); } //stop sampling void stop_sampling(void) { sample_timer.detach(); } //Sample function void sample(void) { double emg_value1 = emg1.read(); double emg_value2 = emg2.read(); double emg_value3 = emg3.read(); double emg_value4 = emg4.read(); scope.set(0,emg_value1); scope.set(1,emg_value2); scope.set(2,emg_value3); scope.set(3,emg_value4); scope.send(); } //Send arm to mechanical limits, and set encoder to 0. Then send arm to starting position. void calibrate(void) { } //use biquadFilter library, output filtered EMG (combine with sample?) double filter(double u) { double test; return test; } //Input error and Kp, Kd, Ki, output control signal void pid() { } //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 void control() { //analyse emg (= velocity, averages?) //calculate reference position based on the average emg (integrate) //calculate error (refpos-currentpos) currentpos = forward kinematics //inverse kinematics (pos error to angle error) //PID controller pid(); //send PWM and DIR to motors } //Start control void start_control(void) { control_timer.attach(&control,SAMPLE_RATE); } //stop control void stop_control(void) { control_timer.detach(); } int main() { // make a menu, user has to initiated next step calibrate(); //Start Calibration start_sampling(); //500 Hz EMG start_control(); //100 Hz control //maybe some stop commands when a button is pressed: detach from timers. //stop_control(); //stop_sampling(); while(1) { }//end of while loop }//end of main void keep_in_range(float * in, float min, float max) { *in > min ? *in < max? : *in = max: *in = min; }