Biorobotics
/
piano_robot
newest version,
move_motor.cpp@0:fc6fa085d591, 2015-10-23 (annotated)
- Committer:
- NickDGreg
- Date:
- Fri Oct 23 13:14:57 2015 +0000
- Revision:
- 0:fc6fa085d591
move_motor compiles, working on filter. Cannot pass AnalogIn as input, says no default constructor. Cannot pass as pinName as analogIn declared in constructor cannot be seen by methods; ;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
NickDGreg | 0:fc6fa085d591 | 1 | /* |
NickDGreg | 0:fc6fa085d591 | 2 | * move_motor.cpp |
NickDGreg | 0:fc6fa085d591 | 3 | * |
NickDGreg | 0:fc6fa085d591 | 4 | * Created on: Oct 20, 2015 |
NickDGreg | 0:fc6fa085d591 | 5 | * Author: User |
NickDGreg | 0:fc6fa085d591 | 6 | */ |
NickDGreg | 0:fc6fa085d591 | 7 | |
NickDGreg | 0:fc6fa085d591 | 8 | #include "QEI.h" |
NickDGreg | 0:fc6fa085d591 | 9 | #include "mbed.h" |
NickDGreg | 0:fc6fa085d591 | 10 | |
NickDGreg | 0:fc6fa085d591 | 11 | |
NickDGreg | 0:fc6fa085d591 | 12 | |
NickDGreg | 0:fc6fa085d591 | 13 | //define the pins for the left and right |
NickDGreg | 0:fc6fa085d591 | 14 | |
NickDGreg | 0:fc6fa085d591 | 15 | // Defining pulses per revolution (calculating pulses to rotations in degree.) |
NickDGreg | 0:fc6fa085d591 | 16 | 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) |
NickDGreg | 0:fc6fa085d591 | 17 | const double timestep = 0.01; // reason ticker works with 100 Hz. ASSUMING TIME STEP IS THE SAME FOR BOTH MOTORS |
NickDGreg | 0:fc6fa085d591 | 18 | |
NickDGreg | 0:fc6fa085d591 | 19 | //we dont we Pi control so this can remain 0, for now |
NickDGreg | 0:fc6fa085d591 | 20 | double motor_error_integral = 0; |
NickDGreg | 0:fc6fa085d591 | 21 | |
NickDGreg | 0:fc6fa085d591 | 22 | // define rotation direction and begin position |
NickDGreg | 0:fc6fa085d591 | 23 | const int cw = 1; |
NickDGreg | 0:fc6fa085d591 | 24 | const int ccw = 0; |
NickDGreg | 0:fc6fa085d591 | 25 | |
NickDGreg | 0:fc6fa085d591 | 26 | // define ticker |
NickDGreg | 0:fc6fa085d591 | 27 | Ticker aansturen; |
NickDGreg | 0:fc6fa085d591 | 28 | |
NickDGreg | 0:fc6fa085d591 | 29 | // defining flags |
NickDGreg | 0:fc6fa085d591 | 30 | volatile bool flag_motor = false; |
NickDGreg | 0:fc6fa085d591 | 31 | |
NickDGreg | 0:fc6fa085d591 | 32 | |
NickDGreg | 0:fc6fa085d591 | 33 | // making function flags. |
NickDGreg | 0:fc6fa085d591 | 34 | void Go_flag_motor() |
NickDGreg | 0:fc6fa085d591 | 35 | { |
NickDGreg | 0:fc6fa085d591 | 36 | flag_motor = true; |
NickDGreg | 0:fc6fa085d591 | 37 | } |
NickDGreg | 0:fc6fa085d591 | 38 | |
NickDGreg | 0:fc6fa085d591 | 39 | |
NickDGreg | 0:fc6fa085d591 | 40 | // defining reusable P controller |
NickDGreg | 0:fc6fa085d591 | 41 | double PI(double error, const double Kp, const double Ki, double Ts, double &e_int) |
NickDGreg | 0:fc6fa085d591 | 42 | { |
NickDGreg | 0:fc6fa085d591 | 43 | |
NickDGreg | 0:fc6fa085d591 | 44 | e_int = e_int + Ts * error; |
NickDGreg | 0:fc6fa085d591 | 45 | |
NickDGreg | 0:fc6fa085d591 | 46 | double PI_output = (Kp * error) + (Ki * e_int); |
NickDGreg | 0:fc6fa085d591 | 47 | return PI_output; |
NickDGreg | 0:fc6fa085d591 | 48 | } |
NickDGreg | 0:fc6fa085d591 | 49 | |
NickDGreg | 0:fc6fa085d591 | 50 | |
NickDGreg | 0:fc6fa085d591 | 51 | // Measure the error and apply the output to the plant |
NickDGreg | 0:fc6fa085d591 | 52 | |
NickDGreg | 0:fc6fa085d591 | 53 | //How to pass Digitalout and PWM? |
NickDGreg | 0:fc6fa085d591 | 54 | void motor_Controller( DigitalOut &directionPin, PwmOut &pwm, double position, double setpoint, double Kp, double Ki, double error_rotation, double error_pulses) |
NickDGreg | 0:fc6fa085d591 | 55 | { |
NickDGreg | 0:fc6fa085d591 | 56 | |
NickDGreg | 0:fc6fa085d591 | 57 | double output = abs(PI( error_rotation, Kp, Ki, timestep, motor_error_integral )); |
NickDGreg | 0:fc6fa085d591 | 58 | |
NickDGreg | 0:fc6fa085d591 | 59 | // 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 |
NickDGreg | 0:fc6fa085d591 | 60 | if(error_pulses > 0) |
NickDGreg | 0:fc6fa085d591 | 61 | { |
NickDGreg | 0:fc6fa085d591 | 62 | directionPin.write(cw); |
NickDGreg | 0:fc6fa085d591 | 63 | } |
NickDGreg | 0:fc6fa085d591 | 64 | |
NickDGreg | 0:fc6fa085d591 | 65 | else if(error_pulses < 0) |
NickDGreg | 0:fc6fa085d591 | 66 | { |
NickDGreg | 0:fc6fa085d591 | 67 | directionPin.write(ccw); |
NickDGreg | 0:fc6fa085d591 | 68 | } |
NickDGreg | 0:fc6fa085d591 | 69 | //else output = 0 |
NickDGreg | 0:fc6fa085d591 | 70 | |
NickDGreg | 0:fc6fa085d591 | 71 | // out of the if loop due to abs output |
NickDGreg | 0:fc6fa085d591 | 72 | pwm.write(output); |
NickDGreg | 0:fc6fa085d591 | 73 | } |
NickDGreg | 0:fc6fa085d591 | 74 | |
NickDGreg | 0:fc6fa085d591 | 75 | |
NickDGreg | 0:fc6fa085d591 | 76 | |
NickDGreg | 0:fc6fa085d591 | 77 | void move_motor(DigitalOut &directionPin, PwmOut &pwm, double position, double setpoint, double Kp, double Ki) //state can be left, right, or keypress |
NickDGreg | 0:fc6fa085d591 | 78 | { |
NickDGreg | 0:fc6fa085d591 | 79 | //attach ticker to run the motor at 100hz |
NickDGreg | 0:fc6fa085d591 | 80 | aansturen.attach( &Go_flag_motor, 0.01f ); // 100 Hz // timer 0.00001f motor keeps spinning // attaching the flag to the ticker |
NickDGreg | 0:fc6fa085d591 | 81 | |
NickDGreg | 0:fc6fa085d591 | 82 | //error rotation initialised to high value to enter the loop, control works until error is a small enough value to ignore |
NickDGreg | 0:fc6fa085d591 | 83 | double error_rotation = 1; |
NickDGreg | 0:fc6fa085d591 | 84 | |
NickDGreg | 0:fc6fa085d591 | 85 | //run the control loop until the error is acceptable (3mm) |
NickDGreg | 0:fc6fa085d591 | 86 | //nEED TO CALCULATE THE VALUE |
NickDGreg | 0:fc6fa085d591 | 87 | while (error_rotation > 0.1) //error is not between -0.05 and 0.05 or something error should be a maximum of 3 mm |
NickDGreg | 0:fc6fa085d591 | 88 | { |
NickDGreg | 0:fc6fa085d591 | 89 | if(flag_motor) |
NickDGreg | 0:fc6fa085d591 | 90 | { |
NickDGreg | 0:fc6fa085d591 | 91 | flag_motor = false; |
NickDGreg | 0:fc6fa085d591 | 92 | |
NickDGreg | 0:fc6fa085d591 | 93 | //calculate the errors here so there is a way for the motor to stop |
NickDGreg | 0:fc6fa085d591 | 94 | //could return it from the control function? |
NickDGreg | 0:fc6fa085d591 | 95 | double error_pulses = (setpoint - position); // calculate the error in pulses |
NickDGreg | 0:fc6fa085d591 | 96 | |
NickDGreg | 0:fc6fa085d591 | 97 | error_rotation = error_pulses / pulses_per_revolution; //calculate how much the rotations the motor has to turn |
NickDGreg | 0:fc6fa085d591 | 98 | |
NickDGreg | 0:fc6fa085d591 | 99 | motor_Controller(directionPin, pwm, position, setpoint, Kp, Ki, error_rotation, error_pulses ); |
NickDGreg | 0:fc6fa085d591 | 100 | } |
NickDGreg | 0:fc6fa085d591 | 101 | } |
NickDGreg | 0:fc6fa085d591 | 102 | } |
NickDGreg | 0:fc6fa085d591 | 103 | |
NickDGreg | 0:fc6fa085d591 | 104 | |
NickDGreg | 0:fc6fa085d591 | 105 |