File content as of revision 0:fc6fa085d591:

/*
 * move_motor.cpp
 *
 *  Created on: Oct 20, 2015
 *      Author: User
 */

#include "QEI.h"
#include "mbed.h"



//define the pins for the left and right

// 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)
const double timestep = 0.01; // reason ticker works with 100 Hz. ASSUMING TIME STEP IS THE SAME FOR BOTH MOTORS

//we dont we Pi control so this can remain 0, for now
double motor_error_integral = 0;

// define rotation direction and begin position
const int cw = 1;
const int ccw = 0;

// define ticker
Ticker aansturen;

// defining flags
volatile bool flag_motor = false;


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


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


// Measure the error and apply the output to the plant

//How to pass Digitalout and PWM?
void motor_Controller( DigitalOut &directionPin, PwmOut &pwm, double position, double setpoint, double Kp, double Ki, double error_rotation, double error_pulses) 
{

    double output = abs(PI( error_rotation, Kp, Ki, timestep, motor_error_integral ));

// MOTORS ROTATE IN DIFFERENT DIRECTIONS WITH THE SAME DIRECTION INPUT SO WE NEED TO CHANGE THE ENCODER WIRES. CHANGING IN CODE IS TRICKIER. WONT BE UNIVERSAL
    if(error_pulses > 0) 
    {
        directionPin.write(cw);
    }
    
    else if(error_pulses < 0) 
    {
        directionPin.write(ccw);
    }
    //else output = 0

    // out of the if loop due to abs output
    pwm.write(output);
}



void move_motor(DigitalOut &directionPin, PwmOut &pwm, double position, double setpoint, double Kp, double Ki) //state can be left, right, or keypress
{
	//attach ticker to run the motor at 100hz
	aansturen.attach( &Go_flag_motor, 0.01f ); // 100 Hz // timer 0.00001f motor keeps spinning // attaching the flag to the ticker
	
	//error rotation initialised to high value to enter the loop, control works until error is a small enough value to ignore
	double error_rotation = 1;
	
	//run the control loop until the error is acceptable (3mm)
	//nEED TO CALCULATE THE VALUE
	while (error_rotation > 0.1) //error is not between -0.05 and 0.05 or something error should be a maximum of 3 mm
	{
		if(flag_motor)
		{
			flag_motor = false;
			
			//calculate the errors here so there is a way for the motor to stop
			//could return it from the control function?
			double error_pulses = (setpoint - position); // calculate the error in pulses

    		error_rotation = error_pulses / pulses_per_revolution; //calculate how much the rotations the motor has to turn
    
			motor_Controller(directionPin, pwm, position, setpoint, Kp, Ki, error_rotation, error_pulses );
		}
	}
}