Diff: main.cpp

Revision:

12:1ecd11dc2c00

Parent:

11:4e3ef6150a2e

Child:

13:0b51846cf9e3

--- a/main.cpp	Mon Oct 15 14:36:33 2018 +0000
+++ b/main.cpp	Mon Oct 15 19:08:42 2018 +0000
@@ -2,113 +2,118 @@
 #include "FastPWM.h"    // FastPWM library
 #include "MODSERIAL.h"
 #include "QEI.h"
+#include <math.h>
 MODSERIAL pc(USBTX, USBRX);
-DigitalOut motor1_direction(D7);
+DigitalOut Motor1DirectionPin(D7);
+FastPWM Motor1MagnitudePin(D6); //FastPWM input
 AnalogIn potMeter1(A4);
+AnalogIn potMeter2(A5);
 InterruptIn button2(D3);
-FastPWM motor1_pwm(D6); //FastPWM input, PES lecture 2
 QEI Encoder (D12, D13, NC, 64, QEI::X4_ENCODING);
-Ticker MotorSpeedCounts;
-Ticker measurevelocity;
+
+//Tickers
+Ticker MeasureControl;
+Ticker MakeMotorRotate;
 
 //Global variables
-volatile double RotationalPosition = 0.0;
-volatile double measuredVelocity = 0.0; 
-const double tickertime = 0.001;
+volatile double measuredPosition = 0.0;
+volatile double referencePosition = 0.0; 
+volatile double motorValue = 0.0;
+volatile double Kp = 0.0;
+volatile double 
 
 /*void Motor()
 {
     // Aflezen Potentiometers voor PWM
     motor1_pwm = potMeter1.read(); // Aflezen PotMeter 1
-    
-    // Encoder counts printen
-    //pc.printf("%i\r\n", Encoder.getPulses());
-}*/
+} */
 
-double ReferencePosition()
+/*void changeDirectionButton()
 {
-   double y_ref = 314.0*potMeter1.read(); // Reference value y, scaled from 0 to 100 pi
-   pc.printf('Value potmeter is %f\r\n', y_ref);
-   return y_ref;
+    Motor1DirectionPin = 1 - Motor1DirectionPin; //
+    pc.printf("Motor direction value %i \r\n", Motor1DirectionPin);
+} */
+
+double GetReferencePosition()
+{
+   double potMeterIn = potMeter1.read();
+   double referencePosition = 4*3.14*potMeterIn - 2*3.14 ; // Reference value y, scaled to -2 to 2 revolutions (or 0 to 100 pi)
+   return referencePosition;
 }
 
-double EncoderPosition()
+void GetMeasuredPosition()
 {
-    double OldRotationalPosition = RotationalPosition;
     double counts = Encoder.getPulses();
-    RotationalPosition = ( counts / 4.0 * 64.0) * 6.28; // Rotational position in radians   
-    return RotationalPosition;
+    measuredPosition = ( counts / 4.0 * 64.0) * 6.28; // Rotational position in radians   
 } 
 
 void PositionGain()
 {
     double Kp = 10.0*potMeter2.read(); // Scale 0 to 10
 }
