File content as of revision 42:0a7898cb3e94:

#include "mbed.h"
#include "HIDScope.h"
#include "QEI.h"
#include "MODSERIAL.h"
#include "biquadFilter.h"
#include "encoder.h"

//                    ___________________________
//                  //                           \\
//                 ||          [Inputs]           ||
//                  \\___________________________//
//


MODSERIAL   pc(USBTX,USBRX);

DigitalOut  debug_led_red(LED_RED);                 // Debug LED
DigitalOut  debug_led_green(LED_GREEN);             // Debug LED 
DigitalOut  debug_led_blue(LED_BLUE);               // Debug LED   

DigitalIn buttonL1(PTC6);                // Button 1 (low on k64f) for testing at the lower board
DigitalIn buttonL2(PTA4);                // Button 2 (low on k64f) for testing at the lower board
DigitalIn buttonH1(D2);                  // Button 3 (high on k64f) for testing at the top board
DigitalIn buttonH2(D6);                  // Button 4 (high on k64f) for testing at the top board   

AnalogIn potmeter(A0);                  // Potmeter that can read a reference value (DEBUG TOOL)

//                    ___________________________
//                  //                           \\
//                 ||        [MOTOR TURN]         ||
//                  \\___________________________//
//

QEI motor_turn(D12,D13,NC,32);          // Encoder for motor Turn
PwmOut pwm_motor_turn(D5);              // Pwm for motor Turn
DigitalOut motordirection_turn(D4);     // Direction of the motor Turn

//                    ___________________________
//                  //                           \\
//                 ||          [HIDSCOPE]         ||
//                  \\___________________________//
//

HIDScope    scope(2); // HIDSCOPE declared

//                    ___________________________
//                  //                           \\
//                 ||         [CONSTANTS]         ||
//                  \\___________________________//
//

volatile bool looptimerflag;
const double  cw=0;                     // zero is clockwise (front view)
const double  ccw=1;                    // one is counterclockwise (front view)
const double off=1;                     //Led off
const double on=0;                      //Led on
const int Fs = 512;                     // sampling frequency (512 Hz)
const double sample_time = 0.001953125;                     // sampling frequency (512 Hz)

// PID motor constants
double integrate_error_turn=0;          // integration error turn motor
double previous_error_turn=0;           // previous error turn motor

double P_gain_turn=10; //0.0067; 
                    // stel setpoint tussen (0 en 360) en position tussen (0 en 360)
                    // max verschil: 360 -> dan pwm_to_motor 1 tot aan een verschil van 15 graden-> bij 15 moet pwm_to_motor ong 0.1 zijn 
                    // dus     0.1=15*gain      gain=0.0067
                    // Als 3 graden verschil 0.1 dan 0.1/3=gain=0.33
                                                        
double I_gain_turn=0.1; //0.025;  //(1/2000) //0.00000134
        // pwm_motor_I=(integrate_error_turn + sample_time*error)*gain;  pwm = (4*0.01 + 4)* Gain => 0.1 pwm gewenst (na 1 seconde een verschil van 4 graden)  
                        // 0.1 / (4.01) = Gain = 0.025
        
double D_gain_turn=50; //0.01;                    
        // error_derivative_turn=(error - previous_error_turn)/sample_time
        // 

double conversion_counts_to_degrees=0.085877862594198;
                    // gear ratio motor = 131
                    // ticks per magnet rotation = 32 (X2 Encoder)
                    // One revolution = 360 degrees
                    // degrees_per_encoder_tick = 360/(gear_ratio*ticks_per_magnet_rotation)=360/131*32=0.085877862594198 

//                    ___________________________
//                  //                           \\
//                 ||     [FILTER CONSTANTS]      ||
//                  \\___________________________//
//

const double mT_f_a1=-1.965293372622690e+00;
const double mT_f_a2=9.658854605688177e-01;
const double mT_f_b0=1.480219865318266e-04;
const double mT_f_b1=2.960439730636533e-04;
const double mT_f_b2=1.480219865318266e-04; // Motor Turn filter constants
                
biquadFilter Lowpassfilter_derivative(mT_f_a1,mT_f_a2,mT_f_b0,mT_f_b1,mT_f_b2);
    
//                    ___________________________
//                  //                           \\
//                 ||       [FUNCTIONS USED]      ||
//                  \\___________________________//
//

void keep_in_range(double * in, double min, double max);
void setlooptimerflag(void);
double get_reference(double input);

// STRIKE: Deactivate PID Controller
void deactivate_PID_Controller(double& P_gain, double& I_gain, double& D_gain);
// TURN: Activate PID Controller 
void activate_PID_Controller_turn(double& P_gain, double& I_gain, double& D_gain);
// STRIKE: Activate PID Controller
void activate_PID_Controller_strike(double& P_gain, double& I_gain, double& D_gain);


// Change reference position (two functions)
void Change_Turn_Position_Left (double& reference, double change_one_bottle);

