File content as of revision 23:78898ddfb103:

#include "QEI.h"
#include "mbed.h"
#include "BiQuad.h"
#include "FastPWM.h"
#include "HIDScope.h"
#include "MODSERIAL.h"

// Pins
MODSERIAL pc(USBTX, USBRX);
InterruptIn stepresponse(D0);
FastPWM     PWM_motor_1(D6);
FastPWM     PWM_motor_2(D5);
DigitalOut  direction_motor_1(D7);
DigitalOut  direction_motor_2(D4);
InterruptIn button_1(SW2);
InterruptIn button_2(SW3);

// variables
Ticker          TickerStateMachine;
Ticker          motor_control;
Ticker          write_scope;
Timer           sinus_time;
enum    states  {start, motor_calibration, demo_mode, emg_calibration, vertical_movement, horizontal_movement};
states  CurrentState = start;
bool    StateChanged = true;
bool    demo    = false;
bool    emg     = false;
bool    next    = false;
HIDScope scope(3);
QEI encoder_1(D10,D11,NC,8400,QEI::X4_ENCODING);
QEI encoder_2(D12,D13,NC,8400,QEI::X4_ENCODING);
volatile double theta_1;
//volatile float  theta_error_1;
volatile float  theta_reference_1;
volatile double theta_2;
//volatile float  theta_error_2;
volatile float  theta_reference_2;
float Ts    = 0.001;
float Kp;
float Ki;
float Kd;

// functies
float CalculateError(float theta_reference,float theta)
{
    float theta_error = theta_reference-theta;
    return theta_error;
}

float Controller(float theta_error, bool motor)
{
    if (motor == false)
    {
        float K     = 1;
        float ti    = 0.1;
        float td    = 10;
        Kp    = K*(1+td/ti);
        Ki    = K/ti;
        Kd    = K*td;
    }
    else
    {
        float K     = 1;
        float ti    = 0.1;
        float td    = 10;
        Kp    = K*(1+td/ti);
        Ki    = K/ti;
        Kd    = K*td;
    }
    static float error_integral = 0;
    static float error_prev = 0;
    static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
    
    // Proportional part:
    float torque_p = Kp * theta_error;
    
    // Integral part:
    error_integral = error_integral + theta_error * Ts;
    float torque_i = Ki * error_integral;
    
    // Derivative part:
    float error_derivative = (theta_error - error_prev)/Ts;
    float filtered_error_derivative = LowPassFilter.step(error_derivative);
    float torque_d = Kd * filtered_error_derivative;
    error_prev = theta_error;
    
    // Sum all parts and return it
    float torque = torque_p + torque_i + torque_d;
    return torque;
}

void CalculateDirectionMotor() 
{
    direction_motor_1 = Controller(CalculateError(theta_reference_1,theta_1),0) <= 0.0f;
    direction_motor_2 = Controller(CalculateError(theta_reference_2,theta_2),1) <= 0.0f;
}

void WriteScope()
{
    scope.set(0,theta_1);
    scope.set(1,CalculateError(theta_reference_1,theta_1));
    scope.set(2,theta_reference_1);
    scope.send();
}

void ReadEncoder()
{
    theta_1 = ((360.0f/64.0f)*(float)encoder_1.getPulses())/131.25f; // 360/64 voor de 64 CPR Encoder, 131.25 omdat de gear ratio 131.25:1 is. Hiermee is 1 omwenteling 360.
    theta_2 = ((360.0f/64.0f)*(float)encoder_2.getPulses())/131.25f;
}

void MotorControl() 
{
    ReadEncoder();
    theta_reference_1 = 360*sin(0.1f*sinus_time.read()*2*3.14f); // voor test, moet weg in eindscript
    CalculateDirectionMotor();
    PWM_motor_1.write(fabs(Controller(CalculateError(theta_reference_1,theta_1),0)/360.0f));
    PWM_motor_2.write(fabs(Controller(CalculateError(theta_reference_2,theta_2),1)/360.0f));
}

void go_next_1()
{
    demo = !demo;
}

void go_next_2()
{
    emg = !emg;
    next = emg;
}

void while_start()
{
    // Do startup stuff
    CurrentState = motor_calibration;
    StateChanged = true;
}

void while_motor_calibration()
{
    // Do motor calibration stuff
    if (demo)   // bool aanmaken voor demo (switch oid aanmaken)
    {
        CurrentState = demo_mode;
        StateChanged = true;
    }
    if (emg)    // bool aanmaken voor EMG (switch oid aanmaken)
    {
        CurrentState = emg_calibration;
        StateChanged = true;
    }
}

void while_demo_mode()
{
    // Do demo mode stuff
    if (!demo)  // bool aanmaken voor demo (switch oid)
    {
        emg = true;
    }
    if (emg)    // bool aanmaken voor EMG (switch oid aanmaken)
    {
        CurrentState = emg_calibration;
        StateChanged = true;
    }
}

void while_emg_calibration()
{
    // Do emg calibration stuff
    if (!emg)   // bool aanmaken voor EMG (switch)
    {
        CurrentState = vertical_movement;
        StateChanged = true;
    }
}

void while_vertical_movement()
{
    // Do vertical movement stuff
    if (next)  // EMG gebaseerde threshold aanmaken
    {
        CurrentState = horizontal_movement;
        StateChanged = true;
    }
}

void while_horizontal_movement()
{
    // Do horizontal movement stuff
    if (!next) // EMG gebaseerde threshold aanmaken
    {
        CurrentState = vertical_movement;
        StateChanged = true;
    }
}

void StateTransition()
{
    if (StateChanged)
    {
        if (CurrentState == start)
        {
            pc.printf("Initiating start.\n\r");
        }
        if (CurrentState == motor_calibration)
        {
            pc.printf("Initiating motor_calibration.\n\r");
        }
        if (CurrentState == demo_mode)
        {
            pc.printf("Initiating demo_mode.\n\r");
        }
        if (CurrentState == emg_calibration)
        {
            pc.printf("Initiating emg_calibration.\n\r");
        }
        if (CurrentState == vertical_movement)
        {
            pc.printf("Initiating vertical_movement.\n\r");
        }
        if (CurrentState == horizontal_movement)
        {
            pc.printf("Initiating horizontal_movement.\n\r");
        }
        StateChanged = false;
    }
}

void StateMachine()
{
    switch(CurrentState)
    {
        case start:
            StateTransition();
            while_start();
            break;
        case motor_calibration:
            StateTransition();
            while_motor_calibration();
            break;
        case demo_mode:
            StateTransition();
            while_demo_mode();
            break;
        case emg_calibration:
            StateTransition();
            while_emg_calibration();
            break;
        case vertical_movement:
            StateTransition();
            while_vertical_movement();
            break;
        case horizontal_movement:
            StateTransition();
            while_horizontal_movement();
            break;
        default:
            pc.printf("Unknown or unimplemented state reached!\n\r");
    }
}

// main
int main(){
    pc.baud(115200);
    pc.printf("Hello World!\n\r");
    button_1.fall(go_next_1);
    button_2.fall(go_next_2);
    sinus_time.start();
    PWM_motor_1.period_ms(10);
    motor_control.attach(&MotorControl, Ts);
    write_scope.attach(&WriteScope, 0.1);
    //TickerStateMachine.attach(StateMachine,1.00f);
    while(true) {
        StateMachine();
    }
}