Biorobotics / Mbed 2 deprecated Controller

Controller futhers (PI)

Dependencies:   QEI mbed

main.cpp@10:3796b6a6c239, 2015-10-27 (annotated)

Committer:
NickDGreg
Date:
Tue Oct 27 14:02:40 2015 +0000
Revision:
10:3796b6a6c239
Parent:
9:ea9d35838861
Child:
11:e2b2ea8a3be8
motor one works, motor two runs in the wrong direction

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