File content as of revision 27:4d7ca91e2e64:

 //--------------------------------------------------------------------------------------------------------------------------//
 // Motorscript voor 2 motoren voor de "SJOEL ROBOT", Groep 7
 //--------------------------------------------------------------------------------------------------------------------------//
 // Libraries
 //--------------------------------------------------------------------------------------------------------------------------//
#include "mbed.h"
#include "MODSERIAL.h"
#include "HIDScope.h"
#include "QEI.h"
#include "biquadFilter.h"

//--------------------------------------------------------------------------------------------------------------------------//
// Constanten/Inputs/Outputs
//--------------------------------------------------------------------------------------------------------------------------//
    MODSERIAL pc(USBTX, USBRX);             // To/From PC
    QEI Encoder2(D3, D2, NC, 32);           // Encoder Motor 2
    QEI Encoder1(D13,D12,NC, 32);           // Encoder Motor 1
    HIDScope scope(4);                      // Scope, 4 channels

// LEDs
    DigitalOut LedR(LED_RED);
    DigitalOut LedG(LED_GREEN);
    DigitalOut LedB(LED_BLUE);

// Motor
    DigitalOut motor1direction(D7);         // Motor 1, Direction & Speed
    PwmOut motor1speed(D6);
    DigitalOut motor2direction(D4);         // Motor 2, Direction & Speed
    PwmOut motor2speed(D5);

// Tickers
    Ticker ScopeTime;
    Ticker myControllerTicker2;
    Ticker myControllerTicker1;

// Constants
    double reference2, reference1;
    double position2 = 0, position1 = 0;
    double m2_ref = 0, m1_ref = 0;
    int count = 0;
    double Grens2 = 90, Grens1 = 90;
    double Stapgrootte = 5;

//Sample time (motor-step)
    const double m2_Ts = 0.01, m1_Ts = 0.01;

//Controller gain Motor 2 & 1
    const double m2_Kp = 0.1,m2_Ki = 0.001, m2_Kd = 1;
    const double m1_Kp = 5,m1_Ki = 0.05, m1_Kd = 2;
    double m2_err_int = 0, m2_prev_err = 0;
    double m1_err_int = 0, m1_prev_err = 0;

//Derivative filter coeffs Motor 2 & 1
    const double BiGainm2f1 = 0.959332;
    const double m2_f1_a1 = -1.55576653052, m2_f1_a2 = 0.61374320375, m2_f1_b0 = 1.0*BiGainm2f1, m2_f1_b1 = -0.90928276835*BiGainm2f1, m2_f1_b2 = 1.0*BiGainm2f1;


//Biquads
    biquadFilter f_Motor2 (m2_f1_a1, m2_f1_a2, m2_f1_b0, m2_f1_b1, m2_f1_b2);         // creates the low pass filter

// Filter variables
    double m2_f_v1 = 0, m2_f_v2 = 0;
    double m1_f_v1 = 0, m1_f_v2 = 0;
    double m2_f2_v1 = 0, m2_f2_v2 = 0;

//--------------------------------------------------------------------------------------------------------------------------//
// General Functions
//--------------------------------------------------------------------------------------------------------------------------//

//HIDScope
    void ScopeSend()//Functie die de gegevens voor de scope uitleest en doorstuurt
    {
        scope.set(0, reference2 - position2);
        scope.set(1, position2);
        scope.set(2, reference2);    
        scope.set(3, position2);
        scope.send();
    
    }  

// Reusable PID controller
    double PID( double e, const double Kp, const double Ki, const double Kd, double Ts, double &e_int, double &e_prev)
    {
    // Derivative
        double e_der = (e-e_prev)/Ts;
        //e_der = f_motor2(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 control functions
//--------------------------------------------------------------------------------------------------------------------------//

// Motor2 control
    void motor2_Controller() 
    {
        // Setpoint motor 2
            reference2 = m2_ref;                           // Reference in degrees
            position2 = Encoder2.getPulses()*360/(32*131); // Position in degrees
        // Speed control
            double m2_P1 = PID( reference2 - position2, m2_Kp, m2_Ki, m2_Kd, m2_Ts, m2_err_int, m2_prev_err);
            double m2_P2 = f_Motor2.step(m2_P1);//(m2_P1, m2_f2_v1, m2_f2_v2, m2_f2_a1, m2_f2_a2,m2_f2_b0, m2_f2_b1, m2_f2_b2); // Filter of motorspeed input
            motor2speed = abs(m2_P2); 
        // Direction control
            if(m2_P2 > 0) 
            {    
                motor2direction = 0;
            }
            else
            {
                motor2direction = 1;
            }
    }   

// Motor1 control
    void motor1_Controller() 
    {
        
    }

//--------------------------------------------------------------------------------------------------------------------------//
// Main function
//--------------------------------------------------------------------------------------------------------------------------//
int main()
{  
//--------------------------------------------------------------------------------------------------------------------------//
// Initalizing
//--------------------------------------------------------------------------------------------------------------------------// 
    //LEDs OFF
        LedR = LedB = LedG = 1;
    
    //PC connection & check
        pc.baud(115200);
        pc.printf("Tot aan loop werkt\n");
    
    // Tickers
        ScopeTime.attach(&ScopeSend, 0.01f);                    // 100 Hz, Scope
        myControllerTicker2.attach(&motor2_Controller, 0.01f ); // 100 Hz, Motor 2
        myControllerTicker1.attach(&motor1_Controller, 0.01f ); // 100 Hz, Motor 1
    
//--------------------------------------------------------------------------------------------------------------------------//
// Control Program
//--------------------------------------------------------------------------------------------------------------------------//
    while(true)
    {
        char c = pc.getc();
    // 1 Program UP
        if(c == 'e') 
        {
            count = count + 1;
            if(count > 2)
                {
                    count = 2;
                }

        }
     // 1 Program DOWN
        if(c == 'd')
        {
            count = count - 1;
            if(count < 0)
                {
                    count = 0;
                }
        }
    // PROGRAM 0: Motor 2 control and indirect control of motor 1, Green LED      
        if(count == 0)
        {
                
                LedR = LedB = 1;
                LedG = 0;
                if(c == 'r')
                {
                    m2_ref = m2_ref + Stapgrootte;
                    m1_ref = m1_ref - Stapgrootte;
                    if (m2_ref > Grens2)
                    {
                        m2_ref = Grens2;
                        m1_ref = -1*Grens1;
                    }
                }
                if(c == 'f')
                {
                    m2_ref = m2_ref - Stapgrootte;
                    m1_ref = m1_ref + Stapgrootte;
                    if (m2_ref < -1*Grens2)
                    {
                        m2_ref = -1*Grens2;
                        m1_ref = Grens1;
                    }
                }
        }
    // PROGRAM 1: Motor 1 control, Red LED
        if(count == 1) 
        {
                LedG = LedB = 1;
                LedR = 0;
                if(c == 't')
                {
                    m1_ref = m1_ref + Stapgrootte;
                    if (m1_ref > Grens1)
                    {
                        m1_ref = Grens1;
                    }
                }
                if(c == 'g')
                {
                    m1_ref = m1_ref - Stapgrootte;
                    if (m1_ref < -1*Grens1)
                    {
                        m1_ref = -1*Grens1;
                    }
                }
        }
    // PROGRAM 2: Firing mechanism & Reset, Blue LED
        if(count == 2) 
        {

                LedR = LedG = 1;
                LedB = 0;
                //VUUUUR!! (To Do)
                wait(1);
                m2_ref = 0;
                m1_ref = 0;   
        }
    }

}