File content as of revision 17:d643e5954165:

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

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

Ticker          controller1, controller2;        // definieer de ticker die controler1 doet


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

// motor 2
PwmOut motor2_aan(D5);
DigitalOut motor2_rich(D4);
// EINDE MOTOR


// ENCODER
Encoder motor1_enc(D12,D11);
Encoder motor2_enc(D10,D9);

//POTMETERS
AnalogIn potright(A0);
AnalogIn potleft(A1);

// RESETBUTTON
DigitalIn   button(PTA4);
int button_pressed = 0;


// controller stuff
double controlfreq = 10 ;    // controlloops frequentie (Hz)
double controlstep = 1/controlfreq; // timestep derived from controlfreq
const double K1 = 1 ;       // P constant motorcontrolle 1
const double K2 = 1;        // p constant motorcontroller 2

// define storage variables for setpoint values
double c_setpoint1 = 0;
double c_setpoint2 = 0;

// define the maximum rate of change for the setpoint (velocity)
double Vmax = 5;            // rad/sec



// 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 &c_setpoint)
{
    double offset = 0.5;            // offset the inputsignal to -0.5->0.5
    double gain = 2;                // increase the signal
    double potset = (input-offset)*gain;
    double setpoint = c_setpoint + potset * controlstep * Vmax ;
        c_setpoint = setpoint;
    return setpoint;
}

// this function is a simple K control called by the motor function
double K_control(double error,double K)
{
    double output = K*error;                            // controller output K*e
    
    // Limit the output to to a number between -1 and 1 because 
    // the motorcode will not handle anything smaller than -1 or larger than 1
    // should be put in own function to improve readability
    if(output>1)                                
    {
        output = 1;
    }
    else if(output < 1 && output > 0)
    {
        output = output;
    }
    else if(output > -1 && output < 0)
    {
        output = output;
    }
    else if(output < -1)
    {
        (output = -1);
    }
    return output;
}

// this function controls the input for one of the electric motors and is called by a ticker
// MOTOR 1
// // // // // // // // // //
void motor1_control()
{
    double setpoint1 = setpoint_f(potright.read(), c_setpoint1);      // determine the setpoint that has been set by the inputsignal
    double rads1 = get_radians(motor1_enc.getPosition());    // determine the position of the motor
    double error1 = (setpoint1 - rads1);                       // determine the error (reference - position)
    // scope.set(0,setpoint1);
    // scope.set(1,rads1);
    // scope.send();
    double output1 = K_control(error1, K1);        // bereken de controller output voor motor 1(zie hierboven)
    if(output1 > 0) {                    // 
        motor1_rich.write(0);
        motor1_aan.write(output1);
    } else if(output1 < 0) {
        motor1_rich.write(1);
        motor1_aan.write(abs(output1));
    }
}

// MOTOR 2
// // // // // // //
void motor2_control()
{
    double setpoint2 = setpoint_f(potleft.read(), c_setpoint2);      // determine the setpoint that has been set by the inputsignal
    double rads2 = get_radians(motor2_enc.getPosition());    // determine the position of the motor
    double error2 = (setpoint2 - rads2);                       // determine the error (reference - position)
    // scope.set(3,setpoint2);
    // scope.set(4,rads2);
    // scope.send();
    double output2 = K_control(error2, K2);        // bereken de controller output voor motor 1(zie hierboven)
    if(output2 > 0) {                    // 
        motor2_rich.write(0);
        motor2_aan.write(output2);
    } else if(output2 < 0) {
        motor2_rich.write(1);
        motor2_aan.write(abs(output2));
    }
}


int main()
{
    controller1.attach(&motor1_control, controlstep);
    controller2.attach(&motor2_control, controlstep);
    while(true) 
    {
        if(button.read() == button_pressed)
        {
            motor1_enc.setPosition(0);
            motor2_enc.setPosition(0);
        }
    }
}