Jacob Kang / Mbed 2 deprecated EW309_turrettest

EW309

Dependencies:   mbed QEI

main.cpp@0:058dc42250db, 2019-04-11 (annotated)

Committer:
kingkang2
Date:
Thu Apr 11 15:08:27 2019 +0000
Revision:
0:058dc42250db
ERC

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kingkang2 0:058dc42250db 1 #include "mbed.h"
kingkang2 0:058dc42250db 2 #include "QEI.h"
kingkang2 0:058dc42250db 3
kingkang2 0:058dc42250db 4 QEI turn(p26,p27,NC,1600);
kingkang2 0:058dc42250db 5 Serial pc(USBTX,USBRX); // serial comms with PC
kingkang2 0:058dc42250db 6 Timer t;
kingkang2 0:058dc42250db 7 Ticker tick;
kingkang2 0:058dc42250db 8 PwmOut in1(p25);
kingkang2 0:058dc42250db 9 DigitalOut in2(p24);
kingkang2 0:058dc42250db 10
kingkang2 0:058dc42250db 11 int pulse_count;
kingkang2 0:058dc42250db 12 int j;
kingkang2 0:058dc42250db 13 float dc = 0.0;
kingkang2 0:058dc42250db 14 float dir;
kingkang2 0:058dc42250db 15 float ang_curr;
kingkang2 0:058dc42250db 16 float ang_old = 0.0;
kingkang2 0:058dc42250db 17 float ctrl_period = 0.01;
kingkang2 0:058dc42250db 18 float ang_des;
kingkang2 0:058dc42250db 19 float dc_old = 0.0;
kingkang2 0:058dc42250db 20 float current_time;
kingkang2 0:058dc42250db 21 float speed;
kingkang2 0:058dc42250db 22 int idx = 0; // counter for figuring out when you reach 20 values of velocity
kingkang2 0:058dc42250db 23 float e_dot_arr[20]; //stores 20 values of the error derivative
kingkang2 0:058dc42250db 24 float e_dot; //this is your error derivative
kingkang2 0:058dc42250db 25 int first = 0;
kingkang2 0:058dc42250db 26 float omega;
kingkang2 0:058dc42250db 27 float err_int;
kingkang2 0:058dc42250db 28 float err_int_old = 0.0;
kingkang2 0:058dc42250db 29 float err_ang;
kingkang2 0:058dc42250db 30 float err_ang_old = 0.0;
kingkang2 0:058dc42250db 31 float deg_ang_curr;
kingkang2 0:058dc42250db 32 float deg_err_ang;
kingkang2 0:058dc42250db 33
kingkang2 0:058dc42250db 34 void measure()
kingkang2 0:058dc42250db 35 {
kingkang2 0:058dc42250db 36 pulse_count = 1.0*turn.getPulses();
kingkang2 0:058dc42250db 37 current_time = t.read();
kingkang2 0:058dc42250db 38 //3200 pulses is one rev for x2
kingkang2 0:058dc42250db 39 ang_curr = (pulse_count /(2.0 * 1600)*(2*3.14159265));
kingkang2 0:058dc42250db 40 //Estimate speed
kingkang2 0:058dc42250db 41
kingkang2 0:058dc42250db 42 omega = (ang_curr - ang_old)/ctrl_period; //this is change in theta over delta time
kingkang2 0:058dc42250db 43
kingkang2 0:058dc42250db 44 //Velocity filter for derivative term
kingkang2 0:058dc42250db 45 if(idx <= 19) {
kingkang2 0:058dc42250db 46 e_dot_arr[idx] = omega; //keep storing values in your array
kingkang2 0:058dc42250db 47 idx = idx+1;
kingkang2 0:058dc42250db 48 } else {
kingkang2 0:058dc42250db 49 idx = 0; //start over with the first entry (rewrite old ones)
kingkang2 0:058dc42250db 50 e_dot_arr[idx] = omega;
kingkang2 0:058dc42250db 51 }
kingkang2 0:058dc42250db 52 if (first == 0) {
kingkang2 0:058dc42250db 53
kingkang2 0:058dc42250db 54 if (idx >= 1) {
kingkang2 0:058dc42250db 55 e_dot = 0.0; //this checks to see if it's the first time the array is populated
kingkang2 0:058dc42250db 56 for (j=0; j <= idx; j++) {
kingkang2 0:058dc42250db 57 e_dot = e_dot + e_dot_arr[j];
kingkang2 0:058dc42250db 58 }
kingkang2 0:058dc42250db 59
kingkang2 0:058dc42250db 60 } else {
kingkang2 0:058dc42250db 61 e_dot = e_dot_arr[idx];
kingkang2 0:058dc42250db 62 }
kingkang2 0:058dc42250db 63 } else {
kingkang2 0:058dc42250db 64 first = 1; //set the value to first to 1 so that it starts to just overwrite old values in the array
kingkang2 0:058dc42250db 65 }
kingkang2 0:058dc42250db 66 for (j=0; j<=19; j++) {
kingkang2 0:058dc42250db 67 e_dot = e_dot + e_dot_arr[j];
kingkang2 0:058dc42250db 68 }
kingkang2 0:058dc42250db 69
kingkang2 0:058dc42250db 70 e_dot = 0.0 - e_dot;
kingkang2 0:058dc42250db 71
kingkang2 0:058dc42250db 72 //ERRORS//
kingkang2 0:058dc42250db 73
kingkang2 0:058dc42250db 74 err_ang = -ang_curr + ang_des;
kingkang2 0:058dc42250db 75
kingkang2 0:058dc42250db 76 err_int = err_int_old + ((err_ang_old + err_ang)/2)*ctrl_period;
kingkang2 0:058dc42250db 77
kingkang2 0:058dc42250db 78 //err_der = (err_ang- err_ang_old)/ctrl_period
kingkang2 0:058dc42250db 79
kingkang2 0:058dc42250db 80 //compute duty cycle for motor (if necessary)
kingkang2 0:058dc42250db 81
kingkang2 0:058dc42250db 82 ////PID////
kingkang2 0:058dc42250db 83 dc = 1.6*err_ang + (0.75*err_int) + (0.000*e_dot); //Kp = .43 0.0418 .35
kingkang2 0:058dc42250db 84 //dc= -dc;
kingkang2 0:058dc42250db 85
kingkang2 0:058dc42250db 86
kingkang2 0:058dc42250db 87 //Age variable
kingkang2 0:058dc42250db 88 err_ang_old = err_ang;
kingkang2 0:058dc42250db 89 err_int_old = err_int;
kingkang2 0:058dc42250db 90
kingkang2 0:058dc42250db 91
kingkang2 0:058dc42250db 92
kingkang2 0:058dc42250db 93 //send duty cycle to motor
kingkang2 0:058dc42250db 94 if(dc>0.0) {
kingkang2 0:058dc42250db 95 dir = 1.0;
kingkang2 0:058dc42250db 96 } else if(dc<0.0) {
kingkang2 0:058dc42250db 97 dir = 0.0;
kingkang2 0:058dc42250db 98 }
kingkang2 0:058dc42250db 99 if(dc>1.0) {
kingkang2 0:058dc42250db 100 dc = 1.0;
kingkang2 0:058dc42250db 101 } else if(dc<-1.0) {
kingkang2 0:058dc42250db 102 dc = 1.0;
kingkang2 0:058dc42250db 103 } else if (dc<0.0){
kingkang2 0:058dc42250db 104 dc = abs(dc);
kingkang2 0:058dc42250db 105 }
kingkang2 0:058dc42250db 106
kingkang2 0:058dc42250db 107 //Age variable
kingkang2 0:058dc42250db 108 //dc_old = dc;
kingkang2 0:058dc42250db 109 in1 = dc;
kingkang2 0:058dc42250db 110 in2 = dir;
kingkang2 0:058dc42250db 111
kingkang2 0:058dc42250db 112 deg_ang_curr = ang_curr*180.0/3.14159265;
kingkang2 0:058dc42250db 113 deg_err_ang= err_ang*180.0/3.14159265;
kingkang2 0:058dc42250db 114
kingkang2 0:058dc42250db 115 printf("%f,%f,%f,%f,%f\n\r", current_time,deg_ang_curr,deg_err_ang,dc,dir);
kingkang2 0:058dc42250db 116
kingkang2 0:058dc42250db 117
kingkang2 0:058dc42250db 118 }
kingkang2 0:058dc42250db 119
kingkang2 0:058dc42250db 120 int main()
kingkang2 0:058dc42250db 121 {
kingkang2 0:058dc42250db 122 in1.period(1.0/20.0e3);
kingkang2 0:058dc42250db 123 pc.baud(115200); // increase baud rate
kingkang2 0:058dc42250db 124 pc.format(8,SerialBase::Even,1);
kingkang2 0:058dc42250db 125 turn.reset();
kingkang2 0:058dc42250db 126
kingkang2 0:058dc42250db 127 in1 = dc;
kingkang2 0:058dc42250db 128 in2 = dir;
kingkang2 0:058dc42250db 129
kingkang2 0:058dc42250db 130 printf("Enter a Position:");
kingkang2 0:058dc42250db 131 pc.scanf("%f",&ang_des); //take in desired position
kingkang2 0:058dc42250db 132
kingkang2 0:058dc42250db 133 //printf("Enter a direction 0 or 1:");
kingkang2 0:058dc42250db 134 //pc.scanf("%f",&dir); //take in direction
kingkang2 0:058dc42250db 135
kingkang2 0:058dc42250db 136 in1 = dc;
kingkang2 0:058dc42250db 137 in2 = dir;
kingkang2 0:058dc42250db 138
kingkang2 0:058dc42250db 139 tick.attach(&measure, 0.01);
kingkang2 0:058dc42250db 140
kingkang2 0:058dc42250db 141
kingkang2 0:058dc42250db 142
kingkang2 0:058dc42250db 143 while(t.read()<10.0) {
kingkang2 0:058dc42250db 144 t.start();
kingkang2 0:058dc42250db 145
kingkang2 0:058dc42250db 146
kingkang2 0:058dc42250db 147 }
kingkang2 0:058dc42250db 148 t.stop();
kingkang2 0:058dc42250db 149 tick.detach();
kingkang2 0:058dc42250db 150 }