File content as of revision 12:614f1439e679:

#include "mbed.h"
#include "QEI.h"
#include <math.h>

Serial pc(USBTX, USBRX);
//QEI wheel_one (PTC10, PTC11, NC, 624); // Pin for counting (analog in)
QEI wheel_one (PTD3, PTD1, NC, 624);
QEI wheel_two (PTD0, PTD2, NC, 624);

DigitalOut led_green(LED_GREEN);
DigitalOut led_red(LED_RED);

// Define pin for motor control
DigitalOut directionPin_one(D4);
PwmOut PWM_one(D5);

PwmOut PWM_two(D6);
DigitalOut directionPin_two(D7);

DigitalIn buttonccw(PTA4);
DigitalIn buttoncw(PTC6);

// define ticker
Ticker aansturen;
Ticker Printen;

// define rotation direction and begin possition
const int cw = 1;
const int ccw = 0;
double setpoint_one = 0; //setpoint is in pulses
double setpoint_two = 0; //""               ""

// saying buttons are not pressed
const int Buttoncw_pressed = 0;
const int Buttonccw_pressed = 0;


// Controller gain proportional and intergrator
const double motor1_Kp = 5; // more or les random number.
const double motor1_Ki = 0;
const double M1_timestep = 0.01; // reason ticker works with 100 Hz.
double motor1_error_integraal = 0; // initial value of integral error


// Defining pulses per revolution (calculating pulses to rotations in degree.)
const double pulses_per_revolution = 4200 ;//8400 counts is aangegeven op de motor for x4.  10 - 30 counts oveshoot. for moter 1(tape)! Motor 2 almost the same(nice)
/*
double Rotation = -2; // rotation 
double movement = Rotation * pulses_per_revolution; // times 360 to make Rotations degree.
*/

// defining flags
volatile bool flag_motor = false;
volatile bool flag_pcprint = false;

// making function flags.
void Go_flag_motor()
{
    flag_motor = true;
}

void Go_flag_pcprint()
{
    flag_pcprint = true;
}

// To make a new setpoint 
double making_setpoint(double direction, double setpoint){
    if ( cw == direction){
        setpoint = setpoint + (pulses_per_revolution/400000);
        }
        if ( ccw == direction){
        setpoint = setpoint - (pulses_per_revolution/400000);
        }
    return(setpoint);
    
    }

// Reusable P controller
double PI(double error, const double Kp, const double Ki, double Ts, double &e_int)
{ 

  e_int = e_int + Ts * error;

    double PI_output = (Kp * error) + (Ki * e_int);
    return PI_output;
}
// Next task, measure the error and apply the output to the plant
void motor1_Controller(DigitalOut directionPin, PwmOut PWM, QEI &wheel, double setpoint)
{
    double reference = setpoint; // setpoint is in pulses
    double position = wheel.getPulses();
    double error_pulses = (reference - position); // calculate the error in pulses
    double error_rotation = error_pulses / pulses_per_revolution; //calculate how much the rotaions the motor has to turn
    
    double output = abs(PI( error_rotation, motor1_Kp, motor1_Ki, M1_timestep, motor1_error_integraal ));

    if(error_pulses > 0) {
        directionPin.write(cw);
    
    }
    else if(error_pulses < 0) {
        directionPin.write(ccw); 
    }
    else{
        output = 0;
        }
    
    PWM.write(output); // out of the if loop due to abs output


}

//control for the keypress
void motor2_Controller(DigitalOut directionPin, PwmOut PWM, QEI &wheel, double setpoint)
{
    double reference = setpoint; // setpoint is in pulses
    double position = wheel_two.getPulses();
    double error_pulses = (reference - position); // calculate the error in pulses
    double error_rotation = error_pulses / pulses_per_revolution; //calculate how much the rotaions the motor has to turn
        
    while (fabs(error_pulses) > 40)
    { 
        //kijken = wheel_two.getPulses()/pulses_per_revolution;       
        
        double output = abs(PI( error_rotation, motor1_Kp, motor1_Ki, M1_timestep, motor1_error_integraal ));
    
        if(error_pulses > 0) {
            directionPin.write(cw);
        
        }
        else if(error_pulses < 0) {
            directionPin.write(ccw); 
        }
        else{
            output = 0;
            }
        
        PWM.write(output); // out of the if loop due to abs output
        
        position = wheel.getPulses();
        error_pulses = (reference - position);
        error_rotation = error_pulses / pulses_per_revolution;
    }

}


void counts_showing(double setpoint, QEI &wheel)
{
    
    //double error_pulses = setpoint - wheel_one.getPulses();
    //double kijken = wheel_one.getPulses()/pulses_per_revolution;
    //pc.printf("ref %.2f rounds%.2f error pulses%.2f \n",setpoint_one/pulses_per_revolution,kijken, error_pulses);
    double position = wheel.getPulses();
    double error_pulses = setpoint - position;
    pc.printf("ref %.2f position%.2f error_pulses%.2f \n",setpoint, position, error_pulses);

}

int main()
{
    aansturen.attach( &Go_flag_motor, 0.01f ); // 100 Hz // timer 0.00001f motor keeps spinning
    Printen.attach(&Go_flag_pcprint,0.1f); // 10 Hz // printstatement here because printing cost to much time. the motor void wouldn't be completed
    while( 1 ) {
// to make the motor move
        led_green = 0;
        if(flag_motor) {
            flag_motor = false;
            //motor1_Controller(directionPin_one, PWM_one, wheel_one, setpoint_one);
            //motor1_Controller(directionPin_two, PWM_two, wheel_two, setpoint_two);
        }

        if(flag_pcprint) {
            flag_pcprint = false;
            counts_showing(setpoint_two, wheel_two);
        }

        led_green = 1;
// Pussing buttons to get input s0ignal
        /*
        if(buttoncw.read() == Buttoncw_pressed and buttonccw.read() == Buttonccw_pressed)
        {
            led_red = 0;
            
            
            setpoint_two = setpoint_two + 1050;
            motor2_Controller(directionPin_two, PWM_two, wheel_two, setpoint_two);
            setpoint_two = setpoint_two - 1050;
            motor2_Controller(directionPin_two, PWM_two, wheel_two, setpoint_two);
            
            led_green = 0;
            wait(1);
            led_green = 1;
            
        }*/
        //led_red = 1; 
        if(buttoncw.read() == Buttoncw_pressed)
        {
            setpoint_one = making_setpoint(cw, setpoint_one);
            //setpoint_two = making_setpoint(ccw, setpoint_two);
        }
        
        if(buttonccw.read() == Buttonccw_pressed) 
        {
            setpoint_one =  making_setpoint(ccw, setpoint_one);
            //setpoint_two = making_setpoint(cw, setpoint_two);
        }

    }
}