2nd draft
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed Servo
Fork of robot_mockup by
Diff: main.cpp
- Revision:
- 18:44905b008f44
- Parent:
- 16:9f7797ffd0fb
- Child:
- 19:0a3ee31dcdb4
--- a/main.cpp Tue Sep 22 07:00:54 2015 +0000 +++ b/main.cpp Mon Oct 12 13:44:29 2015 +0000 @@ -1,32 +1,139 @@ #include "mbed.h" #include "HIDScope.h" +#include "MODSERIAL.h" +#include "biquadFilter.h" +#include "QEI.h" -//Define objects -AnalogIn emg(A0); //Analog input -Ticker sample_timer; -HIDScope scope(1); +//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 - * this function samples the emg and sends it to HIDScope - **/ -void sample() +//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() { - /* First, sample the EMG using the 'read' method of the 'AnalogIn' variable named 'emg' */ - double emg_value = emg.read(); - /* Second, set the sampled emg value in channel zero (the first channel) in the 'HIDScope' variable named 'scope' */ - scope.set(0,emg_value); - /* Repeat the step above if required for more channels (channel 0 up to 5 = 6 channels) */ - /* Finally, send all channels to the PC at once */ - scope.send(); +} + +//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() { - /**Attach the 'sample' function to the timer 'sample_timer'. - * this ensures that 'sample' is executed every... 0.002 seconds - */ - sample_timer.attach(&sample, 0.002); + // 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 - /*empty loop, sample() is executed periodically*/ - while(1) {} +void keep_in_range(float * in, float min, float max) +{ + *in > min ? *in < max? : *in = max: *in = min; } \ No newline at end of file