Diff: main.cpp

Revision:

29:948b0b14f6be

Parent:

28:743485bb51e4

Child:

30:a9fdd3202ca2

--- a/main.cpp	Mon Oct 19 10:23:56 2015 +0000
+++ b/main.cpp	Wed Oct 21 09:19:23 2015 +0000
@@ -95,10 +95,10 @@
 //PID variables
 double u1; double u2;                                               //Output of PID controller (PWM value for motor 1 and 2)
 double m1_error=0; double m1_e_int=0; double m1_e_prev=0;           //Error, integrated error, previous error
-const double m1_kp=1; const double m1_ki=0.0125; const double m1_kd=0.1;   //Proportional, integral and derivative gains.
+const double m1_kp=0.001; const double m1_ki=0.0125; const double m1_kd=0.1;   //Proportional, integral and derivative gains.
 
 double m2_error=0; double m2_e_int=0; double m2_e_prev=0;           //Error, integrated error, previous error
-const double m2_kp=1; const double m2_ki=0.0125; const double m2_kd=0.1;   //Proportional, integral and derivative gains.
+const double m2_kp=0.001; const double m2_ki=0.0125; const double m2_kd=0.1;   //Proportional, integral and derivative gains.
 
 //highpass filter 20 Hz
 const double high_b0 = 0.956543225556877;
@@ -148,7 +148,7 @@
 const double low_a1 = -1.968902268531908;
 const double low_a2 = 0.9693784555043481;
 
-//Forward Kinematics
+//Forward and Inverse Kinematics
 const double l1 = 0.25; const double l2 = 0.25;
 double current_x; double current_y;
 double theta1; double theta2;
@@ -190,7 +190,7 @@
 biquadFilter     lowpass4( low_a1 , low_a2 , low_b0 , low_b1 , low_b2 );                     // EMG envelope    
 
 //PID filter (lowpass ??? Hz)
-biquadFilter     derfilter( 0.0009446914586925257 , 0.0018893829173850514 , 0.0009446914586925257 , -1.911196288237583 , 0.914975054072353 );   // derivative filter 
+biquadFilter     derfilter( low_a1 , low_a2 , low_b0 , low_b1 , low_b2 );   // derivative filter
 
 /*--------------------------------------------------------------------------------------------------------------------
 ---- DECLARE FUNCTION NAMES ------------------------------------------------------------------------------------------
@@ -216,7 +216,7 @@
 void mainMenu();
 void caliMenu();
 void controlMenu();
-void controlMenu2();
+void controlButtons();
 void clearTerminal();
 void emgInstructions();
 void titleBox();
@@ -228,7 +228,7 @@
 
 int main()
 {
-    pc.baud(115200);            //terminal baudrate
+    pc.baud(115200);            //serial baudrate
     red=1; green=1; blue=1;     //Make sure debug LEDS are off  
     
     //Set PwmOut frequency to 10k Hz
@@ -246,13 +246,13 @@
     //calibrate_emg();        
     
     
-    x=0; y=0.5;
+    x=0.5; y=0;
     //start_control();        //100 Hz control
     
     //maybe some stop commands when a button is pressed: detach from timers.
     //stop_control();
-    //stop_sampling();
-    
+    start_sampling();
+    wait(60);
     //char c;
     
     
@@ -313,7 +313,7 @@
                 wait(1);
                 start_control();
                 wait(1);   
-                controlMenu2();
+                controlButtons();
                 break;
             case 'R':
                 red=!red;
@@ -354,7 +354,7 @@
 ---- FUNCTIONS -------------------------------------------------------------------------------------------------------
 --------------------------------------------------------------------------------------------------------------------*/
 
-void controlMenu2()
+void controlButtons()
 {
     controlMenu();
     char c=0;
@@ -432,7 +432,11 @@
     biceps_power = abs(biceps_power); triceps_power = abs(triceps_power); flexor_power = abs(flexor_power); extens_power = abs(extens_power);
     biceps_power = lowpass.step(biceps_power); triceps_power = lowpass2.step(triceps_power); flexor_power = lowpass3.step(flexor_power); extens_power = lowpass4.step(extens_power);
     
-    
+    scope.set(0,emg_biceps);
+    scope.set(1,biceps_power);
+    scope.set(2,biceps_power);
+    scope.set(3,biceps_power);
+    scope.send();
     /* alternative for lowpass filter: moving average
     window=30;                      //30 samples
     int i=0;                        //buffer index