Diff: main.cpp

Revision:

11:4e3ef6150a2e

Parent:

9:b002572e37fd

Child:

12:1ecd11dc2c00

--- a/main.cpp	Mon Oct 15 13:53:15 2018 +0000
+++ b/main.cpp	Mon Oct 15 14:36:33 2018 +0000
@@ -16,21 +16,43 @@
 volatile double measuredVelocity = 0.0; 
 const double tickertime = 0.001;
 
-void Motor()
+/*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()
+{
+   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;
 }
-   
+
+double EncoderPosition()
+{
+    double OldRotationalPosition = RotationalPosition;
+    double counts = Encoder.getPulses();
+    RotationalPosition = ( counts / 4.0 * 64.0) * 6.28; // Rotational position in radians   
+    return RotationalPosition;
+} 
+
+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()
+/* double GetReferenceVelocity()
 {
     // Returns reference velocity in rad/s. 
     // Positive value means clockwise rotation.
@@ -53,9 +75,9 @@
         pc.printf("Something has gone wrong. \r \n");
         }
     return referenceVelocity;
-}
+} */
 
-void GetMeasuredVelocity()
+/*void GetMeasuredVelocity()
 {
     // Get actual velocity from the motor plant
     // Use encoder (counts)
@@ -65,20 +87,28 @@
     // 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;
 }
 
-
 int main()
 {
     pc.baud(115200);
-    motor1_direction = 0;
+    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(Motor, 0.5); // Ticker die de functie Motor aanroept elke halve seconde, meer tijd is tragere respons op potmeter
+    //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);
+    //measurevelocity.attach(GetMeasuredVelocity, tickertime);
     
-    double referenceSnelheid = GetReferenceVelocity();
-    pc.printf("Reference velocity is %f \r\n Measured velocity is %f. \r\n", referenceSnelheid, measuredVelocity);
+    //double referenceSnelheid = GetReferenceVelocity();
+    //pc.printf("Reference velocity is %f \r\n Measured velocity is %f. \r\n", referenceSnelheid, measuredVelocity);
       
-    while(true){} // Endless loop  
-}
\ No newline at end of file
+    while(Error ==! 0)          // when reference postion is reached, stop with the while loop
+    {
+        MotorSpeedCounts.attach(PositionGain,0.5);
+    }
+}