void Change_Turn_Position_Right (double& reference, double change_one_bottle);

                                                     ///////////////////////////////
                                                     //                           //
///////////////////////////////////////////////////////      [MAIN FUNCTION]      ////////////////////////////////////////////////////////////////////////////
                                                     //                           //                                                                        //
                                                     ///////////////////////////////                                                                        //
int main() {
    debug_led_red=off;
    debug_led_blue=off;
    debug_led_green=off;
    
    const double change_one_bottle=45;    
    double position_turn;                   // Set constant to store current position of the Turn motor
    double error_turn;
    double pwm_to_motor_turn;
    double pwm_motor_turn_P;
    double pwm_motor_turn_I;
    double pwm_motor_turn_D;
    double reference_turn=0;                  // Set constant to store reference value of the Turn motor
    
    // CALIBRATE (STARTING POSITION)
    
    
    // TEST: Does change PID gains work? => works correctly
    deactivate_PID_Controller(P_gain_turn,I_gain_turn,D_gain_turn); // Deactivate Turn motor
    pc.printf("P = %d I = %d D = %d \n\r");
    activate_PID_Controller_turn(P_gain_turn,I_gain_turn,D_gain_turn);
    pc.printf("P = %d I = %d D = %d \n\r");
    
    // TEST: Does change reference function work? => still needs to be tested
    pc.printf("initial reference_turn = %d \n\r", reference_turn);
    Change_Turn_Position_Left (reference_turn, change_one_bottle);
    pc.printf("After left turn reference_turn  = %d \n\r", reference_turn);
    Change_Turn_Position_Right (reference_turn, change_one_bottle);
    pc.printf("After right turn reference_turn  = %d \n\r", reference_turn);
    Change_Turn_Position_Right (reference_turn, change_one_bottle);
    Change_Turn_Position_Right (reference_turn, change_one_bottle);
    Change_Turn_Position_Right (reference_turn, change_one_bottle);
    Change_Turn_Position_Right (reference_turn, change_one_bottle);
    pc.printf("After right turn reference_turn  = %d \n\r", reference_turn); // DOES THE LIMITER WORK? (should be -90 degrees)
    
    //START OF CODE 
    pc.printf("Start of code \n\r");
    
    pc.baud(115200);                          // Set the baudrate
    
    // Tickers 
    Ticker looptimer;                                          // Ticker called looptimer to set a looptimerflag
    looptimer.attach(setlooptimerflag,(float)1/Fs);            // calls the looptimer flag every 0.01s
    
    pc.printf("Start infinite loop \n\r");
    wait (3);                                                  // Wait before starting system
    
    
    //INFINITE LOOP
    while(1) 
        {                                                                                                   // Start while loop
        
//                                         ___________________________
//                                       //                           \\
//                                      ||       [DEBUG BUTTONS]       ||
//                                       \\___________________________//
//                             interrupt button Disbalances the current motor position 

                                //if button L2 pressed => disbalance motor                                                  
         if (buttonL2.read() < 0.5){      
             motordirection_turn = cw;                       
             pwm_motor_turn = 0.5f;         
             pc.printf("positie = %d \r\n", motor_turn.getPulses()); }

             
                                // if button L1 pressed => shut down motor for 1000 seconds  
         if (buttonL1.read() < 0.5){      
             motordirection_turn = cw;                       
             pwm_motor_turn = 0;
             wait(1000);        
             pc.printf("positie = %d \r\n", motor_turn.getPulses()); }   
             
            else
            {
//                                         ___________________________
//                                       //                           \\
//                                      ||    [Wait for go signal]     ||
//                                       \\___________________________//
//                     // Wait until looptimer flag is true then execute PID controller loop.                
             
        while(looptimerflag != true);

        looptimerflag = false;
        
                //reference = (potmeter.read()-0.5)*2000;  // Potmeter bepaald reference (uitgeschakeld) 
        
//                                         ___________________________
//                                       //                           \\
//                                      ||     [Calibrate position]    ||
//                                       \\___________________________//
//                             //   Keep motor position between -4200 and 4200 counts        
        
        if ((motor_turn.getPulses()>4200) || (motor_turn.getPulses()<-4200)) // If value is outside -4200 and 4200 (number of counts equal to one revolution) reset to zero
        {
            motor_turn.reset();
            pc.printf("RESET \n\r");
        }   
        
                              //    Convert position to degrees    \\
                              
        position_turn = conversion_counts_to_degrees * motor_turn.getPulses();
        
        pc.printf("calibrated setpoint: %f, calibrated position motor %i, position %f \n\r", reference_turn, motor_turn.getPulses(), position_turn);
        
        
//                                         ___________________________
//                                       //                           \\
//                                      ||     [PID CONTROLLER]        ||
//                                       \\___________________________//
//            // Calculate error then multiply it with the (P, I and D) gains, and store in pwm_to_motor \\

        error_turn=(reference_turn - position_turn);                                     // TURN: Current error (input controller)        
        integrate_error_turn=integrate_error_turn + sample_time*error_turn;              // TURN: integral error output
//                                                                                          // overwrite previous integrate error by adding the current error 
                                                                                            // multiplied by the sample time.
//        
        double error_derivative_turn=(error_turn - previous_error_turn)/sample_time;     // TURN: derivative error output
        error_derivative_turn=Lowpassfilter_derivative.step(error_derivative_turn); // TURN: Filter
        error_derivative_turn=Lowpassfilter_derivative.step(error_derivative_turn);
        
        previous_error_turn=error_turn;                                // current error is saved to memory constant to be used in 
                                                                  // the next loop for calculating the derivative error 

        pwm_to_motor_turn = error_turn*P_gain_turn;                     // output P controller to pwm   

        pwm_motor_turn_P = error_turn*P_gain_turn;                     // output P controller to pwm        
        pwm_motor_turn_I = integrate_error_turn*I_gain_turn;      // output I controller to pwm
        pwm_motor_turn_D = error_derivative_turn*D_gain_turn;     // output D controller to pwm

        pwm_to_motor_turn = pwm_motor_turn_P + pwm_motor_turn_I + pwm_motor_turn_D;
     
//                                         ___________________________
//                                       //                           \\
//                                      ||   [PID error -> pwm motor]  ||
//                                       \\___________________________//
//          // Keep Pwm between -1 and 1 -> and decide the direction of the motor to compensate the error       

        keep_in_range(&pwm_to_motor_turn, -1,1);                     // Pass to motor controller but keep it in range!
        pc.printf("pwm %f \n\r", pwm_to_motor_turn);

        // Check error and decide direction to turn
        if(pwm_to_motor_turn > 0)
            {
            motordirection_turn=ccw;
            pc.printf("if loop pwm > 0 \n\r");
            }
        else
            {
            motordirection_turn=cw;
            pc.printf("else loop pwm < 0 \n\r");
            }
        
//                                         ___________________________
//                                       //                           \\
//                                      ||       [pwm -> Plant]        ||
//                                       \\___________________________//
//                                          // Put pwm to the motor \\

            pwm_motor_turn=(abs(pwm_to_motor_turn));
        
//                                         ___________________________
//                                       //                           \\
//                                      ||         [HIDSCOPE]          ||
//                                       \\___________________________//
//                                      // Check signals inside HIDSCOPE \\
        
            scope.set(0,error_turn);             // HIDSCOPE channel 0 : Current Error
            scope.set(1,position_turn);     // HIDSCOPE channel 1 : Position_turn
            scope.set(2,pwm_to_motor_turn); // HIDSCOPE channel 2 : Pwm_to_motor_turn
            scope.send();                   // Send channel info to HIDSCOPE
            
//                                         ___________________________
//                                       //                           \\
//                                      ||        [Deactivate?]        ||
//                                       \\___________________________//
//                // Check whether the motor has reached reference position and can be shut down            
            
            if(fabs(error_turn)<2)             // Shut down if error is smaller than two degrees
            {deactivate_PID_Controller(P_gain_turn,I_gain_turn,D_gain_turn);}
            else
            {activate_PID_Controller_turn(P_gain_turn,I_gain_turn,D_gain_turn);}
    }
}
}

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
                                                    //                                //  
                                                    //      [Functions Described]     //
                                                    //                                //
                                                    ////////////////////////////////////