-    
-
-void changeDirectionButton()
-{
-    motor1_direction = 1 - motor1_direction; //
-    pc.printf("Motor direction value %i \r\n", motor1_direction);
-}
-
-/* double GetReferenceVelocity()
-{
-    // Returns reference velocity in rad/s. 
-    // Positive value means clockwise rotation.
-    // 60 rpm = 60*2*pi/60 = 6.28 ~ 6.2 rad/s
-    const double maxVelocity=6.2; // in rad/s of course!    
-    double referenceVelocity;  // in rad/s
-    switch (motor1_direction)
-    {
-        case 0: // check if motor1_direction is 0  
-        // Clockwise rotation      
-        referenceVelocity = potMeter1 * maxVelocity; 
-        pc.printf("Reference velocity is %f. \r\n", referenceVelocity);
-        break;
-        case 1: // check if motor1_direction is 1
-        // Counterclockwise rotation       
-        referenceVelocity = -1*potMeter1 * maxVelocity;
-        pc.printf("Reference velocity is %f. \r\n", referenceVelocity);
-        break; 
-        default:  
-        pc.printf("Something has gone wrong. \r \n");
-        }
-    return referenceVelocity;
-} */
-
-/*void GetMeasuredVelocity()
-{
-    // Get actual velocity from the motor plant
-    // Use encoder (counts)
-    double OldRotationalPosition = RotationalPosition;
-    double counts = Encoder.getPulses();
-    RotationalPosition = ( counts / 4 * 64) * 6.28; // Rotational position in radians   
-    // hier komt de berekening van measured velocity
-    double measuredVelocity = (RotationalPosition - OldRotationalPosition) / tickertime;
-    //measuredVelocity defined as global variable, doesn't have to be returned anymore, is stored in memory all the time;
-}*/
 
 double Error()
 {
-    double Error = ReferencePosition()-EncoderPosition();
-    return Error;
+    double error = referencePosition - measuredPosition;
+    return error;
 }
 
+double FeedbackControl(double Error)
+{  // Proportional part:  
+    double u_k = Kp * Error;
+    // Sum all parts and return it 
+    return u_k;   
+ }
+ 
+void SetMotor1(double motorValue)
+{
+    // Given -1<=motorValue<=1, this sets the PWM and direction
+    // bits for motor 1. Positive value makes motor rotating
+    // clockwise. motorValues outside range are truncated to
+    // within range
+    if (motorValue >=0) 
+    {
+        motor1DirectionPin=1;
+    }
+    else
+    {
+        motor1DirectionPin=0;
+    }
+    if (fabs(motorValue)>1) 
+    {
+        motor1MagnitudePin = 1;
+    }
+    else 
+    {
+        motor1MagnitudePin = fabs(motorValue);
+    }
+}
+//-----------------------------------------------------------------------------
+// Ticker
+void MeasureAndControl(void)
+{
+    // This function determines the desired velocity, measures the   
+    // actual velocity, and controls the motor with 
+    // a simple Feedback controller. Call this from a Ticker.
+    float referencePosition = GetReferencePosition();
+    float measuredPosition = GetMeasuredPosition();
+    float error = Error();
+    float motorValue = FeedbackControl(error);
+    SetMotor1(motorValue);
+
+//-----------------------------------------------------------------------------
 int main()
 {
+    //Initialize once
     pc.baud(115200);
-    bool motor1_direction = 0;
-    motor1_pwm.period_us(60.0); // 60 microseconds PWM period, 16.7 kHz, defines all PWM pins (only needs to done once), FastPWM variabele
-    //MotorSpeedCounts.attach(ReferencePosition, 0.5); // Ticker die de functie Motor aanroept elke halve seconde, meer tijd is tragere respons op potmeter
-    button2.rise(changeDirectionButton);
-    //measurevelocity.attach(GetMeasuredVelocity, tickertime);
+    motor1MagnitudePin.period_us(60.0); // 60 microseconds PWM period, 16.7 kHz, defines all PWM pins (only needs to done once), FastPWM variabele
+    MeasureControl.attach(MeasureAndControl, 0.01);
     
-    //double referenceSnelheid = GetReferenceVelocity();
-    //pc.printf("Reference velocity is %f \r\n Measured velocity is %f. \r\n", referenceSnelheid, measuredVelocity);
-      
-    while(Error ==! 0)          // when reference postion is reached, stop with the while loop
+    //Other initializations
+    button2.rise(changeDirectionButton);
+          
+    while(Error != 0)          // when reference postion is reached, stop with the while loop
     {
-        MotorSpeedCounts.attach(PositionGain,0.5);
+        MakeMotorRotate.attach(SetMotor,0.5);
     }
 }