Diff: main.cpp

Revision:

35:ba556f2d0fcc

Parent:

33:ec07e11676ec

Child:

36:077fe5b3189b

Child:

37:c61d7768c18a

--- a/main.cpp	Wed Oct 31 09:26:55 2018 +0000
+++ b/main.cpp	Wed Oct 31 10:22:04 2018 +0000
@@ -21,12 +21,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 +34,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 +58,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 encoder 3
+    {   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
@@ -125,29 +125,54 @@
         return  u_k + u_i + u_d;
     }
 
-        float DesiredAngle()        // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
+        float DesiredAnglel()        // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
         {   float   angle = (pot1*2.0f*pi)-pi;
             return  angle;
         }
-
-
-
-float*    turn1()    // main function for movement of motor 1, all above functions with an extra tab are called
+        
+        float DesiredAngler()        // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
+        {   float   angle = (pot2*2.0f*pi)-pi;
+            return  angle;
+        }        
+        
+float*    turnl()    // main function for movement of motor 1, all above functions with an extra tab are called
 {   
-    //float   refvalue1 = pi/4.0f;
-    float   refvalue1 = DesiredAngle();                             // different minus sign per motor
-    int     counts1 = Counts1input();                               // different encoder pins per motor
-    float   currentangle1 = CurrentAngle(counts1);                  // different minus sign per motor
-    float   PIDcontrol1 = PIDcontroller(refvalue1,currentangle1);   // same for every motor
-    float   error1 = ErrorCalc(refvalue1,currentangle1);            // same for every motor
+    //float   refvalue = pi/4.0f;
+    float   refvalue = DesiredAnglel();                             // different minus sign per motor
+    int     counts = Countslinput();                               // different encoder pins per motor
+    float   currentangle = CurrentAngle(counts);                  // different minus sign per motor
+    float   PIDcontrol = PIDcontroller(refvalue,currentangle);   // same for every motor
+    float   error = ErrorCalc(refvalue,currentangle);            // same for every motor
     
-    pin6 = fabs(PIDcontrol1);                                       // different pins for every motor
-    pin7 = PIDcontrol1 > 0.0f;                                      // different pins for every motor
+    pin6 = fabs(PIDcontrol);                                       // different pins for every motor
+    pin7 = PIDcontrol > 0.0f;                                      // different pins for every motor
     //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*  outcome;
-    float   turninfo[3] = {error1, refvalue1, currentangle1};
+    float   turninfo[3] = {error, refvalue, currentangle};
+    //float ( &fillarr( float (&outcome)[3] ) )[3] { // no decay; argument must be size 3
+    outcome = turninfo;
+    
+    return outcome;
+}
+
+float*    turnr()    // main function for movement of motor 1, all above functions with an extra tab are called
+{   
+    //float   refvalue = pi/4.0f;
+    float   refvalue = DesiredAngler();                             // different minus sign per motor
+    int     counts = Countsrinput();                               // different encoder pins per motor
+    float   currentangle = CurrentAngle(counts);                  // different minus sign per motor
+    float   PIDcontrol = PIDcontroller(refvalue,currentangle);   // same for every motor
+    float   error = ErrorCalc(refvalue,currentangle);            // same for every motor
+    
+    pin6 = fabs(PIDcontrol);                                       // different pins for every motor
+    pin7 = PIDcontrol > 0.0f;                                      // different pins for every motor
+    //pin6 = 0.4+0.6*fabs(PIDcontr);     //geschaald
+    //pin6 = fabs(PIDcontr)/pi;
+    //pc.printf("     error:  %f      ref: %f     angle:  %f \r\n",error,refvalue,currentangle);
+    float*  outcome;
+    float   turninfo[3] = {error, refvalue, currentangle};
     //float ( &fillarr( float (&outcome)[3] ) )[3] { // no decay; argument must be size 3
     outcome = turninfo;
     
@@ -173,12 +198,18 @@
             
     while   (true)
     {   
-        float* motoroutcome1 = turn1();
-        float motor11 = motoroutcome1[0];
-        float motor12 = motoroutcome1[1];
-        float motor13 = motoroutcome1[2];
+        float* motoroutcomel = turnl();
+        float motorl1 = motoroutcomel[0];
+        float motorl2 = motoroutcomel[1];
+        float motorl3 = motoroutcomel[2];
         
-        pc.printf("     error1:  %f      ref1: %f     angle1:  %f \r\n",motor11,motor12,motor13);
+        //float* motoroutcomer = turnr();
+        //float motorr1 = motoroutcomer[0];
+        //float motorr2 = motoroutcomer[1];
+        //float motorr3 = motoroutcomer[2];
+        
+        pc.printf("     error1:  %f      ref1: %f     angle1:  %f \r\n",motorl1,motorl2,motorl3);
+        //pc.printf("     error1:  %f      ref1: %f     angle1:  %f \r\n",motorr1,motorr2,motorr3);
         wait(dt);
         //wait(dt*10);
         //wait(printingfreq);  --> still needs to be defined