Biorobotics / Mbed 2 deprecated piano_robot

newest version,

Dependencies:   QEI mbed

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?

UserRevisionLine numberNew 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