Biorobotics / Mbed 2 deprecated Controller

Controller futhers (PI)

Dependencies:   QEI mbed

main.cpp@12:614f1439e679, 2015-10-28 (annotated)

Committer:
NickDGreg
Date:
Wed Oct 28 10:02:33 2015 +0000
Revision:
12:614f1439e679
Parent:
11:e2b2ea8a3be8 
pressing both buttons moves motor two one way and then the other. It does not work as a combination moving left and right as well

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