Biorobotics / Mbed 2 deprecated Controller

Controller futhers (PI)

Dependencies:   QEI mbed

main.cpp@9:ea9d35838861, 2015-10-27 (annotated)

Committer:
NickDGreg
Date:
Tue Oct 27 10:30:54 2015 +0000
Revision:
9:ea9d35838861
Parent:
8:998f4575089b
Child:
10:3796b6a6c239
generalising the function, pwm, direction, the encoder (by reference) and setpoint are now passed to the functions so theoretically can now work for two motors;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yohoo15 0:2b481a8db14f 1 #include "mbed.h"
yohoo15 0:2b481a8db14f 2 #include "QEI.h"
yohoo15 0:2b481a8db14f 3
yohoo15 0:2b481a8db14f 4 Serial pc(USBTX, USBRX);
yohoo15 0:2b481a8db14f 5 QEI wheel (PTC10, PTC11, NC, 624); // Pin for counting (analog in)
yohoo15 0:2b481a8db14f 6
yohoo15 0:2b481a8db14f 7 // Define pin for motor control
NickDGreg 9:ea9d35838861 8 DigitalOut directionPin_one(D4);
NickDGreg 9:ea9d35838861 9 PwmOut PWM_one(D5);
NickDGreg 9:ea9d35838861 10
yohoo15 7:6006a473ea0b 11 DigitalIn buttonccw(PTA4);
yohoo15 7:6006a473ea0b 12 DigitalIn buttoncw(PTC6);
yohoo15 0:2b481a8db14f 13
yohoo15 0:2b481a8db14f 14 // define ticker
yohoo15 0:2b481a8db14f 15 Ticker aansturen;
yohoo15 1:71617831bc4e 16 Ticker Printen;
yohoo15 0:2b481a8db14f 17
yohoo15 7:6006a473ea0b 18 // define rotation direction and begin possition
yohoo15 0:2b481a8db14f 19 const int cw = 1;
yohoo15 0:2b481a8db14f 20 const int ccw = 0;
yohoo15 7:6006a473ea0b 21 double setpoint = 0; //setpoint is in pulses
yohoo15 7:6006a473ea0b 22
yohoo15 7:6006a473ea0b 23 // saying buttons are not pressed
yohoo15 7:6006a473ea0b 24 const int Buttoncw_pressed = 0;
yohoo15 7:6006a473ea0b 25 const int Buttonccw_pressed = 0;
yohoo15 7:6006a473ea0b 26
yohoo15 0:2b481a8db14f 27
yohoo15 3:4abcd40682fa 28 // Controller gain proportional and intergrator
yohoo15 1:71617831bc4e 29 const double motor1_Kp = 5; // more or les random number.
yohoo15 8:998f4575089b 30 const double motor1_Ki = 0;
yohoo15 6:8ab07cce3098 31 const double M1_timestep = 0.01; // reason ticker works with 100 Hz.
yohoo15 3:4abcd40682fa 32 double motor1_error_integraal = 0; // initial value of integral error
yohoo15 0:2b481a8db14f 33
yohoo15 6:8ab07cce3098 34
yohoo15 7:6006a473ea0b 35 // Defining pulses per revolution (calculating pulses to rotations in degree.)
yohoo15 6:8ab07cce3098 36 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)
yohoo15 7:6006a473ea0b 37 /*
yohoo15 8:998f4575089b 38 double Rotation = -2; // rotation
yohoo15 6:8ab07cce3098 39 double movement = Rotation * pulses_per_revolution; // times 360 to make Rotations degree.
yohoo15 7:6006a473ea0b 40 */
yohoo15 0:2b481a8db14f 41
yohoo15 5:d1ab07fd3355 42 // defining flags
yohoo15 5:d1ab07fd3355 43 volatile bool flag_motor = false;
yohoo15 5:d1ab07fd3355 44 volatile bool flag_pcprint = false;
yohoo15 0:2b481a8db14f 45
yohoo15 6:8ab07cce3098 46 // making function flags.
yohoo15 5:d1ab07fd3355 47 void Go_flag_motor()
yohoo15 5:d1ab07fd3355 48 {
yohoo15 5:d1ab07fd3355 49 flag_motor = true;
yohoo15 5:d1ab07fd3355 50 }
yohoo15 5:d1ab07fd3355 51
yohoo15 5:d1ab07fd3355 52 void Go_flag_pcprint()
yohoo15 5:d1ab07fd3355 53 {
yohoo15 5:d1ab07fd3355 54 flag_pcprint = true;
yohoo15 5:d1ab07fd3355 55 }
yohoo15 0:2b481a8db14f 56
yohoo15 7:6006a473ea0b 57 // To make a new setpoint
NickDGreg 9:ea9d35838861 58 double making_setpoint(double direction, double setpoint){
yohoo15 7:6006a473ea0b 59 if ( cw == direction){
NickDGreg 9:ea9d35838861 60 setpoint = setpoint + (pulses_per_revolution/400000);
yohoo15 7:6006a473ea0b 61 }
yohoo15 7:6006a473ea0b 62 if ( ccw == direction){
NickDGreg 9:ea9d35838861 63 setpoint = setpoint - (pulses_per_revolution/400000);
yohoo15 7:6006a473ea0b 64 }
yohoo15 7:6006a473ea0b 65 return(setpoint);
yohoo15 8:998f4575089b 66
yohoo15 7:6006a473ea0b 67 }
yohoo15 7:6006a473ea0b 68
yohoo15 0:2b481a8db14f 69 // Reusable P controller
yohoo15 3:4abcd40682fa 70 double PI(double error, const double Kp, const double Ki, double Ts, double &e_int)
yohoo15 6:8ab07cce3098 71 {
yohoo15 6:8ab07cce3098 72
yohoo15 6:8ab07cce3098 73 e_int = e_int + Ts * error;
yohoo15 6:8ab07cce3098 74
yohoo15 6:8ab07cce3098 75 double PI_output = (Kp * error) + (Ki * e_int);
yohoo15 3:4abcd40682fa 76 return PI_output;
yohoo15 0:2b481a8db14f 77 }
yohoo15 0:2b481a8db14f 78 // Next task, measure the error and apply the output to the plant
NickDGreg 9:ea9d35838861 79 void motor1_Controller(DigitalOut directionPin, PwmOut PWM, QEI &wheel, double setpoint)
yohoo15 0:2b481a8db14f 80 {
yohoo15 7:6006a473ea0b 81 double reference = setpoint; // setpoint is in pulses
yohoo15 0:2b481a8db14f 82 double position = wheel.getPulses();
yohoo15 6:8ab07cce3098 83 double error_pulses = (reference - position); // calculate the error in pulses
yohoo15 6:8ab07cce3098 84 double error_rotation = error_pulses / pulses_per_revolution; //calculate how much the rotaions the motor has to turn
yohoo15 6:8ab07cce3098 85
yohoo15 6:8ab07cce3098 86 double output = abs(PI( error_rotation, motor1_Kp, motor1_Ki, M1_timestep, motor1_error_integraal ));
yohoo15 0:2b481a8db14f 87
yohoo15 6:8ab07cce3098 88 if(error_pulses > 0) {
yohoo15 0:2b481a8db14f 89 directionPin.write(cw);
yohoo15 6:8ab07cce3098 90
yohoo15 6:8ab07cce3098 91 }
yohoo15 6:8ab07cce3098 92 else if(error_pulses < 0) {
yohoo15 6:8ab07cce3098 93 directionPin.write(ccw);
yohoo15 0:2b481a8db14f 94 }
yohoo15 6:8ab07cce3098 95 else{
yohoo15 6:8ab07cce3098 96 output = 0;
yohoo15 6:8ab07cce3098 97 }
yohoo15 6:8ab07cce3098 98 PWM.write(output); // out of the if loop due to abs output
yohoo15 0:2b481a8db14f 99
yohoo15 0:2b481a8db14f 100
yohoo15 0:2b481a8db14f 101 }
yohoo15 0:2b481a8db14f 102
yohoo15 0:2b481a8db14f 103
yohoo15 1:71617831bc4e 104 void counts_showing()
yohoo15 6:8ab07cce3098 105 {
yohoo15 1:71617831bc4e 106
yohoo15 8:998f4575089b 107 double kijken = wheel.getPulses()/pulses_per_revolution;
yohoo15 8:998f4575089b 108 pc.printf("ref %.2f rounds%.2f \n",setpoint/pulses_per_revolution,kijken);
yohoo15 6:8ab07cce3098 109
yohoo15 6:8ab07cce3098 110 }
yohoo15 0:2b481a8db14f 111
yohoo15 0:2b481a8db14f 112 int main()
yohoo15 0:2b481a8db14f 113 {
yohoo15 6:8ab07cce3098 114 aansturen.attach( &Go_flag_motor, 0.01f ); // 100 Hz // timer 0.00001f motor keeps spinning
yohoo15 5:d1ab07fd3355 115 Printen.attach(&Go_flag_pcprint,0.1f); // 10 Hz // printstatement here because printing cost to much time. the motor void wouldn't be completed
yohoo15 5:d1ab07fd3355 116 while( 1 ) {
yohoo15 7:6006a473ea0b 117 // to make the motor move
yohoo15 5:d1ab07fd3355 118 if(flag_motor) {
yohoo15 5:d1ab07fd3355 119 flag_motor = false;
NickDGreg 9:ea9d35838861 120 motor1_Controller(directionPin, PWM, wheel, setpoint);
yohoo15 5:d1ab07fd3355 121 }
yohoo15 5:d1ab07fd3355 122
yohoo15 5:d1ab07fd3355 123 if(flag_pcprint) {
yohoo15 5:d1ab07fd3355 124 flag_pcprint = false;
yohoo15 5:d1ab07fd3355 125 counts_showing();
yohoo15 5:d1ab07fd3355 126 }
yohoo15 5:d1ab07fd3355 127
yohoo15 7:6006a473ea0b 128 // Pussing buttons to get input signal
yohoo15 7:6006a473ea0b 129
yohoo15 7:6006a473ea0b 130 if(buttoncw.read() == Buttoncw_pressed){
NickDGreg 9:ea9d35838861 131 setpoint = making_setpoint(cw, setpoint);
yohoo15 7:6006a473ea0b 132 }
NickDGreg 9:ea9d35838861 133 if(buttonccw.read() == Buttonccw_pressed) {
NickDGreg 9:ea9d35838861 134 setpoint = making_setpoint(ccw, setpoint);
yohoo15 7:6006a473ea0b 135 }
yohoo15 7:6006a473ea0b 136
yohoo15 5:d1ab07fd3355 137 }
yohoo15 0:2b481a8db14f 138 }
yohoo15 0:2b481a8db14f 139
yohoo15 0:2b481a8db14f 140
yohoo15 0:2b481a8db14f 141