Biorobotics / Mbed 2 deprecated Controller

Controller futhers (PI)

Dependencies:   QEI mbed

main.cpp@7:6006a473ea0b, 2015-10-15 (annotated)

Committer:
yohoo15
Date:
Thu Oct 15 13:39:59 2015 +0000
Revision:
7:6006a473ea0b
Parent:
6:8ab07cce3098
Child:
8:998f4575089b
Letting the controller move with buttons. not yet tested;

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