Diff: main.cpp

Revision:

34:30100c1901d4

Parent:

32:a5b411833d1e

diff -r a5b411833d1e -r 30100c1901d4 main.cpp
--- a/main.cpp	Tue Oct 30 18:28:26 2018 +0000
+++ b/main.cpp	Wed Oct 31 09:56:35 2018 +0000
@@ -9,8 +9,9 @@
 // Input
 AnalogIn    pot1(A1);
 AnalogIn    pot2(A2);
-InterruptIn button1(D0);
-InterruptIn button2(D1);
+InterruptIn buttonbio1(D0);
+InterruptIn buttonbio2(D1);
+InterruptIn buttonK64F(SW3);
 InterruptIn emergencybutton(SW2);   //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
@@ -21,12 +22,12 @@
 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
+DigitalOut  pin2(D2);       // Motor 3 direction = motor flip
+FastPWM     pin3(D3);       // Motor 3 pwm = motor flip
+DigitalOut  pin4(D4);       // Motor 2 direction = motor right
+FastPWM     pin5(D5);       // Motor 2 pwm = motor right
+FastPWM     pin6(D6);       // Motor 1 pwm = motor left
+DigitalOut  pin7(D7);       // Motor 1 direction = motor left
 
 DigitalOut  greenled(LED_GREEN,1);
 DigitalOut  redled(LED_RED,1);
@@ -34,9 +35,9 @@
 
 // Utilisation of libraries
 MODSERIAL   pc(USBTX, USBRX);
-QEI         Encoder1(D9,D8,NC,4200);        // Counterclockwise motor rotation is the positive direction
-QEI         Encoder2(D11,D10,NC,4200);      // Counterclockwise motor rotation is the positive direction
-QEI         Encoder3(D13,D12,NC,4200);      // Counterclockwise motor rotation is the positive direction
+QEI         Encoderl(D9,D8,NC,4200);        // Counterclockwise motor rotation is the positive direction
+QEI         Encoderr(D11,D10,NC,4200);      // Counterclockwise motor rotation is the positive direction
+QEI         Encoderf(D13,D12,NC,4200);      // Counterclockwise motor rotation is the positive direction
 Ticker      motor;
 Ticker      encoders;
 
@@ -58,20 +59,20 @@
 }
 
     // Subfunctions
-    int Counts1input()      // Gets the counts from encoder 1
-    {   int     counts1;
-        counts1 = Encoder1.getPulses();
-        return  counts1;
+    int Countslinput()      // Gets the counts from encoder 1
+    {   int     countsl;
+        countsl = Encoderl.getPulses();
+        return  countsl;
     }
-    int Counts2input()      // Gets the counts from encoder 2
-    {   int     counts2;
-        counts2 = Encoder2.getPulses();
-        return  counts2;
+    int Countsrinput()      // Gets the counts from encoder 2
+    {   int     countsr;
+        countsr = Encoderr.getPulses();
+        return  countsr;
     }
-    int Counts3input()      // Gets the counts from encoder3
-    {   int     counts3;
-        counts3 = Encoder3.getPulses();
-        return  counts3;
+    int Countsfinput()      // Gets the counts from encoder3
+    {   int     countsf;
+        countsf = Encoderf.getPulses();
+        return  countsf;
     }
 
     float   CurrentAngle(float counts)      // Calculates the current angle of the motor (between -2*pi to 2*pi) based on the counts from the encoder
@@ -121,7 +122,7 @@
         float   u_d = Kd * filtered_error_derivative;
         error_prev = error;
         // Sum all parts and return it
-        pc.printf ("Kp = %f     Kd = %f",Kp,Kd);
+        //pc.printf ("Kp = %f     Kd = %f",Kp,Kd);
         return  u_k + u_i + u_d;
     }
 
@@ -130,20 +131,20 @@
             return  angle;
         }
 
-void    turn1()    // main function for movement of motor 1, all above functions with an extra tab are called
+void    turnl()    // main function for movement of motor 1, all above functions with an extra tab are called
 {   
     //float   refvalue1 = pi/4.0;
-    float   refvalue1 = DesiredAngle();
-    int     counts1 = Counts1input();
-    float   currentangle1 = CurrentAngle(counts1);
-    float   PIDcontrol1 = PIDcontroller(refvalue1,currentangle1);
-    float   error1 = ErrorCalc(refvalue1,currentangle1);
+    float   refvalue = DesiredAngle();
+    int     counts = Counts1input();
+    float   currentangle = CurrentAngle(counts);
+    float   PIDcontrol = PIDcontroller(refvalue,currentangle);
+    float   error = ErrorCalc(refvalue,currentangle);
     
-    pin6 = fabs(PIDcontrol1);
-    pin7 = PIDcontrol1 > 0.0f;
+    pin6 = fabs(PIDcontrol);
+    pin7 = PIDcontrol > 0.0f;
     //pin6 = 0.4+0.6*fabs(PIDcontr);     //geschaald
     //pin6 = fabs(PIDcontr)/pi;
-    pc.printf("     error:  %f      ref: %f     angle:  %f \r\n",error1,refvalue1,currentangle1);
+    //pc.printf("     error:  %f      ref: %f     angle:  %f \r\n",error,refvalue,currentangle);
 }
 
 float ActualPosition(int counts, int offsetcounts)      // After calibration, this function is used to return the counts (and thus the angle of the system) to 0
@@ -158,14 +159,14 @@
     pc.baud(115200);  
     pin3.period_us(15);     // If you give a period on one pin, c++ gives all pins this period
         
-    //motor.attach(turn1,dt);
+    motor.attach(turnl,dt);
 
     emergencybutton.rise(Emergency);              //If the button is pressed, stop program
     //pin6 = 0.01; 
             
     while   (true)
     {   
-        turn1();
-        wait(dt);    
+        //turnl();
+        //wait(dt);    
     }
 }