Isro Syaeful Iman / Mbed 2 deprecated Thrower_KRAI

utk magang

Dependencies:   Motor Ultrasonic encoderKRAI mbed millis odometryKRAI pidKRAI

main.cpp@3:4801d83f397c, 2017-10-19 (annotated)

Committer:
calmantara186
Date:
Thu Oct 19 13:32:49 2017 +0000
Revision:
3:4801d83f397c
Parent:
2:dea57b3f84cd
Child:
6:5632ff2142c0
v1.2

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 * Updated :
calmantara186 2:dea57b3f84cd 6 * -
calmantara186 0:a6918b57d3fa 7 *
calmantara186 0:a6918b57d3fa 8 * To do List :
calmantara186 0:a6918b57d3fa 9 * -Coba ODOMETRY !!
calmantara186 1:5e0cb74f950e 10 * -Coba baca encoder kanan dan kiri dulu
calmantara186 0:a6918b57d3fa 11 *
calmantara186 2:dea57b3f84cd 12 * UPDATED : 18/10
calmantara186 2:dea57b3f84cd 13 * - Dapet nilai mundur Kp = 0.00083, pwm kanan kiri = 0.4
calmantara186 2:dea57b3f84cd 14 * - Dapet nilai maju Kp = 0.00035
calmantara186 2:dea57b3f84cd 15 * Last Edited by : Calmantara & Robertsen
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 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 2:dea57b3f84cd 34 //Primitive Function
calmantara186 3:4801d83f397c 35 void limitMotor(); //procedure untuk mengecek limit pwm motor
calmantara186 3:4801d83f397c 36 float getY(float y1, float y2); //fungsi untuk menghitung jarak Y
calmantara186 2:dea57b3f84cd 37
calmantara186 0:a6918b57d3fa 38 //Deklarasi Motor
calmantara186 0:a6918b57d3fa 39 Motor motor_depan(PB_6, PB_13, PA_10); //pwm, fwd, rev
calmantara186 0:a6918b57d3fa 40 Motor motor_kanan(PB_8, PA_9, PA_5); //pwm, fwd, rev
calmantara186 0:a6918b57d3fa 41 Motor motor_kiri(PB_9, PA_12, PA_11); //pwm, fwd, rev
calmantara186 0:a6918b57d3fa 42
calmantara186 2:dea57b3f84cd 43 //Variabel Kecepatan
calmantara186 2:dea57b3f84cd 44 float pwm_kanan = 0.4;
calmantara186 2:dea57b3f84cd 45 float pwm_kiri = -0.4;
calmantara186 2:dea57b3f84cd 46 float limit_pwm = 0.8;
calmantara186 0:a6918b57d3fa 47
calmantara186 0:a6918b57d3fa 48 //Variabel Serial
calmantara186 0:a6918b57d3fa 49 Serial pc(USBTX, USBRX); //Deklarasi serial pc TX RX
calmantara186 0:a6918b57d3fa 50
calmantara186 0:a6918b57d3fa 51 //Deklarasi Rotary Encoder
calmantara186 0:a6918b57d3fa 52 encoderKRAI encMotor_depan(PC_0, PC_1, 540, encoderKRAI::X4_ENCODING); //chA, chB, revPulse, interruptType
calmantara186 2:dea57b3f84cd 53 encoderKRAI encMotor_kanan(PC_10, PC_11, 540, encoderKRAI::X4_ENCODING); //chA, chB, revPulse, interruptType
calmantara186 2:dea57b3f84cd 54 encoderKRAI encMotor_kiri(PC_2, PC_3, 540, encoderKRAI::X4_ENCODING); //chA, chB, revPulse, interruptType
calmantara186 0:a6918b57d3fa 55
calmantara186 0:a6918b57d3fa 56 //Variabel Encoder
calmantara186 0:a6918b57d3fa 57 float pulses_depan; //variabel untuk membaca nilai encoder motor depan
calmantara186 0:a6918b57d3fa 58 float pulses_kanan; //variabel untuk membaca nilai encoder motor kanan
calmantara186 0:a6918b57d3fa 59 float pulses_kiri; //variabel untuk membaca nilai encoder motor kiri
calmantara186 2:dea57b3f84cd 60 float delta_kaki; //variabel untuk membaca nilai delta kanan kiri
calmantara186 2:dea57b3f84cd 61 float rev_depan; //variabel untuk membaca nilai putaran roda depan
calmantara186 2:dea57b3f84cd 62 float rev_kanan; //variabel untuk membaca nilai putaran roda kanan
calmantara186 2:dea57b3f84cd 63 float rev_kiri; //variabel untuk membaca nilai putaran roda kiri
calmantara186 0:a6918b57d3fa 64
calmantara186 0:a6918b57d3fa 65 //Target nilai encoder
calmantara186 2:dea57b3f84cd 66 float target_depan = 0; //Target untuk jumlah rotary encoder gerakan
calmantara186 0:a6918b57d3fa 67
calmantara186 0:a6918b57d3fa 68 //Variabel PID_1
calmantara186 0:a6918b57d3fa 69 unsigned long int current_millis; //variabel untuk mendapatkan millis yang berjalan
calmantara186 0:a6918b57d3fa 70 unsigned long int prev_millis1; //variabel untuk mendapatkan millis sebelumnya
calmantara186 0:a6918b57d3fa 71 unsigned short time_sampling = 12; //variabel untuk time sampling
calmantara186 2:dea57b3f84cd 72 float kp_1 = 0.00035; //variabel konstanta PID
calmantara186 2:dea57b3f84cd 73 float current_error1, prev_error1; //variabel untuk millis PID
calmantara186 2:dea57b3f84cd 74 float speed, prev_speed; //variabel untuk penambahan pwm PID
calmantara186 2:dea57b3f84cd 75
calmantara186 2:dea57b3f84cd 76 //Variabel cari jarak
calmantara186 3:4801d83f397c 77 float target_jarak = 1100.0; //variabel yang digunakan untuk jarak odometry
calmantara186 2:dea57b3f84cd 78 float jarak_y; //variabel untuk increment jarak y
calmantara186 0:a6918b57d3fa 79
calmantara186 0:a6918b57d3fa 80 //Variabel Motor Berhenti
calmantara186 0:a6918b57d3fa 81 unsigned long int prev_millis2; //variabel untuk mendapatkan millis sebelumnya
calmantara186 0:a6918b57d3fa 82
calmantara186 0:a6918b57d3fa 83 /***************************
calmantara186 0:a6918b57d3fa 84 * Main Function *
calmantara186 0:a6918b57d3fa 85 ***************************/
calmantara186 0:a6918b57d3fa 86
calmantara186 0:a6918b57d3fa 87 int main(){
calmantara186 0:a6918b57d3fa 88
calmantara186 0:a6918b57d3fa 89 //Inisiasi Serial
calmantara186 0:a6918b57d3fa 90 pc.baud(115200);
calmantara186 0:a6918b57d3fa 91
calmantara186 0:a6918b57d3fa 92 //Start Millis
calmantara186 2:dea57b3f84cd 93 wait_ms(3000);
calmantara186 0:a6918b57d3fa 94 startMillis();
calmantara186 0:a6918b57d3fa 95
calmantara186 0:a6918b57d3fa 96 //Looping Program
calmantara186 0:a6918b57d3fa 97 while(1){
calmantara186 0:a6918b57d3fa 98 current_millis = millis();
calmantara186 1:5e0cb74f950e 99
calmantara186 0:a6918b57d3fa 100 /* Speed Motor */
calmantara186 2:dea57b3f84cd 101 motor_kanan.speed(pwm_kanan);
calmantara186 2:dea57b3f84cd 102 motor_kiri.speed(pwm_kiri);
calmantara186 2:dea57b3f84cd 103
calmantara186 3:4801d83f397c 104 pc.printf("%.2f %.2f %.2f %.2f\n", speed, pulses_kanan, pulses_kiri,jarak_y);
calmantara186 2:dea57b3f84cd 105
calmantara186 2:dea57b3f84cd 106 if(current_millis - prev_millis1 >= time_sampling){
calmantara186 2:dea57b3f84cd 107 /* masuk ke perhitungan ketika sudah pada time sampling */
calmantara186 2:dea57b3f84cd 108 pulses_kanan = (float) encMotor_kanan.getPulses();
calmantara186 2:dea57b3f84cd 109 pulses_kiri = (float) encMotor_kiri.getPulses();
calmantara186 3:4801d83f397c 110 rev_kanan = (float) pulses_kanan/540;
calmantara186 3:4801d83f397c 111 rev_kiri = (float) pulses_kiri/540;
calmantara186 3:4801d83f397c 112 /* masuk ke perhitungan parameter */
calmantara186 3:4801d83f397c 113 jarak_y += getY(rev_kanan, rev_kiri);
calmantara186 2:dea57b3f84cd 114 delta_kaki = pulses_kanan + pulses_kiri;
calmantara186 3:4801d83f397c 115
calmantara186 2:dea57b3f84cd 116 current_error1 = target_depan - delta_kaki;
calmantara186 2:dea57b3f84cd 117 speed = prev_speed + kp_1*current_error1 + (-kp_1)*prev_error1;
calmantara186 2:dea57b3f84cd 118 /* Kondisi untuk menambahkan speed */
calmantara186 2:dea57b3f84cd 119 pwm_kiri = pwm_kiri + speed;
calmantara186 2:dea57b3f84cd 120 pwm_kanan = pwm_kanan + speed;
calmantara186 2:dea57b3f84cd 121 prev_speed = speed;
calmantara186 2:dea57b3f84cd 122 prev_error1 = current_error1;
calmantara186 2:dea57b3f84cd 123 prev_millis1 = current_millis;
calmantara186 2:dea57b3f84cd 124 encMotor_kanan.reset();
calmantara186 2:dea57b3f84cd 125 encMotor_kiri.reset();
calmantara186 2:dea57b3f84cd 126 }
calmantara186 2:dea57b3f84cd 127
calmantara186 3:4801d83f397c 128 if(jarak_y > target_jarak){
calmantara186 2:dea57b3f84cd 129 pwm_kiri = 0;
calmantara186 2:dea57b3f84cd 130 pwm_kanan = 0;
calmantara186 2:dea57b3f84cd 131 }
calmantara186 2:dea57b3f84cd 132 }
calmantara186 2:dea57b3f84cd 133 } /* end of main fuction */
calmantara186 2:dea57b3f84cd 134
calmantara186 2:dea57b3f84cd 135 /************************************
calmantara186 2:dea57b3f84cd 136 * Deklarasi Fungsi dan Prosedur *
calmantara186 2:dea57b3f84cd 137 ************************************/
calmantara186 2:dea57b3f84cd 138 void limitMotor(){
calmantara186 2:dea57b3f84cd 139 /* Prosedur yang digunakan untuk mengecek batas limit pwm motor */
calmantara186 2:dea57b3f84cd 140 if(pwm_kanan > limit_pwm){
calmantara186 1:5e0cb74f950e 141 pwm_kanan = limit_pwm;
calmantara186 0:a6918b57d3fa 142 }
calmantara186 2:dea57b3f84cd 143 else if(pwm_kanan < (-limit_pwm)){
calmantara186 1:5e0cb74f950e 144 pwm_kanan = -limit_pwm;
calmantara186 0:a6918b57d3fa 145 }
calmantara186 1:5e0cb74f950e 146 if(pwm_kiri > limit_pwm){
calmantara186 1:5e0cb74f950e 147 pwm_kiri = limit_pwm;
calmantara186 0:a6918b57d3fa 148 }
calmantara186 2:dea57b3f84cd 149 else if(pwm_kiri < (-limit_pwm)){
calmantara186 1:5e0cb74f950e 150 pwm_kiri = -limit_pwm;
calmantara186 0:a6918b57d3fa 151 }
calmantara186 3:4801d83f397c 152 }/* end of limitMotor */
calmantara186 3:4801d83f397c 153
calmantara186 3:4801d83f397c 154 float getY(float y1, float y2){
calmantara186 3:4801d83f397c 155 /* fungsi untuk menghitung nilai Y dari roda kanan kiri */
calmantara186 3:4801d83f397c 156 float jarak_y1 = (kll_roda*y1)/4;
calmantara186 3:4801d83f397c 157 float jarak_y2 = (kll_roda*y2)/4;
calmantara186 3:4801d83f397c 158
calmantara186 3:4801d83f397c 159 return jarak_y1 - jarak_y2;
calmantara186 3:4801d83f397c 160 }/* end of getY */