Diff: main.cpp

Revision:

0:ce44e2a8e87a

Child:

1:76e57b695115

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Oct 15 13:48:56 2018 +0000
@@ -0,0 +1,118 @@
+#include "mbed.h"
+#include "math.h"
+#include "MODSERIAL.h"
+#include "QEI.h"
+
+//Tickers
+Ticker TickerMeasureAndControl;
+Ticker TickerSetMotor;
+Ticker TickerPrintToScreen;
+//Communication
+MODSERIAL pc(USBTX,USBRX);
+QEI Encoder(D10,D11,NC,32);
+
+//Global pin variables 
+PwmOut PwmPin(D5);
+DigitalOut DirectionPin(D4);
+AnalogIn Potmeter1(A1);
+AnalogIn Potmeter2(A0);
+DigitalIn button1(D8);
+
+//Global variables
+const double Ts = 0.01; //Sample time of Ticker measure and control (100 Hz)
+volatile bool PrintFlag = false;
+
+volatile float PosRefPrint; // for printing value on screen
+volatile float PosMotorPrint; // for printing value on screen
+//-----------------------------------------------------------------------------
+//The double-functions
+
+//Get reference position
+double GetReferencePosition()
+{
+// This function set the reference position to determine the position of the signal.
+// Reference velocity is set as proportional to duty cycle.
+// Positive velocity (if button is pressed) means clockwise(CW) rotation.
+    
+
+    const double MaxVelocity = 6.28; //60 RPM max velocity in rad/s
+    double VelocityRef; // Reference Velocity
+    double ValuePot = Potmeter1.read(); // Read value from potmeter (range from 0-1)
+    static double PositionRef = 0; // Initial position value in rad
+    
+        if (button1)
+        {
+            VelocityRef = ValuePot*MaxVelocity; //CW
+        }
+        else 
+        {
+            VelocityRef = -1*ValuePot*MaxVelocity; //CCW
+        }
+        
+        PositionRef = PositionRef + VelocityRef*Ts;
+        
+        return PositionRef; //rad
+}
+
+// actual position of the motor
+double GetActualPosition()
+{
+    //This function determines the actual position of the motor
+    //The count:radians relation is 8400:2pi
+    double EncoderCounts = Encoder.getPulses();    //number of counts
+    double PositionMotor = EncoderCounts/8400*(6.283); // in rad (6.283 = pi)
+    
+    return PositionMotor;
+}
+
+
+
+///The controller
+//double P_Controller(double error)
+//{
+    
+//}
+
+// ----------------------------------------------------------------------------
+//Ticker functions
+
+//Ticker function Data 
+void MeasureAndControl(void)
+{
+    double PositionRef = GetReferencePosition();
+    double PositionMotor = GetActualPosition();
+    
+    PosRefPrint = PositionRef;
+    PosMotorPrint = PositionMotor;
+}
+
+//Ticker function set motorvalues
+void SetMotor()
+{
+
+} 
+
+void PrintToScreen()
+{
+    PrintFlag = true;
+}
+
+
+//-----------------------------------------------------------------------------
+int main()
+{
+    pc.baud(115200);
+    pc.printf("Hello World\n\r");
+    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
+    
+    while (true) 
+    {
+        if(PrintFlag)
+        {
+            pc.printf("Pos ref = %f and Pos motor = %f\r",PosRefPrint,PosMotorPrint);
+            PrintFlag = false;
+        }
+    }
+}
\ No newline at end of file