Diff: main.cpp

Revision:

20:84cc373e6fbb

Parent:

19:9417d2011e8b

Child:

21:9e6461109547

--- a/main.cpp	Fri Oct 02 07:27:12 2015 +0000
+++ b/main.cpp	Tue Nov 01 11:24:52 2016 +0000
@@ -9,19 +9,26 @@
 DigitalOut LedR(LED_RED);
 DigitalOut LedG(LED_GREEN);
 DigitalOut LedB(LED_BLUE);
-DigitalOut motor2direction(D4); //D4 en D5 zijn motor 2 (op het motorshield)
-PwmOut motor2speed(D5);
-AnalogIn potmeter2(A5);
-QEI Encoder(D3, D2, NC, 128);
+DigitalOut motor2direction(D7); //D6 en D7 zijn motor 2 (op het motorshield)
+PwmOut motor2speed(D6);
+AnalogIn potmeter2(A1);
+QEI Encoder(D12, D13, NC, 64, QEI::X4_ENCODING);
 HIDScope scope(3);
 
 Ticker ScopeTime;
 Ticker myControllerTicker;
 
-double reference;
-double position;
-double m2_ref = 0;
-int count = 0;
+int q = 0 ;
+double P2 ;
+double m_ref = 0 ;
+double reference = 0 ;
+double position ;
+double h_m = 0 ;
+int count = 0 ;
+int f_motor = 100 ;
+int t =0 ;
+int x1 = -1 ;
+int x2 = -1 ;
 
 void ScopeSend()//Functie die de gegevens voor de scope uitleest en doorstuurt
 {
@@ -55,7 +62,6 @@
     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)
@@ -70,73 +76,93 @@
     return Kp * e + Ki*e_int + Kd*e_der;
 }
 
+void m2_ref()
+{
+    double pi = 3.14 ;
+    double t_m = 1 ;
+    double start1 ;
+    double start2 ; 
+        
+    switch (q)
+    {
+    case 1 :                                // clockwise
+                   
+    start1 = x1*h_m - (x2+1)*h_m ;                  
+    if (t >= 0 & t <= f_motor) {
+        reference = start1 + (h_m* t_m *t)*1/f_motor - h_m/(2*pi)*sin(2*pi/t_m*t/f_motor) ;
+        t++ ; }
+    else if (t > f_motor){
+        motor2speed.write(0.0) ; }
+        
+    pc.printf("\r\n t = %i reference1 = %d pos1 = %d ",t,reference,position) ;
+    break ;
+    
+    case 2  :                               // counterclockwise
+    
+    start2 = (x1+1)*h_m - x2*h_m ;    
+    if (t >= 0 & t <= f_motor) {
+        reference = start2 - ((h_m* t_m *t)*1/f_motor - h_m/(2*pi)*sin(2*pi/t_m*t/f_motor)) ;
+        t++ ; }
+    else if (t > f_motor){
+        motor2speed.write(0.0) ; }
+        
+    pc.printf("\r\n start1 = %d reference2 = %d pos2 = %d ",start1,reference,position) ; 
+    break ;    
+    }  
+}    
+
 // Motor2 control
 void motor2_Controller() 
 {
-    reference = m2_ref; // Setpoint
+    m2_ref() ;
+    // Setpoint
     double pulses = Encoder.getPulses();
-    position = Encoder.getPulses()*360/(0.5*128*131); // Aantal Degs
-    double 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, 
+    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);
-    motor2speed = abs(P2); // Speed control
-    if(P2 > 0) // Direction control
+
+    if (P2 > 0.6)
+    {   P2 = 0.6 ; }
+    else if (P2 < 0.6)
+    {   P2 = -0.6 ; }
+    
+    const int frequency_pwm = 20000 ;
+    motor2speed.period(1/frequency_pwm) ;
+    motor2speed = abs(P2); // Speed control   
+    
+    if (P2 > 0) // Direction control
     {  
         motor2direction = 0;
     }
-    else
+    else if (P2 < 0)
     {
         motor2direction = 1;
     }
-    pc.printf("position = %f aantal degs = %f, pulses = %f\n",reference,position, pulses);
 }
 
 int main()
 {   
     pc.baud(115200);
-    pc.printf("Tot aan loop werkt\n");
+        
+    ScopeTime.attach_us(&ScopeSend, 10e4);
+    myControllerTicker.attach( &motor2_Controller, 0.01); // 100 Hz
     
-    ScopeTime.attach_us(&ScopeSend, 10e4);
-    myControllerTicker.attach( &motor2_Controller, 0.01f ); // 100 Hz
     while(true)
-    {      
-       switch (count)
-       {
-           case (0):
-           {
-                LedR.write(0);
-                char c = pc.getc();
-                if(c == 'r')
-                {
-                    m2_ref = m2_ref + 5;
-                    if (m2_ref > 90)
-                    {
-                        m2_ref = 90;
-                    }
-                }
-                if(c == 'f')
-                {
-                    m2_ref = m2_ref - 5;
-                    if (m2_ref < -90)
-                    {
-                        m2_ref = -90;
-                    }
-                }
-                break;
-            }
-            
-            case (1):
-            {
-                LedG.write(0);
-                // code voor het besturen van de 2e motor
-                break;
-            }
-            
-            default:
-            {
-                LedB.write(1);
-                break;
-            }
-        } 
+    {
+       char c = pc.getc();
+       if(c == 'r')
+        {
+           q = 1 ;              // case 1
+           t = 0 ;
+           h_m = 3 ;
+           x1++ ;
+        }
+        if(c == 'f')
+        {
+           q = 2 ;              // case 2
+           t = 0 ;
+           h_m = 3 ;
+           x2++ ;
+        }        
     }
-
 }
\ No newline at end of file