Revision 51:e4a0ce7ff4b8, committed 2015-10-27

Comitter:

Vigilance88

Date:

Tue Oct 27 09:09:56 2015 +0000

Parent:

50:54f71544964c

Child:

52:8a8c53dc8547

Commit message:

set der filter to 60 hz

Changed in this revision

--- a/main.cpp	Tue Oct 27 08:44:29 2015 +0000
+++ b/main.cpp	Tue Oct 27 09:09:56 2015 +0000
@@ -167,6 +167,13 @@
 const double low_a1 = -1.968902268531908;
 const double low_a2 = 0.9693784555043481;
 
+//Derivative lowpass filter 60 Hz  - remove error noise
+const double derlow_b0 = 0.027859766117136;
+const double derlow_b1 = 0.0557195322342721;
+const double derlow_b2 = 0.027859766117136;
+const double derlow_a1 = -1.47548044359265;
+const double derlow_a2 = 0.58691950806119;
+
 //Forward Kinematics
 const double l1 = 0.25; const double l2 = 0.25;     //Arm lengths
 double current_x; double current_y;                 //Current position
@@ -229,9 +236,9 @@
 biquadFilter     notch2_4( notch2_a1 , notch2_a2 , notch2_b0 , notch2_b1 , notch2_b2 );      //
 biquadFilter     lowpass4( low_a1 , low_a2 , low_b0 , low_b1 , low_b2 );                     // EMG envelope    
 
-//PID filter (lowpass ??? Hz)
-biquadFilter     derfilter1( low_a1 , low_a2 , low_b0 , low_b1 , low_b2 );   // derivative filter
-biquadFilter     derfilter2( low_a1 , low_a2 , low_b0 , low_b1 , low_b2 );   // derivative filter
+//PID filter (lowpass 60 Hz)
+biquadFilter     derfilter1( derlow_a1 , derlow_a2 , derlow_b0 , derlow_b1 , derlow_b2 );   // derivative filter
+biquadFilter     derfilter2( derlow_a1 , derlow_a2 , derlow_b0 , derlow_b1 , derlow_b2 );   // derivative filter
 
 /*--------------------------------------------------------------------------------------------------------------------
 ---- DECLARE FUNCTION NAMES ------------------------------------------------------------------------------------------
@@ -448,7 +455,7 @@
     wait(1);
     }//end if
     
-    /*if( pc.readable() ){
+    if( pc.readable() ){
         c = pc.getc();
         switch (c) 
             {
@@ -488,7 +495,7 @@
     
     }
     //end if pc readable
-    */
+    
     
     }
     //end of while loop
@@ -520,14 +527,14 @@
 }
 
 //Limit switch - if hit: set pulsecount of encoder to angle of lower mechanical limit
-void shoulder(){
+void shoulder()
+{
     pwm_motor1=0;
     done1 = true;
     pc.printf("Shoulder Limit hit - shutting down motor 1\r\n");
     //mechanical angle limits -> pulses. If 40 degrees -> counts = floor( 40 * (4200/360) )
     theta1_cal = floor(theta1_lower * (4200/(2*PI)));
-    Encoder1.setPulses(theta1_cal);       //edited QEI library: added setPulses(int p)
-    
+    Encoder1.setPulses(theta1_cal);       //edited QEI library: added setPulses(int p)  
 }
 
 void elbow(){
@@ -537,8 +544,7 @@
     
     //Mechanical limit 43 degrees -> 43*(4200/360) = 350
     theta2_cal = floor(theta2_lower * (4200/(2*PI)));
-    Encoder2.setPulses(theta2_cal);       //edited QEI library: added setPulses(int p)
-    
+    Encoder2.setPulses(theta2_cal);       //edited QEI library: added setPulses(int p)    
 }
 
 //Run motors slowly clockwise to mechanical limit. Attached to 100Hz ticker
@@ -552,9 +558,6 @@
             pwm_motor2=0.3;             //start moving forearm slowly cw
             pc.printf("Motor 2 running %f \r\n");    
         }   
-  //  if(done1==true && done2==true)      //if both limits are hit
-   //      pwm_motor2=0;                  //stop motor2
-    //     calibrating=false;             //stop calibrating
 }
     
 //Send arm to mechanical limits, and set encoder to 0. Then send arm to starting position.