Biorobotics / Mbed 2 deprecated piano_robot

newest version,

Dependencies:   QEI mbed

move_motor.cpp@3:11c2175b4478, 2015-10-26 (annotated)

Committer:
roosbulthuis
Date:
Mon Oct 26 11:42:17 2015 +0000
Revision:
3:11c2175b4478
Parent:
0:fc6fa085d591 
New file for filters without classes, check if input and output are correct. Should be pasted into main code.

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