Isro Syaeful Iman / Mbed 2 deprecated Thrower_KRAI

utk magang

Dependencies:   Motor Ultrasonic encoderKRAI mbed millis odometryKRAI pidKRAI

main.cpp@8:4a536a26ee07, 2017-10-22 (annotated)

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