Diff: main.cpp

Revision:

0:85965fd37d39

Child:

1:3f88aa5bf2ff

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Oct 03 19:38:35 2017 +0000
@@ -0,0 +1,74 @@
+#include "mbed.h"
+
+Ticker Treecko;                         //We maken een geweldige ticker aan zodat de potmeter en alles telkens opnieuw worden uitgelezen 
+AnalogIn potMeter2In(A1);               //Analoge input van potmeter 2 (zal gebruikt worden voor de snelheids bepaling van de motor)
+AnalogIn button2(A2);                   //Analoge input van button 2 (zal gebruikt worden voor het inschakelen van de motor)
+AnalogIn button1(A4);                   //Analago input van button 1 (zal gebruikt worden voor het uitschakelen van de motor)
+AnalogIn potMeter1In(A3);               //Analoge input van potmeter 1 (zal gebruikt worden voor de bepaling van de draairichting van de motor)
+PwmOut motor1MagnitudePin(D6);          //Biorobotics Motor 1 PWM controle van de snelheid 
+DigitalOut motor1DirectionPin(D7);      //Biorobotics Motor 1 draairichting controle 
+
+float PwmPeriod = 1.0/5000.0;           //PWM periode instellen (5000 Hz, want 5000 periodes in 1 seconde)
+
+float SetMotor1(float motorValue)
+{
+    //Hiermee wordt de richting bepaald met behulp van potmeter 1 en wordt de snelheid bepaald aan de hand van de motorValue.
+    if (potMeter1In >= 0.5)
+    {   //Clockwise rotation
+        motor1DirectionPin = 0;         //de motor draait met de klok mee
+    }
+    else 
+    {   //Counterclockwise rotation    
+        motor1DirectionPin = 1;         //de motor draait tegen de klok in
+    }
+    if  (fabs(motorValue) > 1)    
+    {
+        motor1MagnitudePin = 1;         //de snelheid wordt teruggeschaald naar 8.4 rad/s (maximale snelheid, dus waarde 1)
+    }
+    else
+    {    
+        motor1MagnitudePin = fabs(motorValue);      //de absolute snelheid wordt bepaald
+    }
+}
+
+float GetReferenceVelocity()
+{
+    //Returns reference velocity in rad/s
+    //Positieve waarden betekend met de klok mee draaien
+    const float maxVelocity=8.4;        //in rad/s    
+    float referenceVelocity = potMeter2In * maxVelocity;
+    return referenceVelocity;
+}
+        
+float FeedForwardControl(float referenceVelocity)   //Koppelt de referenceVelocity aan de motorValue
+{
+    //eenvoudige lineaire feed-forward control
+    const float MotorGain = 8.4;        //eenheid: (rad/s) / PWM
+    float motorValue = referenceVelocity / MotorGain;   //bepalen van de fractie van de snelheid t.o.v. max snelheid
+    return motorValue;
+}
+
+void MeasureAndControl(void)
+{
+    //Deze functie neemt de potmeter positie waar, leidt hier een referentie snelheid vanaf en controleert de motor 
+    //met een eenvoudige feed-forward controller. Deze functie wordt aangeroepen met behulp van de ticker Treecko
+    float referenceVelocity = GetReferenceVelocity();
+    float motorValue = FeedForwardControl(referenceVelocity);
+    SetMotor1(motorValue);
+}
+
+int main ()
+{
+    if (button2)
+    {
+        Treecko.attach(MeasureAndControl, 1);   //Elke 1 seconde zorgt de ticker voor het runnen en uitlezen van de verschillende 
+                                                //functies en analoge signalen. Veranderingen worden elke 1 seconde doorgevoerd.
+    }
+    else if (button1)
+    {
+        Treecko.dettach;                        //De ticker wordt uitgeschakeld, de motor stopt met draaien.
+    }
+    else
+    {
+    }
+ }
\ No newline at end of file