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