File content as of revision 9:697134d3564e:

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

MODSERIAL pc(USBTX,USBRX);

// (DEBUGGING AND TESTING BUTTONS) (0 when pressed and 1 when not pressed) 
DigitalIn buttonL1(PTC6);                // Button 1 (laag op board) for testing at the lower board
DigitalIn buttonL2(PTA4);                // Button 2 (laag op board) for testing at the lower board
DigitalIn buttonH1(D2);                  // Button 3 (hoog op board) for testing at the top board
DigitalIn buttonH2(D6);                  // Button 4 (hoog op board) for testing at the top board

volatile bool looptimerflag;
const double  cw=0;                     // zero is clockwise (front view)
const double  ccw=1;                    // one is counterclockwise (front view)

const double Gain_P_turn=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

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 

const double sample_time=0.01;          // tijd voor een sample (100Hz)
    
// PID motor constants
double integrate_error_turn=0;          // integration error turn motor
double previous_error_turn=0;           // previous error turn motor


// Functions used (described after main)
void keep_in_range(double * in, double min, double max);
void setlooptimerflag(void);
double get_reference(double input);


// MAIN function
int main() {
    AnalogIn potmeter(A0);                  // Potmeter that can read a reference value (DEBUG TOOL)
    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
    double reference_turn;                  // Set constant to store reference value of the Turn motor
    double position_turn;                   // Set constant to store current position of the Turn motor
    double error;
    
    //START OF CODE 
    pc.printf("Start of code \n\r");
    
    pc.baud(9600);                          // Set the baudrate
    
    // Tickers 
    Ticker looptimer;                                          // Ticker called looptimer to set a looptimerflag
    looptimer.attach(setlooptimerflag,sample_time);            // calls the looptimer flag every 0.01s
    
    pc.printf("Start infinite loop \n\r");
    wait (3);                                                  // Rest before starting system
    
    //INFINITE LOOP
    while(1) 
        {                                                                                                   // Start while loop
        // DEBUGGING BUTTON: interrupt button Disbalances the current motor position 
         if (buttonL2.read() < 0.5){      //if button pressed
             motordirection_turn = cw;                       
             pwm_motor_turn = 0.5f;       // motorspeed            
             pc.printf("positie = %d \r\n", motor_turn.getPulses()); }
             
        // Wait until looptimer flag is true then execute PID controller.  
        else
            {
        while(looptimerflag != true);

        looptimerflag = false;
        
        //reference = (potmeter.read()-0.5)*2000;  // Potmeter bepaald reference (uitgeschakeld) 
        reference_turn = 15;
        
        // 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);
        
        
        // P-CONTROLLER
        // Calculate error then multiply it with the gain, and store in pwm_to_motor
        
        error=(reference_turn - position_turn);                             // Current error (input controller)
        
   //     integrate_error_turn=integrate_error_turn + sample_time*error;      // integral error output
//                                                                                        // overwrite previous integrate error by adding the current error multiplied by the sample time.
//        
        //double error_derivative_turn=(error - previous_error_turn)/sample_time;    // derivative error output
    
        // FILTER error_derivative_turn (lowpassfilter)
            // biquadFilter Lowpassfilter(mT_f_a1,mT_f_a2,mT_f_b0,mT_f_b1,mT_f_b2);
                // 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
            // Lowpassfilter.step(e_der)
        
        //previous_error_turn=error;                                // current error is saved to memory constant to be used in 
                                                                  // the next loop for calculating the derivative error 
                                                                  
        // double Gain_I_turn=1;
        // double Gain_D_turn=1;

        double pwm_motor_turn = error*Gain_P_turn;                     // output P controller to pwm   

   //     double pwm_motor_turn_P = error*Gain_P_turn;                     // output P controller to pwm        
//        double pwm_motor_turn_I = integrate_error_turn*Gain_I_turn;      // output I controller to pwm
//        double pwm_motor_turn_D = error_derivative_turn*Gain_D_turn;     // output D controller to pwm
//        
//        double pwm_motor_turn = pwm_motor_turn_P + pwm_motor_turn_I + pwm_motor_turn_D; // Total output PID controller to pwm
//        
        
        // Keep Pwm between -1 and 1
        keep_in_range(&pwm_motor_turn, -1,1);                     // Pass to motor controller but keep it in range!
        pc.printf("pwm %f \n\r", pwm_motor_turn);

        // Check error and decide direction to turn
        if(pwm_motor_turn > 0)
            {
            motordirection_turn=ccw;
            pc.printf("if loop pwm_to_motor > 0 \n\r");
            }
        else
            {
            motordirection_turn=cw;
            pc.printf("else loop pwm_to_motor < 0 \n\r");
            }
        
        // Put pwm_motor to the motor
        pwm_motor_turn=(abs(pwm_motor_turn));
    }
}
}

// 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;
}