![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
tugas akhir
Dependencies: Motor mbed millis
Fork of DagonFly__RoadToJapan_15Mei_Ultimate by
main.cpp@25:054d3048dd03, 2017-02-10 (annotated)
- Committer:
- Joshua23
- Date:
- Fri Feb 10 10:29:45 2017 +0000
- Revision:
- 25:054d3048dd03
- Parent:
- 24:b3e632cc4533
- Child:
- 26:256160a1a82d
update closeloop encoder ; ; masih perlu tuning
Who changed what in which revision?
User | Revision | Line number | New 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" |
rahmadirizki18 | 5:3aa203218306 | 44 | |
calmantara186 | 16:90119f03c5d1 | 45 | /***********************************************/ |
calmantara186 | 16:90119f03c5d1 | 46 | /* Konstanta dan Variabel */ |
calmantara186 | 16:90119f03c5d1 | 47 | /***********************************************/ |
calmantara186 | 16:90119f03c5d1 | 48 | #define PI 3.14159265 |
Joshua23 | 25:054d3048dd03 | 49 | #define D_ENCODER 10 // Diameter Roda Encoder |
Joshua23 | 25:054d3048dd03 | 50 | #define D_ROBOT 80 // Diameter Roda Robot |
MarchioKevin | 22:4632f58461e0 | 51 | #define VMAX 0.3 // Kiri Kanan |
rahmadirizki18 | 17:e4229d77a5ab | 52 | #define PIVOT 0.4 // Pivka, Pivki |
rahmadirizki18 | 17:e4229d77a5ab | 53 | #define PERPINDAHAN 1 // Perpindahan ke kanan dan kiri |
rahmadirizki18 | 5:3aa203218306 | 54 | |
Joshua23 | 25:054d3048dd03 | 55 | float Vt; |
Joshua23 | 25:054d3048dd03 | 56 | |
calmantara186 | 16:90119f03c5d1 | 57 | float k_enc = PI*D_ENCODER; |
Joshua23 | 25:054d3048dd03 | 58 | float k_robot = PI*D_ROBOT; |
rahmadirizki18 | 5:3aa203218306 | 59 | |
MarchioKevin | 22:4632f58461e0 | 60 | float speedT = 0.2; |
MarchioKevin | 22:4632f58461e0 | 61 | float speedB = 0.43; |
MarchioKevin | 22:4632f58461e0 | 62 | float speedL = 0.4; |
rahmadirizki18 | 5:3aa203218306 | 63 | |
MarchioKevin | 22:4632f58461e0 | 64 | float vpid = 0; |
MarchioKevin | 22:4632f58461e0 | 65 | |
MarchioKevin | 22:4632f58461e0 | 66 | float kpX = 0.5, kpY = 0.5, kp_tetha = 0.03; |
calmantara186 | 16:90119f03c5d1 | 67 | |
calmantara186 | 16:90119f03c5d1 | 68 | /* Deklarasi encoder */ |
Joshua23 | 25:054d3048dd03 | 69 | encoderKRAI encoderKiri(PC_4, PB_15, 2000, encoderKRAI::X2_ENCODING); //inA, inB, pin Indeks (NC = tak ada), resolusi, pembacaan |
rahmadirizki18 | 15:98f0d56b14f0 | 70 | |
calmantara186 | 16:90119f03c5d1 | 71 | /* Deklarasi Motor Base */ |
Joshua23 | 25:054d3048dd03 | 72 | Motor motor1(PB_9, PC_5, PA_12); // pwm, fwd, rev, Motor Depan |
Joshua23 | 25:054d3048dd03 | 73 | Motor motor2(PB_6, PB_1, PB_12); // pwm, fwd, rev, Motor Belakang |
fanny868 | 0:9072e932503c | 74 | |
calmantara186 | 16:90119f03c5d1 | 75 | /* Deklarasi Motor Launcher */ |
rahmadirizki18 | 23:023b522977b2 | 76 | //Motor motorld(PA_8, PC_1, PC_2); // pwm, fwd, rev |
rahmadirizki18 | 23:023b522977b2 | 77 | //Motor motorlb(PA_0, PA_4, PC_15 ); // pwm, fwd, rev |
rahmadirizki18 | 5:3aa203218306 | 78 | |
calmantara186 | 16:90119f03c5d1 | 79 | /* Deklarasi Servo Launcher */ |
rahmadirizki18 | 23:023b522977b2 | 80 | //Servo servoS(PB_2); |
rahmadirizki18 | 23:023b522977b2 | 81 | //Servo servoB(PA_5); |
rahmadirizki18 | 6:68293bed71ea | 82 | |
calmantara186 | 16:90119f03c5d1 | 83 | /** |
calmantara186 | 16:90119f03c5d1 | 84 | * posX dan posY berdasarkan arah robot |
calmantara186 | 16:90119f03c5d1 | 85 | * encoder Depan & Belakang sejajar sumbu Y |
calmantara186 | 16:90119f03c5d1 | 86 | * encoder Kanan & Kiri sejajar sumbu X |
calmantara186 | 16:90119f03c5d1 | 87 | **/ |
rahmadirizki18 | 5:3aa203218306 | 88 | |
calmantara186 | 16:90119f03c5d1 | 89 | /* Variabel Encoder */ |
MarchioKevin | 22:4632f58461e0 | 90 | float errT, Tetha; // Variabel yang didapatkan encoder |
rahmadirizki18 | 5:3aa203218306 | 91 | |
calmantara186 | 16:90119f03c5d1 | 92 | /* Fungsi dan Procedur Encoder */ |
calmantara186 | 16:90119f03c5d1 | 93 | void setCenter(); // Fungsi reset agar robot di tengah |
MarchioKevin | 20:54dc93e7b016 | 94 | float getTetha(); // Fungsi mendapatkan jarak Tetha |
calmantara186 | 16:90119f03c5d1 | 95 | |
calmantara186 | 16:90119f03c5d1 | 96 | /* Inisialisasi Pin TX-RX Joystik dan PC */ |
Joshua23 | 25:054d3048dd03 | 97 | joysticknucleo joystick(PA_0,PA_1); |
rahmadirizki18 | 23:023b522977b2 | 98 | Serial pc(USBTX,USBRX); |
fanny868 | 0:9072e932503c | 99 | |
calmantara186 | 16:90119f03c5d1 | 100 | /* Variabel Stick */ |
fanny868 | 0:9072e932503c | 101 | char case_ger; |
MarchioKevin | 22:4632f58461e0 | 102 | bool launcher = false, servoGo = false; |
fanny868 | 0:9072e932503c | 103 | |
MarchioKevin | 22:4632f58461e0 | 104 | /****************************************************/ |
MarchioKevin | 22:4632f58461e0 | 105 | /* Deklarasi Fungsi dan Procedure */ |
MarchioKevin | 22:4632f58461e0 | 106 | /****************************************************/ |
Joshua23 | 25:054d3048dd03 | 107 | |
Joshua23 | 25:054d3048dd03 | 108 | float pid(float Kp, float Ki, float Kd) |
Joshua23 | 25:054d3048dd03 | 109 | { |
Joshua23 | 25:054d3048dd03 | 110 | int errorP; |
Joshua23 | 25:054d3048dd03 | 111 | |
Joshua23 | 25:054d3048dd03 | 112 | errorP = getTetha(); |
Joshua23 | 25:054d3048dd03 | 113 | |
Joshua23 | 25:054d3048dd03 | 114 | return (float)Kp*errorP; |
Joshua23 | 25:054d3048dd03 | 115 | } |
Joshua23 | 25:054d3048dd03 | 116 | |
calmantara186 | 16:90119f03c5d1 | 117 | int case_gerak(){ |
rahmadirizki18 | 23:023b522977b2 | 118 | /**************************************************** |
calmantara186 | 16:90119f03c5d1 | 119 | ** Gerak Motor Base |
calmantara186 | 16:90119f03c5d1 | 120 | ** Case 1 : Pivot kanan |
calmantara186 | 16:90119f03c5d1 | 121 | ** Case 2 : Pivot Kiri |
MarchioKevin | 22:4632f58461e0 | 122 | ** Case 3 : Kanan |
MarchioKevin | 22:4632f58461e0 | 123 | ** Case 4 : Kiri |
MarchioKevin | 22:4632f58461e0 | 124 | ** Case 5 : Break |
calmantara186 | 16:90119f03c5d1 | 125 | ****************************************************/ |
MarchioKevin | 22:4632f58461e0 | 126 | |
fanny868 | 0:9072e932503c | 127 | int casegerak; |
calmantara186 | 16:90119f03c5d1 | 128 | if (!joystick.L1 && joystick.R1) { |
fanny868 | 0:9072e932503c | 129 | // Pivot Kanan |
fanny868 | 0:9072e932503c | 130 | casegerak = 1; |
calmantara186 | 16:90119f03c5d1 | 131 | } |
calmantara186 | 16:90119f03c5d1 | 132 | else if (!joystick.R1 && joystick.L1) { |
fanny868 | 0:9072e932503c | 133 | // Pivot Kiri |
fanny868 | 0:9072e932503c | 134 | casegerak = 2; |
calmantara186 | 16:90119f03c5d1 | 135 | } |
calmantara186 | 16:90119f03c5d1 | 136 | else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) { |
fanny868 | 0:9072e932503c | 137 | // Kanan |
MarchioKevin | 22:4632f58461e0 | 138 | casegerak = 3; |
rahmadirizki18 | 23:023b522977b2 | 139 | pc.printf("kanan"); |
calmantara186 | 16:90119f03c5d1 | 140 | } |
calmantara186 | 16:90119f03c5d1 | 141 | else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) { |
fanny868 | 0:9072e932503c | 142 | // Kiri |
rahmadirizki18 | 23:023b522977b2 | 143 | casegerak = 4; |
rahmadirizki18 | 23:023b522977b2 | 144 | pc.printf("kiri"); |
calmantara186 | 16:90119f03c5d1 | 145 | } |
calmantara186 | 16:90119f03c5d1 | 146 | else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) { |
MarchioKevin | 22:4632f58461e0 | 147 | // Break |
MarchioKevin | 22:4632f58461e0 | 148 | casegerak = 5; |
calmantara186 | 16:90119f03c5d1 | 149 | } |
fanny868 | 0:9072e932503c | 150 | return(casegerak); |
fanny868 | 0:9072e932503c | 151 | } |
fanny868 | 0:9072e932503c | 152 | |
calmantara186 | 16:90119f03c5d1 | 153 | void aktuator(){ |
calmantara186 | 16:90119f03c5d1 | 154 | /* Fungsi untuk menggerakkan servo */ |
calmantara186 | 16:90119f03c5d1 | 155 | // Servo |
rahmadirizki18 | 23:023b522977b2 | 156 | /* if (servoGo){ |
franshendri | 12:e07c59c28c29 | 157 | servoS.position(20); |
franshendri | 10:f0f0dc3904e0 | 158 | wait_ms(500); |
franshendri | 12:e07c59c28c29 | 159 | servoS.position(-28); |
Joshua23 | 8:0711dea61312 | 160 | wait_ms(500); |
franshendri | 12:e07c59c28c29 | 161 | servoS.position(20); |
Joshua23 | 8:0711dea61312 | 162 | wait_ms(500); |
franshendri | 12:e07c59c28c29 | 163 | for (int i = -0; i<=70; i++){ |
Joshua23 | 8:0711dea61312 | 164 | servoB.position(i); |
Joshua23 | 8:0711dea61312 | 165 | wait_ms(10); |
Joshua23 | 8:0711dea61312 | 166 | } |
Joshua23 | 8:0711dea61312 | 167 | wait_ms(500); |
Joshua23 | 8:0711dea61312 | 168 | servoB.position(0); |
calmantara186 | 16:90119f03c5d1 | 169 | servoGo = false; |
calmantara186 | 16:90119f03c5d1 | 170 | } |
calmantara186 | 16:90119f03c5d1 | 171 | else{ |
franshendri | 12:e07c59c28c29 | 172 | servoS.position(20); |
rahmadirizki18 | 6:68293bed71ea | 173 | servoB.position(0); |
rahmadirizki18 | 7:d138c56dab20 | 174 | } |
rahmadirizki18 | 6:68293bed71ea | 175 | |
rahmadirizki18 | 6:68293bed71ea | 176 | // Motor Atas |
calmantara186 | 16:90119f03c5d1 | 177 | if (launcher) { |
rahmadirizki18 | 7:d138c56dab20 | 178 | motorld.speed(speedL); |
franshendri | 12:e07c59c28c29 | 179 | motorlb.speed(speedB); |
rahmadirizki18 | 6:68293bed71ea | 180 | }else{ |
rahmadirizki18 | 6:68293bed71ea | 181 | motorld.speed(0); |
rahmadirizki18 | 6:68293bed71ea | 182 | motorlb.speed(0); |
rahmadirizki18 | 6:68293bed71ea | 183 | } |
rahmadirizki18 | 23:023b522977b2 | 184 | */ |
rahmadirizki18 | 6:68293bed71ea | 185 | // MOTOR Bawah |
MarchioKevin | 22:4632f58461e0 | 186 | switch (case_ger) { |
rahmadirizki18 | 17:e4229d77a5ab | 187 | case (1): { |
rahmadirizki18 | 17:e4229d77a5ab | 188 | // Pivot Kanan |
Joshua23 | 25:054d3048dd03 | 189 | motor1.speed(PIVOT); |
Joshua23 | 25:054d3048dd03 | 190 | motor2.speed(PIVOT); |
Joshua23 | 25:054d3048dd03 | 191 | setCenter(); |
fanny868 | 0:9072e932503c | 192 | break; |
rahmadirizki18 | 17:e4229d77a5ab | 193 | } |
rahmadirizki18 | 17:e4229d77a5ab | 194 | case (2): { |
rahmadirizki18 | 17:e4229d77a5ab | 195 | // Pivot Kiri |
Joshua23 | 25:054d3048dd03 | 196 | motor1.speed(-PIVOT); |
Joshua23 | 25:054d3048dd03 | 197 | motor2.speed(-PIVOT); |
Joshua23 | 25:054d3048dd03 | 198 | setCenter(); |
fanny868 | 0:9072e932503c | 199 | break; |
MarchioKevin | 22:4632f58461e0 | 200 | } |
MarchioKevin | 22:4632f58461e0 | 201 | case (3) : { |
MarchioKevin | 22:4632f58461e0 | 202 | // Kanan |
Joshua23 | 25:054d3048dd03 | 203 | //motor1.speed(-VMAX-vpid); |
Joshua23 | 25:054d3048dd03 | 204 | //motor2.speed(0.2+vpid); |
Joshua23 | 25:054d3048dd03 | 205 | motor1.speed(-0.365+pid(0.09,0,0)); |
Joshua23 | 25:054d3048dd03 | 206 | motor2.speed(0.46+pid(0.09,0,0)); |
rahmadirizki18 | 17:e4229d77a5ab | 207 | break; |
MarchioKevin | 22:4632f58461e0 | 208 | } |
MarchioKevin | 22:4632f58461e0 | 209 | case (4) : { |
rahmadirizki18 | 17:e4229d77a5ab | 210 | // Kiri |
Joshua23 | 25:054d3048dd03 | 211 | //motor1.speed(VMAX-vpid); |
Joshua23 | 25:054d3048dd03 | 212 | //motor2.speed(-0.2+vpid); |
Joshua23 | 25:054d3048dd03 | 213 | motor1.speed(0.365+pid(0.09,0,0));//belakang |
Joshua23 | 25:054d3048dd03 | 214 | motor2.speed(-0.46+pid(0.09,0,0));//depan |
fanny868 | 0:9072e932503c | 215 | break; |
rahmadirizki18 | 17:e4229d77a5ab | 216 | } |
rahmadirizki18 | 17:e4229d77a5ab | 217 | default : { |
MarchioKevin | 22:4632f58461e0 | 218 | motor1.brake(1); |
MarchioKevin | 22:4632f58461e0 | 219 | motor2.brake(1); |
rahmadirizki18 | 17:e4229d77a5ab | 220 | } |
MarchioKevin | 22:4632f58461e0 | 221 | } // End Switch |
rahmadirizki18 | 5:3aa203218306 | 222 | } |
rahmadirizki18 | 5:3aa203218306 | 223 | |
calmantara186 | 16:90119f03c5d1 | 224 | void setCenter(){ |
calmantara186 | 16:90119f03c5d1 | 225 | /* Fungsi untuk menentukan center dari robot */ |
rahmadirizki18 | 5:3aa203218306 | 226 | encoderKiri.reset(); |
rahmadirizki18 | 5:3aa203218306 | 227 | } |
rahmadirizki18 | 5:3aa203218306 | 228 | |
calmantara186 | 16:90119f03c5d1 | 229 | float getTetha(){ |
calmantara186 | 16:90119f03c5d1 | 230 | /* Fungsi untuk mendapatkan nilai tetha */ |
Joshua23 | 25:054d3048dd03 | 231 | float busurKir, tetha; |
MarchioKevin | 22:4632f58461e0 | 232 | busurKir = ((encoderKiri.getPulses())/(float)(2000.0)*k_enc); |
Joshua23 | 25:054d3048dd03 | 233 | tetha = busurKir/k_robot*360; |
calmantara186 | 16:90119f03c5d1 | 234 | |
Joshua23 | 25:054d3048dd03 | 235 | return -(tetha); |
calmantara186 | 16:90119f03c5d1 | 236 | } |
rahmadirizki18 | 5:3aa203218306 | 237 | |
rahmadirizki18 | 24:b3e632cc4533 | 238 | void speedKalibrasiMotor(){ |
calmantara186 | 16:90119f03c5d1 | 239 | /* Fungsi untuk speed launcher */ |
franshendri | 12:e07c59c28c29 | 240 | if (joystick.R3_click and speedL < 0.8){ |
MarchioKevin | 22:4632f58461e0 | 241 | speedL = speedL + 0.01; // PWM++ Motor Belakang |
calmantara186 | 16:90119f03c5d1 | 242 | } |
rahmadirizki18 | 7:d138c56dab20 | 243 | if (joystick.L3_click and speedL > 0.1){ |
MarchioKevin | 22:4632f58461e0 | 244 | speedL = speedL - 0.01; // PWM-- Motor Belakang |
calmantara186 | 16:90119f03c5d1 | 245 | } |
rahmadirizki18 | 13:8ab42383a2ca | 246 | if (joystick.R2_click and speedB < 0.8 ){ |
MarchioKevin | 22:4632f58461e0 | 247 | speedB = speedB + 0.01; // PWM++ Motor Depan |
calmantara186 | 16:90119f03c5d1 | 248 | } |
rahmadirizki18 | 13:8ab42383a2ca | 249 | if (joystick.L2_click and speedB > 0.1 ){ |
MarchioKevin | 22:4632f58461e0 | 250 | speedB = speedB - 0.01; // PWM-- Motor Depan |
rahmadirizki18 | 24:b3e632cc4533 | 251 | } |
rahmadirizki18 | 24:b3e632cc4533 | 252 | // pc.printf("Pwm depan = %.3f\t Pwm belakang = %.3f\n", speedL, speedB); |
rahmadirizki18 | 7:d138c56dab20 | 253 | } |
franshendri | 12:e07c59c28c29 | 254 | |
MarchioKevin | 22:4632f58461e0 | 255 | /*********************************************************/ |
MarchioKevin | 22:4632f58461e0 | 256 | /* Main Function */ |
MarchioKevin | 22:4632f58461e0 | 257 | /*********************************************************/ |
calmantara186 | 16:90119f03c5d1 | 258 | |
calmantara186 | 16:90119f03c5d1 | 259 | int main (void){ |
calmantara186 | 16:90119f03c5d1 | 260 | /* Set baud rate - 115200 */ |
fanny868 | 0:9072e932503c | 261 | joystick.setup(); |
rahmadirizki18 | 23:023b522977b2 | 262 | pc.baud(115200); |
rahmadirizki18 | 6:68293bed71ea | 263 | wait_ms(1000); |
rahmadirizki18 | 5:3aa203218306 | 264 | setCenter(); |
rahmadirizki18 | 5:3aa203218306 | 265 | wait_ms(500); |
rahmadirizki18 | 23:023b522977b2 | 266 | pc.printf("ready...."); |
calmantara186 | 16:90119f03c5d1 | 267 | |
calmantara186 | 16:90119f03c5d1 | 268 | /* Untuk mendapatkan serial dari Arduino */ |
fanny868 | 0:9072e932503c | 269 | while(1) |
fanny868 | 0:9072e932503c | 270 | { |
fanny868 | 0:9072e932503c | 271 | // Interrupt Serial |
calmantara186 | 16:90119f03c5d1 | 272 | joystick.idle(); |
Joshua23 | 25:054d3048dd03 | 273 | //pc.printf("enco : %d \n",encoderKiri.getPulses()); |
calmantara186 | 16:90119f03c5d1 | 274 | if(joystick.readable() ) { |
fanny868 | 0:9072e932503c | 275 | // Panggil fungsi pembacaan joystik |
fanny868 | 0:9072e932503c | 276 | joystick.baca_data(); |
calmantara186 | 16:90119f03c5d1 | 277 | |
fanny868 | 0:9072e932503c | 278 | // Panggil fungsi pengolahan data joystik |
fanny868 | 0:9072e932503c | 279 | joystick.olah_data(); |
calmantara186 | 16:90119f03c5d1 | 280 | |
calmantara186 | 16:90119f03c5d1 | 281 | // Masuk ke case gerak |
fanny868 | 0:9072e932503c | 282 | case_ger = case_gerak(); |
rahmadirizki18 | 3:1287fccc11be | 283 | aktuator(); |
calmantara186 | 16:90119f03c5d1 | 284 | |
rahmadirizki18 | 17:e4229d77a5ab | 285 | if (joystick.segitiga_click) launcher = !launcher; |
rahmadirizki18 | 17:e4229d77a5ab | 286 | if (joystick.lingkaran_click) servoGo = true; |
rahmadirizki18 | 24:b3e632cc4533 | 287 | speedKalibrasiMotor(); |
calmantara186 | 16:90119f03c5d1 | 288 | } |
calmantara186 | 16:90119f03c5d1 | 289 | else { |
calmantara186 | 16:90119f03c5d1 | 290 | joystick.idle(); |
Joshua23 | 25:054d3048dd03 | 291 | //motor1.brake(1); |
Joshua23 | 25:054d3048dd03 | 292 | //motor2.brake(1); |
MarchioKevin | 21:da2f3d04468f | 293 | } |
fanny868 | 0:9072e932503c | 294 | } |
fanny868 | 0:9072e932503c | 295 | } |