Diff: main.cpp

Revision:

20:695140b8db2f

Parent:

19:1d0b25d4d775

Parent:

18:ca084c362855

Child:

21:363271dcfe1f

--- a/main.cpp	Mon Oct 22 14:19:25 2018 +0000
+++ b/main.cpp	Mon Oct 22 14:29:37 2018 +0000
@@ -1,3 +1,4 @@
+// Inclusion of libraries
 #include "mbed.h"
 #include "FastPWM.h"    
 #include "QEI.h"        // Includes library for encoder
@@ -6,37 +7,39 @@
 #include "BiQuad.h"
 
 // Input
-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
 
 // 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
-//float u1  = pot1;
+
 
-// Implementing libraries
-MODSERIAL pc(USBTX, USBRX);
-Ticker motor;
+// Utilisation of libraries
+MODSERIAL   pc(USBTX, USBRX);
+QEI         Encoder1(D11,D10,NC,4200);      // Counterclockwise motor rotation is the positive direction
+QEI         Encoder2(D9,D8,NC,4200);        // Counterclockwise motor rotation is the positive direction
+QEI         Encoder3(D13,D12,NC,4200);      // Counterclockwise motor rotation is the positive direction
+Ticker      motor;
+
+// Global variables
+const float    pi = 3.14159265358979;
 float u3 = 0.0;         // Normalised variable for the movement of motor 3
 
 // Functions
+void Encoderinput()
+{   int counts1;
+    int counts2;
+    int counts3;
+    float  angle1;
+    float  angle2;
+    float  angle3;
+    counts1 = Encoder1.getPulses();
+    counts2 = Encoder2.getPulses();
+    counts3 = Encoder3.getPulses();
+    angle1 = ((float)counts1*2.0*pi)/4200.0;
+    angle2 = ((float)counts2*2.0*pi)/4200.0;
+    angle3 = ((float)counts3*2.0*pi)/4200.0;
+         
+    pc.printf("Counts1: %i  Angle1: %f      Counts2: %i  Angle2: %f\r\n",counts1,angle1,counts2,angle2);
+}
+
 void draaibuttons()         
 {   /*  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
@@ -57,8 +60,6 @@
             }
         }
     
-}
-
 void draai()    
 /*  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
@@ -95,24 +96,13 @@
     pin3 = fabs(u3);   
 }
 
-// Main code
+// Main program
 int main()
-{
+{   
     pc.baud(115200);
     
-    pin5.period(1.0/10000);
-    button1.rise(&draaibuttons);       
-    button2.rise(&draaibuttons);
-    
-    pin3.period_us(50);
-    motor.attach(draai, 0.001);
-   
-    pin5.period_us(50);
-    motor.attach(draai, 0.001);
-    
-    pin6.period_us(50);
-    motor.attach(draai, 0.001);
-    while(true)
-    {
+            
+    while   (true)
+    {   Encoderinput();
     }
 }
\ No newline at end of file