Isro Syaeful Iman / Mbed 2 deprecated Thrower_KRAI

utk magang

Dependencies:   Motor Ultrasonic encoderKRAI mbed millis odometryKRAI pidKRAI

main.cpp@7:cf822f5e7b12, 2017-10-22 (annotated)

Committer:
calmantara186
Date:
Sun Oct 22 03:24:21 2017 +0000
Revision:
7:cf822f5e7b12
Parent:
6:5632ff2142c0
Child:
8:4a536a26ee07
Bismillah semua update dan to do list ada di program

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 6:5632ff2142c0 7 * - Buat library odometry
calmantara186 7:cf822f5e7b12 8 * - Rapihkan kodingan :)
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 0:a6918b57d3fa 24
calmantara186 0:a6918b57d3fa 25 /***************************
calmantara186 0:a6918b57d3fa 26 * Konstanta dan Variabel *
calmantara186 0:a6918b57d3fa 27 ***************************/
calmantara186 2:dea57b3f84cd 28 #define PI 3.141592
calmantara186 2:dea57b3f84cd 29 #define DRODA 100
calmantara186 2:dea57b3f84cd 30
calmantara186 2:dea57b3f84cd 31 //Variabel Robot
calmantara186 2:dea57b3f84cd 32 float kll_roda = (float) PI*DRODA; //variabel keliling robot
calmantara186 2:dea57b3f84cd 33
calmantara186 6:5632ff2142c0 34 //Variabel Program dan Planning
calmantara186 6:5632ff2142c0 35 unsigned short in_program = 1; //variabel untuk masuk ke looping program
calmantara186 7:cf822f5e7b12 36 unsigned short state = 0;
calmantara186 6:5632ff2142c0 37
calmantara186 2:dea57b3f84cd 38 //Primitive Function
calmantara186 6:5632ff2142c0 39 void limitMotor(); //procedure untuk mengecek limit pwm motor
calmantara186 6:5632ff2142c0 40 void pidMotor(int x); //procedure untuk arah gerak motor
calmantara186 6:5632ff2142c0 41 float getY(float y1, float y2); //fungsi untuk menghitung jarak Y
calmantara186 2:dea57b3f84cd 42
calmantara186 0:a6918b57d3fa 43 //Deklarasi Motor
calmantara186 0:a6918b57d3fa 44 Motor motor_depan(PB_6, PB_13, PA_10); //pwm, fwd, rev
calmantara186 0:a6918b57d3fa 45 Motor motor_kanan(PB_8, PA_9, PA_5); //pwm, fwd, rev
calmantara186 0:a6918b57d3fa 46 Motor motor_kiri(PB_9, PA_12, PA_11); //pwm, fwd, rev
calmantara186 0:a6918b57d3fa 47
calmantara186 2:dea57b3f84cd 48 //Variabel Kecepatan
calmantara186 6:5632ff2142c0 49 float pwm_kanan; //pwm motor kanan
calmantara186 6:5632ff2142c0 50 float pwm_kiri; //pwm motor kiri
calmantara186 6:5632ff2142c0 51 float pwm_depan; //pwm motor depan
calmantara186 6:5632ff2142c0 52 float limit_pwm = 0.9; //limit pwm semua motor
calmantara186 0:a6918b57d3fa 53
calmantara186 0:a6918b57d3fa 54 //Variabel Serial
calmantara186 0:a6918b57d3fa 55 Serial pc(USBTX, USBRX); //Deklarasi serial pc TX RX
calmantara186 0:a6918b57d3fa 56
calmantara186 0:a6918b57d3fa 57 //Deklarasi Rotary Encoder
calmantara186 6:5632ff2142c0 58 encoderKRAI encMotor_depan(PC_0, PC_1, 540, encoderKRAI::X4_ENCODING); //chA, chB, revPulse, interruptType
calmantara186 6:5632ff2142c0 59 encoderKRAI encMotor_kanan(PC_10, PC_11, 540, encoderKRAI::X4_ENCODING); //chA, chB, revPulse, interruptType
calmantara186 6:5632ff2142c0 60 encoderKRAI encMotor_kiri(PC_2, PC_3, 540, encoderKRAI::X4_ENCODING); //chA, chB, revPulse, interruptType
calmantara186 0:a6918b57d3fa 61
calmantara186 0:a6918b57d3fa 62 //Variabel Encoder
calmantara186 0:a6918b57d3fa 63 float pulses_depan; //variabel untuk membaca nilai encoder motor depan
calmantara186 0:a6918b57d3fa 64 float pulses_kanan; //variabel untuk membaca nilai encoder motor kanan
calmantara186 0:a6918b57d3fa 65 float pulses_kiri; //variabel untuk membaca nilai encoder motor kiri
calmantara186 6:5632ff2142c0 66
calmantara186 2:dea57b3f84cd 67 float delta_kaki; //variabel untuk membaca nilai delta kanan kiri
calmantara186 6:5632ff2142c0 68 float delta_semua; //variabel untuk membaca nilai delta semua encoder
calmantara186 6:5632ff2142c0 69
calmantara186 2:dea57b3f84cd 70 float rev_depan; //variabel untuk membaca nilai putaran roda depan
calmantara186 2:dea57b3f84cd 71 float rev_kanan; //variabel untuk membaca nilai putaran roda kanan
calmantara186 6:5632ff2142c0 72 float rev_kiri; //variabel untuk membaca nilai putaran roda kiri
calmantara186 0:a6918b57d3fa 73
calmantara186 0:a6918b57d3fa 74 //Target nilai encoder
calmantara186 6:5632ff2142c0 75 float tn_y = 0; //target untuk jumlah rotary encoder maju atau mundur
calmantara186 6:5632ff2142c0 76 float tn_x = 0; //target untuk jumlah rotary encoder kanan atau kiri
calmantara186 0:a6918b57d3fa 77
calmantara186 6:5632ff2142c0 78 //Variabel PID
calmantara186 6:5632ff2142c0 79 unsigned short ts_base = 12; //variabel untuk time sampling
calmantara186 6:5632ff2142c0 80 float speed_roty; //variabel untuk penambahan pwm PID rotasi y
calmantara186 6:5632ff2142c0 81 float speed_disy; //variabel untuk penambahan pwm PID distance y
calmantara186 7:cf822f5e7b12 82 float limit_y = 0.5;
calmantara186 6:5632ff2142c0 83 float speed_rotx; //variabel untuk penambahan pwm PID rotasi x
calmantara186 6:5632ff2142c0 84 float speed_disx; //variabel untuk penambahan pwm PID distance x
calmantara186 6:5632ff2142c0 85
calmantara186 6:5632ff2142c0 86 //Deklarasi PID
calmantara186 7:cf822f5e7b12 87 pidKRAI pidRotY((float) 0.00334, (float) 0.0, (float) 0.00000, ts_base); //kp ki kd ts
calmantara186 6:5632ff2142c0 88 pidKRAI pidRotX(); //kp ki kd ts
calmantara186 6:5632ff2142c0 89
calmantara186 7:cf822f5e7b12 90 pidKRAI pidDisY((float)0.228,(float).005534,(float)0.0,ts_base); //kp ki kd ts
calmantara186 6:5632ff2142c0 91 pidKRAI pidDisX(); //kp ki kd ts
calmantara186 2:dea57b3f84cd 92
calmantara186 2:dea57b3f84cd 93 //Variabel cari jarak
calmantara186 7:cf822f5e7b12 94 float target_y[100] = {-5000.0, -5000.0}; //variabel yang digunakan untuk target jarak y
calmantara186 6:5632ff2142c0 95 float jarak_y; //variabel untuk increment jarak y
calmantara186 0:a6918b57d3fa 96
calmantara186 6:5632ff2142c0 97 float target_x[100]; //variabel yang digunakan untuk target jarak x
calmantara186 6:5632ff2142c0 98 float jarak_x; //variabel untuk increment jarak x
calmantara186 6:5632ff2142c0 99
calmantara186 6:5632ff2142c0 100 //Variabel Waktu
calmantara186 6:5632ff2142c0 101 unsigned long int current_millis; //variabel untuk mendapatkan millis yang berjalan
calmantara186 6:5632ff2142c0 102 unsigned long int prev_millis1; //variabel untuk masuk ke perhitungan PID
calmantara186 6:5632ff2142c0 103 unsigned long int prev_millis2; //variabel untuk matikan motor
calmantara186 0:a6918b57d3fa 104
calmantara186 0:a6918b57d3fa 105 /***************************
calmantara186 0:a6918b57d3fa 106 * Main Function *
calmantara186 0:a6918b57d3fa 107 ***************************/
calmantara186 0:a6918b57d3fa 108
calmantara186 0:a6918b57d3fa 109 int main(){
calmantara186 0:a6918b57d3fa 110
calmantara186 0:a6918b57d3fa 111 //Inisiasi Serial
calmantara186 0:a6918b57d3fa 112 pc.baud(115200);
calmantara186 0:a6918b57d3fa 113
calmantara186 0:a6918b57d3fa 114 //Start Millis
calmantara186 2:dea57b3f84cd 115 wait_ms(3000);
calmantara186 0:a6918b57d3fa 116 startMillis();
calmantara186 6:5632ff2142c0 117 //Looping Utama
calmantara186 6:5632ff2142c0 118 while(1){
calmantara186 6:5632ff2142c0 119 //Looping Program Planning
calmantara186 6:5632ff2142c0 120 while(in_program == 0){
calmantara186 6:5632ff2142c0 121 }/* end of planning program */
calmantara186 6:5632ff2142c0 122 //Looping Program Utama
calmantara186 6:5632ff2142c0 123 while(in_program == 1){
calmantara186 7:cf822f5e7b12 124 //current_millis = millis();
calmantara186 7:cf822f5e7b12 125 if((jarak_y <= target_y[state]-(float)10.0)||(jarak_y >= target_y[state]+(float)10.0)){
calmantara186 7:cf822f5e7b12 126 current_millis = millis();
calmantara186 7:cf822f5e7b12 127 if(current_millis - prev_millis1 >= 12){
calmantara186 7:cf822f5e7b12 128 pidMotor(0);
calmantara186 7:cf822f5e7b12 129 pc.printf("%.2f %.2f\n", jarak_y, delta_kaki);
calmantara186 7:cf822f5e7b12 130 }
calmantara186 6:5632ff2142c0 131
calmantara186 7:cf822f5e7b12 132 /* Speed Motor */
calmantara186 7:cf822f5e7b12 133 limitMotor();
calmantara186 7:cf822f5e7b12 134 motor_kanan.speed(pwm_kanan);
calmantara186 7:cf822f5e7b12 135 motor_kiri.speed(pwm_kiri);
calmantara186 6:5632ff2142c0 136 }
calmantara186 7:cf822f5e7b12 137 else{
calmantara186 7:cf822f5e7b12 138 pwm_kanan = 0.0;
calmantara186 7:cf822f5e7b12 139 pwm_kiri = 0.0;
calmantara186 7:cf822f5e7b12 140 motor_kanan.brake(1);
calmantara186 7:cf822f5e7b12 141 motor_kiri.brake(1);
calmantara186 7:cf822f5e7b12 142 }
calmantara186 7:cf822f5e7b12 143 pulses_kanan = (float) encMotor_kanan.getPulses();
calmantara186 7:cf822f5e7b12 144 pulses_kiri = (float) encMotor_kiri.getPulses();
calmantara186 7:cf822f5e7b12 145 rev_kanan = (float) pulses_kanan/540;
calmantara186 7:cf822f5e7b12 146 rev_kiri = (float) pulses_kiri/540;
calmantara186 7:cf822f5e7b12 147
calmantara186 7:cf822f5e7b12 148 /* masuk ke perhitungan parameter */
calmantara186 7:cf822f5e7b12 149 jarak_y += getY(rev_kanan, rev_kiri);
calmantara186 7:cf822f5e7b12 150 delta_kaki = pulses_kanan + pulses_kiri;
calmantara186 7:cf822f5e7b12 151 encMotor_kanan.reset();
calmantara186 7:cf822f5e7b12 152 encMotor_kiri.reset();
calmantara186 7:cf822f5e7b12 153 pc.printf("%.2f %.2f\n", jarak_y, delta_kaki);
calmantara186 2:dea57b3f84cd 154 }
calmantara186 6:5632ff2142c0 155 } /* end of main program */
calmantara186 2:dea57b3f84cd 156 }
calmantara186 2:dea57b3f84cd 157
calmantara186 2:dea57b3f84cd 158 /************************************
calmantara186 2:dea57b3f84cd 159 * Deklarasi Fungsi dan Prosedur *
calmantara186 2:dea57b3f84cd 160 ************************************/
calmantara186 2:dea57b3f84cd 161 void limitMotor(){
calmantara186 2:dea57b3f84cd 162 /* Prosedur yang digunakan untuk mengecek batas limit pwm motor */
calmantara186 6:5632ff2142c0 163 if(pwm_kanan > limit_pwm){
calmantara186 1:5e0cb74f950e 164 pwm_kanan = limit_pwm;
calmantara186 6:5632ff2142c0 165 }
calmantara186 6:5632ff2142c0 166 else if(pwm_kanan < (-limit_pwm)){
calmantara186 6:5632ff2142c0 167 pwm_kanan = -limit_pwm;
calmantara186 6:5632ff2142c0 168 }
calmantara186 6:5632ff2142c0 169 if(pwm_kiri > limit_pwm){
calmantara186 6:5632ff2142c0 170 pwm_kiri = limit_pwm;
calmantara186 6:5632ff2142c0 171 }
calmantara186 6:5632ff2142c0 172 else if(pwm_kiri < (-limit_pwm)){
calmantara186 6:5632ff2142c0 173 pwm_kiri = -limit_pwm;
calmantara186 6:5632ff2142c0 174 }
calmantara186 6:5632ff2142c0 175 if(pwm_depan > limit_pwm){
calmantara186 6:5632ff2142c0 176 pwm_depan = limit_pwm;
calmantara186 6:5632ff2142c0 177 }
calmantara186 6:5632ff2142c0 178 else if(pwm_depan < (-limit_pwm)){
calmantara186 6:5632ff2142c0 179 pwm_depan = -limit_pwm;
calmantara186 6:5632ff2142c0 180 }
calmantara186 3:4801d83f397c 181 }/* end of limitMotor */
calmantara186 3:4801d83f397c 182
calmantara186 6:5632ff2142c0 183 void pidMotor(int x){
calmantara186 6:5632ff2142c0 184 /**
calmantara186 6:5632ff2142c0 185 * prosedur untuk gerak motor
calmantara186 6:5632ff2142c0 186 * 0 : maju dan mundur
calmantara186 6:5632ff2142c0 187 * 1 : kanan dan kiri
calmantara186 6:5632ff2142c0 188 **/
calmantara186 6:5632ff2142c0 189 if(x==0){
calmantara186 6:5632ff2142c0 190 /* masuk ke perhitungan ketika sudah pada time sampling */
calmantara186 6:5632ff2142c0 191 pulses_kanan = (float) encMotor_kanan.getPulses();
calmantara186 6:5632ff2142c0 192 pulses_kiri = (float) encMotor_kiri.getPulses();
calmantara186 6:5632ff2142c0 193 rev_kanan = (float) pulses_kanan/540;
calmantara186 6:5632ff2142c0 194 rev_kiri = (float) pulses_kiri/540;
calmantara186 6:5632ff2142c0 195
calmantara186 6:5632ff2142c0 196 /* masuk ke perhitungan parameter */
calmantara186 6:5632ff2142c0 197 jarak_y += getY(rev_kanan, rev_kiri);
calmantara186 6:5632ff2142c0 198 delta_kaki = pulses_kanan + pulses_kiri;
calmantara186 6:5632ff2142c0 199
calmantara186 7:cf822f5e7b12 200 speed_roty = pidRotY.pidCount(tn_y, delta_kaki, limit_y);
calmantara186 7:cf822f5e7b12 201 speed_disy = pidDisY.pidCount(target_y[0], jarak_y, limit_y);
calmantara186 6:5632ff2142c0 202 /* Kondisi untuk menambahkan speed */
calmantara186 7:cf822f5e7b12 203 pwm_kiri = -speed_disy + speed_roty;
calmantara186 6:5632ff2142c0 204 pwm_kanan = speed_disy + speed_roty;
calmantara186 6:5632ff2142c0 205
calmantara186 6:5632ff2142c0 206 prev_millis1 = current_millis;
calmantara186 6:5632ff2142c0 207 encMotor_kanan.reset();
calmantara186 6:5632ff2142c0 208 encMotor_kiri.reset();
calmantara186 6:5632ff2142c0 209
calmantara186 6:5632ff2142c0 210 }
calmantara186 7:cf822f5e7b12 211 else if(x==1){
calmantara186 7:cf822f5e7b12 212 /* masuk ke perhitungan ketika sudah pada time sampling */
calmantara186 7:cf822f5e7b12 213 pulses_kanan = (float) encMotor_kanan.getPulses();
calmantara186 7:cf822f5e7b12 214 pulses_kiri = (float) encMotor_kiri.getPulses();
calmantara186 7:cf822f5e7b12 215 pulses_depan = (float) encMotor_depan.getPulses();
calmantara186 7:cf822f5e7b12 216 rev_kanan = (float) pulses_kanan/540;
calmantara186 7:cf822f5e7b12 217 rev_kiri = (float) pulses_kiri/540;
calmantara186 7:cf822f5e7b12 218 rev_depan = (float) pulses_depan/540;
calmantara186 7:cf822f5e7b12 219
calmantara186 7:cf822f5e7b12 220 /* masuk ke perhitungan parameter */
calmantara186 7:cf822f5e7b12 221 //jarak_x += getX(rev_kanan+rev_kiri, rev_depan);
calmantara186 7:cf822f5e7b12 222 delta_semua = pulses_kanan + pulses_kiri + pulses_depan;
calmantara186 7:cf822f5e7b12 223
calmantara186 7:cf822f5e7b12 224 //speed_rotx = pidRotY.pidCount(tn_x, delta_semua);
calmantara186 7:cf822f5e7b12 225 //speed_disx = pidDisY.pidCount(target_y[0], jarak_y);
calmantara186 7:cf822f5e7b12 226 /* Kondisi untuk menambahkan speed */
calmantara186 7:cf822f5e7b12 227 pwm_kiri = speed_disx + speed_rotx;
calmantara186 7:cf822f5e7b12 228 pwm_kanan = speed_disx + speed_rotx;
calmantara186 7:cf822f5e7b12 229 pwm_depan = -(speed_disx + speed_rotx);
calmantara186 7:cf822f5e7b12 230
calmantara186 7:cf822f5e7b12 231 prev_millis1 = current_millis;
calmantara186 7:cf822f5e7b12 232 encMotor_kanan.reset();
calmantara186 7:cf822f5e7b12 233 encMotor_kiri.reset();
calmantara186 7:cf822f5e7b12 234 encMotor_depan.reset();
calmantara186 7:cf822f5e7b12 235 }
calmantara186 6:5632ff2142c0 236 }/* end of pidMotor */
calmantara186 6:5632ff2142c0 237
calmantara186 3:4801d83f397c 238 float getY(float y1, float y2){
calmantara186 3:4801d83f397c 239 /* fungsi untuk menghitung nilai Y dari roda kanan kiri */
calmantara186 7:cf822f5e7b12 240 float jarak_y1 =(float) 1.732*(kll_roda*y1)/2;
calmantara186 7:cf822f5e7b12 241 float jarak_y2 =(float) 1.732*(kll_roda*y2)/2;
calmantara186 3:4801d83f397c 242
calmantara186 7:cf822f5e7b12 243 return (jarak_y1 - jarak_y2)*2;
calmantara186 6:5632ff2142c0 244 }/* end of getY */