Diff: main.cpp

Revision:

39:dcf3e5019a63

Parent:

37:c61d7768c18a

Child:

40:1be9dfad0a10

--- a/main.cpp	Wed Oct 31 10:41:37 2018 +0000
+++ b/main.cpp	Wed Oct 31 11:26:49 2018 +0000
@@ -35,7 +35,7 @@
 // Utilisation of libraries
 MODSERIAL   pc(USBTX, USBRX);
 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         Encoderr(D10,D11,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;
@@ -102,9 +102,9 @@
                
     float PIDcontroller(float refvalue,float CurAngle)  // PID controller for the motors, based on the reference value and the current angle of the motor
     {   //float   Kp = Kpcontr();
-        float   Kp = 10.37f; 
+        float   Kp = 10.42f; 
         float   Ki = 1.02f;
-        float   Kd = 0.0514f;
+        float   Kd = 0.0493f;
         //float   Kd = Kdcontr();
         float   error = ErrorCalc(refvalue,CurAngle);
         static float    error_integral = 0.0;
@@ -195,17 +195,16 @@
                 
     while   (true)
     {   
-        //float* motoroutcomel = turnl();
-        //float motorl1 = motoroutcomel[0];
-        //float motorl2 = motoroutcomel[1];
-        //float motorl3 = motoroutcomel[2];
+        float* motoroutcomel = turnl();
+        float motorl1 = motoroutcomel[0];
+        float motorl2 = motoroutcomel[1];
+        float motorl3 = motoroutcomel[2];
+        pc.printf("     errorl:  %f      refl: %f     anglel:  %f \r\n",motorl1,motorl2,motorl3);
         
         float* motoroutcomer = turnr();
         float motorr1 = motoroutcomer[0];
         float motorr2 = motoroutcomer[1];
-        float motorr3 = motoroutcomer[2];
-        
-        //pc.printf("     errorl:  %f      refl: %f     anglel:  %f \r\n",motorl1,motorl2,motorl3);
+        float motorr3 = motoroutcomer[2];              
         pc.printf("     errorr:  %f      refr: %f     angler:  %f \r\n",motorr1,motorr2,motorr3);
         wait(dt);
         //wait(dt*10);