File content as of revision 2:b541085b55e9:

#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.detach();                         //De ticker wordt uitgeschakeld, de motor stopt met draaien.
    }
    else
    {
    }
 }