Isro Syaeful Iman / Mbed 2 deprecated Thrower_KRAI

utk magang

Dependencies:   Motor Ultrasonic encoderKRAI mbed millis odometryKRAI pidKRAI

main.cpp@6:5632ff2142c0, 2017-10-21 (annotated)

Committer:
calmantara186
Date:
Sat Oct 21 06:34:05 2017 +0000
Revision:
6:5632ff2142c0
Parent:
3:4801d83f397c
Child:
7:cf822f5e7b12
thrower robot : 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 6:5632ff2142c0 6 * - Cari Fungsi Output Open Loop
calmantara186 6:5632ff2142c0 7 * - Buat library odometry
calmantara186 0:a6918b57d3fa 8 *
calmantara186 2:dea57b3f84cd 9 * UPDATED : 18/10
calmantara186 2:dea57b3f84cd 10 * - Dapet nilai mundur Kp = 0.00083, pwm kanan kiri = 0.4
calmantara186 6:5632ff2142c0 11 * - Dapet nilai maju Kp = 0.00035, pwm kanan kiri = 0.4
calmantara186 6:5632ff2142c0 12 *
calmantara186 6:5632ff2142c0 13 * Update : 19/10
calmantara186 6:5632ff2142c0 14 * - Inlcude library PID
calmantara186 6:5632ff2142c0 15 * - Lebih rapi
calmantara186 6:5632ff2142c0 16 *
calmantara186 6:5632ff2142c0 17 * Last Edited by : Thrower KRAI
calmantara186 0:a6918b57d3fa 18 **/
calmantara186 2:dea57b3f84cd 19
calmantara186 0:a6918b57d3fa 20
calmantara186 0:a6918b57d3fa 21 //LIBRARY
calmantara186 0:a6918b57d3fa 22 #include "mbed.h"
calmantara186 0:a6918b57d3fa 23 #include "Motor.h"
calmantara186 0:a6918b57d3fa 24 #include "encoderKRAI.h"
calmantara186 0:a6918b57d3fa 25 #include "millis.h"
calmantara186 6:5632ff2142c0 26 #include "pidKRAI.h"
calmantara186 0:a6918b57d3fa 27
calmantara186 0:a6918b57d3fa 28 /***************************
calmantara186 0:a6918b57d3fa 29 * Konstanta dan Variabel *
calmantara186 0:a6918b57d3fa 30 ***************************/
calmantara186 2:dea57b3f84cd 31 #define PI 3.141592
calmantara186 2:dea57b3f84cd 32 #define DRODA 100
calmantara186 2:dea57b3f84cd 33
calmantara186 2:dea57b3f84cd 34 //Variabel Robot
calmantara186 2:dea57b3f84cd 35 float kll_roda = (float) PI*DRODA; //variabel keliling robot
calmantara186 2:dea57b3f84cd 36
calmantara186 6:5632ff2142c0 37 //Variabel Program dan Planning
calmantara186 6:5632ff2142c0 38 unsigned short in_program = 1; //variabel untuk masuk ke looping program
calmantara186 6:5632ff2142c0 39
calmantara186 2:dea57b3f84cd 40 //Primitive Function
calmantara186 6:5632ff2142c0 41 void limitMotor(); //procedure untuk mengecek limit pwm motor
calmantara186 6:5632ff2142c0 42 void pidMotor(int x); //procedure untuk arah gerak motor
calmantara186 6:5632ff2142c0 43 float getY(float y1, float y2); //fungsi untuk menghitung jarak Y
calmantara186 2:dea57b3f84cd 44
calmantara186 0:a6918b57d3fa 45 //Deklarasi Motor
calmantara186 0:a6918b57d3fa 46 Motor motor_depan(PB_6, PB_13, PA_10); //pwm, fwd, rev
calmantara186 0:a6918b57d3fa 47 Motor motor_kanan(PB_8, PA_9, PA_5); //pwm, fwd, rev
calmantara186 0:a6918b57d3fa 48 Motor motor_kiri(PB_9, PA_12, PA_11); //pwm, fwd, rev
calmantara186 0:a6918b57d3fa 49
calmantara186 2:dea57b3f84cd 50 //Variabel Kecepatan
calmantara186 6:5632ff2142c0 51 float pwm_kanan; //pwm motor kanan
calmantara186 6:5632ff2142c0 52 float pwm_kiri; //pwm motor kiri
calmantara186 6:5632ff2142c0 53 float pwm_depan; //pwm motor depan
calmantara186 6:5632ff2142c0 54 float limit_pwm = 0.9; //limit pwm semua motor
calmantara186 0:a6918b57d3fa 55
calmantara186 0:a6918b57d3fa 56 //Variabel Serial
calmantara186 0:a6918b57d3fa 57 Serial pc(USBTX, USBRX); //Deklarasi serial pc TX RX
calmantara186 0:a6918b57d3fa 58
calmantara186 0:a6918b57d3fa 59 //Deklarasi Rotary Encoder
calmantara186 6:5632ff2142c0 60 encoderKRAI encMotor_depan(PC_0, PC_1, 540, encoderKRAI::X4_ENCODING); //chA, chB, revPulse, interruptType
calmantara186 6:5632ff2142c0 61 encoderKRAI encMotor_kanan(PC_10, PC_11, 540, encoderKRAI::X4_ENCODING); //chA, chB, revPulse, interruptType
calmantara186 6:5632ff2142c0 62 encoderKRAI encMotor_kiri(PC_2, PC_3, 540, encoderKRAI::X4_ENCODING); //chA, chB, revPulse, interruptType
calmantara186 0:a6918b57d3fa 63
calmantara186 0:a6918b57d3fa 64 //Variabel Encoder
calmantara186 0:a6918b57d3fa 65 float pulses_depan; //variabel untuk membaca nilai encoder motor depan
calmantara186 0:a6918b57d3fa 66 float pulses_kanan; //variabel untuk membaca nilai encoder motor kanan
calmantara186 0:a6918b57d3fa 67 float pulses_kiri; //variabel untuk membaca nilai encoder motor kiri
calmantara186 6:5632ff2142c0 68
calmantara186 2:dea57b3f84cd 69 float delta_kaki; //variabel untuk membaca nilai delta kanan kiri
calmantara186 6:5632ff2142c0 70 float delta_semua; //variabel untuk membaca nilai delta semua encoder
calmantara186 6:5632ff2142c0 71
calmantara186 2:dea57b3f84cd 72 float rev_depan; //variabel untuk membaca nilai putaran roda depan
calmantara186 2:dea57b3f84cd 73 float rev_kanan; //variabel untuk membaca nilai putaran roda kanan
calmantara186 6:5632ff2142c0 74 float rev_kiri; //variabel untuk membaca nilai putaran roda kiri
calmantara186 0:a6918b57d3fa 75
calmantara186 0:a6918b57d3fa 76 //Target nilai encoder
calmantara186 6:5632ff2142c0 77 float tn_y = 0; //target untuk jumlah rotary encoder maju atau mundur
calmantara186 6:5632ff2142c0 78 float tn_x = 0; //target untuk jumlah rotary encoder kanan atau kiri
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 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 6:5632ff2142c0 88 pidKRAI pidRotY((float) 0.0, (float) 0.0, (float) 0.0, ts_base); //kp ki kd ts
calmantara186 6:5632ff2142c0 89 pidKRAI pidRotX(); //kp ki kd ts
calmantara186 6:5632ff2142c0 90
calmantara186 6:5632ff2142c0 91 pidKRAI pidDisY((float)0.0,(float)0.0,(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 6:5632ff2142c0 95 float target_y[100] = {1100.0, 0.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 6:5632ff2142c0 125 current_millis = millis();
calmantara186 6:5632ff2142c0 126
calmantara186 6:5632ff2142c0 127 /* Speed Motor */
calmantara186 6:5632ff2142c0 128 motor_kanan.speed(pwm_kanan);
calmantara186 6:5632ff2142c0 129 motor_kiri.speed(pwm_kiri);
calmantara186 6:5632ff2142c0 130
calmantara186 6:5632ff2142c0 131 pc.printf("%.2f \n", delta_kaki);
calmantara186 6:5632ff2142c0 132
calmantara186 6:5632ff2142c0 133 if(current_millis - prev_millis1 >= ts_base){
calmantara186 6:5632ff2142c0 134 //pidMotor(1);
calmantara186 6:5632ff2142c0 135 pulses_kanan = (float) encMotor_kanan.getPulses();
calmantara186 6:5632ff2142c0 136 pulses_kiri = (float) encMotor_kiri.getPulses();
calmantara186 6:5632ff2142c0 137 rev_kanan = (float) encMotor_kanan.getRevolutions();
calmantara186 6:5632ff2142c0 138 rev_kiri = (float) encMotor_kiri.getRevolutions();
calmantara186 6:5632ff2142c0 139
calmantara186 6:5632ff2142c0 140 /* masuk ke perhitungan parameter */
calmantara186 6:5632ff2142c0 141 jarak_y += getY(rev_kanan, rev_kiri);
calmantara186 6:5632ff2142c0 142 delta_kaki = pulses_kanan + pulses_kiri;
calmantara186 6:5632ff2142c0 143 }
calmantara186 6:5632ff2142c0 144
calmantara186 6:5632ff2142c0 145 if(jarak_y > target_y[0]){
calmantara186 6:5632ff2142c0 146 pwm_kiri = 0;
calmantara186 6:5632ff2142c0 147 pwm_kanan = 0;
calmantara186 6:5632ff2142c0 148 }
calmantara186 2:dea57b3f84cd 149 }
calmantara186 6:5632ff2142c0 150 } /* end of main program */
calmantara186 2:dea57b3f84cd 151 }
calmantara186 2:dea57b3f84cd 152
calmantara186 2:dea57b3f84cd 153 /************************************
calmantara186 2:dea57b3f84cd 154 * Deklarasi Fungsi dan Prosedur *
calmantara186 2:dea57b3f84cd 155 ************************************/
calmantara186 2:dea57b3f84cd 156 void limitMotor(){
calmantara186 2:dea57b3f84cd 157 /* Prosedur yang digunakan untuk mengecek batas limit pwm motor */
calmantara186 6:5632ff2142c0 158 if(pwm_kanan > limit_pwm){
calmantara186 1:5e0cb74f950e 159 pwm_kanan = limit_pwm;
calmantara186 6:5632ff2142c0 160 }
calmantara186 6:5632ff2142c0 161 else if(pwm_kanan < (-limit_pwm)){
calmantara186 6:5632ff2142c0 162 pwm_kanan = -limit_pwm;
calmantara186 6:5632ff2142c0 163 }
calmantara186 6:5632ff2142c0 164 if(pwm_kiri > limit_pwm){
calmantara186 6:5632ff2142c0 165 pwm_kiri = limit_pwm;
calmantara186 6:5632ff2142c0 166 }
calmantara186 6:5632ff2142c0 167 else if(pwm_kiri < (-limit_pwm)){
calmantara186 6:5632ff2142c0 168 pwm_kiri = -limit_pwm;
calmantara186 6:5632ff2142c0 169 }
calmantara186 6:5632ff2142c0 170 if(pwm_depan > limit_pwm){
calmantara186 6:5632ff2142c0 171 pwm_depan = limit_pwm;
calmantara186 6:5632ff2142c0 172 }
calmantara186 6:5632ff2142c0 173 else if(pwm_depan < (-limit_pwm)){
calmantara186 6:5632ff2142c0 174 pwm_depan = -limit_pwm;
calmantara186 6:5632ff2142c0 175 }
calmantara186 3:4801d83f397c 176 }/* end of limitMotor */
calmantara186 3:4801d83f397c 177
calmantara186 6:5632ff2142c0 178 void pidMotor(int x){
calmantara186 6:5632ff2142c0 179 /**
calmantara186 6:5632ff2142c0 180 * prosedur untuk gerak motor
calmantara186 6:5632ff2142c0 181 * 0 : maju dan mundur
calmantara186 6:5632ff2142c0 182 * 1 : kanan dan kiri
calmantara186 6:5632ff2142c0 183 **/
calmantara186 6:5632ff2142c0 184 if(x==0){
calmantara186 6:5632ff2142c0 185 /* masuk ke perhitungan ketika sudah pada time sampling */
calmantara186 6:5632ff2142c0 186 pulses_kanan = (float) encMotor_kanan.getPulses();
calmantara186 6:5632ff2142c0 187 pulses_kiri = (float) encMotor_kiri.getPulses();
calmantara186 6:5632ff2142c0 188 rev_kanan = (float) pulses_kanan/540;
calmantara186 6:5632ff2142c0 189 rev_kiri = (float) pulses_kiri/540;
calmantara186 6:5632ff2142c0 190
calmantara186 6:5632ff2142c0 191 /* masuk ke perhitungan parameter */
calmantara186 6:5632ff2142c0 192 jarak_y += getY(rev_kanan, rev_kiri);
calmantara186 6:5632ff2142c0 193 delta_kaki = pulses_kanan + pulses_kiri;
calmantara186 6:5632ff2142c0 194
calmantara186 6:5632ff2142c0 195 speed_roty = pidRotY.pidCount(tn_y, delta_kaki);
calmantara186 6:5632ff2142c0 196 speed_disy = pidDisY.pidCount(target_y[0], jarak_y);
calmantara186 6:5632ff2142c0 197 /* Kondisi untuk menambahkan speed */
calmantara186 6:5632ff2142c0 198 pwm_kiri = speed_disy + speed_roty;
calmantara186 6:5632ff2142c0 199 pwm_kanan = speed_disy + speed_roty;
calmantara186 6:5632ff2142c0 200
calmantara186 6:5632ff2142c0 201 prev_millis1 = current_millis;
calmantara186 6:5632ff2142c0 202 encMotor_kanan.reset();
calmantara186 6:5632ff2142c0 203 encMotor_kiri.reset();
calmantara186 6:5632ff2142c0 204
calmantara186 6:5632ff2142c0 205 }
calmantara186 6:5632ff2142c0 206 else if(x==1){}
calmantara186 6:5632ff2142c0 207 }/* end of pidMotor */
calmantara186 6:5632ff2142c0 208
calmantara186 3:4801d83f397c 209 float getY(float y1, float y2){
calmantara186 3:4801d83f397c 210 /* fungsi untuk menghitung nilai Y dari roda kanan kiri */
calmantara186 3:4801d83f397c 211 float jarak_y1 = (kll_roda*y1)/4;
calmantara186 3:4801d83f397c 212 float jarak_y2 = (kll_roda*y2)/4;
calmantara186 3:4801d83f397c 213
calmantara186 3:4801d83f397c 214 return jarak_y1 - jarak_y2;
calmantara186 6:5632ff2142c0 215 }/* end of getY */