File content as of revision 5:0d3e8694726e:

#include "BiQuad.h"
#include "FastPWM.h"
#include "HIDScope.h"
#include <math.h>
#include "mbed.h"
#include "MODSERIAL.h"
#include "QEI.h"

const double pi = 3.1415926535897;                                              // Definition of pi

// SERIAL COMMUNICATION WITH PC
MODSERIAL pc(USBTX, USBRX);

// STATES
enum states{MOTORS_OFF, MOVING, HITTING};
states currentState = MOTORS_OFF;                                               // Start with motors off
bool stateChanged = true;                                                       // Make sure the initialization of first state is executed

// ENCODER
QEI Encoder1(D10,D11,NC,32);                                                    // CONNECT ENC1A TO D10, ENC1B TO D11
QEI Encoder2(D12,D13,NC,32);                                                    // CONNECT ENC2A TO D12, ENC2B TO D13

// PINS
DigitalOut motor1DirectionPin(D4);                                              // Value 0: CCW; 1: CW
PwmOut motor1MagnitudePin(D5);
DigitalOut motor2DirectionPin(D7);                                              // Value 0: CW or CCW?; 1: CW or CCW?
PwmOut motor2MagnitudePin(D6);
InterruptIn button1(D2);                                                        // CONNECT BUT1 TO D2
InterruptIn button2(D3);                                                        // CONNECT BUT2 TO D3

// MOTOR CONTROL
Ticker controllerTicker;
const double controller_Ts = 1/5000;                                            // Time step for controllerTicker; Should be between 5kHz and 10kHz [s]
const double motorRatio = 131.25;                                               // Ratio of the gearbox in the motors []
const double radPerPulse = 2*pi/(32*motorRatio);
// Motor 1
// Controller gains
const double motor1_KP = 2.5;                                                   // Position gain []
const double motor1_KI = 1.0;                                                   // Integral gain []
const double motor1_KD = 0.5;                                                   // Derivative gain []
double motor1_err_int = 0, motor1_prev_err = 0;
// Derivative filter coefficients
const double motor1_f_a1 = 1.0, motor1_f_a2 = 2.0;
const double motor1_f_b0 = 1.0, motor1_f_b1 = 3.0, motor1_f_b2 = 4.0;
// Filter variables
double motor1_f_v1 = 0, motor1_f_v2 = 0;
double reference1 = 0.0;                                                        // Wanted rotation of motor 1 [rad]
// Motor 2
// Controller gains
// Derivative filter coefficients
// Filter variables


// FUNCTIONS
// BIQUAD FILTER FOR DERIVATIVE OF ERROR
double biquad(double u, double &v1, double &v2, const double a1,
 const double a2, const double b0, const double b1, const double b2)
{
    double v = u - a1*v1 - a2*v2;
    double y = b0*v + b1*v1 + b2*v2;
    v2 = v1; v1 = v;
    return y;
}

// P-CONTROLLER
double P_Controller(double error, const double Kp)
{
    return Kp * error;
}

// PID-CONTROLLER WITH FILTER
double PID_Controller(double e, const double Kp, const double Ki,
 const double Kd, double Ts, double &e_int, double &e_prev, double &f_v1,
 double &f_v2, const double f_a1, const double f_a2, const double f_b0,
 const double f_b1, const double f_b2)
{
    // Derivative
    double e_der = (e - e_prev)/Ts;
    e_der = biquad(e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2);
    e_prev = e;
    // Integral
    e_int = e_int + Ts*e;
    // PID
    return Kp*e + Ki*e_int + Kd*e_der;
}

// MOTOR 1
void RotateMotor1(double motor1Value)
{
    if(currentState == MOVING)                                                  // Checks if state is MOVING
    {
        if(motor1Value >= 0) motor1DirectionPin = 1;                            // Rotate motor 1 CW
        else motor1DirectionPin = 0;                                            // Rotate motor 1 CCW
        
        if(fabs(motor1Value) > 1) motor1MagnitudePin = 1;
        else motor1MagnitudePin = fabs(motor1Value);
    }
    else motor1MagnitudePin = 0;
}

// MOTOR 1 P-CONTROLLER
void Motor1PController()
{
    double position1 = radPerPulse * Encoder1.getPulses();                      // Calculates the rotation of motor 1
    double motor1Value = P_Controller(reference1 - position1, motor1_KP);
    RotateMotor1(motor1Value);
}

// MOTOR 1 PID-CONTROLLER
void Motor1Controller()
{
    double position1 = radPerPulse * Encoder1.getPulses();
    double motor1Value = PID_Controller(reference1 - position1, motor1_KP, 
     motor1_KI, motor1_KD, controller_Ts, motor1_err_int, motor1_prev_err,
     motor1_f_v1, motor1_f_v2, motor1_f_a1, motor1_f_a2, motor1_f_b0,
     motor1_f_b1, motor1_f_b2);
    RotateMotor1(motor1Value);
}

// TURN OFF MOTORS
void TurnMotorsOff()
{
    motor1MagnitudePin = 0;
    motor2MagnitudePin = 0;
}

// STATES
void ProcessStateMachine()
{
    switch(currentState)
    {
        case MOTORS_OFF:
        {
            // State initialization
            if(stateChanged)
            {
                pc.printf("Entering MOTORS_OFF \r\n");
                TurnMotorsOff();                                                // Turn motors off
                stateChanged = false;
            }
            
            // Home command
            if(!button1)
            {
                currentState = MOVING;
                stateChanged = true;
                break;
            }
            break;
        }
        
        case MOVING:
        {
            // State initialization
            if(stateChanged)
            {
                pc.printf("Entering MOVING \r\n");
                stateChanged = false;
            }
            
            // Hit command    
            if(!button1)
            {
                currentState = HITTING;
                stateChanged = true;
                break;
            }
            
            break;
        }
        
        case HITTING:
        {
            // State initialization
            if(stateChanged)
            {
                pc.printf("Entering HITTING \r\n");
                stateChanged = false;
                //HitBall();                                                    // Hit the ball
                currentState = MOVING;
                stateChanged = true;
                break;
            }
            break;
        }
        
        default:
        {
            TurnMotorsOff();                                                    // Turn motors off for safety
            break;
        }
    }
}

// MAIN FUNCTION
int main()
{
    // Serial communication
    pc.baud(115200);
    
    pc.printf("START \r\n");
    
    controllerTicker.attach(&Motor1PController, controller_Ts);                 // Ticker to control motor 1
    //controllerTicker.attach(&Motor1Controller, controller_Ts);
    
    while(true)
    {
        ProcessStateMachine();                                                  // Execute states function
    }
}