KRAI 2017 / Mbed 2 deprecated MainProgram_BaseBaru_20Feb_malam

ping + display on

Dependencies:   DigitDisplay Motor PID Ping mbed millis

Fork of MainProgram_BaseBaru_20Feb by KRAI 2017

main.cpp@26:256160a1a82d, 2017-02-10 (annotated)

Committer:
be_bryan
Date:
Fri Feb 10 13:22:02 2017 +0000
Revision:
26:256160a1a82d
Parent:
25:054d3048dd03
Child:
27:68efb1622985
base baru + pid launcher

Who changed what in which revision?

UserRevisionLine numberNew contents of line
rahmadirizki18 5:3aa203218306 1 /****************************************************************************/
rahmadirizki18 5:3aa203218306 2 /* PROGRAM UNTUK PID CLOSED LOOP */
rahmadirizki18 5:3aa203218306 3 /* */
rahmadirizki18 5:3aa203218306 4 /* - Digunakan encoder autonics */
rahmadirizki18 5:3aa203218306 5 /* - Konfigurasi Motor dan Encoder sbb : */
MarchioKevin 22:4632f58461e0 6 /* ______________________ */
MarchioKevin 22:4632f58461e0 7 /* / \ Rode Depan Belakang: */
MarchioKevin 22:4632f58461e0 8 /* / 2 (Belakang) \ Omniwheel */
MarchioKevin 22:4632f58461e0 9 /* | | */
MarchioKevin 22:4632f58461e0 10 /* | 3 (Encoder) 4 | Roda Kiri Kanan: */
MarchioKevin 22:4632f58461e0 11 /* | | Fixed Wheel */
MarchioKevin 22:4632f58461e0 12 /* \ 1 (Depan) / */
MarchioKevin 22:4632f58461e0 13 /* \______________________/ Putaran berlawanan arah */
MarchioKevin 22:4632f58461e0 14 /* jarum jam positif */
rahmadirizki18 5:3aa203218306 15 /* SETTINGS (WAJIB!) : */
rahmadirizki18 5:3aa203218306 16 /* 1. Settings Pin Encoder, Resolusi, dan Tipe encoding di omniPos.h */
rahmadirizki18 5:3aa203218306 17 /* 2. Deklarasi penggunaan library omniPos pada bagian deklarasi encoder */
rahmadirizki18 5:3aa203218306 18 /* */
rahmadirizki18 5:3aa203218306 19 /****************************************************************************/
rahmadirizki18 6:68293bed71ea 20 /* */
MarchioKevin 20:54dc93e7b016 21 /* Joystick */
MarchioKevin 20:54dc93e7b016 22 /* Kanan => Posisi target x ditambah 'perpindahan' */
MarchioKevin 20:54dc93e7b016 23 /* Kiri => Posisi target x dikurang 'perpindahan' */
rahmadirizki18 6:68293bed71ea 24 /* */
rahmadirizki18 6:68293bed71ea 25 /* Tombol silang => Kembali keposisi Awal */
rahmadirizki18 6:68293bed71ea 26 /* Tombol segitiga => Aktif motor Launcher */
rahmadirizki18 13:8ab42383a2ca 27 /* Tombol lingkaran=> Aktif servo Launcher */
MarchioKevin 22:4632f58461e0 28 /* Tombol L1 => Pivot Kiri */
MarchioKevin 22:4632f58461e0 29 /* Tombol R1 => Pivot Kanan */
rahmadirizki18 24:b3e632cc4533 30 /* Tombol L3 => PWM Motor Belakang Dikurangi */
rahmadirizki18 24:b3e632cc4533 31 /* Tombol R3 => PWM Motor Belakang Ditambah */
rahmadirizki18 24:b3e632cc4533 32 /* Tombol L2 => PWM Motor Depan Dikurangi */
rahmadirizki18 24:b3e632cc4533 33 /* Tombol R2 => PWM Motor Depan Ditambah */
rahmadirizki18 13:8ab42383a2ca 34 /* */
calmantara186 16:90119f03c5d1 35 /* Bismillahirahmanirrahim */
MarchioKevin 20:54dc93e7b016 36 /* Jagalah Kebersihan Kodingan */
rahmadirizki18 6:68293bed71ea 37 /****************************************************************************/
rahmadirizki18 6:68293bed71ea 38
fanny868 0:9072e932503c 39 #include "mbed.h"
fanny868 0:9072e932503c 40 #include "JoystickPS3.h"
fanny868 0:9072e932503c 41 #include "Motor.h"
rahmadirizki18 6:68293bed71ea 42 #include "Servo.h"
rahmadirizki18 5:3aa203218306 43 #include "encoderKRAI.h"
be_bryan 26:256160a1a82d 44 #include "millis.h"
rahmadirizki18 5:3aa203218306 45
calmantara186 16:90119f03c5d1 46 /***********************************************/
calmantara186 16:90119f03c5d1 47 /* Konstanta dan Variabel */
calmantara186 16:90119f03c5d1 48 /***********************************************/
calmantara186 16:90119f03c5d1 49 #define PI 3.14159265
be_bryan 26:256160a1a82d 50 #define D_ENCODER 10 // Diameter Roda Encoder
be_bryan 26:256160a1a82d 51 #define D_ROBOT 80 // Diameter Roda Robot
MarchioKevin 22:4632f58461e0 52 #define VMAX 0.3 // Kiri Kanan
rahmadirizki18 17:e4229d77a5ab 53 #define PIVOT 0.4 // Pivka, Pivki
rahmadirizki18 17:e4229d77a5ab 54 #define PERPINDAHAN 1 // Perpindahan ke kanan dan kiri
rahmadirizki18 5:3aa203218306 55
be_bryan 26:256160a1a82d 56 // Variable Atas
be_bryan 26:256160a1a82d 57 double speed, maxSpeed = 0.8, minSpeed = -0.8;
be_bryan 26:256160a1a82d 58 double kpA=0.6757, kdA=0.6757, kiA=0.00006757;
be_bryan 26:256160a1a82d 59 double p,i,d;
be_bryan 26:256160a1a82d 60 double last_error = 0, current_error, sum_error = 0;
be_bryan 26:256160a1a82d 61 double rpm, target_rpm = 9.0;
be_bryan 26:256160a1a82d 62
be_bryan 26:256160a1a82d 63 // Variable Bawah
Joshua23 25:054d3048dd03 64 float Vt;
calmantara186 16:90119f03c5d1 65 float k_enc = PI*D_ENCODER;
Joshua23 25:054d3048dd03 66 float k_robot = PI*D_ROBOT;
MarchioKevin 22:4632f58461e0 67 float speedT = 0.2;
MarchioKevin 22:4632f58461e0 68 float speedB = 0.43;
MarchioKevin 22:4632f58461e0 69 float speedL = 0.4;
MarchioKevin 22:4632f58461e0 70 float vpid = 0;
MarchioKevin 22:4632f58461e0 71
calmantara186 16:90119f03c5d1 72
be_bryan 26:256160a1a82d 73 /* Deklarasi Encoder Base */
Joshua23 25:054d3048dd03 74 encoderKRAI encoderKiri(PC_4, PB_15, 2000, encoderKRAI::X2_ENCODING); //inA, inB, pin Indeks (NC = tak ada), resolusi, pembacaan
rahmadirizki18 15:98f0d56b14f0 75
be_bryan 26:256160a1a82d 76 /* Deklarasi Encoder Launcher */
be_bryan 26:256160a1a82d 77 encoderKRAI encoderAtas( PB_13, PB_14, 14, encoderKRAI::X2_ENCODING);
be_bryan 26:256160a1a82d 78
calmantara186 16:90119f03c5d1 79 /* Deklarasi Motor Base */
be_bryan 26:256160a1a82d 80 Motor motor1(PB_9, PC_5, PA_12); // pwm, fwd, rev, Motor Depan
be_bryan 26:256160a1a82d 81 Motor motor2(PB_6, PB_1, PB_12); // pwm, fwd, rev, Motor Belakang
fanny868 0:9072e932503c 82
calmantara186 16:90119f03c5d1 83 /* Deklarasi Motor Launcher */
be_bryan 26:256160a1a82d 84 Motor motor3(PA_8,PC_1,PC_2); // pwm ,fwd, rev, Motor Atas
rahmadirizki18 23:023b522977b2 85 //Motor motorld(PA_8, PC_1, PC_2); // pwm, fwd, rev
rahmadirizki18 23:023b522977b2 86 //Motor motorlb(PA_0, PA_4, PC_15 ); // pwm, fwd, rev
rahmadirizki18 5:3aa203218306 87
be_bryan 26:256160a1a82d 88 /* Deklarasi Penumatik Launcher */
be_bryan 26:256160a1a82d 89 DigitalOut pneumatik(PB_2, 0);
be_bryan 26:256160a1a82d 90
be_bryan 26:256160a1a82d 91 /* Deklarasi Servo */
rahmadirizki18 23:023b522977b2 92 //Servo servoS(PB_2);
rahmadirizki18 23:023b522977b2 93 //Servo servoB(PA_5);
rahmadirizki18 6:68293bed71ea 94
calmantara186 16:90119f03c5d1 95 /**
calmantara186 16:90119f03c5d1 96 * posX dan posY berdasarkan arah robot
calmantara186 16:90119f03c5d1 97 * encoder Depan & Belakang sejajar sumbu Y
calmantara186 16:90119f03c5d1 98 * encoder Kanan & Kiri sejajar sumbu X
calmantara186 16:90119f03c5d1 99 **/
rahmadirizki18 5:3aa203218306 100
calmantara186 16:90119f03c5d1 101 /* Variabel Encoder */
MarchioKevin 22:4632f58461e0 102 float errT, Tetha; // Variabel yang didapatkan encoder
rahmadirizki18 5:3aa203218306 103
calmantara186 16:90119f03c5d1 104 /* Fungsi dan Procedur Encoder */
calmantara186 16:90119f03c5d1 105 void setCenter(); // Fungsi reset agar robot di tengah
MarchioKevin 20:54dc93e7b016 106 float getTetha(); // Fungsi mendapatkan jarak Tetha
calmantara186 16:90119f03c5d1 107
calmantara186 16:90119f03c5d1 108 /* Inisialisasi Pin TX-RX Joystik dan PC */
Joshua23 25:054d3048dd03 109 joysticknucleo joystick(PA_0,PA_1);
rahmadirizki18 23:023b522977b2 110 Serial pc(USBTX,USBRX);
fanny868 0:9072e932503c 111
be_bryan 26:256160a1a82d 112 /* Deklarasi Variable Milis */
be_bryan 26:256160a1a82d 113 unsigned long int previousMillis = 0;
be_bryan 26:256160a1a82d 114 unsigned long int currentMillis;
be_bryan 26:256160a1a82d 115
calmantara186 16:90119f03c5d1 116 /* Variabel Stick */
fanny868 0:9072e932503c 117 char case_ger;
be_bryan 26:256160a1a82d 118 bool launcher = false;
be_bryan 26:256160a1a82d 119 //bool pneumatikGo = false;
fanny868 0:9072e932503c 120
MarchioKevin 22:4632f58461e0 121 /****************************************************/
MarchioKevin 22:4632f58461e0 122 /* Deklarasi Fungsi dan Procedure */
MarchioKevin 22:4632f58461e0 123 /****************************************************/
be_bryan 26:256160a1a82d 124 void init_speed();
be_bryan 26:256160a1a82d 125 void speedKalibrasiMotor();
be_bryan 26:256160a1a82d 126 float pidBase(float Kp, float Ki, float Kd)
Joshua23 25:054d3048dd03 127 {
Joshua23 25:054d3048dd03 128 int errorP;
Joshua23 25:054d3048dd03 129 errorP = getTetha();
Joshua23 25:054d3048dd03 130 return (float)Kp*errorP;
Joshua23 25:054d3048dd03 131 }
Joshua23 25:054d3048dd03 132
calmantara186 16:90119f03c5d1 133 int case_gerak(){
rahmadirizki18 23:023b522977b2 134 /****************************************************
calmantara186 16:90119f03c5d1 135 ** Gerak Motor Base
calmantara186 16:90119f03c5d1 136 ** Case 1 : Pivot kanan
calmantara186 16:90119f03c5d1 137 ** Case 2 : Pivot Kiri
MarchioKevin 22:4632f58461e0 138 ** Case 3 : Kanan
MarchioKevin 22:4632f58461e0 139 ** Case 4 : Kiri
MarchioKevin 22:4632f58461e0 140 ** Case 5 : Break
calmantara186 16:90119f03c5d1 141 ****************************************************/
MarchioKevin 22:4632f58461e0 142
fanny868 0:9072e932503c 143 int casegerak;
calmantara186 16:90119f03c5d1 144 if (!joystick.L1 && joystick.R1) {
fanny868 0:9072e932503c 145 // Pivot Kanan
fanny868 0:9072e932503c 146 casegerak = 1;
calmantara186 16:90119f03c5d1 147 }
calmantara186 16:90119f03c5d1 148 else if (!joystick.R1 && joystick.L1) {
fanny868 0:9072e932503c 149 // Pivot Kiri
fanny868 0:9072e932503c 150 casegerak = 2;
calmantara186 16:90119f03c5d1 151 }
calmantara186 16:90119f03c5d1 152 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {
fanny868 0:9072e932503c 153 // Kanan
MarchioKevin 22:4632f58461e0 154 casegerak = 3;
rahmadirizki18 23:023b522977b2 155 pc.printf("kanan");
calmantara186 16:90119f03c5d1 156 }
calmantara186 16:90119f03c5d1 157 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {
fanny868 0:9072e932503c 158 // Kiri
rahmadirizki18 23:023b522977b2 159 casegerak = 4;
rahmadirizki18 23:023b522977b2 160 pc.printf("kiri");
calmantara186 16:90119f03c5d1 161 }
calmantara186 16:90119f03c5d1 162 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
MarchioKevin 22:4632f58461e0 163 // Break
MarchioKevin 22:4632f58461e0 164 casegerak = 5;
calmantara186 16:90119f03c5d1 165 }
fanny868 0:9072e932503c 166 return(casegerak);
fanny868 0:9072e932503c 167 }
fanny868 0:9072e932503c 168
calmantara186 16:90119f03c5d1 169 void aktuator(){
be_bryan 26:256160a1a82d 170 /* Fungsi untuk menggerakkan Penumatik */
be_bryan 26:256160a1a82d 171 // Pneumatik
be_bryan 26:256160a1a82d 172 /* if (pneumatikGo){
franshendri 12:e07c59c28c29 173 servoS.position(20);
franshendri 10:f0f0dc3904e0 174 wait_ms(500);
franshendri 12:e07c59c28c29 175 servoS.position(-28);
Joshua23 8:0711dea61312 176 wait_ms(500);
franshendri 12:e07c59c28c29 177 servoS.position(20);
Joshua23 8:0711dea61312 178 wait_ms(500);
franshendri 12:e07c59c28c29 179 for (int i = -0; i<=70; i++){
Joshua23 8:0711dea61312 180 servoB.position(i);
Joshua23 8:0711dea61312 181 wait_ms(10);
Joshua23 8:0711dea61312 182 }
Joshua23 8:0711dea61312 183 wait_ms(500);
Joshua23 8:0711dea61312 184 servoB.position(0);
calmantara186 16:90119f03c5d1 185 servoGo = false;
calmantara186 16:90119f03c5d1 186 }
calmantara186 16:90119f03c5d1 187 else{
franshendri 12:e07c59c28c29 188 servoS.position(20);
rahmadirizki18 6:68293bed71ea 189 servoB.position(0);
rahmadirizki18 7:d138c56dab20 190 }
be_bryan 26:256160a1a82d 191 */
be_bryan 26:256160a1a82d 192 //Motor Atas
calmantara186 16:90119f03c5d1 193 if (launcher) {
be_bryan 26:256160a1a82d 194 startMillis();
be_bryan 26:256160a1a82d 195 currentMillis = millis();
be_bryan 26:256160a1a82d 196 if (currentMillis-previousMillis>=10)
be_bryan 26:256160a1a82d 197 {
be_bryan 26:256160a1a82d 198 rpm = (double)encoderAtas.getPulses();
be_bryan 26:256160a1a82d 199 current_error = target_rpm - rpm;
be_bryan 26:256160a1a82d 200 sum_error = sum_error + current_error;
be_bryan 26:256160a1a82d 201 p = current_error*kpA;
be_bryan 26:256160a1a82d 202 d = (current_error-last_error)*kdA/10.0;
be_bryan 26:256160a1a82d 203 i = sum_error*kiA*10.0;
be_bryan 26:256160a1a82d 204 speed = p + d + i;
be_bryan 26:256160a1a82d 205 init_speed();
be_bryan 26:256160a1a82d 206 motor3.speed(speed);
be_bryan 26:256160a1a82d 207 last_error = current_error;
be_bryan 26:256160a1a82d 208 encoderAtas.reset();
be_bryan 26:256160a1a82d 209 //pc.printf("%.04lf\n",rpm);
be_bryan 26:256160a1a82d 210 previousMillis = currentMillis;
be_bryan 26:256160a1a82d 211 }
be_bryan 26:256160a1a82d 212 else
be_bryan 26:256160a1a82d 213 {
be_bryan 26:256160a1a82d 214 motor3.speed(0);
be_bryan 26:256160a1a82d 215 }
rahmadirizki18 6:68293bed71ea 216 }
rahmadirizki18 6:68293bed71ea 217 // MOTOR Bawah
MarchioKevin 22:4632f58461e0 218 switch (case_ger) {
rahmadirizki18 17:e4229d77a5ab 219 case (1): {
rahmadirizki18 17:e4229d77a5ab 220 // Pivot Kanan
Joshua23 25:054d3048dd03 221 motor1.speed(PIVOT);
Joshua23 25:054d3048dd03 222 motor2.speed(PIVOT);
Joshua23 25:054d3048dd03 223 setCenter();
fanny868 0:9072e932503c 224 break;
rahmadirizki18 17:e4229d77a5ab 225 }
rahmadirizki18 17:e4229d77a5ab 226 case (2): {
rahmadirizki18 17:e4229d77a5ab 227 // Pivot Kiri
Joshua23 25:054d3048dd03 228 motor1.speed(-PIVOT);
Joshua23 25:054d3048dd03 229 motor2.speed(-PIVOT);
Joshua23 25:054d3048dd03 230 setCenter();
fanny868 0:9072e932503c 231 break;
MarchioKevin 22:4632f58461e0 232 }
MarchioKevin 22:4632f58461e0 233 case (3) : {
MarchioKevin 22:4632f58461e0 234 // Kanan
Joshua23 25:054d3048dd03 235 //motor1.speed(-VMAX-vpid);
Joshua23 25:054d3048dd03 236 //motor2.speed(0.2+vpid);
be_bryan 26:256160a1a82d 237 motor1.speed(-0.365+pidBase(0.09,0,0));
be_bryan 26:256160a1a82d 238 motor2.speed(0.46+pidBase(0.09,0,0));
rahmadirizki18 17:e4229d77a5ab 239 break;
MarchioKevin 22:4632f58461e0 240 }
MarchioKevin 22:4632f58461e0 241 case (4) : {
rahmadirizki18 17:e4229d77a5ab 242 // Kiri
Joshua23 25:054d3048dd03 243 //motor1.speed(VMAX-vpid);
Joshua23 25:054d3048dd03 244 //motor2.speed(-0.2+vpid);
be_bryan 26:256160a1a82d 245 motor1.speed(0.365+pidBase(0.09,0,0));//belakang
be_bryan 26:256160a1a82d 246 motor2.speed(-0.46+pidBase(0.09,0,0));//depan
fanny868 0:9072e932503c 247 break;
rahmadirizki18 17:e4229d77a5ab 248 }
rahmadirizki18 17:e4229d77a5ab 249 default : {
MarchioKevin 22:4632f58461e0 250 motor1.brake(1);
MarchioKevin 22:4632f58461e0 251 motor2.brake(1);
be_bryan 26:256160a1a82d 252 break;
rahmadirizki18 17:e4229d77a5ab 253 }
MarchioKevin 22:4632f58461e0 254 } // End Switch
rahmadirizki18 5:3aa203218306 255 }
rahmadirizki18 5:3aa203218306 256
calmantara186 16:90119f03c5d1 257 void setCenter(){
calmantara186 16:90119f03c5d1 258 /* Fungsi untuk menentukan center dari robot */
rahmadirizki18 5:3aa203218306 259 encoderKiri.reset();
rahmadirizki18 5:3aa203218306 260 }
rahmadirizki18 5:3aa203218306 261
calmantara186 16:90119f03c5d1 262 float getTetha(){
calmantara186 16:90119f03c5d1 263 /* Fungsi untuk mendapatkan nilai tetha */
Joshua23 25:054d3048dd03 264 float busurKir, tetha;
MarchioKevin 22:4632f58461e0 265 busurKir = ((encoderKiri.getPulses())/(float)(2000.0)*k_enc);
Joshua23 25:054d3048dd03 266 tetha = busurKir/k_robot*360;
calmantara186 16:90119f03c5d1 267
Joshua23 25:054d3048dd03 268 return -(tetha);
calmantara186 16:90119f03c5d1 269 }
rahmadirizki18 5:3aa203218306 270
rahmadirizki18 24:b3e632cc4533 271 void speedKalibrasiMotor(){
calmantara186 16:90119f03c5d1 272 /* Fungsi untuk speed launcher */
franshendri 12:e07c59c28c29 273 if (joystick.R3_click and speedL < 0.8){
MarchioKevin 22:4632f58461e0 274 speedL = speedL + 0.01; // PWM++ Motor Belakang
calmantara186 16:90119f03c5d1 275 }
rahmadirizki18 7:d138c56dab20 276 if (joystick.L3_click and speedL > 0.1){
MarchioKevin 22:4632f58461e0 277 speedL = speedL - 0.01; // PWM-- Motor Belakang
calmantara186 16:90119f03c5d1 278 }
rahmadirizki18 13:8ab42383a2ca 279 if (joystick.R2_click and speedB < 0.8 ){
MarchioKevin 22:4632f58461e0 280 speedB = speedB + 0.01; // PWM++ Motor Depan
calmantara186 16:90119f03c5d1 281 }
rahmadirizki18 13:8ab42383a2ca 282 if (joystick.L2_click and speedB > 0.1 ){
MarchioKevin 22:4632f58461e0 283 speedB = speedB - 0.01; // PWM-- Motor Depan
rahmadirizki18 24:b3e632cc4533 284 }
rahmadirizki18 24:b3e632cc4533 285 // pc.printf("Pwm depan = %.3f\t Pwm belakang = %.3f\n", speedL, speedB);
rahmadirizki18 7:d138c56dab20 286 }
be_bryan 26:256160a1a82d 287
be_bryan 26:256160a1a82d 288 void init_speed (){
be_bryan 26:256160a1a82d 289 if (speed>maxSpeed){
be_bryan 26:256160a1a82d 290 speed = maxSpeed;
be_bryan 26:256160a1a82d 291 }
be_bryan 26:256160a1a82d 292
be_bryan 26:256160a1a82d 293 if (speed<minSpeed){
be_bryan 26:256160a1a82d 294 speed = minSpeed;
be_bryan 26:256160a1a82d 295 }
be_bryan 26:256160a1a82d 296 }
MarchioKevin 22:4632f58461e0 297 /*********************************************************/
MarchioKevin 22:4632f58461e0 298 /* Main Function */
MarchioKevin 22:4632f58461e0 299 /*********************************************************/
calmantara186 16:90119f03c5d1 300
calmantara186 16:90119f03c5d1 301 int main (void){
calmantara186 16:90119f03c5d1 302 /* Set baud rate - 115200 */
fanny868 0:9072e932503c 303 joystick.setup();
rahmadirizki18 23:023b522977b2 304 pc.baud(115200);
rahmadirizki18 6:68293bed71ea 305 wait_ms(1000);
rahmadirizki18 5:3aa203218306 306 setCenter();
rahmadirizki18 5:3aa203218306 307 wait_ms(500);
rahmadirizki18 23:023b522977b2 308 pc.printf("ready....");
calmantara186 16:90119f03c5d1 309
calmantara186 16:90119f03c5d1 310 /* Untuk mendapatkan serial dari Arduino */
fanny868 0:9072e932503c 311 while(1)
fanny868 0:9072e932503c 312 {
fanny868 0:9072e932503c 313 // Interrupt Serial
calmantara186 16:90119f03c5d1 314 joystick.idle();
Joshua23 25:054d3048dd03 315 //pc.printf("enco : %d \n",encoderKiri.getPulses());
be_bryan 26:256160a1a82d 316 if(joystick.readable()) {
fanny868 0:9072e932503c 317 // Panggil fungsi pembacaan joystik
fanny868 0:9072e932503c 318 joystick.baca_data();
fanny868 0:9072e932503c 319 // Panggil fungsi pengolahan data joystik
fanny868 0:9072e932503c 320 joystick.olah_data();
calmantara186 16:90119f03c5d1 321 // Masuk ke case gerak
fanny868 0:9072e932503c 322 case_ger = case_gerak();
rahmadirizki18 3:1287fccc11be 323 aktuator();
be_bryan 26:256160a1a82d 324 if (joystick.segitiga_click){
be_bryan 26:256160a1a82d 325 launcher = !launcher;
be_bryan 26:256160a1a82d 326 }
be_bryan 26:256160a1a82d 327 if (joystick.lingkaran_click){
be_bryan 26:256160a1a82d 328 pneumatik = 1;
be_bryan 26:256160a1a82d 329 wait_ms(500);
be_bryan 26:256160a1a82d 330 pneumatik = 0;
be_bryan 26:256160a1a82d 331 }
be_bryan 26:256160a1a82d 332 speedKalibrasiMotor();
be_bryan 26:256160a1a82d 333 }
calmantara186 16:90119f03c5d1 334 else {
calmantara186 16:90119f03c5d1 335 joystick.idle();
be_bryan 26:256160a1a82d 336 motor1.brake(1);
be_bryan 26:256160a1a82d 337 motor2.brake(1);
MarchioKevin 21:da2f3d04468f 338 }
fanny868 0:9072e932503c 339 }
fanny868 0:9072e932503c 340 }