Diff: main.cpp

Revision:

6:10ac8c7cac26

Parent:

4:0251290a2fc0

--- a/main.cpp	Fri Oct 19 12:02:58 2018 +0000
+++ b/main.cpp	Fri Oct 19 14:09:23 2018 +0000
@@ -10,11 +10,17 @@
 //Communication
 MODSERIAL pc(USBTX,USBRX);
 QEI Encoder(D10,D11,NC,32);
+QEI Encoder2(D12,D13,NC,32);
 
-//Global pin variables 
+//Global pin variables Motor 1
 PwmOut PwmPin(D5);
 DigitalOut DirectionPin(D4);
 AnalogIn Potmeter1(A1);
+
+
+//Global pin variables Motor 2
+PwmOut PwmPin2(D6);
+DigitalOut DirectionPin2(D7);
 AnalogIn Potmeter2(A0);
 
 //Global variables
@@ -24,11 +30,14 @@
 volatile float PosRefPrint; // for printing value on screen
 volatile float PosMotorPrint; // for printing value on screen
 volatile float ErrorPrint;
+volatile float PosRefPrint2; // for printing value on screen
+volatile float PosMotorPrint2; // for printing value on screen
+volatile float ErrorPrint2;
 
 //-----------------------------------------------------------------------------
 //The double-functions
 
-//Get reference position
+//Get reference position 1
 double GetReferencePosition()
 {
 // This function set the reference position to determine the position of the signal.
@@ -39,12 +48,28 @@
     double ValuePot = Potmeter1.read(); // Read value from potmeter (range from 0-1)
     
         
-        double PositionRef = 3*6.2830*ValuePot - 3*3.1415; // position reference ranging from -1.5 rotations till 1.5 rotations
+        double PositionRef = 6.2830*ValuePot - 3.1415; // position reference ranging from -1.5 rotations till 1.5 rotations
         
         return PositionRef; //rad
 }
 
-// actual position of the motor
+//Get reference position 2
+double GetReferencePosition2()
+{
+// This function set the reference position to determine the position of the signal.
+// a positive position is defined as a counterclockwise (CCW) rotation
+    
+
+    
+    double ValuePot2 = Potmeter2.read(); // Read value from potmeter (range from 0-1)
+    
+        
+        double PositionRef2 = 6.2830*ValuePot2 - 3.1415; // position reference ranging from -1.5 rotations till 1.5 rotations
+        
+        return PositionRef2; //rad
+}
+
+// actual position of the motor 1
 double GetActualPosition()
 {
     //This function determines the actual position of the motor
@@ -55,15 +80,25 @@
     return PositionMotor;
 }
 
+// actual position of the motor 2
+double GetActualPosition2()
+{
+    //This function determines the actual position of the motor
+    //The count:radians relation is 8400:2pi
+    double EncoderCounts2 = Encoder2.getPulses();    //number of counts
+    double PositionMotor2 = EncoderCounts2/8400*(2*6.283); // in rad (6.283 = 2pi), 2* is for compensating X4 --> X2 encoding
+    
+    return PositionMotor2;
+}
 
 
-///The controller
+//The controller motor 1
 double PI_Controller(double Error)
 {
 
    double Ts = 0.01; //Sampling time 100 Hz
    double Kp = 2; // proportional gain
-   double Ki = 2*Potmeter2.read(); //integral gain ranging from 0-2.
+   double Ki = 0.6;
    static double ErrorIntegral = 0;
    
    //Proportional part:
@@ -72,12 +107,32 @@
    //Integral part:
    ErrorIntegral = ErrorIntegral + Error*Ts;
    double u_i = Ki * ErrorIntegral;
-   return u_k + u_i; //This will become the MotorValue
+   return u_k + u_i; //This will become the MotorValue 
+}
+
+//The controller motor 2
+double PI_Controller2(double Error)
+{
+
+   double Ts = 0.01; //Sampling time 100 Hz
+   double Kp = 2; // proportional gain
+   double Ki = 0.6;
+   static double ErrorIntegral = 0;
+   
+   //Proportional part:
+   double u_k = Kp * Error;
+   
+   //Integral part:
+   ErrorIntegral = ErrorIntegral + Error*Ts;
+   double u_i = Ki * ErrorIntegral;
+   return u_k + u_i; //This will become the MotorValue2
 }
 
 //Ticker function set motorvalues
-void SetMotor(double MotorValue)
+void SetMotor(double MotorValue, double MotorValue2)
 {
+    
+    // motor 1
     if (MotorValue >=0)
     {
         DirectionPin = 1;
@@ -95,22 +150,53 @@
     {
         PwmPin = fabs(MotorValue);
     }
+    
+    //Motor 2
+    
+        if (MotorValue2 >=0)
+    {
+        DirectionPin2 = 1;
+    }
+    else
+    {
+        DirectionPin2 = 0;
+    }
+    
+    if (fabs(MotorValue2)>1) // if error more than 1 radian, full duty cycle
+    {
+        PwmPin2 = 1; 
+    }
+    else
+    {
+        PwmPin2 = fabs(MotorValue2);
+    }
+    
 }
 
 // ----------------------------------------------------------------------------
 //Ticker function
 void MeasureAndControl(void)
 {
+    
+    //motor 1
     double PositionRef = GetReferencePosition();
     double PositionMotor = GetActualPosition();
     double MotorValue = PI_Controller(PositionRef - PositionMotor); // input is error
-    SetMotor(MotorValue);
+    
+    //motor 2
+    double PositionRef2 = GetReferencePosition2();
+    double PositionMotor2 = GetActualPosition2();
+    double MotorValue2 = PI_Controller2(PositionRef2 - PositionMotor2); // input is error
+    
+    SetMotor(MotorValue,MotorValue2);
     
     //for printing on screen
     PosRefPrint = PositionRef;
     PosMotorPrint = PositionMotor;
     ErrorPrint = PositionRef - PositionMotor;
-    
+    PosRefPrint2 = PositionRef2;
+    PosMotorPrint2 = PositionMotor2;
+    ErrorPrint2 = PositionRef2 - PositionMotor2;
 }
 
 
@@ -134,9 +220,7 @@
     {
         if(PrintFlag) // if-statement for printing every second four times to screen
         {
-            double KpPrint = 2;
-            double KiPrint = 2*Potmeter2.read();
-            pc.printf("Pos ref = %f, Pos motor = %f, Error = %f, Kp = %f and Ki = %f\r",PosRefPrint,PosMotorPrint,ErrorPrint,KpPrint,KiPrint);
+            pc.printf("Pos ref1 = %f, Pos motor1 = %f, Error1 = %f, Pos ref2 = %f, Pos motor2 = %f, Error2 = %f\r",PosRefPrint,PosMotorPrint,ErrorPrint,PosRefPrint2,PosMotorPrint2,ErrorPrint2);
             PrintFlag = false;
         }
     }