KRAI ITB GARUDAGO
/
MR1_New
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 /* 00002 * Author : Garudago ITB 2019 00003 * Developer : Garudago ITB 2019 00004 * Maintainer : Calmantara Sumpono Putra 00005 * Version : 2.0.0 00006 */ 00007 00008 #include "mbed.h" 00009 #include "millis.h" 00010 #include <garudago_conf/robotpin.h> 00011 #include <garudago_conf/variable.h> 00012 #include <garudago_conf/config.h> 00013 #include <garudago_hardware_interface/actuator.h> 00014 #include <garudago_hardware_interface/sensor.h> 00015 #include <pid_dagoz/PID.h> 00016 00017 PID locomotion_right_top(right_top_kp, 00018 right_top_ki, 00019 right_top_kd, 00020 right_top_N, 00021 right_top_TS, 00022 right_top_FF, 00023 PID::PI_MODE); 00024 00025 PID locomotion_left_top(left_top_kp, 00026 left_top_ki, 00027 left_top_kd, 00028 left_top_N, 00029 left_top_TS, 00030 left_top_FF, 00031 PID::PI_MODE); 00032 00033 PID locomotion_right_back(right_back_kp, 00034 right_back_ki, 00035 right_back_kd, 00036 right_back_N, 00037 right_back_TS, 00038 right_back_FF, 00039 PID::PI_MODE); 00040 00041 PID locomotion_left_back(left_back_kp, 00042 left_back_ki, 00043 left_back_kd, 00044 left_back_N, 00045 left_back_TS, 00046 left_back_FF, 00047 PID::PI_MODE); 00048 00049 Base base; 00050 Compass compass; 00051 00052 void locomotionMovement(); 00053 void shagaiMovement(int _state, float _theta, bool _up); 00054 void shagaiParameter(); 00055 void pneumaticMovement(); 00056 void stickState(); 00057 00058 int main(){ 00059 stick.setup(); 00060 compass.compass_reset((float)compass_sensor.getAngle()/10); 00061 startMillis(); 00062 stick.idle(); //start stick 00063 00064 //start robot geometry 00065 robot_geometry[0] = 0.0; 00066 robot_geometry[1] = 0.0; 00067 robot_geometry[2] = 0.0; 00068 00069 while(1){ 00070 if(stick.readable()){ 00071 //procedure to read stick data 00072 stick.baca_data(); 00073 stick.olah_data(); 00074 } 00075 // if(millis() - prev_compass_timer > 50){ 00076 // //before external encoder fixed, use faster sampling rate 00077 // compass.compass_update((float)compass_sensor.getAngle()/10.0); 00078 // robot_geometry[2] = compass.compass_value(); 00079 // prev_compass_timer = millis(); 00080 // } 00081 if(millis() - prev_motor_timer > 10){ 00082 stickState(); 00083 locomotionMovement(); 00084 //debug encoder direction 00085 // pc.printf("lt=%.2f rt=%.2f lb=%.2f rb=%.2f lt=%.2f rt=%.2f lb=%.2f rb=%.2f cmps=%.2f\n\r", left_top_vel, 00086 // right_top_vel, 00087 // left_back_vel, 00088 // right_back_vel, 00089 // wheels_target_velocity[0], 00090 // wheels_target_velocity[1], 00091 // wheels_target_velocity[2], 00092 // wheels_target_velocity[3], 00093 // robot_geometry[2]); 00094 prev_motor_timer = millis(); 00095 } 00096 if(millis() - prev_pneumatic_timer > 15){ 00097 shagaiParameter(); 00098 pneumaticMovement(); 00099 shagaiMovement(state_condition, theta_shagai, up); 00100 prev_pneumatic_timer = millis(); 00101 } 00102 } 00103 } 00104 00105 /*Function and Procedure Declaration*/ 00106 void locomotionMovement(){ 00107 left_top_vel = (enc_left_top.getPulses() * 2 * PI * WHEEL_RAD)/(0.01*537.6); 00108 right_top_vel = (enc_right_top.getPulses() * 2 * PI * WHEEL_RAD)/(0.01*537.6); 00109 left_back_vel = (enc_left_back.getPulses() * 2 * PI * WHEEL_RAD)/(0.01*537.6); 00110 right_back_vel = (enc_right_back.getPulses() * 2 * PI * WHEEL_RAD)/(0.01*537.6); 00111 00112 //compute PID 00113 float left_top_target_rate = locomotion_left_top.createpwm(wheels_target_velocity[0], 00114 left_top_vel); 00115 float right_top_target_rate = locomotion_right_top.createpwm(wheels_target_velocity[1], 00116 right_top_vel); 00117 float left_back_target_rate = locomotion_left_back.createpwm(wheels_target_velocity[2], 00118 left_back_vel); 00119 float right_back_target_rate = locomotion_right_back.createpwm(wheels_target_velocity[3], 00120 right_back_vel); 00121 //rate to motor 00122 motor_left_top.speed(left_top_target_rate); 00123 motor_right_top.speed(right_top_target_rate); 00124 motor_left_back.speed(left_back_target_rate); 00125 motor_right_back.speed(right_back_target_rate); 00126 00127 enc_left_back.reset(); 00128 enc_right_back.reset(); 00129 enc_left_top.reset(); 00130 enc_right_top.reset(); 00131 } 00132 00133 void shagaiMovement(int _state, float _theta, bool _up){ 00134 if ((_state == 5) && (!(_up))){ 00135 if(_theta<30){ 00136 motor_shagai.speed(0.5); 00137 }else if(_theta<100){ 00138 motor_shagai.speed(0.3); 00139 }else if((_theta>220) && (_theta<250)){ 00140 motor_shagai.speed(-0.3); 00141 } 00142 else{ 00143 motor_shagai.brake(1); 00144 } 00145 }else if ((_state == 8)&&(_up)){ 00146 if(fabs(_theta)<150){ 00147 motor_shagai.speed(-0.8); 00148 }else if(fabs(_theta)<210){ 00149 motor_shagai.speed(-0.5); 00150 } 00151 else{ 00152 motor_shagai.brake(1); 00153 } 00154 }else{ 00155 motor_shagai.brake(1); 00156 } 00157 } 00158 00159 void shagaiParameter(){ 00160 float shagai_pulse = enc_shagai.getPulses()*360.0; 00161 theta_shagai += shagai_pulse; 00162 enc_shagai.reset(); 00163 } 00164 00165 void pneumaticMovement(){ 00166 //Kondisi untuk pneumatic pembelok gerege 00167 if(belok){ 00168 pneumatic_pembelok=0; 00169 }else{ 00170 pneumatic_pembelok=1; 00171 } 00172 00173 //state 00174 if (state_condition<0) 00175 state_condition=0; 00176 if (state_condition>15) 00177 state_condition=5; 00178 //State 0: Kondisi awal 00179 if (state_condition == 0){ 00180 up=0; 00181 theta_shagai=0; 00182 count_reset_arm=0; 00183 00184 pneumatic_extension = 1; //Pendek 00185 pneumatic_penaik = 1; //Naik 00186 pneumatic_pelontar = 0; //Pendek 00187 pneumatic_pencapit = 1; //Buka 00188 pneumatic_pengambil = 1; //Tutup 00189 } 00190 //state 1: kondisi pencapit tertutup 00191 if (state_condition == 1){ 00192 pneumatic_extension = 1; //Pendek 00193 pneumatic_penaik = 1; //Naik 00194 pneumatic_pelontar = 0; //Pendek 00195 pneumatic_pencapit = 0; //Tutup 00196 pneumatic_pengambil = 1; //Tutup 00197 } 00198 //state 2: tangan gerege turun untuk persiapan menyerahkan gerege ke kuda 00199 if (state_condition == 2){ 00200 pneumatic_extension = 1; //Pendek 00201 pneumatic_penaik = 0; //Turun 00202 pneumatic_pelontar = 0; //Pendek 00203 pneumatic_pencapit = 0; //Tutup 00204 pneumatic_pengambil = 1; //Tutup 00205 } 00206 //state 3: penjepit gerege terbuka,gerege akan jatuh ke kuda 00207 if (state_condition == 3){ 00208 pneumatic_extension = 1; //Pendek 00209 pneumatic_penaik = 0; //Turun 00210 pneumatic_pelontar = 0; //Pendek 00211 pneumatic_pencapit = 1; //Buka 00212 pneumatic_pengambil = 1; //Tutup 00213 } 00214 //state4: penaik gerege naik ke atas 00215 if (state_condition==4){ 00216 pneumatic_extension = 1; //Pendek 00217 pneumatic_penaik = 1; //Naik 00218 pneumatic_pelontar = 0; //Pendek 00219 pneumatic_pencapit = 1; //Buka 00220 pneumatic_pengambil = 1; //Tutup 00221 } 00222 //state 5: Motor pelontar turun 00223 if(state_condition==5){ 00224 up=0; 00225 } 00226 //state 6: arm terbuka untuk mengambil shagai 00227 if (state_condition == 6) { 00228 //Reset encoder motor Shagai 00229 if(count_reset_arm==0){ 00230 theta_shagai=0; 00231 count_reset_arm++; 00232 } 00233 pneumatic_extension = 1; //Pendek 00234 pneumatic_penaik = 1; //Naik 00235 pneumatic_pelontar = 0; //Pendek 00236 pneumatic_pencapit = 1; //Buka 00237 pneumatic_pengambil = 0; //Buka 00238 } 00239 //state 7: arm tertutup untuk menjepit shagai 00240 if (state_condition == 7) { 00241 pneumatic_extension = 1; //Pendek 00242 pneumatic_penaik = 1; //Naik 00243 pneumatic_pelontar = 0; //Pendek 00244 pneumatic_pencapit = 1; //Buka 00245 pneumatic_pengambil = 1; //Tutup 00246 } 00247 if(state_condition==8){ 00248 up=1; 00249 } 00250 //state 9: pengambil shagai terbuka 00251 if (state_condition==9){ 00252 //Reset encoder motor Shagai 00253 if(count_reset_arm==1){ 00254 theta_shagai=0; 00255 count_reset_arm--; 00256 } 00257 pneumatic_extension = 1; //Pendek 00258 pneumatic_penaik = 1; //Naik 00259 pneumatic_pelontar = 0; //Pendek 00260 pneumatic_pencapit = 1; //Buka 00261 pneumatic_pengambil = 0; //Buka 00262 } 00263 //state 10: pengambil shagai tertutup 00264 if (state_condition==10){ 00265 pneumatic_extension = 1; //Pendek 00266 pneumatic_penaik = 1; //Naik 00267 pneumatic_pelontar = 0; //Pendek 00268 pneumatic_pencapit = 1; //Buka 00269 pneumatic_pengambil = 1; //Tutup 00270 } 00271 //state 11 : arm ekstension maju 00272 if (state_condition == 11) { 00273 pneumatic_extension = 0; //Panjang 00274 pneumatic_penaik = 1; //Naik 00275 pneumatic_pelontar = 0; //Pendek 00276 pneumatic_pencapit = 1; //Buka 00277 pneumatic_pengambil = 1; //Tutup 00278 } 00279 //state 12 : arm terbuka, persiapan menembak 00280 if (state_condition == 12){ 00281 pneumatic_extension = 0; //Panjang 00282 pneumatic_penaik = 1; //Naik 00283 pneumatic_pelontar = 0; //Pendek 00284 pneumatic_pencapit = 1; //Buka 00285 pneumatic_pengambil = 0; //Buka 00286 } 00287 //state 13 : shagai ditembak ke depan 00288 if (state_condition == 13) { 00289 pneumatic_extension = 0; //Panjang 00290 pneumatic_penaik = 1; //Naik 00291 pneumatic_pelontar = 1; //Panjang 00292 pneumatic_pencapit = 1; //Buka 00293 pneumatic_pengambil = 0; //Buka 00294 } 00295 //state 14: penjepit shagai tertutup dan pelontarnya memendek 00296 if (state_condition==14){ 00297 pneumatic_extension = 1; //Pendek 00298 pneumatic_penaik = 1; //Naik 00299 pneumatic_pelontar = 0; //Pendek 00300 pneumatic_pencapit = 1; //Buka 00301 pneumatic_pengambil = 1; //Tutup 00302 } 00303 //state 15 kembali ke state 5 00304 if (state_condition==15){ 00305 //motor turun 00306 up=0; 00307 state_condition=5; 00308 } 00309 00310 } 00311 00312 void stickState(){ 00313 00314 // RESET STATE // 00315 if (stick.START) {} 00316 else if (stick.SELECT){} 00317 00318 // STATE-STATE PENUMATIC // 00319 if ((millis()-prev_stick_symbol_timer>700)&&(stick.segitiga)){ 00320 state_condition++; 00321 prev_stick_symbol_timer = millis(); 00322 } else if ((millis()-prev_stick_symbol_timer>700)&&(stick.kotak)){ 00323 state_condition--; 00324 prev_stick_symbol_timer = millis(); 00325 } else if((millis()-prev_stick_symbol_timer>700)&&(stick.lingkaran)){ 00326 state_condition=0; 00327 prev_stick_symbol_timer = millis(); 00328 } else if((millis()-prev_stick_symbol_timer>700)&&(stick.silang)){ 00329 belok=!(belok); 00330 prev_stick_symbol_timer = millis(); 00331 } 00332 00333 00334 // STICK ROTATION STATE // 00335 if (stick.L1 && !stick.R1){ 00336 linear_velocity = 0; 00337 rotation_velocity = 3.0; 00338 manual_alpha = 0; 00339 wheels_target_velocity = base._wheels_from_base(linear_velocity, rotation_velocity, manual_alpha); 00340 theta_target = compass.compass_value(); 00341 } 00342 else if (stick.R1 && !stick.L1){ 00343 linear_velocity = 0; 00344 rotation_velocity = -3.0; 00345 manual_alpha = 0; 00346 wheels_target_velocity = base._wheels_from_base(linear_velocity, rotation_velocity, manual_alpha); 00347 theta_target = compass.compass_value(); 00348 } 00349 00350 00351 // STICK ARROW STATE // 00352 if ((!stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(!stick.kiri) 00353 &&(!stick.R2) && (!stick.L2)&&(!stick.R1) && (!stick.L1)) { 00354 //no input condition 00355 linear_velocity = 0; 00356 manual_alpha = 0; 00357 wheels_target_velocity = base.manual_movement(robot_geometry[2], 00358 theta_target, 00359 linear_velocity, 00360 manual_alpha); 00361 } 00362 else if ((!stick.lingkaran)&&(!stick.segitiga)&&(!stick.silang) 00363 &&(stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(!stick.kiri)&&(!stick.R2)){ 00364 //stick up 00365 linear_velocity = manual_linear_velocity; 00366 manual_alpha = PI/2; 00367 wheels_target_velocity = base.manual_movement(robot_geometry[2], 00368 theta_target, 00369 linear_velocity, 00370 manual_alpha); 00371 } 00372 else if ((!stick.lingkaran)&&(!stick.segitiga)&&(!stick.silang) 00373 &&(!stick.atas)&&(stick.bawah)&&(!stick.kanan)&&(!stick.kiri)&&(!stick.R2)){ 00374 //stick down 00375 linear_velocity = manual_linear_velocity; 00376 manual_alpha = -PI/2; 00377 wheels_target_velocity = base.manual_movement(robot_geometry[2], 00378 theta_target, 00379 linear_velocity, 00380 manual_alpha); 00381 } 00382 else if ( ((!stick.lingkaran)&&(!stick.segitiga)&&(!stick.silang) 00383 &&(!stick.atas)&&(!stick.bawah)&&(stick.kanan)&&(!stick.kiri)&&(!stick.R2))){ 00384 //stick right 00385 linear_velocity = manual_linear_velocity; 00386 manual_alpha = 0; 00387 wheels_target_velocity = base.manual_movement(robot_geometry[2], 00388 theta_target, 00389 linear_velocity, 00390 manual_alpha); 00391 } 00392 else if ((!stick.lingkaran)&&(!stick.segitiga)&&(!stick.silang) 00393 &&(!stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(stick.kiri)&&(!stick.R2)){ 00394 //stick left 00395 linear_velocity = manual_linear_velocity; 00396 manual_alpha = PI; 00397 wheels_target_velocity = base.manual_movement(robot_geometry[2], 00398 theta_target, 00399 linear_velocity, 00400 manual_alpha); 00401 } 00402 else if ((!stick.lingkaran)&&(!stick.segitiga)&&(!stick.silang) 00403 &&(stick.atas)&&(!stick.bawah)&&(stick.kanan)&&(!stick.kiri)&&(!stick.R2)){ 00404 //stick right up 00405 linear_velocity = manual_linear_velocity; 00406 manual_alpha = PI/4; 00407 wheels_target_velocity = base.manual_movement(robot_geometry[2], 00408 theta_target, 00409 linear_velocity, 00410 manual_alpha); 00411 } 00412 else if ((!stick.lingkaran)&&(!stick.segitiga)&&(!stick.silang) 00413 &&(stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(stick.kiri)&&(!stick.R2)){ 00414 //stick left up 00415 linear_velocity = manual_linear_velocity; 00416 manual_alpha = 3*PI/4; 00417 wheels_target_velocity = base.manual_movement(robot_geometry[2], 00418 theta_target, 00419 linear_velocity, 00420 manual_alpha); 00421 } 00422 else if ((!stick.lingkaran)&&(!stick.segitiga)&&(!stick.silang) 00423 &&(!stick.atas)&&(stick.bawah)&&(stick.kanan)&&(!stick.kiri)&&(!stick.R2)){ 00424 //stick right down 00425 linear_velocity = manual_linear_velocity; 00426 manual_alpha = -PI/4; 00427 wheels_target_velocity = base.manual_movement(robot_geometry[2], 00428 theta_target, 00429 linear_velocity, 00430 manual_alpha); 00431 } 00432 else if ((!stick.lingkaran)&&(!stick.segitiga)&&(!stick.silang) 00433 &&(!stick.atas)&&(stick.bawah)&&(!stick.kanan)&&(stick.kiri)&&(!stick.R2)){ 00434 //stick left down 00435 linear_velocity = manual_linear_velocity; 00436 manual_alpha = -3*PI/4; 00437 wheels_target_velocity = base.manual_movement(robot_geometry[2], 00438 theta_target, 00439 linear_velocity, 00440 manual_alpha); 00441 } 00442 }
Generated on Tue Jul 12 2022 22:57:42 by
1.7.2