Yolanda Tania / Mbed 2 deprecated PID_tangan

udah bisa looo

Dependencies:   mbed

main.cpp@0:aa8e05bc0533, 2020-02-27 (annotated)

Committer:
Yolandataniaa
Date:
Thu Feb 27 12:40:03 2020 +0000
Revision:
0:aa8e05bc0533
Child:
1:0122c72f6e1b
good pid

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Yolandataniaa 0:aa8e05bc0533 1 /***************************************************************************
Yolandataniaa 0:aa8e05bc0533 2 * Title : Program Debug Motor dan Encoder
Yolandataniaa 0:aa8e05bc0533 3 * Name : debug_motor.cpp
Yolandataniaa 0:aa8e05bc0533 4 * Version : 1.0
Yolandataniaa 0:aa8e05bc0533 5 * Author : Gian Arjuna EL 16
Yolandataniaa 0:aa8e05bc0533 6 * Date : 12 Desember 2019
Yolandataniaa 0:aa8e05bc0533 7 * Description:
Yolandataniaa 0:aa8e05bc0533 8 *
Yolandataniaa 0:aa8e05bc0533 9 * Program ini dapat digunakan untuk mengambil data pulse encoder, menggerakan
Yolandataniaa 0:aa8e05bc0533 10 * motor, mengambil data kecepatan, dan mengambil data PID. Untuk menggunakan
Yolandataniaa 0:aa8e05bc0533 11 * program ini UBAH DEKLARASI PIN PADA BAGIAN PIN LIST DIBAWAH agar sesuai
Yolandataniaa 0:aa8e05bc0533 12 * dengan pin yang sedang digunakan. Kemudian pilih mode debug yang ingin
Yolandataniaa 0:aa8e05bc0533 13 * dilakukan dengan comment/uncomment "#define" pada bagian mode debug
Yolandataniaa 0:aa8e05bc0533 14 *
Yolandataniaa 0:aa8e05bc0533 15 ***************************************************************************/
Yolandataniaa 0:aa8e05bc0533 16
Yolandataniaa 0:aa8e05bc0533 17
Yolandataniaa 0:aa8e05bc0533 18 /* LIBRARY */
Yolandataniaa 0:aa8e05bc0533 19 #include "mbed.h"
Yolandataniaa 0:aa8e05bc0533 20 #include "encoderKRAI.h"
Yolandataniaa 0:aa8e05bc0533 21 #include "Motor.h"
Yolandataniaa 0:aa8e05bc0533 22 #include "pid_dagoz/PID.h"
Yolandataniaa 0:aa8e05bc0533 23
Yolandataniaa 0:aa8e05bc0533 24 /* MODE DEBUG YANG DIINGINKAN */
Yolandataniaa 0:aa8e05bc0533 25 //#define AMBIL_ENCODER
Yolandataniaa 0:aa8e05bc0533 26 //#define GERAK_MOTOR1
Yolandataniaa 0:aa8e05bc0533 27 //#define GERAK_MOTOR2
Yolandataniaa 0:aa8e05bc0533 28 //#define AMBIL_KECEPATAN
Yolandataniaa 0:aa8e05bc0533 29 //#define TES_PID
Yolandataniaa 0:aa8e05bc0533 30 #define TES_PID_TANGAN
Yolandataniaa 0:aa8e05bc0533 31
Yolandataniaa 0:aa8e05bc0533 32 /***** PIN LIST *****/
Yolandataniaa 0:aa8e05bc0533 33 /* pin assignment untuk encoder */
Yolandataniaa 0:aa8e05bc0533 34 encoderKRAI enc_arm1(PB_7, PB_6, 538, encoderKRAI::X4_ENCODING);
Yolandataniaa 0:aa8e05bc0533 35 //encoderKRAI enc_arm2(PB_2, PD_2, 538, encoderKRAI::X4_ENCODING);
Yolandataniaa 0:aa8e05bc0533 36
Yolandataniaa 0:aa8e05bc0533 37 /* pin assignment untuk motor */
Yolandataniaa 0:aa8e05bc0533 38
Yolandataniaa 0:aa8e05bc0533 39 Motor arm1(PA_15, PA_13, PA_14);
Yolandataniaa 0:aa8e05bc0533 40 //Motor arm2(PC_8, PC_6, PC_5);
Yolandataniaa 0:aa8e05bc0533 41
Yolandataniaa 0:aa8e05bc0533 42 float kp1 = 0.0204622,kp2 = 0.0102622;
Yolandataniaa 0:aa8e05bc0533 43 float kd1 = 0.06,kd2 = 0.065;
Yolandataniaa 0:aa8e05bc0533 44 float ki1 = 0 ,ki2 = 0;
Yolandataniaa 0:aa8e05bc0533 45 float ff1 = 0,ff2 = 0;
Yolandataniaa 0:aa8e05bc0533 46 float n1 = 0,n2 = 0;
Yolandataniaa 0:aa8e05bc0533 47 float ts1 = 0.009127;
Yolandataniaa 0:aa8e05bc0533 48 float PI = 3.14159;
Yolandataniaa 0:aa8e05bc0533 49 float wheel_radius = 0.075;
Yolandataniaa 0:aa8e05bc0533 50
Yolandataniaa 0:aa8e05bc0533 51 PID pid1(kp1, ki1, kd1, n1, ts1, ff1, PID::PI_MODE);
Yolandataniaa 0:aa8e05bc0533 52 PID pid2(kp2, ki2, kd2, n2, ts1, ff2, PID::PI_MODE);
Yolandataniaa 0:aa8e05bc0533 53
Yolandataniaa 0:aa8e05bc0533 54 /* pin assignment lainnya */
Yolandataniaa 0:aa8e05bc0533 55 DigitalIn mybutton(USER_BUTTON, PullUp);
Yolandataniaa 0:aa8e05bc0533 56
Yolandataniaa 0:aa8e05bc0533 57 /* komunikasi serial dengan UART */
Yolandataniaa 0:aa8e05bc0533 58 Serial pc(USBTX, USBRX, 115200);
Yolandataniaa 0:aa8e05bc0533 59
Yolandataniaa 0:aa8e05bc0533 60 /* timer untuk mendapatkan waktu */
Yolandataniaa 0:aa8e05bc0533 61 Timer timer1;
Yolandataniaa 0:aa8e05bc0533 62
Yolandataniaa 0:aa8e05bc0533 63
Yolandataniaa 0:aa8e05bc0533 64 /* deklarasi variable global */
Yolandataniaa 0:aa8e05bc0533 65 /* array untuk menyimpan data kecepatan */
Yolandataniaa 0:aa8e05bc0533 66 float theta1[400];
Yolandataniaa 0:aa8e05bc0533 67 float theta2[400];
Yolandataniaa 0:aa8e05bc0533 68 float input[400];
Yolandataniaa 0:aa8e05bc0533 69 float time_array[400];
Yolandataniaa 0:aa8e05bc0533 70
Yolandataniaa 0:aa8e05bc0533 71 /* variable kecepatan dan posisi*/
Yolandataniaa 0:aa8e05bc0533 72 float curr_theta, prev_theta;
Yolandataniaa 0:aa8e05bc0533 73 float en1,en2;
Yolandataniaa 0:aa8e05bc0533 74
Yolandataniaa 0:aa8e05bc0533 75 /* variable sampling time */
Yolandataniaa 0:aa8e05bc0533 76 int samp, samp_pid, TS;
Yolandataniaa 0:aa8e05bc0533 77 int i;
Yolandataniaa 0:aa8e05bc0533 78 int counttt;
Yolandataniaa 0:aa8e05bc0533 79
Yolandataniaa 0:aa8e05bc0533 80
Yolandataniaa 0:aa8e05bc0533 81 /* variable pid */
Yolandataniaa 0:aa8e05bc0533 82 float curr_theta1,curr_theta2;
Yolandataniaa 0:aa8e05bc0533 83 float prev_error, lowpass_error, prev_lowpass_error;
Yolandataniaa 0:aa8e05bc0533 84 float pwm1,pwm2;
Yolandataniaa 0:aa8e05bc0533 85
Yolandataniaa 0:aa8e05bc0533 86 float teta_ref;
Yolandataniaa 0:aa8e05bc0533 87 float teta_act;
Yolandataniaa 0:aa8e05bc0533 88 float kp_teta = 0.005;
Yolandataniaa 0:aa8e05bc0533 89 float ki_teta = 0;
Yolandataniaa 0:aa8e05bc0533 90 float kd_teta = 0;
Yolandataniaa 0:aa8e05bc0533 91 float TS_pid;
Yolandataniaa 0:aa8e05bc0533 92 float w_ref;
Yolandataniaa 0:aa8e05bc0533 93 float prev_teta_ref;
Yolandataniaa 0:aa8e05bc0533 94 float w_act;
Yolandataniaa 0:aa8e05bc0533 95 float kp_w = 0.005;
Yolandataniaa 0:aa8e05bc0533 96 float ki_w = 0;
Yolandataniaa 0:aa8e05bc0533 97 float kd_w = 0.005;
Yolandataniaa 0:aa8e05bc0533 98 float pwm;
Yolandataniaa 0:aa8e05bc0533 99
Yolandataniaa 0:aa8e05bc0533 100 /* variable penyimpan waktu */
Yolandataniaa 0:aa8e05bc0533 101 uint32_t last_baca, last_pid, last_motor;
Yolandataniaa 0:aa8e05bc0533 102
Yolandataniaa 0:aa8e05bc0533 103 /* prototipe fungsi */
Yolandataniaa 0:aa8e05bc0533 104 //void pid (float ref, float curr_feed, float prev_feed, float feedforward, float actual, float kp, float ki, float kd, float TS, float* output);
Yolandataniaa 0:aa8e05bc0533 105
Yolandataniaa 0:aa8e05bc0533 106 int main() {
Yolandataniaa 0:aa8e05bc0533 107 arm1.period(0.02);
Yolandataniaa 0:aa8e05bc0533 108 /* ================================================================== */
Yolandataniaa 0:aa8e05bc0533 109 #ifdef AMBIL_ENCODER
Yolandataniaa 0:aa8e05bc0533 110 while(1)
Yolandataniaa 0:aa8e05bc0533 111 {
Yolandataniaa 0:aa8e05bc0533 112 en1= (float)enc_arm1.getPulses();
Yolandataniaa 0:aa8e05bc0533 113 //en2= (float)enc_arm2.getPulses();
Yolandataniaa 0:aa8e05bc0533 114 pc.printf("%f\t %f\n", en1, en2);
Yolandataniaa 0:aa8e05bc0533 115 }
Yolandataniaa 0:aa8e05bc0533 116 #endif
Yolandataniaa 0:aa8e05bc0533 117
Yolandataniaa 0:aa8e05bc0533 118 /* ================================================================== */
Yolandataniaa 0:aa8e05bc0533 119
Yolandataniaa 0:aa8e05bc0533 120 #ifdef GERAK_MOTOR1
Yolandataniaa 0:aa8e05bc0533 121 while(1)
Yolandataniaa 0:aa8e05bc0533 122 {
Yolandataniaa 0:aa8e05bc0533 123 float pwm = 0.6;
Yolandataniaa 0:aa8e05bc0533 124 arm1.speed(pwm);
Yolandataniaa 0:aa8e05bc0533 125 en1= (float)enc_arm1.getPulses();
Yolandataniaa 0:aa8e05bc0533 126 pc.printf("%f\t %f\n", en1, en2);
Yolandataniaa 0:aa8e05bc0533 127 }
Yolandataniaa 0:aa8e05bc0533 128 #endif
Yolandataniaa 0:aa8e05bc0533 129
Yolandataniaa 0:aa8e05bc0533 130 #ifdef GERAK_MOTOR2
Yolandataniaa 0:aa8e05bc0533 131 while(1)
Yolandataniaa 0:aa8e05bc0533 132 {
Yolandataniaa 0:aa8e05bc0533 133 float pwm = 0.6;
Yolandataniaa 0:aa8e05bc0533 134 arm2.speed(pwm);
Yolandataniaa 0:aa8e05bc0533 135 //en2= (float)enc_arm2.getPulses();
Yolandataniaa 0:aa8e05bc0533 136 pc.printf("%f\t %f\n", en1, en2);
Yolandataniaa 0:aa8e05bc0533 137 }
Yolandataniaa 0:aa8e05bc0533 138 #endif
Yolandataniaa 0:aa8e05bc0533 139 /* ================================================================== */
Yolandataniaa 0:aa8e05bc0533 140 #ifdef AMBIL_KECEPATAN // (misal arm1(?))
Yolandataniaa 0:aa8e05bc0533 141
Yolandataniaa 0:aa8e05bc0533 142 /* setup and initialization*/
Yolandataniaa 0:aa8e05bc0533 143 timer1.start();
Yolandataniaa 0:aa8e05bc0533 144
Yolandataniaa 0:aa8e05bc0533 145 /* command move motor and sample data*/
Yolandataniaa 0:aa8e05bc0533 146 while(counttt <= 400)
Yolandataniaa 0:aa8e05bc0533 147 {
Yolandataniaa 0:aa8e05bc0533 148 if (t.read_us()-samp >= 5000)
Yolandataniaa 0:aa8e05bc0533 149 {
Yolandataniaa 0:aa8e05bc0533 150 TS = t.read_us()-samp;
Yolandataniaa 0:aa8e05bc0533 151 curr_theta = (float)enc.getPulses()*360/538;
Yolandataniaa 0:aa8e05bc0533 152 theta1[counttt] = curr_theta1;
Yolandataniaa 0:aa8e05bc0533 153 enc_arm1.reset();
Yolandataniaa 0:aa8e05bc0533 154 arm1.speed(0.3);
Yolandataniaa 0:aa8e05bc0533 155 counttt ++;
Yolandataniaa 0:aa8e05bc0533 156 samp = t.read_us();
Yolandataniaa 0:aa8e05bc0533 157 }
Yolandataniaa 0:aa8e05bc0533 158 }
Yolandataniaa 0:aa8e05bc0533 159 arm1.speed(0); /* turn off motor after sampling done */
Yolandataniaa 0:aa8e05bc0533 160
Yolandataniaa 0:aa8e05bc0533 161 /* print data */
Yolandataniaa 0:aa8e05bc0533 162 for(i = 0; i < 400; i++)
Yolandataniaa 0:aa8e05bc0533 163 {
Yolandataniaa 0:aa8e05bc0533 164 pc.printf("%f\n", theta[i]);
Yolandataniaa 0:aa8e05bc0533 165 }
Yolandataniaa 0:aa8e05bc0533 166 #endif
Yolandataniaa 0:aa8e05bc0533 167 /* ================================================================== */
Yolandataniaa 0:aa8e05bc0533 168
Yolandataniaa 0:aa8e05bc0533 169 #ifdef TES_PID
Yolandataniaa 0:aa8e05bc0533 170 /* setup and initialization*/
Yolandataniaa 0:aa8e05bc0533 171 timer1.start();
Yolandataniaa 0:aa8e05bc0533 172 float period = 0.0005;
Yolandataniaa 0:aa8e05bc0533 173 arm1.period(period);
Yolandataniaa 0:aa8e05bc0533 174 //arm2.period(period);
Yolandataniaa 0:aa8e05bc0533 175 // float pwmz = 0.7;
Yolandataniaa 0:aa8e05bc0533 176
Yolandataniaa 0:aa8e05bc0533 177 /* command move motor and sample data*/
Yolandataniaa 0:aa8e05bc0533 178 while(counttt <= 400)
Yolandataniaa 0:aa8e05bc0533 179 {
Yolandataniaa 0:aa8e05bc0533 180 if (timer1.read_us()-samp >= 7123)
Yolandataniaa 0:aa8e05bc0533 181 {
Yolandataniaa 0:aa8e05bc0533 182 TS = timer1.read_us()-samp;
Yolandataniaa 0:aa8e05bc0533 183 time_array[counttt] = (float)timer1.read_us()/1000000;
Yolandataniaa 0:aa8e05bc0533 184 input[counttt] = pwmz*24;
Yolandataniaa 0:aa8e05bc0533 185 curr_theta1 = (float)enc_arm1.getPulses()*360/538;
Yolandataniaa 0:aa8e05bc0533 186 //curr_theta2 = (float)enc_arm2.getPulses()*360/538;
Yolandataniaa 0:aa8e05bc0533 187 theta1[counttt] = curr_theta1;
Yolandataniaa 0:aa8e05bc0533 188 //theta2[counttt] = curr_theta2;
Yolandataniaa 0:aa8e05bc0533 189 enc_arm1.reset();
Yolandataniaa 0:aa8e05bc0533 190 //enc_arm2.reset();
Yolandataniaa 0:aa8e05bc0533 191
Yolandataniaa 0:aa8e05bc0533 192 counttt ++;
Yolandataniaa 0:aa8e05bc0533 193 samp = timer1.read_us();
Yolandataniaa 0:aa8e05bc0533 194 }
Yolandataniaa 0:aa8e05bc0533 195 if (timer1.read_us() - samp_pid > 9127)
Yolandataniaa 0:aa8e05bc0533 196 {
Yolandataniaa 0:aa8e05bc0533 197 float setpoint = 180;
Yolandataniaa 0:aa8e05bc0533 198 pwm1 = pid1.createpwm(setpoint,curr_theta1);
Yolandataniaa 0:aa8e05bc0533 199 //pwm2 = pid2.createpwm(setpoint,curr_theta2);
Yolandataniaa 0:aa8e05bc0533 200
Yolandataniaa 0:aa8e05bc0533 201
Yolandataniaa 0:aa8e05bc0533 202 arm1.speed(pwm1);
Yolandataniaa 0:aa8e05bc0533 203 //arm2.speed(pwm2);
Yolandataniaa 0:aa8e05bc0533 204 if (curr_theta1>180){
Yolandataniaa 0:aa8e05bc0533 205 arm1.speed(0);
Yolandataniaa 0:aa8e05bc0533 206 }
Yolandataniaa 0:aa8e05bc0533 207 samp_pid = timer1.read_us();
Yolandataniaa 0:aa8e05bc0533 208 }
Yolandataniaa 0:aa8e05bc0533 209
Yolandataniaa 0:aa8e05bc0533 210 }
Yolandataniaa 0:aa8e05bc0533 211
Yolandataniaa 0:aa8e05bc0533 212
Yolandataniaa 0:aa8e05bc0533 213 arm1.speed(0);
Yolandataniaa 0:aa8e05bc0533 214 arm2.speed(0);
Yolandataniaa 0:aa8e05bc0533 215 /* turn off motor after sampling done */
Yolandataniaa 0:aa8e05bc0533 216
Yolandataniaa 0:aa8e05bc0533 217 /* print data */
Yolandataniaa 0:aa8e05bc0533 218 for(i = 0; i < 400; i++)
Yolandataniaa 0:aa8e05bc0533 219 {
Yolandataniaa 0:aa8e05bc0533 220 pc.printf("%f %f %f %f \n", theta1[i], theta2[i], time_array[i], input[i]);
Yolandataniaa 0:aa8e05bc0533 221 }
Yolandataniaa 0:aa8e05bc0533 222 #endif
Yolandataniaa 0:aa8e05bc0533 223
Yolandataniaa 0:aa8e05bc0533 224 #ifdef TES_PID_TANGAN
Yolandataniaa 0:aa8e05bc0533 225 /* setup and initialization*/
Yolandataniaa 0:aa8e05bc0533 226 timer1.start();
Yolandataniaa 0:aa8e05bc0533 227 float period = 0.0005;
Yolandataniaa 0:aa8e05bc0533 228 arm1.period(period);
Yolandataniaa 0:aa8e05bc0533 229
Yolandataniaa 0:aa8e05bc0533 230 /* command move motor and sample data*/
Yolandataniaa 0:aa8e05bc0533 231 while(1)
Yolandataniaa 0:aa8e05bc0533 232 {
Yolandataniaa 0:aa8e05bc0533 233 if (timer1.read_us()-samp >= 7123)
Yolandataniaa 0:aa8e05bc0533 234 {
Yolandataniaa 0:aa8e05bc0533 235 curr_theta1 += (float)enc_arm1.getPulses()*360/538;
Yolandataniaa 0:aa8e05bc0533 236 //curr_theta2 += (float)enc_arm2.getPulses()*360/538;
Yolandataniaa 0:aa8e05bc0533 237 enc_arm1.reset();
Yolandataniaa 0:aa8e05bc0533 238 //enc_arm2.reset();
Yolandataniaa 0:aa8e05bc0533 239
Yolandataniaa 0:aa8e05bc0533 240 samp = timer1.read_us();
Yolandataniaa 0:aa8e05bc0533 241 }
Yolandataniaa 0:aa8e05bc0533 242 if (timer1.read_us() - samp_pid > 9127)
Yolandataniaa 0:aa8e05bc0533 243 {
Yolandataniaa 0:aa8e05bc0533 244 // float setpoint = 110;
Yolandataniaa 0:aa8e05bc0533 245 float setpoint = 150;
Yolandataniaa 0:aa8e05bc0533 246 float error = setpoint - curr_theta1;
Yolandataniaa 0:aa8e05bc0533 247
Yolandataniaa 0:aa8e05bc0533 248 lowpass_error = 0.1*error + 0.9*prev_error;
Yolandataniaa 0:aa8e05bc0533 249 // lowpass_error = 0.1*lowpass_error + 0.9*prev_lowpass_error;
Yolandataniaa 0:aa8e05bc0533 250 // prev_lowpass_error= lowpass_error;
Yolandataniaa 0:aa8e05bc0533 251
Yolandataniaa 0:aa8e05bc0533 252 pwm1 = kp2*(lowpass_error) + kd2*(lowpass_error - prev_lowpass_error);
Yolandataniaa 0:aa8e05bc0533 253 pwm1 = fabs(pwm1) > 0.85 ? 0.85*fabs(pwm1)/pwm1 : pwm1;
Yolandataniaa 0:aa8e05bc0533 254
Yolandataniaa 0:aa8e05bc0533 255 prev_lowpass_error = lowpass_error;
Yolandataniaa 0:aa8e05bc0533 256
Yolandataniaa 0:aa8e05bc0533 257 prev_error = error;
Yolandataniaa 0:aa8e05bc0533 258
Yolandataniaa 0:aa8e05bc0533 259 //arm2.speed(pwm2);
Yolandataniaa 0:aa8e05bc0533 260 // if (curr_theta1>110 && curr_theta1 < 140 && setpoint >= 120){
Yolandataniaa 0:aa8e05bc0533 261 if (curr_theta1>140 && curr_theta1 < 170 && setpoint >= 145){
Yolandataniaa 0:aa8e05bc0533 262 arm1.speed(0);
Yolandataniaa 0:aa8e05bc0533 263 arm1.brake();
Yolandataniaa 0:aa8e05bc0533 264 }
Yolandataniaa 0:aa8e05bc0533 265 else if (curr_theta1>130 && curr_theta1 <= 140){
Yolandataniaa 0:aa8e05bc0533 266 arm1.speed(0.075*pwm1);
Yolandataniaa 0:aa8e05bc0533 267 }
Yolandataniaa 0:aa8e05bc0533 268 else if (curr_theta1>115 && curr_theta1 <= 130){
Yolandataniaa 0:aa8e05bc0533 269 arm1.speed(0.65*pwm1);
Yolandataniaa 0:aa8e05bc0533 270 }
Yolandataniaa 0:aa8e05bc0533 271 else {
Yolandataniaa 0:aa8e05bc0533 272 arm1.speed(pwm1);
Yolandataniaa 0:aa8e05bc0533 273 }
Yolandataniaa 0:aa8e05bc0533 274 samp_pid = timer1.read_us();
Yolandataniaa 0:aa8e05bc0533 275
Yolandataniaa 0:aa8e05bc0533 276
Yolandataniaa 0:aa8e05bc0533 277 }
Yolandataniaa 0:aa8e05bc0533 278 if(timer1.read_us() - last_baca > 100000){
Yolandataniaa 0:aa8e05bc0533 279 pc.printf("%.2f %.2f\n", curr_theta1, pwm1);
Yolandataniaa 0:aa8e05bc0533 280 }
Yolandataniaa 0:aa8e05bc0533 281
Yolandataniaa 0:aa8e05bc0533 282 }
Yolandataniaa 0:aa8e05bc0533 283 /* turn off motor after sampling done */
Yolandataniaa 0:aa8e05bc0533 284 #endif
Yolandataniaa 0:aa8e05bc0533 285 /* ================================================================== */
Yolandataniaa 0:aa8e05bc0533 286
Yolandataniaa 0:aa8e05bc0533 287 }
Yolandataniaa 0:aa8e05bc0533 288
Yolandataniaa 0:aa8e05bc0533 289
Yolandataniaa 0:aa8e05bc0533 290 /* definisi fungsi */