Daniqe Kottelenberg / Mbed 2 deprecated Wubs_demo

met knopjes, voor Wubs, zit PID in dus restrictie.

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

Fork of a_pid_kal_end_def by Floor Couwenberg

main.cpp@60:c165691c4e86, 2016-11-14 (annotated)

Committer:
daniQQue
Date:
Mon Nov 14 12:13:38 2016 +0000
Revision:
60:c165691c4e86
Parent:
59:1725a3f02f37 
met buttons

Who changed what in which revision?

UserRevisionLine numberNew contents of line
daniQQue 57:c546edf67c5c 1 //=======================================================================================================================================================
daniQQue 0:34c739fcc3e0 2 //libraries
daniQQue 49:818a0e90ed9c 3 #include "mbed.h" //mbed revision 113
daniQQue 49:818a0e90ed9c 4 #include "HIDScope.h" //Hidscope by Tom Lankhorst
daniQQue 49:818a0e90ed9c 5 #include "BiQuad.h" //BiQuad by Tom Lankhorst
daniQQue 49:818a0e90ed9c 6 #include "MODSERIAL.h" //Modserial
sivuu 50:2c03357de7cc 7 #include "QEI.h" //QEI library for the encoders
daniQQue 0:34c739fcc3e0 8
daniQQue 46:4a8889f9dc9f 9
daniQQue 57:c546edf67c5c 10 //=======================================================================================================================================================
daniQQue 0:34c739fcc3e0 11 //Define objects
daniQQue 49:818a0e90ed9c 12 //Tickers
sivuu 50:2c03357de7cc 13 Ticker ticker_referenceangle; //ticker for the reference angle
sivuu 50:2c03357de7cc 14 Ticker ticker_controllerm1; //ticker for the controller (PID) of motor 1
daniQQue 57:c546edf67c5c 15 Ticker ticker_encoder; //ticker for encoderfunction motor 1
FloorC 55:d742332ced11 16
FloorC 55:d742332ced11 17 //Timer
FloorC 55:d742332ced11 18 Timer timer;
daniQQue 49:818a0e90ed9c 19
daniQQue 49:818a0e90ed9c 20 //Monitoring
daniQQue 46:4a8889f9dc9f 21 MODSERIAL pc(USBTX, USBRX); //pc connection
daniQQue 49:818a0e90ed9c 22 DigitalOut red(LED_RED); //LED on K64F board, 1 is out; 0 is on
daniQQue 49:818a0e90ed9c 23 DigitalOut green(LED_GREEN); //LED on K64f board, 1 is out; o is on
daniQQue 57:c546edf67c5c 24
daniQQue 57:c546edf67c5c 25 //buttons
daniQQue 60:c165691c4e86 26 DigitalIn button__right_biceps(D9);
daniQQue 60:c165691c4e86 27 DigitalIn button__left_biceps(PTC12);
daniQQue 60:c165691c4e86 28 InterruptIn button_switch(SW3);
daniQQue 40:187ef29de53d 29
daniQQue 40:187ef29de53d 30 //motors
FloorC 59:1725a3f02f37 31 DigitalOut richting_motor1(D7); //motor 1 connected to motor 1 at k64f board; for turningtable
daniQQue 47:ddaa59d48aca 32 PwmOut pwm_motor1(D6);
FloorC 59:1725a3f02f37 33 DigitalOut richting_motor2(D4); //motor 2 connected to motor 2 at k64f board; for linear actuator
daniQQue 47:ddaa59d48aca 34 PwmOut pwm_motor2(D5);
daniQQue 0:34c739fcc3e0 35
sivuu 50:2c03357de7cc 36 //encoders
sivuu 50:2c03357de7cc 37 DigitalIn encoder1A(D13);
sivuu 50:2c03357de7cc 38 DigitalIn encoder1B(D12);
sivuu 50:2c03357de7cc 39
sivuu 50:2c03357de7cc 40 //controller
FloorC 59:1725a3f02f37 41 BiQuad PID_controller;
sivuu 50:2c03357de7cc 42
daniQQue 57:c546edf67c5c 43 //=======================================================================================================================================================
daniQQue 0:34c739fcc3e0 44 //define variables
daniQQue 47:ddaa59d48aca 45
daniQQue 49:818a0e90ed9c 46 //on/off and switch signals
FloorC 59:1725a3f02f37 47 int switch_signal = 0; //start of counter, switch made by even and odd numbers
daniQQue 46:4a8889f9dc9f 48
daniQQue 49:818a0e90ed9c 49 //motorvariables
FloorC 59:1725a3f02f37 50 float speedmotor1=0.18; //speed of motor 1 is 0.18 pwm at start
daniQQue 49:818a0e90ed9c 51 float speedmotor2=1.0; //speed of motor 2 is 1.0 pwm at start
daniQQue 46:4a8889f9dc9f 52
daniQQue 49:818a0e90ed9c 53 int cw=0; //clockwise direction
daniQQue 49:818a0e90ed9c 54 int ccw=1; //counterclockwise direction
sivuu 50:2c03357de7cc 55
sivuu 50:2c03357de7cc 56 //encoder
FloorC 59:1725a3f02f37 57 int counts_encoder1; //variable to count the pulses given by the encoder, 1 indicates motor 1
FloorC 59:1725a3f02f37 58 float rev_counts_motor1; //calculated revolutions
daniQQue 57:c546edf67c5c 59 float rev_counts_motor1_rad; //calculated revolutions in rad!
FloorC 59:1725a3f02f37 60 const float gearboxratio=131.25; //gearboxratio from encoder to motor
FloorC 59:1725a3f02f37 61 const float rev_rond=64.0; //number of revolutions per rotation
daniQQue 57:c546edf67c5c 62
FloorC 59:1725a3f02f37 63 QEI Encoder1(D13,D12,NC,rev_rond,QEI::X4_ENCODING); //To set the encoder
sivuu 50:2c03357de7cc 64
sivuu 50:2c03357de7cc 65 //reference
FloorC 59:1725a3f02f37 66 volatile float d_ref = 0; //reference angle, starts off 0
FloorC 59:1725a3f02f37 67 const float w_ref = 1.5; //reference speed, constant
FloorC 59:1725a3f02f37 68 volatile double t_start; //starttime of the timer
FloorC 59:1725a3f02f37 69 volatile double w_var; //variable reference speed for making the reference signal
FloorC 59:1725a3f02f37 70 const double Ts = 0.001; //time step for diverse tickers. It is comparable to a frequency of 1000Hz
sivuu 50:2c03357de7cc 71
sivuu 50:2c03357de7cc 72 //controller
FloorC 59:1725a3f02f37 73 const double Kp = 1.2614; //calculated value for the proportional action of the PID
FloorC 59:1725a3f02f37 74 const double Ki = 0.4219; //calculated value for the integral action of the PID
FloorC 59:1725a3f02f37 75 const double Kd = 0.8312; //calculated value for the derivative action of the PID
FloorC 59:1725a3f02f37 76 const double N = 100; //specified value for the filter coefficient of the PID
FloorC 59:1725a3f02f37 77 volatile double error1; //calculated error
FloorC 59:1725a3f02f37 78 volatile double controlOutput; //output of the PID-controller
FloorC 59:1725a3f02f37 79 bool start_motor = true; //bool to start the reference when the motor turns
FloorC 59:1725a3f02f37 80
daniQQue 57:c546edf67c5c 81
daniQQue 57:c546edf67c5c 82 //=======================================================================================================================================================
daniQQue 57:c546edf67c5c 83 //=======================================================================================================================================================
daniQQue 45:08bddea67bd8 84 //voids
daniQQue 57:c546edf67c5c 85 //=======================================================================================================================================================
daniQQue 45:08bddea67bd8 86
daniQQue 40:187ef29de53d 87 //function teller
FloorC 59:1725a3f02f37 88 void switch_function() { // The switch function. Makes it possible to switch between the motors. It simply adds one at switch_signal.
daniQQue 60:c165691c4e86 89 if(button_switch==0){
daniQQue 45:08bddea67bd8 90 switch_signal++;
daniQQue 49:818a0e90ed9c 91
FloorC 59:1725a3f02f37 92 // To monitor what is happening: we will show the text in putty and change led color from red to green or vice versa.
daniQQue 49:818a0e90ed9c 93
daniQQue 40:187ef29de53d 94 green=!green;
daniQQue 40:187ef29de53d 95 red=!red;
daniQQue 47:ddaa59d48aca 96
FloorC 59:1725a3f02f37 97 if (switch_signal%2==0){
daniQQue 60:c165691c4e86 98 pc.printf("If you push button 1 , the robot will go right \r\n");
daniQQue 60:c165691c4e86 99 pc.printf("If you push button 2, the robot will go left \r\n");
FloorC 59:1725a3f02f37 100 pc.printf("\r\n");
FloorC 59:1725a3f02f37 101 }
daniQQue 47:ddaa59d48aca 102
daniQQue 47:ddaa59d48aca 103
FloorC 59:1725a3f02f37 104 else{
daniQQue 60:c165691c4e86 105 pc.printf("If you push button 1, the robot will go up \r\n");
daniQQue 60:c165691c4e86 106 pc.printf("If you push button 2, the robot will go down \r\n");
FloorC 59:1725a3f02f37 107 pc.printf("\r\n");
FloorC 59:1725a3f02f37 108 }
daniQQue 47:ddaa59d48aca 109
daniQQue 40:187ef29de53d 110 }
FloorC 59:1725a3f02f37 111 }
daniQQue 60:c165691c4e86 112 //=======================================================================================================================================================
daniQQue 57:c546edf67c5c 113
daniQQue 57:c546edf67c5c 114 //reference void makes the reference that the controllor should follow. There is only a controller for motor 1.
FloorC 59:1725a3f02f37 115 void reference(){
FloorC 59:1725a3f02f37 116 if (start_motor == true){ //bool that is true when the motor starts turning
FloorC 59:1725a3f02f37 117 timer.start(); //timer that starts counting in milliseconds
sivuu 50:2c03357de7cc 118 }
daniQQue 60:c165691c4e86 119 if (button__left_biceps==0 && switch_signal%2==0){ //the signal of the biceps is -1 and the switch is even, so motor 1 is being controlled
FloorC 59:1725a3f02f37 120 t_start = timer.read_ms(); //read the current time passed from the timer
FloorC 59:1725a3f02f37 121 start_motor = false; //it means that the motor is not running or has started up
FloorC 55:d742332ced11 122
FloorC 59:1725a3f02f37 123 if (t_start < 1.0){ //the time passed is less than one second
FloorC 59:1725a3f02f37 124 w_var = t_start*1.5; //the reference velocity is the time passed multiplied with the eventual constant velocity it should reach
FloorC 59:1725a3f02f37 125 }
FloorC 55:d742332ced11 126
FloorC 59:1725a3f02f37 127 else{
FloorC 59:1725a3f02f37 128 w_var = 1.5; //if the time passed is more than one second, the velocity is constant
FloorC 59:1725a3f02f37 129 }
FloorC 59:1725a3f02f37 130
FloorC 59:1725a3f02f37 131 d_ref = d_ref + w_var * Ts; //makes the reference angle
FloorC 59:1725a3f02f37 132
FloorC 59:1725a3f02f37 133 }
FloorC 59:1725a3f02f37 134 if (d_ref > 12){ //set the restrictions
FloorC 59:1725a3f02f37 135 d_ref = 12;
FloorC 59:1725a3f02f37 136 start_motor = true; //after the restriction is reached the motor (if turned the other way) will start up again so the bool has to be set to true
FloorC 55:d742332ced11 137 }
FloorC 55:d742332ced11 138
sivuu 50:2c03357de7cc 139 else{
FloorC 59:1725a3f02f37 140 d_ref = d_ref; //if there is no signal, the referance angle is constant
sivuu 50:2c03357de7cc 141 }
sivuu 50:2c03357de7cc 142
daniQQue 60:c165691c4e86 143 if (button__right_biceps==0 && switch_signal%2==0){ //the signal of the biceps is -1 and the switch is even, so motor 1 is being controlled
FloorC 55:d742332ced11 144 t_start = timer.read_ms();
FloorC 55:d742332ced11 145 start_motor = false;
FloorC 55:d742332ced11 146
FloorC 55:d742332ced11 147 if (t_start < 1.0){
FloorC 55:d742332ced11 148 w_var = t_start*1.5;
FloorC 59:1725a3f02f37 149 }
FloorC 59:1725a3f02f37 150
FloorC 55:d742332ced11 151 else {
FloorC 55:d742332ced11 152 w_var = 1.5;
FloorC 59:1725a3f02f37 153 }
FloorC 59:1725a3f02f37 154 d_ref = d_ref - w_var * Ts; //the motor should turn the other way now so the reference becomes negative
sivuu 50:2c03357de7cc 155 }
FloorC 59:1725a3f02f37 156
FloorC 59:1725a3f02f37 157 if (d_ref < -12){ //negative restriction
FloorC 59:1725a3f02f37 158 d_ref = -12;
FloorC 59:1725a3f02f37 159 start_motor = true;
FloorC 55:d742332ced11 160 }
FloorC 59:1725a3f02f37 161
sivuu 50:2c03357de7cc 162 else{
sivuu 50:2c03357de7cc 163 d_ref = d_ref;
sivuu 50:2c03357de7cc 164 }
sivuu 50:2c03357de7cc 165
sivuu 50:2c03357de7cc 166 }
daniQQue 57:c546edf67c5c 167 //=======================================================================================================================================================
daniQQue 57:c546edf67c5c 168 //This void calculates the error and makes the control output.
sivuu 50:2c03357de7cc 169 void m1_controller(){
FloorC 59:1725a3f02f37 170 error1 = d_ref-rev_counts_motor1_rad; //calculate the error = reference-output
FloorC 59:1725a3f02f37 171 controlOutput = PID_controller.step(error1); //give the error as input to the controller
sivuu 50:2c03357de7cc 172 }
daniQQue 57:c546edf67c5c 173 //=======================================================================================================================================================
sivuu 50:2c03357de7cc 174
daniQQue 57:c546edf67c5c 175 //This void calculated the number of rotations that the motor has done in rad. It is put in a void because with the ticker, this ensures that it is updated continuously.
FloorC 55:d742332ced11 176 void encoders(){
FloorC 55:d742332ced11 177 counts_encoder1 = Encoder1.getPulses();
FloorC 55:d742332ced11 178 rev_counts_motor1 = (float)counts_encoder1/(gearboxratio*rev_rond);
daniQQue 60:c165691c4e86 179 rev_counts_motor1_rad = rev_counts_motor1*6.28318530718;
daniQQue 60:c165691c4e86 180
FloorC 55:d742332ced11 181 }
daniQQue 57:c546edf67c5c 182 //=======================================================================================================================================================
daniQQue 0:34c739fcc3e0 183 //program
daniQQue 57:c546edf67c5c 184 //=======================================================================================================================================================
FloorC 59:1725a3f02f37 185 int main(){
daniQQue 49:818a0e90ed9c 186
FloorC 59:1725a3f02f37 187 pc.baud(115200); //connect with pc with baudrate 115200
FloorC 59:1725a3f02f37 188 green=1; //led is off (1), at beginning
FloorC 59:1725a3f02f37 189 red=0; //led is on (0), at beginning
FloorC 59:1725a3f02f37 190
FloorC 59:1725a3f02f37 191 //attach tickers to functions
FloorC 59:1725a3f02f37 192 ticker_referenceangle.attach(&reference, Ts);
FloorC 59:1725a3f02f37 193 ticker_controllerm1.attach(&m1_controller, Ts);
FloorC 59:1725a3f02f37 194 ticker_encoder.attach(&encoders, Ts);
daniQQue 60:c165691c4e86 195
FloorC 59:1725a3f02f37 196 //PID controller
FloorC 59:1725a3f02f37 197 PID_controller.PIDF(Kp,Ki,Kd,N,Ts);
FloorC 59:1725a3f02f37 198
FloorC 59:1725a3f02f37 199 //Encoder
FloorC 59:1725a3f02f37 200 //QEI Encoder1(D13,D12, NC, rev_rond,QEI::X4_ENCODING);
FloorC 59:1725a3f02f37 201
FloorC 59:1725a3f02f37 202 //Show the user what the starting motor will be and what will happen
FloorC 59:1725a3f02f37 203 pc.printf("We will start the demonstration\r\n");
FloorC 59:1725a3f02f37 204 pc.printf("\r\n\r\n\r\n");
daniQQue 44:c79e5944ac91 205
FloorC 59:1725a3f02f37 206 if (switch_signal%2==0){
FloorC 59:1725a3f02f37 207 pc.printf("If you contract the biceps, the robot will go right \r\n");
FloorC 59:1725a3f02f37 208 pc.printf("If you contract the triceps, the robot will go left \r\n");
FloorC 59:1725a3f02f37 209 pc.printf("\r\n");
FloorC 59:1725a3f02f37 210 }
daniQQue 47:ddaa59d48aca 211
daniQQue 47:ddaa59d48aca 212
FloorC 59:1725a3f02f37 213 else{
FloorC 59:1725a3f02f37 214 pc.printf("If you contract the biceps, the robot will go up \r\n");
FloorC 59:1725a3f02f37 215 pc.printf("If you contract the triceps, the robot will go down \r\n");
FloorC 59:1725a3f02f37 216 pc.printf("\r\n");
FloorC 59:1725a3f02f37 217 }
daniQQue 45:08bddea67bd8 218
daniQQue 57:c546edf67c5c 219 //=======================================================================================================================================================
daniQQue 0:34c739fcc3e0 220 //endless loop
daniQQue 0:34c739fcc3e0 221
daniQQue 40:187ef29de53d 222
FloorC 59:1725a3f02f37 223 while (true) { //neverending loop
daniQQue 60:c165691c4e86 224 button_switch.fall(&switch_function);
daniQQue 60:c165691c4e86 225 if (button__left_biceps==0){ //left biceps contracted
FloorC 56:a38412383477 226
FloorC 59:1725a3f02f37 227 if (switch_signal%2==0){ //switch even
FloorC 56:a38412383477 228
FloorC 59:1725a3f02f37 229 speedmotor1=controlOutput; //output PID-controller is the speed for motor1
daniQQue 57:c546edf67c5c 230
FloorC 59:1725a3f02f37 231 if (speedmotor1<0){ //if the output of the controller is negative, the direction is clockwise
FloorC 59:1725a3f02f37 232 richting_motor1 = cw; //motor 1, right
sivuu 51:b344a92b6a5f 233 }
FloorC 59:1725a3f02f37 234 else { //if the output is positive, the direction is counterclockwise
FloorC 59:1725a3f02f37 235 richting_motor1 = ccw; //motor 1, left
sivuu 51:b344a92b6a5f 236 }
FloorC 59:1725a3f02f37 237 pwm_motor1 = fabs(speedmotor1); //speed of motor 1, absolute because pwm cannot be negative
daniQQue 57:c546edf67c5c 238
FloorC 59:1725a3f02f37 239 }
sivuu 51:b344a92b6a5f 240
daniQQue 40:187ef29de53d 241
FloorC 59:1725a3f02f37 242 else{ //switch odd
FloorC 59:1725a3f02f37 243
FloorC 59:1725a3f02f37 244 richting_motor2 = ccw; //motor 2, up
FloorC 59:1725a3f02f37 245 pwm_motor2 = speedmotor2; //speed of motor 2
daniQQue 40:187ef29de53d 246
FloorC 59:1725a3f02f37 247 }
daniQQue 40:187ef29de53d 248
FloorC 59:1725a3f02f37 249 }
daniQQue 60:c165691c4e86 250 else if (button__right_biceps==0){ //right biceps contracted
FloorC 59:1725a3f02f37 251
FloorC 59:1725a3f02f37 252 if (switch_signal%2==0){ //switch signal even
FloorC 59:1725a3f02f37 253 speedmotor1=controlOutput;
daniQQue 57:c546edf67c5c 254
FloorC 59:1725a3f02f37 255 if (speedmotor1<0){ //the same as for the left biceps, the robot turns in the right direction because of the reference signal
FloorC 59:1725a3f02f37 256 richting_motor1 = cw; //motor 1, right
sivuu 51:b344a92b6a5f 257 }
FloorC 59:1725a3f02f37 258 else {
FloorC 59:1725a3f02f37 259 richting_motor1 = ccw; //motor 1, left
sivuu 51:b344a92b6a5f 260 }
sivuu 51:b344a92b6a5f 261 pwm_motor1 = fabs(speedmotor1); //speed of motor 1
daniQQue 57:c546edf67c5c 262
FloorC 59:1725a3f02f37 263 }
FloorC 59:1725a3f02f37 264 else{ //switch signal odd
FloorC 59:1725a3f02f37 265 richting_motor2 = cw; //motor 2, down
FloorC 59:1725a3f02f37 266 pwm_motor2 = speedmotor2; //speed motor 2
daniQQue 40:187ef29de53d 267
FloorC 59:1725a3f02f37 268 }
FloorC 59:1725a3f02f37 269 }
FloorC 59:1725a3f02f37 270 else{
FloorC 59:1725a3f02f37 271 //no contraction of biceps, thus no motoraction.
FloorC 59:1725a3f02f37 272 pwm_motor2=0;
FloorC 59:1725a3f02f37 273 pwm_motor1=0;
FloorC 59:1725a3f02f37 274 start_motor = true; //every time the motor is off, the bool is reset so that the reference void can start when the motor starts
FloorC 56:a38412383477 275
FloorC 59:1725a3f02f37 276 }
daniQQue 40:187ef29de53d 277
FloorC 59:1725a3f02f37 278 } //while true closed
daniQQue 0:34c739fcc3e0 279
daniQQue 49:818a0e90ed9c 280 } //int main closed
daniQQue 49:818a0e90ed9c 281
daniQQue 57:c546edf67c5c 282 //=======================================================================================================================================================