Diff: main.cpp

Revision:

13:0b51846cf9e3

Parent:

12:1ecd11dc2c00

Child:

14:29236a33b5e4

--- a/main.cpp	Mon Oct 15 19:08:42 2018 +0000
+++ b/main.cpp	Tue Oct 16 09:06:44 2018 +0000
@@ -1,11 +1,11 @@
 #include "mbed.h"
-#include "FastPWM.h"    // FastPWM library
+#include "FastPWM.h"    
 #include "MODSERIAL.h"
 #include "QEI.h"
 #include <math.h>
 MODSERIAL pc(USBTX, USBRX);
-DigitalOut Motor1DirectionPin(D7);
-FastPWM Motor1MagnitudePin(D6); //FastPWM input
+DigitalOut motor1DirectionPin(D7);
+FastPWM motor1MagnitudePin(D6); 
 AnalogIn potMeter1(A4);
 AnalogIn potMeter2(A5);
 InterruptIn button2(D3);
@@ -13,56 +13,38 @@
 
 //Tickers
 Ticker MeasureControl;
-Ticker MakeMotorRotate;
+/*Ticker MakeMotorRotate;*/
 
 //Global variables
 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
-} */
-
-/*void changeDirectionButton()
-{
-    Motor1DirectionPin = 1 - Motor1DirectionPin; //
-    pc.printf("Motor direction value %i \r\n", Motor1DirectionPin);
-} */
+//------------------------------------------------------------------------------
+// Functions
 
 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)
+   referencePosition = 4.0*3.14*potMeterIn - 2.0*3.14 ; // Reference value y, scaled to -2 to 2 revolutions (or 0 to 100 pi)
    return referencePosition;
 }
 
-void GetMeasuredPosition()
+double GetMeasuredPosition()
 {
     double counts = Encoder.getPulses();
-    measuredPosition = ( counts / 4.0 * 64.0) * 6.28; // Rotational position in radians   
+    measuredPosition = ( counts / 4.0 * 64.0) * 6.28; // Rotational position in radians 
+    return measuredPosition;  
 } 
 
-void PositionGain()
-{
-    double Kp = 10.0*potMeter2.read(); // Scale 0 to 10
-}
-
-double Error()
-{
-    double error = referencePosition - measuredPosition;
-    return error;
-}
-
 double FeedbackControl(double Error)
-{  // Proportional part:  
+{  
+    double Kp = 20.0*potMeter2.read(); // Scale 0 to 20
+    // Proportional part:  
     double u_k = Kp * Error;
     // Sum all parts and return it 
-    return u_k;   
+    return u_k; //motorValue
  }
  
 void SetMotor1(double motorValue)
@@ -95,25 +77,24 @@
     // 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);
+    double referencePosition = GetReferencePosition();
+    double measuredPosition = GetMeasuredPosition();
+    double motorValue = FeedbackControl(referencePosition - measuredPosition);
     SetMotor1(motorValue);
+}
 
 //-----------------------------------------------------------------------------
 int main()
 {
     //Initialize once
     pc.baud(115200);
-    motor1MagnitudePin.period_us(60.0); // 60 microseconds PWM period, 16.7 kHz, defines all PWM pins (only needs to done once), FastPWM variabele
+    motor1MagnitudePin.period_us(60.0); // 60 microseconds PWM period: 16.7 kHz.
     MeasureControl.attach(MeasureAndControl, 0.01);
+    /*MakeMotorRotate.attach(SetMotor1, 0.01);*/
     
     //Other initializations
-    button2.rise(changeDirectionButton);
           
-    while(Error != 0)          // when reference postion is reached, stop with the while loop
+    while(true)
     {
-        MakeMotorRotate.attach(SetMotor,0.5);
     }
 }