// Keep in range function
void keep_in_range(double * in, double min, double max)
{
    *in > min ? *in < max? : *in = max: *in = min;
}

// Looptimerflag function
void setlooptimerflag(void)
{
    looptimerflag = true;
}

// Get setpoint -> potmeter (MOMENTEEL UITGESCHAKELD)
double get_reference(double input)
{
const float offset = 0.5;
const float gain = 4.0;
return (input-offset)*gain;
}

// Change reference
void Change_Turn_Position_Left (double& reference, double change_one_bottle)
    {
        reference = reference + change_one_bottle;
        keep_in_range(&reference, -90, 90); // reference position stays between -90 and 90 degrees (IF bottles at -90, -45, 0, 45, 90 degrees)
    }
    
void Change_Turn_Position_Right (double& reference, double change_one_bottle)
    {
        reference = reference - change_one_bottle;
        keep_in_range(&reference, -90, 90);
    }

// Deactivate or Activate PID_Controller
void deactivate_PID_Controller(double& P_gain, double& I_gain, double& D_gain)
{
    P_gain=0; 
    I_gain=0; 
    D_gain=0;
}

void activate_PID_Controller_turn(double& P_gain, double& I_gain, double& D_gain)
{
    P_gain=0; // hier waardes P,I,D veranderen (waardes bovenaan doen (tijdelijk) niks meer
    I_gain=0; 
    D_gain=0;
}

void activate_PID_Controller_strike(double& P_gain, double& I_gain, double& D_gain)
{
    P_gain=0; // hier waardes P,I,D veranderen (waardes bovenaan doen (tijdelijk) niks meer
    I_gain=0; 
    D_gain=0;
}