Isro Syaeful Iman / Mbed 2 deprecated Thrower_KRAI

utk magang

Dependencies:   Motor Ultrasonic encoderKRAI mbed millis odometryKRAI pidKRAI

main.cpp@11:d4cf81f59601, 2017-10-25 (annotated)

Committer:
calmantara186
Date:
Wed Oct 25 15:16:27 2017 +0000
Revision:
11:d4cf81f59601
Parent:
10:e550e5daed74
Child:
12:84cb23216f78
FAK. SAYA PERLU TAMASYA !!!!

Who changed what in which revision?

UserRevisionLine numberNew contents of line
calmantara186 0:a6918b57d3fa 1 /**
calmantara186 0:a6918b57d3fa 2 * THROWER ROBOT
calmantara186 0:a6918b57d3fa 3 * Bismillahirahmanirrahim
calmantara186 0:a6918b57d3fa 4 *
calmantara186 0:a6918b57d3fa 5 * To do List :
calmantara186 7:cf822f5e7b12 6 * - Cari konstanta Kanan Kiri
calmantara186 10:e550e5daed74 7 * - Cari Konstanta lagi :(
calmantara186 9:e2c3175936fb 8 *
calmantara186 7:cf822f5e7b12 9 * - JAGA SEMANGAT DAN KESEHATAN KITA !!! :)
calmantara186 0:a6918b57d3fa 10 *
calmantara186 7:cf822f5e7b12 11 * UPDATED : 22/10
calmantara186 7:cf822f5e7b12 12 * - PID maju/mundur enak enak jos
calmantara186 6:5632ff2142c0 13 *
calmantara186 6:5632ff2142c0 14 * Last Edited by : Thrower KRAI
calmantara186 0:a6918b57d3fa 15 **/
calmantara186 2:dea57b3f84cd 16
calmantara186 0:a6918b57d3fa 17
calmantara186 0:a6918b57d3fa 18 //LIBRARY
calmantara186 0:a6918b57d3fa 19 #include "mbed.h"
calmantara186 0:a6918b57d3fa 20 #include "Motor.h"
calmantara186 0:a6918b57d3fa 21 #include "encoderKRAI.h"
calmantara186 0:a6918b57d3fa 22 #include "millis.h"
calmantara186 6:5632ff2142c0 23 #include "pidKRAI.h"
calmantara186 9:e2c3175936fb 24 #include "odometryKRAI.h"
calmantara186 0:a6918b57d3fa 25
calmantara186 0:a6918b57d3fa 26 /***************************
calmantara186 0:a6918b57d3fa 27 * Konstanta dan Variabel *
calmantara186 0:a6918b57d3fa 28 ***************************/
calmantara186 9:e2c3175936fb 29 #define DRODA 100.0
calmantara186 2:dea57b3f84cd 30
calmantara186 2:dea57b3f84cd 31 //Variabel Robot
calmantara186 9:e2c3175936fb 32 double kll_roda = PI*DRODA; //variabel keliling robot
calmantara186 2:dea57b3f84cd 33
calmantara186 6:5632ff2142c0 34 //Variabel Program dan Planning
calmantara186 11:d4cf81f59601 35 unsigned short in_program = 0; //variabel untuk masuk ke looping program
calmantara186 11:d4cf81f59601 36 short state =-1;
calmantara186 11:d4cf81f59601 37
calmantara186 11:d4cf81f59601 38 DigitalIn mybutton(USER_BUTTON);
calmantara186 6:5632ff2142c0 39
calmantara186 2:dea57b3f84cd 40 //Primitive Function
calmantara186 9:e2c3175936fb 41 void limitMotor(); //procedure untuk mengecek limit pwm motor
calmantara186 9:e2c3175936fb 42 void pidMotor(int x); //procedure untuk arah gerak motor
calmantara186 9:e2c3175936fb 43 void getValue(); //procedure untuk menghitung pulses dan rev
calmantara186 2:dea57b3f84cd 44
calmantara186 0:a6918b57d3fa 45 //Deklarasi Motor
calmantara186 0:a6918b57d3fa 46 Motor motor_depan(PB_6, PB_13, PA_10); //pwm, fwd, rev
calmantara186 0:a6918b57d3fa 47 Motor motor_kanan(PB_8, PA_9, PA_5); //pwm, fwd, rev
calmantara186 0:a6918b57d3fa 48 Motor motor_kiri(PB_9, PA_12, PA_11); //pwm, fwd, rev
calmantara186 0:a6918b57d3fa 49
calmantara186 2:dea57b3f84cd 50 //Variabel Kecepatan
calmantara186 6:5632ff2142c0 51 float pwm_kanan; //pwm motor kanan
calmantara186 6:5632ff2142c0 52 float pwm_kiri; //pwm motor kiri
calmantara186 6:5632ff2142c0 53 float pwm_depan; //pwm motor depan
calmantara186 6:5632ff2142c0 54 float limit_pwm = 0.9; //limit pwm semua motor
calmantara186 0:a6918b57d3fa 55
calmantara186 0:a6918b57d3fa 56 //Variabel Serial
calmantara186 0:a6918b57d3fa 57 Serial pc(USBTX, USBRX); //Deklarasi serial pc TX RX
calmantara186 0:a6918b57d3fa 58
calmantara186 0:a6918b57d3fa 59 //Deklarasi Rotary Encoder
calmantara186 6:5632ff2142c0 60 encoderKRAI encMotor_depan(PC_0, PC_1, 540, encoderKRAI::X4_ENCODING); //chA, chB, revPulse, interruptType
calmantara186 6:5632ff2142c0 61 encoderKRAI encMotor_kanan(PC_10, PC_11, 540, encoderKRAI::X4_ENCODING); //chA, chB, revPulse, interruptType
calmantara186 6:5632ff2142c0 62 encoderKRAI encMotor_kiri(PC_2, PC_3, 540, encoderKRAI::X4_ENCODING); //chA, chB, revPulse, interruptType
calmantara186 0:a6918b57d3fa 63
calmantara186 10:e550e5daed74 64 //Deklarasi Odometry
calmantara186 10:e550e5daed74 65 odometryKRAI odometry(DRODA); //deklarasi odometry dengan input diameter roda
calmantara186 10:e550e5daed74 66
calmantara186 0:a6918b57d3fa 67 //Variabel Encoder
calmantara186 0:a6918b57d3fa 68 float pulses_depan; //variabel untuk membaca nilai encoder motor depan
calmantara186 0:a6918b57d3fa 69 float pulses_kanan; //variabel untuk membaca nilai encoder motor kanan
calmantara186 0:a6918b57d3fa 70 float pulses_kiri; //variabel untuk membaca nilai encoder motor kiri
calmantara186 6:5632ff2142c0 71
calmantara186 10:e550e5daed74 72 float d_tetha; //variabel untuk membaca nilai delta kanan kiri depan
calmantara186 11:d4cf81f59601 73 //float d_tetha2; //variabel untuk membaca nilai delta kanan kiri
calmantara186 6:5632ff2142c0 74
calmantara186 2:dea57b3f84cd 75 float rev_depan; //variabel untuk membaca nilai putaran roda depan
calmantara186 2:dea57b3f84cd 76 float rev_kanan; //variabel untuk membaca nilai putaran roda kanan
calmantara186 6:5632ff2142c0 77 float rev_kiri; //variabel untuk membaca nilai putaran roda kiri
calmantara186 0:a6918b57d3fa 78
calmantara186 0:a6918b57d3fa 79 //Target nilai encoder
calmantara186 9:e2c3175936fb 80 float tn_tetha = 0; //target untuk nilai tetha robot
calmantara186 0:a6918b57d3fa 81
calmantara186 6:5632ff2142c0 82 //Variabel PID
calmantara186 6:5632ff2142c0 83 unsigned short ts_base = 12; //variabel untuk time sampling
calmantara186 6:5632ff2142c0 84 float speed_roty; //variabel untuk penambahan pwm PID rotasi y
calmantara186 6:5632ff2142c0 85 float speed_disy; //variabel untuk penambahan pwm PID distance y
calmantara186 11:d4cf81f59601 86 float limit_dis = 0.4;
calmantara186 11:d4cf81f59601 87 float limit_rot = 0.2;
calmantara186 9:e2c3175936fb 88
calmantara186 6:5632ff2142c0 89 float speed_rotx; //variabel untuk penambahan pwm PID rotasi x
calmantara186 6:5632ff2142c0 90 float speed_disx; //variabel untuk penambahan pwm PID distance x
calmantara186 6:5632ff2142c0 91
calmantara186 6:5632ff2142c0 92 //Deklarasi PID
calmantara186 11:d4cf81f59601 93 pidKRAI pidRotY((float) 0.234, (float) 0.0098, (float) 7.9123, ts_base); //kp ki kd ts
calmantara186 11:d4cf81f59601 94 pidKRAI pidRotX((float) 0.234, (float) 0.0098, (float) 7.9123, ts_base); //kp ki kd ts
calmantara186 6:5632ff2142c0 95
calmantara186 11:d4cf81f59601 96 pidKRAI pidDisY((float)0.2208,(float)0.0105,(float)4.26231,ts_base); //kp ki kd ts
calmantara186 11:d4cf81f59601 97 pidKRAI pidDisX((float)0.228,(float)0.0234,(float)9.6231,ts_base); //kp ki kd ts
calmantara186 2:dea57b3f84cd 98
calmantara186 2:dea57b3f84cd 99 //Variabel cari jarak
calmantara186 11:d4cf81f59601 100 float target_y[50] = {2000.0, -2000.0}; //variabel yang digunakan untuk target jarak y
calmantara186 9:e2c3175936fb 101 double jarak_y; //variabel untuk increment jarak y
calmantara186 0:a6918b57d3fa 102
calmantara186 11:d4cf81f59601 103 float target_x[50] = {2000.0, -2000.0}; //variabel yang digunakan untuk target jarak x
calmantara186 9:e2c3175936fb 104 double jarak_x; //variabel untuk increment jarak x
calmantara186 6:5632ff2142c0 105
calmantara186 6:5632ff2142c0 106 //Variabel Waktu
calmantara186 6:5632ff2142c0 107 unsigned long int current_millis; //variabel untuk mendapatkan millis yang berjalan
calmantara186 6:5632ff2142c0 108 unsigned long int prev_millis1; //variabel untuk masuk ke perhitungan PID
calmantara186 10:e550e5daed74 109 unsigned long int prev_millis2;
calmantara186 0:a6918b57d3fa 110
calmantara186 0:a6918b57d3fa 111 /***************************
calmantara186 0:a6918b57d3fa 112 * Main Function *
calmantara186 0:a6918b57d3fa 113 ***************************/
calmantara186 0:a6918b57d3fa 114
calmantara186 0:a6918b57d3fa 115 int main(){
calmantara186 0:a6918b57d3fa 116
calmantara186 0:a6918b57d3fa 117 //Inisiasi Serial
calmantara186 0:a6918b57d3fa 118 pc.baud(115200);
calmantara186 0:a6918b57d3fa 119
calmantara186 0:a6918b57d3fa 120 //Start Millis
calmantara186 11:d4cf81f59601 121 wait_ms(1000);
calmantara186 0:a6918b57d3fa 122 startMillis();
calmantara186 6:5632ff2142c0 123 //Looping Utama
calmantara186 6:5632ff2142c0 124 while(1){
calmantara186 6:5632ff2142c0 125 //Looping Program Planning
calmantara186 6:5632ff2142c0 126 while(in_program == 0){
calmantara186 11:d4cf81f59601 127
calmantara186 11:d4cf81f59601 128 if (mybutton == 0){
calmantara186 11:d4cf81f59601 129 pidRotY.reset();
calmantara186 11:d4cf81f59601 130 pidDisY.reset();
calmantara186 11:d4cf81f59601 131 pidRotX.reset();
calmantara186 11:d4cf81f59601 132 pidDisX.reset();
calmantara186 11:d4cf81f59601 133 jarak_y = 0;
calmantara186 11:d4cf81f59601 134 jarak_x = 0;
calmantara186 11:d4cf81f59601 135 state+=1;
calmantara186 11:d4cf81f59601 136 in_program = 1;
calmantara186 11:d4cf81f59601 137 }
calmantara186 6:5632ff2142c0 138 }/* end of planning program */
calmantara186 6:5632ff2142c0 139 //Looping Program Utama
calmantara186 6:5632ff2142c0 140 while(in_program == 1){
calmantara186 10:e550e5daed74 141 current_millis = millis();
calmantara186 11:d4cf81f59601 142 if((jarak_y <= target_y[state]-(float)5.0)||(jarak_y >= target_y[state]+(float)5.0)){
calmantara186 9:e2c3175936fb 143 /* masuk ke case sumbu Y */
calmantara186 7:cf822f5e7b12 144 if(current_millis - prev_millis1 >= 12){
calmantara186 7:cf822f5e7b12 145 pidMotor(0);
calmantara186 7:cf822f5e7b12 146 }
calmantara186 6:5632ff2142c0 147
calmantara186 7:cf822f5e7b12 148 /* Speed Motor */
calmantara186 7:cf822f5e7b12 149 limitMotor();
calmantara186 10:e550e5daed74 150 motor_depan.speed(pwm_depan);
calmantara186 7:cf822f5e7b12 151 motor_kanan.speed(pwm_kanan);
calmantara186 7:cf822f5e7b12 152 motor_kiri.speed(pwm_kiri);
calmantara186 11:d4cf81f59601 153 //prev_millis2 = current_millis;
calmantara186 11:d4cf81f59601 154 //pc.printf("%.2f %.2f %.2f %.2f %.2f\n", jarak_y, d_tetha, pulses_depan, pulses_kanan, pulses_kiri);
calmantara186 6:5632ff2142c0 155 }
calmantara186 11:d4cf81f59601 156 if((jarak_x <= target_x[state]-(float)5.0)||(jarak_x >= target_x[state]+(float)5.0)){
calmantara186 10:e550e5daed74 157 /* masuk ke case sumbu X */
calmantara186 10:e550e5daed74 158 if(current_millis - prev_millis1 >= 12){
calmantara186 10:e550e5daed74 159 pidMotor(1);
calmantara186 10:e550e5daed74 160 }
calmantara186 10:e550e5daed74 161
calmantara186 10:e550e5daed74 162 /* Speed Motor */
calmantara186 10:e550e5daed74 163 limitMotor();
calmantara186 10:e550e5daed74 164 motor_depan.speed(pwm_depan);
calmantara186 10:e550e5daed74 165 motor_kanan.speed(pwm_kanan);
calmantara186 11:d4cf81f59601 166 motor_kiri.speed(pwm_kiri);
calmantara186 11:d4cf81f59601 167 //pc.printf("%.2f %.2f %.2f %.2f %.2f\n", jarak_x, d_tetha, rev_depan, rev_kanan, rev_kiri);
calmantara186 9:e2c3175936fb 168 }
calmantara186 7:cf822f5e7b12 169 else{
calmantara186 9:e2c3175936fb 170 pwm_depan = 0.0;
calmantara186 7:cf822f5e7b12 171 pwm_kanan = 0.0;
calmantara186 7:cf822f5e7b12 172 pwm_kiri = 0.0;
calmantara186 9:e2c3175936fb 173 motor_depan.brake(1);
calmantara186 7:cf822f5e7b12 174 motor_kanan.brake(1);
calmantara186 7:cf822f5e7b12 175 motor_kiri.brake(1);
calmantara186 11:d4cf81f59601 176 in_program = 0;
calmantara186 9:e2c3175936fb 177 }
calmantara186 9:e2c3175936fb 178
calmantara186 9:e2c3175936fb 179 /* mengambil data encoder motor */
calmantara186 9:e2c3175936fb 180 getValue();
calmantara186 7:cf822f5e7b12 181
calmantara186 7:cf822f5e7b12 182 /* masuk ke perhitungan parameter */
calmantara186 10:e550e5daed74 183 jarak_y += odometry.getY(rev_kanan, rev_kiri);
calmantara186 10:e550e5daed74 184 jarak_x += odometry.getX(rev_depan, rev_kanan, rev_kiri);
calmantara186 10:e550e5daed74 185 d_tetha = odometry.getTetha(pulses_depan, pulses_kanan, pulses_kiri);
calmantara186 9:e2c3175936fb 186 encMotor_depan.reset();
calmantara186 7:cf822f5e7b12 187 encMotor_kanan.reset();
calmantara186 7:cf822f5e7b12 188 encMotor_kiri.reset();
calmantara186 11:d4cf81f59601 189 //pc.printf("%.2f %.2f %.2f %.2f\n", jarak_y, d_tetha, pulses_kanan, pulses_kiri);
calmantara186 2:dea57b3f84cd 190 }
calmantara186 6:5632ff2142c0 191 } /* end of main program */
calmantara186 2:dea57b3f84cd 192 }
calmantara186 2:dea57b3f84cd 193
calmantara186 2:dea57b3f84cd 194 /************************************
calmantara186 2:dea57b3f84cd 195 * Deklarasi Fungsi dan Prosedur *
calmantara186 2:dea57b3f84cd 196 ************************************/
calmantara186 2:dea57b3f84cd 197 void limitMotor(){
calmantara186 2:dea57b3f84cd 198 /* Prosedur yang digunakan untuk mengecek batas limit pwm motor */
calmantara186 6:5632ff2142c0 199 if(pwm_kanan > limit_pwm){
calmantara186 1:5e0cb74f950e 200 pwm_kanan = limit_pwm;
calmantara186 6:5632ff2142c0 201 }
calmantara186 6:5632ff2142c0 202 else if(pwm_kanan < (-limit_pwm)){
calmantara186 6:5632ff2142c0 203 pwm_kanan = -limit_pwm;
calmantara186 6:5632ff2142c0 204 }
calmantara186 6:5632ff2142c0 205 if(pwm_kiri > limit_pwm){
calmantara186 6:5632ff2142c0 206 pwm_kiri = limit_pwm;
calmantara186 6:5632ff2142c0 207 }
calmantara186 6:5632ff2142c0 208 else if(pwm_kiri < (-limit_pwm)){
calmantara186 6:5632ff2142c0 209 pwm_kiri = -limit_pwm;
calmantara186 6:5632ff2142c0 210 }
calmantara186 6:5632ff2142c0 211 if(pwm_depan > limit_pwm){
calmantara186 6:5632ff2142c0 212 pwm_depan = limit_pwm;
calmantara186 6:5632ff2142c0 213 }
calmantara186 6:5632ff2142c0 214 else if(pwm_depan < (-limit_pwm)){
calmantara186 6:5632ff2142c0 215 pwm_depan = -limit_pwm;
calmantara186 6:5632ff2142c0 216 }
calmantara186 3:4801d83f397c 217 }/* end of limitMotor */
calmantara186 3:4801d83f397c 218
calmantara186 6:5632ff2142c0 219 void pidMotor(int x){
calmantara186 6:5632ff2142c0 220 /**
calmantara186 6:5632ff2142c0 221 * prosedur untuk gerak motor
calmantara186 6:5632ff2142c0 222 * 0 : maju dan mundur
calmantara186 6:5632ff2142c0 223 * 1 : kanan dan kiri
calmantara186 6:5632ff2142c0 224 **/
calmantara186 6:5632ff2142c0 225 if(x==0){
calmantara186 6:5632ff2142c0 226 /* masuk ke perhitungan ketika sudah pada time sampling */
calmantara186 9:e2c3175936fb 227 /* mengambil data encoder motor */
calmantara186 9:e2c3175936fb 228 getValue();
calmantara186 6:5632ff2142c0 229
calmantara186 6:5632ff2142c0 230 /* masuk ke perhitungan parameter */
calmantara186 10:e550e5daed74 231 jarak_y += odometry.getY(rev_kanan, rev_kiri);
calmantara186 10:e550e5daed74 232 d_tetha = odometry.getTetha(pulses_depan, pulses_kanan, pulses_kiri);
calmantara186 6:5632ff2142c0 233
calmantara186 10:e550e5daed74 234 speed_roty = pidRotY.pidCount(tn_tetha, d_tetha, limit_rot);
calmantara186 10:e550e5daed74 235 speed_disy = pidDisY.pidCount(target_y[0], jarak_y, limit_dis);
calmantara186 9:e2c3175936fb 236
calmantara186 6:5632ff2142c0 237 /* Kondisi untuk menambahkan speed */
calmantara186 7:cf822f5e7b12 238 pwm_kiri = -speed_disy + speed_roty;
calmantara186 6:5632ff2142c0 239 pwm_kanan = speed_disy + speed_roty;
calmantara186 9:e2c3175936fb 240 pwm_depan = speed_roty;
calmantara186 6:5632ff2142c0 241
calmantara186 6:5632ff2142c0 242 prev_millis1 = current_millis;
calmantara186 9:e2c3175936fb 243 encMotor_depan.reset();
calmantara186 6:5632ff2142c0 244 encMotor_kanan.reset();
calmantara186 6:5632ff2142c0 245 encMotor_kiri.reset();
calmantara186 6:5632ff2142c0 246
calmantara186 6:5632ff2142c0 247 }
calmantara186 7:cf822f5e7b12 248 else if(x==1){
calmantara186 7:cf822f5e7b12 249 /* masuk ke perhitungan ketika sudah pada time sampling */
calmantara186 9:e2c3175936fb 250 /* mengambil data encoder motor */
calmantara186 9:e2c3175936fb 251 getValue();
calmantara186 7:cf822f5e7b12 252
calmantara186 7:cf822f5e7b12 253 /* masuk ke perhitungan parameter */
calmantara186 10:e550e5daed74 254 jarak_x += odometry.getX(rev_depan, rev_kanan, rev_kiri);
calmantara186 10:e550e5daed74 255 d_tetha = odometry.getTetha(pulses_depan, pulses_kanan, pulses_kiri);
calmantara186 7:cf822f5e7b12 256
calmantara186 10:e550e5daed74 257 speed_rotx = pidRotX.pidCount(tn_tetha, d_tetha, limit_rot);
calmantara186 10:e550e5daed74 258 speed_disx = pidDisX.pidCount(target_x[0], jarak_x, limit_dis);
calmantara186 9:e2c3175936fb 259
calmantara186 7:cf822f5e7b12 260 /* Kondisi untuk menambahkan speed */
calmantara186 11:d4cf81f59601 261 pwm_kiri = ((float)0.63*speed_disx) + speed_rotx;
calmantara186 11:d4cf81f59601 262 pwm_kanan = ((float)0.63*speed_disx) + speed_rotx;
calmantara186 10:e550e5daed74 263 pwm_depan = -(speed_disx) + speed_rotx;
calmantara186 7:cf822f5e7b12 264
calmantara186 7:cf822f5e7b12 265 prev_millis1 = current_millis;
calmantara186 9:e2c3175936fb 266 encMotor_depan.reset();
calmantara186 7:cf822f5e7b12 267 encMotor_kanan.reset();
calmantara186 7:cf822f5e7b12 268 encMotor_kiri.reset();
calmantara186 7:cf822f5e7b12 269 }
calmantara186 6:5632ff2142c0 270 }/* end of pidMotor */
calmantara186 6:5632ff2142c0 271
calmantara186 9:e2c3175936fb 272 void getValue(){
calmantara186 9:e2c3175936fb 273 /*
calmantara186 9:e2c3175936fb 274 * Procedure yang digunakan untuk menghitung value pulses encoder
calmantara186 9:e2c3175936fb 275 * dan revolusi dari motor
calmantara186 9:e2c3175936fb 276 */
calmantara186 9:e2c3175936fb 277 pulses_depan = (float)encMotor_depan.getPulses();
calmantara186 9:e2c3175936fb 278 pulses_kanan = (float) encMotor_kanan.getPulses();
calmantara186 9:e2c3175936fb 279 pulses_kiri = (float) encMotor_kiri.getPulses();
calmantara186 10:e550e5daed74 280 rev_depan = (float) pulses_depan/540;
calmantara186 9:e2c3175936fb 281 rev_kanan = (float) pulses_kanan/540;
calmantara186 9:e2c3175936fb 282 rev_kiri = (float) pulses_kiri/540;
calmantara186 9:e2c3175936fb 283 }/* end of getValue */