Diff: main.cpp

Revision:

16:720365110953

Parent:

15:a52be6368cd5

Child:

17:0ae9e8c958f8

--- a/main.cpp	Mon Oct 15 14:40:50 2018 +0000
+++ b/main.cpp	Fri Oct 19 10:52:31 2018 +0000
@@ -1,3 +1,4 @@
+// Inclusion of libraries
 #include "mbed.h"
 #include "FastPWM.h"    
 #include "QEI.h"        // Includes library for encoder
@@ -6,84 +7,92 @@
 #include "BiQuad.h"
 
 // Input
-AnalogIn pot1(A1);
-AnalogIn pot2(A2);
+AnalogIn    pot1(A1);
+AnalogIn    pot2(A2);
 InterruptIn button1(D0);
 InterruptIn button2(D1);
 InterruptIn emergencybutton(SW2);  /* This is not yet implemented! 
 The button SW2 on the K64F is the emergency button: if you press this, 
 everything will abort as soon as possible
 */
-
-DigitalIn pin8(D8);     // Encoder 1 B
-DigitalIn pin9(D9);     // Encoder 1 A
-DigitalIn pin10(D10);   // Encoder 2 B
-DigitalIn pin11(D11);   // Encoder 2 A
-DigitalIn pin12(D12);   // Encoder 3 B
-DigitalIn pin13(D13);   // Encoder 3 A
+/* Isn't needed anymore --> directly used in QEI
+DigitalIn pin8(D8);       // Encoder 1 B
+DigitalIn pin9(D9);       // Encoder 1 A
+InterruptIn pin10(D10);     // Encoder 2 B
+InterruptIn pin11(D11);     // Encoder 2 A
+InterruptIn pin12(D12);     // Encoder 3 B
+InterruptIn pin13(D13);     // Encoder 3 A
+*/
 
 // Output
-DigitalOut pin2(D2);    // Motor 3 direction
-FastPWM pin3(D3);       // Motor 3 pwm
-DigitalOut pin4(D4);    // Motor 2 direction
-FastPWM pin5(D5);       // Motor 2 pwm
-FastPWM pin6(D6);       // Motor 1 pwm
-DigitalOut pin7(D7);    // Motor 1 direction
-//double u1  = pot1;
+DigitalOut  pin2(D2);       // Motor 3 direction
+FastPWM     pin3(D3);       // Motor 3 pwm
+DigitalOut  pin4(D4);       // Motor 2 direction
+FastPWM     pin5(D5);       // Motor 2 pwm
+FastPWM     pin6(D6);       // Motor 1 pwm
+DigitalOut  pin7(D7);       // Motor 1 direction
 
-MODSERIAL pc(USBTX, USBRX);
-Ticker motor;
-double u3 = 0.0;         // Normalised variable for the movement of motor 3
+// Utilisation of libraries
+MODSERIAL   pc(USBTX, USBRX);
+Ticker      motor;
+QEI         Encoder1(D8,D9,NC,32);      // Order of D8 and D9 still needs to be determined
+QEI         Encoder2(D10,D11,NC,32);   
+QEI         Encoder3(D12,D13,NC,32);
+//Ticker      derivative;
 
-void draaibuttons()         
+// Global variables
+//double    u1  = pot1;
+double  u3 = 0.0;         // Normalised variable for the movement of motor 3
+
+// Functions
+void    turnbuttons()         
 {   /*  Pressing button 2 concludes in a change of speed. While button 1 is pressed,
         the direction of change of speed is reversed. So pressing button 1 and 2
         simultaneously results for the turning speed of motor 3 in a slower movement,
         and eventually the motor will turn the other way around.
     */
-    if (button1 == 1 && button2 == 1)
+    if  (button1 == 1 && button2 == 1)
         {   u3 = u3 + 0.1f;  //In stapjes van 0.1            
-            if (u3>1.0f)
+            if  (u3>1.0f)
             {   u3 = 1.0f;
             }
         }
-    
     else if (button1 == 0 && button2 == 1)
         {   u3 = u3 - 0.1f;
-            if (u3>1.0f)
+            if  (u3>1.0f)
             {   u3 = 1.0f;
             }
         }
 }
 
