Diff: main.cpp

Revision:

8:ceb9abb5a4a8

Parent:

7:3b503177ff5c

Child:

9:b002572e37fd

Child:

10:076eb8beea30

diff -r 3b503177ff5c -r ceb9abb5a4a8 main.cpp
--- a/main.cpp	Mon Oct 15 10:37:27 2018 +0000
+++ b/main.cpp	Mon Oct 15 12:13:42 2018 +0000
@@ -4,32 +4,68 @@
 #include "QEI.h"
 MODSERIAL pc(USBTX, USBRX);
 DigitalOut motor1_direction(D7);
-AnalogIn pot1(A4);
-InterruptIn but2(D3);
+AnalogIn potMeter1(A4);
+InterruptIn button2(D3);
 FastPWM motor1_pwm(D6); //FastPWM input, PES lecture 2
-Ticker MotorInterrupt;
+Ticker MotorSpeedCounts;
 QEI Encoder (D12, D13, NC, 64, QEI::X4_ENCODING);
 
 void Motor()
 {
     // Aflezen Potentiometers voor PWM
-    motor1_pwm = pot1.read(); // Aflezen PotMeter 1 (volatile omdat je altijd de potmeter wil blijven lezen)
+    float potMeterIn = potMeter1.read(); // Aflezen PotMeter 1
+    motor1_pwm = potMeterIn;
+    // Encoder counts printen
     pc.printf("%i\r\n", Encoder.getPulses());
 }
    
-void buttonpress()
+void changeDirectionButton()
 {
     motor1_direction = 1 - motor1_direction;
-    float motor1_velocity = pot1.read() *2.0;
-   
-    pc.printf("Velocity is %f \r\n", motor1_velocity);
+    
+    //float motor1_velocity = pot1.read() *6.2;
+    //pc.printf("Velocity is %f \r\n", motor1_velocity);
 }
 
+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 float maxVelocity=6.2; // in rad/s of course!    
+    double referenceVelocity;  // in rad/s
+    if (button2)
+        {
+        // Clockwise rotation      
+        referenceVelocity = potMeterIn * maxVelocity;  
+        } 
+        else
+        {
+        // Counterclockwise rotation       
+        referenceVelocity = -1*potMeterIn * maxVelocity;   
+        }
+    return referenceVelocity;
+}
+
+double GetMeasuredVelocity()
+{
+    // Get actual velocity from the motor plant
+    // Use encoder (counts)
+    
+    int counts = Encoder.getPulses();
+    double RotationalPosition = ( counts / 4 * 64) * 6.28; // Rotational position in radians   
+    
+    double measuredVelocity;
+    // hier komt de berekening van measured velocity
+    return measuredVelocity;
+}
+
+
 int main()
 {
     pc.baud(115200);
     motor1_pwm.period_us(60.0); // 60 microseconds PWM period, 16.7 kHz, defines all PWM pins (only needs to done once), FastPWM variabele
-    MotorInterrupt.attach(Motor, 0.5); // Ticker die de functie Motor aanroept elke halve seconde, meer tijd is tragere respons op potmeter
-    but2.rise(buttonpress);
+    MotorSpeedCounts.attach(Motor, 0.5); // Ticker die de functie Motor aanroept elke halve seconde, meer tijd is tragere respons op potmeter
+    button2.rise(changeDirectionButton);
     while(true){} // Endless loop  
 }
\ No newline at end of file