File content as of revision 12:e3c5c5acbd09:

#include "mbed.h"
// #include "MODSERIAL.h"
#include "encoder.h"
#include "HIDScope.h"

// Serial pc(USBTX,USBRX);
HIDScope scope(1);          // definieerd het aantal kanalen van de scope

Ticker          mod;        // definieer de ticker die alles bijhoud.

//motor 1 gegevens
PwmOut motor1_aan(D6);      // PWM signaal motor 2 (uit sheets)
DigitalOut motor1_rich(D7); // digitaal signaal voor richting
// einde motor 1

// ENCODER
Encoder motor1_enc(D12,D11);

//POTMETERS
AnalogIn potright(A0);


double setpoint;
const double K = 2 ;


// send specified data to hidscope
void send()
{
    scope.set(0,setpoint);
    scope.send();
}

// counts 2 radians
// this function takes the counts from the encoder and converts it to 
// the amount of radians from the zero position.
double get_radians(double counts)       
{
    double pi = 3.14159265359;
    double radians = (counts/4200)*2*pi;
    return radians;
}


// this function takes a 0->1 input (in this case a potmeter and converts it
// to a -2->2 range
double setpoint_f(double input)
{
    double offset = 0.5;            // offset the inputsignal to -0.5->0.5
    double gain = 4;                // increase the signal
    double output = (input-offset)*gain;
    return output;
}

// this function is a simple K control called by the motor function
double K_control()
{
    double setpoint = setpoint_f(potright.read());      // determine the setpoint that has been set by the inputsignal
    double rads = get_radians(motor1_enc.getPosition());    // determine the position of the motor
    double error = (setpoint - rads);                       // determine the error (reference - position)
    double output = K*error;                            // controller output K*e
    return output;
}

// this function controls the input for one of the electric motore and is called by a ticker
void motor1_control()
{
    double output = K_control();
    if(output > 0) {
        motor1_rich.write(0);
        motor1_aan.write(1);
    } else if(output < 0) {
        motor1_rich.write(1);
        motor1_aan.write(1);
    }
}


int main()
{
    double output ;
    mod.attach(&send,0.01);
    mod.attach(&motor1_control, 0.1);
    while(true) 
    {
    }
}