move_motor.cpp

00001 /*
00002  * move_motor.cpp
00003  *
00004  *  Created on: Oct 20, 2015
00005  *      Author: User
00006  */
00007 
00008 #include "QEI.h"
00009 #include "mbed.h"
00010 
00011 
00012 
00013 //define the pins for the left and right
00014 
00015 // Defining pulses per revolution (calculating pulses to rotations in degree.)
00016 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)
00017 const double timestep = 0.01; // reason ticker works with 100 Hz. ASSUMING TIME STEP IS THE SAME FOR BOTH MOTORS
00018 
00019 //we dont we Pi control so this can remain 0, for now
00020 double motor_error_integral = 0;
00021 
00022 // define rotation direction and begin position
00023 const int cw = 1;
00024 const int ccw = 0;
00025 
00026 // define ticker
00027 Ticker aansturen;
00028 
00029 // defining flags
00030 volatile bool flag_motor = false;
00031 
00032 
00033 // making function flags.
00034 void Go_flag_motor()
00035 {
00036     flag_motor = true;
00037 }
00038 
00039 
00040 // defining reusable P controller
00041 double PI(double error, const double Kp, const double Ki, double Ts, double &e_int)
00042 {
00043 
00044   e_int = e_int + Ts * error;
00045 
00046     double PI_output = (Kp * error) + (Ki * e_int);
00047     return PI_output;
00048 }
00049 
00050 
00051 // Measure the error and apply the output to the plant
00052 
00053 //How to pass Digitalout and PWM?
00054 void motor_Controller( DigitalOut &directionPin, PwmOut &pwm, double position, double setpoint, double Kp, double Ki, double error_rotation, double error_pulses) 
00055 {
00056 
00057     double output = abs(PI( error_rotation, Kp, Ki, timestep, motor_error_integral ));
00058 
00059 // 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
00060     if(error_pulses > 0) 
00061     {
00062         directionPin.write(cw);
00063     }
00064     
00065     else if(error_pulses < 0) 
00066     {
00067         directionPin.write(ccw);
00068     }
00069     //else output = 0
00070 
00071     // out of the if loop due to abs output
00072     pwm.write(output);
00073 }
00074 
00075 
00076 
00077 void move_motor(DigitalOut &directionPin, PwmOut &pwm, double position, double setpoint, double Kp, double Ki) //state can be left, right, or keypress
00078 {
00079     //attach ticker to run the motor at 100hz
00080     aansturen.attach( &Go_flag_motor, 0.01f ); // 100 Hz // timer 0.00001f motor keeps spinning // attaching the flag to the ticker
00081     
00082     //error rotation initialised to high value to enter the loop, control works until error is a small enough value to ignore
00083     double error_rotation = 1;
00084     
00085     //run the control loop until the error is acceptable (3mm)
00086     //nEED TO CALCULATE THE VALUE
00087     while (error_rotation > 0.1) //error is not between -0.05 and 0.05 or something error should be a maximum of 3 mm
00088     {
00089         if(flag_motor)
00090         {
00091             flag_motor = false;
00092             
00093             //calculate the errors here so there is a way for the motor to stop
00094             //could return it from the control function?
00095             double error_pulses = (setpoint - position); // calculate the error in pulses
00096 
00097             error_rotation = error_pulses / pulses_per_revolution; //calculate how much the rotations the motor has to turn
00098     
00099             motor_Controller(directionPin, pwm, position, setpoint, Kp, Ki, error_rotation, error_pulses );
00100         }
00101     }
00102 }
00103 
00104 
00105