Biorobotics
/
piano_robot
newest version,
move_motor.cpp
- Committer:
- NickDGreg
- Date:
- 2015-10-23
- Revision:
- 0:fc6fa085d591
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 ); } } }