Isro Syaeful Iman / Mbed 2 deprecated Thrower_KRAI

utk magang

Dependencies:   Motor Ultrasonic encoderKRAI mbed millis odometryKRAI pidKRAI

main.cpp@10:e550e5daed74, 2017-10-24 (annotated)

Committer:
calmantara186
Date:
Tue Oct 24 23:56:39 2017 +0000
Revision:
10:e550e5daed74
Parent:
9:e2c3175936fb
Child:
11:d4cf81f59601
Update ! To do list available in 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 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 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 9:e2c3175936fb 39 void limitMotor(); //procedure untuk mengecek limit pwm motor
calmantara186 9:e2c3175936fb 40 void pidMotor(int x); //procedure untuk arah gerak motor
calmantara186 9:e2c3175936fb 41 void getValue(); //procedure untuk menghitung pulses dan rev
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 10:e550e5daed74 62 //Deklarasi Odometry
calmantara186 10:e550e5daed74 63 odometryKRAI odometry(DRODA); //deklarasi odometry dengan input diameter roda
calmantara186 10:e550e5daed74 64
calmantara186 0:a6918b57d3fa 65 //Variabel Encoder
calmantara186 0:a6918b57d3fa 66 float pulses_depan; //variabel untuk membaca nilai encoder motor depan
calmantara186 0:a6918b57d3fa 67 float pulses_kanan; //variabel untuk membaca nilai encoder motor kanan
calmantara186 0:a6918b57d3fa 68 float pulses_kiri; //variabel untuk membaca nilai encoder motor kiri
calmantara186 6:5632ff2142c0 69
calmantara186 10:e550e5daed74 70 float d_tetha; //variabel untuk membaca nilai delta kanan kiri depan
calmantara186 10:e550e5daed74 71 float d_tetha2; //variabel untuk membaca nilai delta kanan kiri
calmantara186 6:5632ff2142c0 72
calmantara186 2:dea57b3f84cd 73 float rev_depan; //variabel untuk membaca nilai putaran roda depan
calmantara186 2:dea57b3f84cd 74 float rev_kanan; //variabel untuk membaca nilai putaran roda kanan
calmantara186 6:5632ff2142c0 75 float rev_kiri; //variabel untuk membaca nilai putaran roda kiri
calmantara186 0:a6918b57d3fa 76
calmantara186 0:a6918b57d3fa 77 //Target nilai encoder
calmantara186 9:e2c3175936fb 78 float tn_tetha = 0; //target untuk nilai tetha robot
calmantara186 0:a6918b57d3fa 79
calmantara186 6:5632ff2142c0 80 //Variabel PID
calmantara186 6:5632ff2142c0 81 unsigned short ts_base = 12; //variabel untuk time sampling
calmantara186 6:5632ff2142c0 82 float speed_roty; //variabel untuk penambahan pwm PID rotasi y
calmantara186 6:5632ff2142c0 83 float speed_disy; //variabel untuk penambahan pwm PID distance y
calmantara186 10:e550e5daed74 84 float limit_dis = 0.5;
calmantara186 10:e550e5daed74 85 float limit_rot = 0.3;
calmantara186 9:e2c3175936fb 86
calmantara186 6:5632ff2142c0 87 float speed_rotx; //variabel untuk penambahan pwm PID rotasi x
calmantara186 6:5632ff2142c0 88 float speed_disx; //variabel untuk penambahan pwm PID distance x
calmantara186 6:5632ff2142c0 89
calmantara186 6:5632ff2142c0 90 //Deklarasi PID
calmantara186 10:e550e5daed74 91 pidKRAI pidRotY((float) 0.134, (float) 0.0, (float) 0.2123, ts_base); //kp ki kd ts
calmantara186 10:e550e5daed74 92 pidKRAI pidRotX((float) 0.134, (float) 0.0, (float) 0.2123, ts_base); //kp ki kd ts
calmantara186 6:5632ff2142c0 93
calmantara186 10:e550e5daed74 94 pidKRAI pidDisY((float)0.228,(float).005534,(float)0.3231,ts_base); //kp ki kd ts
calmantara186 10:e550e5daed74 95 pidKRAI pidDisX((float)0.228,(float).005534,(float)0.3231,ts_base); //kp ki kd ts
calmantara186 2:dea57b3f84cd 96
calmantara186 2:dea57b3f84cd 97 //Variabel cari jarak
calmantara186 10:e550e5daed74 98 float target_y[100] = {2000.0, -5000.0}; //variabel yang digunakan untuk target jarak y
calmantara186 9:e2c3175936fb 99 double jarak_y; //variabel untuk increment jarak y
calmantara186 0:a6918b57d3fa 100
calmantara186 10:e550e5daed74 101 float target_x[100] = {2000.0, -5000.0}; //variabel yang digunakan untuk target jarak x
calmantara186 9:e2c3175936fb 102 double jarak_x; //variabel untuk increment jarak x
calmantara186 6:5632ff2142c0 103
calmantara186 6:5632ff2142c0 104 //Variabel Waktu
calmantara186 6:5632ff2142c0 105 unsigned long int current_millis; //variabel untuk mendapatkan millis yang berjalan
calmantara186 6:5632ff2142c0 106 unsigned long int prev_millis1; //variabel untuk masuk ke perhitungan PID
calmantara186 10:e550e5daed74 107 unsigned long int prev_millis2;
calmantara186 0:a6918b57d3fa 108
calmantara186 0:a6918b57d3fa 109 /***************************
calmantara186 0:a6918b57d3fa 110 * Main Function *
calmantara186 0:a6918b57d3fa 111 ***************************/
calmantara186 0:a6918b57d3fa 112
calmantara186 0:a6918b57d3fa 113 int main(){
calmantara186 0:a6918b57d3fa 114
calmantara186 0:a6918b57d3fa 115 //Inisiasi Serial
calmantara186 0:a6918b57d3fa 116 pc.baud(115200);
calmantara186 0:a6918b57d3fa 117
calmantara186 0:a6918b57d3fa 118 //Start Millis
calmantara186 2:dea57b3f84cd 119 wait_ms(3000);
calmantara186 0:a6918b57d3fa 120 startMillis();
calmantara186 6:5632ff2142c0 121 //Looping Utama
calmantara186 6:5632ff2142c0 122 while(1){
calmantara186 6:5632ff2142c0 123 //Looping Program Planning
calmantara186 6:5632ff2142c0 124 while(in_program == 0){
calmantara186 6:5632ff2142c0 125 }/* end of planning program */
calmantara186 6:5632ff2142c0 126 //Looping Program Utama
calmantara186 6:5632ff2142c0 127 while(in_program == 1){
calmantara186 10:e550e5daed74 128 current_millis = millis();
calmantara186 7:cf822f5e7b12 129 if((jarak_y <= target_y[state]-(float)10.0)||(jarak_y >= target_y[state]+(float)10.0)){
calmantara186 9:e2c3175936fb 130 /* masuk ke case sumbu Y */
calmantara186 7:cf822f5e7b12 131 if(current_millis - prev_millis1 >= 12){
calmantara186 7:cf822f5e7b12 132 pidMotor(0);
calmantara186 10:e550e5daed74 133 //pc.printf("%.2f %.2f %.2f %.2f %.2f\n", jarak_y, jarak_x, d_tetha, speed_roty, speed_disy);
calmantara186 7:cf822f5e7b12 134 }
calmantara186 6:5632ff2142c0 135
calmantara186 7:cf822f5e7b12 136 /* Speed Motor */
calmantara186 7:cf822f5e7b12 137 limitMotor();
calmantara186 10:e550e5daed74 138 motor_depan.speed(pwm_depan);
calmantara186 7:cf822f5e7b12 139 motor_kanan.speed(pwm_kanan);
calmantara186 7:cf822f5e7b12 140 motor_kiri.speed(pwm_kiri);
calmantara186 10:e550e5daed74 141 prev_millis2 = current_millis;
calmantara186 6:5632ff2142c0 142 }
calmantara186 9:e2c3175936fb 143 else if((jarak_x <= target_x[state]-(float)10.0)||(jarak_x >= target_x[state]+(float)10.0)){
calmantara186 10:e550e5daed74 144 /* masuk ke case sumbu X */
calmantara186 10:e550e5daed74 145 //current_millis = millis();
calmantara186 10:e550e5daed74 146 if(current_millis - prev_millis1 >= 12){
calmantara186 10:e550e5daed74 147 pidMotor(1);
calmantara186 10:e550e5daed74 148 //pc.printf("%.2f %.2f %.2f %.2f %.2f %.2f %.2f\n", jarak_x, d_tetha, speed_rotx, speed_disx, pulses_depan, pulses_kanan, pulses_kiri);
calmantara186 10:e550e5daed74 149 }
calmantara186 10:e550e5daed74 150
calmantara186 10:e550e5daed74 151 /* Speed Motor */
calmantara186 10:e550e5daed74 152 limitMotor();
calmantara186 10:e550e5daed74 153 motor_depan.speed(pwm_depan);
calmantara186 10:e550e5daed74 154 motor_kanan.speed(pwm_kanan);
calmantara186 10:e550e5daed74 155 motor_kiri.speed(pwm_kiri);
calmantara186 10:e550e5daed74 156 prev_millis2 = current_millis;
calmantara186 9:e2c3175936fb 157 }
calmantara186 7:cf822f5e7b12 158 else{
calmantara186 9:e2c3175936fb 159 pwm_depan = 0.0;
calmantara186 7:cf822f5e7b12 160 pwm_kanan = 0.0;
calmantara186 7:cf822f5e7b12 161 pwm_kiri = 0.0;
calmantara186 9:e2c3175936fb 162 motor_depan.brake(1);
calmantara186 7:cf822f5e7b12 163 motor_kanan.brake(1);
calmantara186 7:cf822f5e7b12 164 motor_kiri.brake(1);
calmantara186 9:e2c3175936fb 165 }
calmantara186 9:e2c3175936fb 166
calmantara186 9:e2c3175936fb 167 /* mengambil data encoder motor */
calmantara186 9:e2c3175936fb 168 getValue();
calmantara186 7:cf822f5e7b12 169
calmantara186 7:cf822f5e7b12 170 /* masuk ke perhitungan parameter */
calmantara186 10:e550e5daed74 171 jarak_y += odometry.getY(rev_kanan, rev_kiri);
calmantara186 10:e550e5daed74 172 jarak_x += odometry.getX(rev_depan, rev_kanan, rev_kiri);
calmantara186 10:e550e5daed74 173 d_tetha = odometry.getTetha(pulses_depan, pulses_kanan, pulses_kiri);
calmantara186 9:e2c3175936fb 174 encMotor_depan.reset();
calmantara186 7:cf822f5e7b12 175 encMotor_kanan.reset();
calmantara186 7:cf822f5e7b12 176 encMotor_kiri.reset();
calmantara186 10:e550e5daed74 177 pc.printf("%.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f\n", jarak_y, jarak_x, d_tetha, speed_rotx, speed_disx, pulses_depan, pulses_kanan, pulses_kiri);
calmantara186 2:dea57b3f84cd 178 }
calmantara186 6:5632ff2142c0 179 } /* end of main program */
calmantara186 2:dea57b3f84cd 180 }
calmantara186 2:dea57b3f84cd 181
calmantara186 2:dea57b3f84cd 182 /************************************
calmantara186 2:dea57b3f84cd 183 * Deklarasi Fungsi dan Prosedur *
calmantara186 2:dea57b3f84cd 184 ************************************/
calmantara186 2:dea57b3f84cd 185 void limitMotor(){
calmantara186 2:dea57b3f84cd 186 /* Prosedur yang digunakan untuk mengecek batas limit pwm motor */
calmantara186 6:5632ff2142c0 187 if(pwm_kanan > limit_pwm){
calmantara186 1:5e0cb74f950e 188 pwm_kanan = limit_pwm;
calmantara186 6:5632ff2142c0 189 }
calmantara186 6:5632ff2142c0 190 else if(pwm_kanan < (-limit_pwm)){
calmantara186 6:5632ff2142c0 191 pwm_kanan = -limit_pwm;
calmantara186 6:5632ff2142c0 192 }
calmantara186 6:5632ff2142c0 193 if(pwm_kiri > limit_pwm){
calmantara186 6:5632ff2142c0 194 pwm_kiri = limit_pwm;
calmantara186 6:5632ff2142c0 195 }
calmantara186 6:5632ff2142c0 196 else if(pwm_kiri < (-limit_pwm)){
calmantara186 6:5632ff2142c0 197 pwm_kiri = -limit_pwm;
calmantara186 6:5632ff2142c0 198 }
calmantara186 6:5632ff2142c0 199 if(pwm_depan > limit_pwm){
calmantara186 6:5632ff2142c0 200 pwm_depan = limit_pwm;
calmantara186 6:5632ff2142c0 201 }
calmantara186 6:5632ff2142c0 202 else if(pwm_depan < (-limit_pwm)){
calmantara186 6:5632ff2142c0 203 pwm_depan = -limit_pwm;
calmantara186 6:5632ff2142c0 204 }
calmantara186 3:4801d83f397c 205 }/* end of limitMotor */
calmantara186 3:4801d83f397c 206
calmantara186 6:5632ff2142c0 207 void pidMotor(int x){
calmantara186 6:5632ff2142c0 208 /**
calmantara186 6:5632ff2142c0 209 * prosedur untuk gerak motor
calmantara186 6:5632ff2142c0 210 * 0 : maju dan mundur
calmantara186 6:5632ff2142c0 211 * 1 : kanan dan kiri
calmantara186 6:5632ff2142c0 212 **/
calmantara186 6:5632ff2142c0 213 if(x==0){
calmantara186 6:5632ff2142c0 214 /* masuk ke perhitungan ketika sudah pada time sampling */
calmantara186 9:e2c3175936fb 215 /* mengambil data encoder motor */
calmantara186 9:e2c3175936fb 216 getValue();
calmantara186 6:5632ff2142c0 217
calmantara186 6:5632ff2142c0 218 /* masuk ke perhitungan parameter */
calmantara186 10:e550e5daed74 219 jarak_y += odometry.getY(rev_kanan, rev_kiri);
calmantara186 10:e550e5daed74 220 d_tetha = odometry.getTetha(pulses_depan, pulses_kanan, pulses_kiri);
calmantara186 6:5632ff2142c0 221
calmantara186 10:e550e5daed74 222 speed_roty = pidRotY.pidCount(tn_tetha, d_tetha, limit_rot);
calmantara186 10:e550e5daed74 223 speed_disy = pidDisY.pidCount(target_y[0], jarak_y, limit_dis);
calmantara186 9:e2c3175936fb 224
calmantara186 6:5632ff2142c0 225 /* Kondisi untuk menambahkan speed */
calmantara186 7:cf822f5e7b12 226 pwm_kiri = -speed_disy + speed_roty;
calmantara186 6:5632ff2142c0 227 pwm_kanan = speed_disy + speed_roty;
calmantara186 9:e2c3175936fb 228 pwm_depan = speed_roty;
calmantara186 6:5632ff2142c0 229
calmantara186 6:5632ff2142c0 230 prev_millis1 = current_millis;
calmantara186 9:e2c3175936fb 231 encMotor_depan.reset();
calmantara186 6:5632ff2142c0 232 encMotor_kanan.reset();
calmantara186 6:5632ff2142c0 233 encMotor_kiri.reset();
calmantara186 6:5632ff2142c0 234
calmantara186 6:5632ff2142c0 235 }
calmantara186 7:cf822f5e7b12 236 else if(x==1){
calmantara186 7:cf822f5e7b12 237 /* masuk ke perhitungan ketika sudah pada time sampling */
calmantara186 9:e2c3175936fb 238 /* mengambil data encoder motor */
calmantara186 9:e2c3175936fb 239 getValue();
calmantara186 7:cf822f5e7b12 240
calmantara186 7:cf822f5e7b12 241 /* masuk ke perhitungan parameter */
calmantara186 10:e550e5daed74 242 jarak_x += odometry.getX(rev_depan, rev_kanan, rev_kiri);
calmantara186 10:e550e5daed74 243 d_tetha = odometry.getTetha(pulses_depan, pulses_kanan, pulses_kiri);
calmantara186 7:cf822f5e7b12 244
calmantara186 10:e550e5daed74 245 speed_rotx = pidRotX.pidCount(tn_tetha, d_tetha, limit_rot);
calmantara186 10:e550e5daed74 246 speed_disx = pidDisX.pidCount(target_x[0], jarak_x, limit_dis);
calmantara186 9:e2c3175936fb 247
calmantara186 7:cf822f5e7b12 248 /* Kondisi untuk menambahkan speed */
calmantara186 10:e550e5daed74 249 pwm_kiri = ((float)0.5*speed_disx) + speed_rotx;
calmantara186 10:e550e5daed74 250 pwm_kanan = ((float)0.5*speed_disx) + speed_rotx;
calmantara186 10:e550e5daed74 251 pwm_depan = -(speed_disx) + speed_rotx;
calmantara186 7:cf822f5e7b12 252
calmantara186 7:cf822f5e7b12 253 prev_millis1 = current_millis;
calmantara186 9:e2c3175936fb 254 encMotor_depan.reset();
calmantara186 7:cf822f5e7b12 255 encMotor_kanan.reset();
calmantara186 7:cf822f5e7b12 256 encMotor_kiri.reset();
calmantara186 7:cf822f5e7b12 257 }
calmantara186 6:5632ff2142c0 258 }/* end of pidMotor */
calmantara186 6:5632ff2142c0 259
calmantara186 9:e2c3175936fb 260 void getValue(){
calmantara186 9:e2c3175936fb 261 /*
calmantara186 9:e2c3175936fb 262 * Procedure yang digunakan untuk menghitung value pulses encoder
calmantara186 9:e2c3175936fb 263 * dan revolusi dari motor
calmantara186 9:e2c3175936fb 264 */
calmantara186 9:e2c3175936fb 265 pulses_depan = (float)encMotor_depan.getPulses();
calmantara186 9:e2c3175936fb 266 pulses_kanan = (float) encMotor_kanan.getPulses();
calmantara186 9:e2c3175936fb 267 pulses_kiri = (float) encMotor_kiri.getPulses();
calmantara186 10:e550e5daed74 268 rev_depan = (float) pulses_depan/540;
calmantara186 9:e2c3175936fb 269 rev_kanan = (float) pulses_kanan/540;
calmantara186 9:e2c3175936fb 270 rev_kiri = (float) pulses_kiri/540;
calmantara186 9:e2c3175936fb 271 }/* end of getValue */