Diff: main.cpp

Revision:

1:76e57b695115

Parent:

0:ce44e2a8e87a

Child:

2:0a61483f4515

--- a/main.cpp	Mon Oct 15 13:48:56 2018 +0000
+++ b/main.cpp	Mon Oct 15 14:57:07 2018 +0000
@@ -5,7 +5,6 @@
 
 //Tickers
 Ticker TickerMeasureAndControl;
-Ticker TickerSetMotor;
 Ticker TickerPrintToScreen;
 //Communication
 MODSERIAL pc(USBTX,USBRX);
@@ -22,8 +21,10 @@
 const double Ts = 0.01; //Sample time of Ticker measure and control (100 Hz)
 volatile bool PrintFlag = false;
 
+//Global variables for printing on screen
 volatile float PosRefPrint; // for printing value on screen
 volatile float PosMotorPrint; // for printing value on screen
+volatile float ErrorPrint;
 //-----------------------------------------------------------------------------
 //The double-functions
 
@@ -68,29 +69,54 @@
 
 
 ///The controller
-//double P_Controller(double error)
-//{
+double P_Controller(double Error)
+{
+   double Kp = 35*Potmeter2.read(); // 35 is just a try
+   
+   double u_k = Kp * Error;
+   
+   return u_k; //This will become the MotorValue
+}
+
+//Ticker function set motorvalues
+void SetMotor(double MotorValue)
+{
+    if (MotorValue >=0)
+    {
+        DirectionPin = 1;
+    }
+    else
+    {
+        DirectionPin = 0;
+    }
     
-//}
+    if (fabs(MotorValue)>1)
+    {
+        PwmPin = 1; // if error more than 1 radian, full duty cycle
+    }
+    else
+    {
+        PwmPin = fabs(MotorValue);
+    }
+}
 
 // ----------------------------------------------------------------------------
-//Ticker functions
-
-//Ticker function Data 
+//Ticker function
 void MeasureAndControl(void)
 {
     double PositionRef = GetReferencePosition();
     double PositionMotor = GetActualPosition();
+    double MotorValue = P_Controller(PositionRef - PositionMotor); // input is error
+    SetMotor(MotorValue);
     
+    //for printing on screen
     PosRefPrint = PositionRef;
     PosMotorPrint = PositionMotor;
+    ErrorPrint = MotorValue;
+    
 }
 
-//Ticker function set motorvalues
-void SetMotor()
-{
 
-} 
 
 void PrintToScreen()
 {
@@ -103,15 +129,16 @@
 {
     pc.baud(115200);
     pc.printf("Hello World\n\r");
+    PwmPin.period_us(60); // 16.66667 kHz (default period is too slow!)
     TickerMeasureAndControl.attach(&MeasureAndControl,0.01); //100 Hz
-    TickerSetMotor.attach(&SetMotor,0.0025); //Set value for motor with 400 Hz
-    TickerPrintToScreen.attach(&PrintToScreen,0.5); //Every second twice the values on screen
+    TickerPrintToScreen.attach(&PrintToScreen,0.25); //Every second four times the values on screen
     
     while (true) 
     {
-        if(PrintFlag)
+        if(PrintFlag) // if-statement for printing every second four times to screen
         {
-            pc.printf("Pos ref = %f and Pos motor = %f\r",PosRefPrint,PosMotorPrint);
+            double KpPrint = 35*Potmeter2.read();
+            pc.printf("Pos ref = %f, Pos motor = %f, Error = %f and Kp = %f\r",PosRefPrint,PosMotorPrint,ErrorPrint,KpPrint);
             PrintFlag = false;
         }
     }