-void draai()    
+void    turn()    
 /*  Function for the movement of all motors, using the potmeters for the moving
     direction and speed of motor 1 and 2, and using button 1 and 2 on the biorobotics
     shield for the moving direction and speed of motor 3.
 */
-{      
-    double u1 = 2.0*(pot1 - 0.5);    // Normalised variable for the movement of motor 1
-    if (u1>0)
+{   // Controls motor 1 velocity through potmeter 1   
+    double  u1 = 2.0*(pot1 - 0.5);    // Normalised variable for the movement of motor 1
+    if  (u1>0)
     {   pin4 = true;
     }
-    else if(u1<0)
+    else if (u1<0)
     {   pin4 = false;    
     }
     pin5 = fabs(u1);
-    
-    double u2 = 2.0*(pot2 - 0.5);    // Normalised variable for the movement of motor 2
-    if (u2<0)
+    // Controls motor 2 velocity through potmeter 2
+    double  u2 = 2.0*(pot2 - 0.5);    // Normalised variable for the movement of motor 2
+    if  (u2<0)
     {   pin7 = true;
     }
-    else if(u2>0)
+    else if (u2>0)
     {   pin7 = false;    
     }
-    pin6 = fabs(u2);  
-    
-    if (u3>0)           //misschien weg?
+    pin6 = fabs(u2);
+    // Controls motor 3 velocity through the buttons
+    if  (u3>0)           //can maybe be deleted?
     {   pin2 = true;
     }
-    else if(u3<0)
+    else if (u3<0)
     {   pin2 = false; 
     }
     else
@@ -92,33 +101,23 @@
     pin3 = fabs(u3);   
 }
 /*
-double Kp = 17.5;
-double Ki = 1.02;
-double Kd = 23.2;
-double Ts = 0.01;       // Sample time in seconds
-
-double PID_controller(double error)
-{   static double error_integral = 0;
-    static double error_prev = error;  
-           
-    // initialization with this value only done once!
-    static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);      
+//Derivative program to test, needs to work at 1000 Hz
+void    derivefunction(double x, double xprev, double tsample)
+{   double deriv = (x - xprev)/tsample;
     
-    // Proportional part:
-    double u_k = Kp * e; 
-    // Integral part  
-    error_integral = error_integral + error * Ts;
-    double u_i = Ki * error_integral;       
-    // Derivative part
-    double error_derivative = (error - error_prev)/Ts;
-    double filtered_error_derivative = LowPassFilter.step(error_derivative);
-    double u_d = Kd * filtered_error_derivative;  error_prev = error; 
-       
-    // Sum all parts and return it
-    return u_k + u_i + u_d;   
 }
 */
 
+/* Emergency program to test
+void    Emergency()
+if emergencybutton == 1;
+{   while (true)
+    {
+    }
+}
+*/
+
+// Main program
 int main()
 {
     pc.baud(115200);
@@ -128,13 +127,21 @@
     pin5.period_us(50);    
     pin6.period_us(50);
     
-    //Ticker
-    motor.attach(draai, 0.001);
+    // Ticker
+    motor.attach(turn, 0.001);
+    //derivative.attach(derivefunction, 1/1000)
+    
+    // Interrupts
+    button1.rise(&turnbuttons);       
+    button2.rise(&turnbuttons);
     
-    //Interrupts
-    button1.rise(&draaibuttons);       
-    button2.rise(&draaibuttons);
-    while(true)
-    {
+    // Encoder, this code doesn't work
+    // Our encoder doesn't have a NC, what do we need to fill in for that?
+    int counts1 = Encoder1.getPulses();
+    int counts2 = Encoder2.getPulses();
+    int counts3 = Encoder3.getPulses();
+        
+    while   (true)
+    {        
     }
 }
\ No newline at end of file