Revision 21:9e6461109547, committed 2016-11-02

Comitter:

RickB

Date:

Wed Nov 02 09:24:32 2016 +0000

Parent:

20:84cc373e6fbb

Commit message:

groep 6;

Changed in this revision

--- a/main.cpp	Tue Nov 01 11:24:52 2016 +0000
+++ b/main.cpp	Wed Nov 02 09:24:32 2016 +0000
@@ -14,10 +14,10 @@
 AnalogIn potmeter2(A1);
 QEI Encoder(D12, D13, NC, 64, QEI::X4_ENCODING);
 HIDScope scope(3);
-
+ 
 Ticker ScopeTime;
 Ticker myControllerTicker;
-
+ 
 int q = 0 ;
 double P2 ;
 double m_ref = 0 ;
@@ -29,30 +29,30 @@
 int t =0 ;
 int x1 = -1 ;
 int x2 = -1 ;
-
+ 
 void ScopeSend()//Functie die de gegevens voor de scope uitleest en doorstuurt
 {
     scope.set(0, motor2direction.read());
     scope.set(1, motor2speed.read());
     scope.set(2, position);
-
+ 
     scope.send();
     
 }
 // Sample time (motor2-step)
 const double m2_Ts = 0.01;
-
+ 
 // Controller gain
 const double m2_Kp = 0.5,m2_Ki = 0.005, m2_Kd = 0.5;
 double m2_err_int = 0, m2_prev_err = 0;
-
+ 
 //Derivative filter coeffs
 const double BiGain = 0.016955;
 const double m2_f_a1 = -0.96608908283*BiGain, m2_f_a2 = 0.0*BiGain, m2_f_b0 = 1.0*BiGain, m2_f_b1 = 1.0*BiGain, m2_f_b2 = 0.0*BiGain;
-
+ 
 // Filter variables
 double m2_f_v1 = 0, m2_f_v2 = 0;
-
+ 
 // Biquad filter
 double biquad( double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2 )
 {
@@ -61,7 +61,7 @@
     v2 = v1; v1 = v;
     return y;
 }    
-
+ 
 // Reusable PID controller
 double PID( double e, const double Kp, const double Ki, const double Kd, double Ts, double &e_int, double &e_prev, double &f_v1, double &f_v2,
 const double f_a1,const double f_a2, const double f_b0, const double f_b1, const double f_b2)
@@ -75,7 +75,7 @@
     // PID
     return Kp * e + Ki*e_int + Kd*e_der;
 }
-
+ 
 void m2_ref()
 {
     double pi = 3.14 ;
@@ -94,7 +94,7 @@
     else if (t > f_motor){
         motor2speed.write(0.0) ; }
         
-    pc.printf("\r\n t = %i reference1 = %d pos1 = %d ",t,reference,position) ;
+    pc.printf("\r\n t = %i reference1 = %f pos1 = %f ",t,reference,position) ;
     break ;
     
     case 2  :                               // counterclockwise
@@ -106,7 +106,7 @@
     else if (t > f_motor){
         motor2speed.write(0.0) ; }
         
-    pc.printf("\r\n start1 = %d reference2 = %d pos2 = %d ",start1,reference,position) ; 
+    pc.printf("\r\n start1 = %i reference2 = %f pos2 = %f ",start1,reference,position) ; 
     break ;    
     }  
 }    
@@ -120,7 +120,7 @@
     position = pulses*360/(64*131.25); // Aantal Degs
     P2 = PID( reference - position, m2_Kp, m2_Ki, m2_Kd, m2_Ts, m2_err_int, m2_prev_err, m2_f_v1, m2_f_v2, m2_f_a1, m2_f_a2, 
     m2_f_b0, m2_f_b1, m2_f_b2);
-
+ 
     if (P2 > 0.6)
     {   P2 = 0.6 ; }
     else if (P2 < 0.6)
@@ -139,7 +139,7 @@
         motor2direction = 1;
     }
 }
-
+ 
 int main()
 {   
     pc.baud(115200);
@@ -165,4 +165,5 @@
            x2++ ;
         }        
     }
-}
\ No newline at end of file
+}
+            
\ No newline at end of file