Isro Syaeful Iman / Mbed 2 deprecated Thrower_KRAI

utk magang

Dependencies:   Motor Ultrasonic encoderKRAI mbed millis odometryKRAI pidKRAI

main.cpp@12:84cb23216f78, 2018-03-31 (annotated)

Committer:
gatulz
Date:
Sat Mar 31 01:44:42 2018 +0000
Revision:
12:84cb23216f78
Parent:
11:d4cf81f59601 
utk magang

Who changed what in which revision?

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