KRAI 2017 / Mbed 2 deprecated MainProgram_BaseBaru_fix_omni12Feb

Update 12 Maret 2017. Tambahan kombinasi tombol

Dependencies:   DigitDisplay Motor PID Ping mbed millis

Fork of MainProgram_BaseBaru_20Feb_malam by KRAI 2017

main.cpp@30:d69cc27ac644, 2017-02-12 (annotated)

Committer:
Sufa
Date:
Sun Feb 12 11:19:48 2017 +0000
Revision:
30:d69cc27ac644
Parent:
29:7b372b0aaa61
Child:
31:d5cbda07fd95
Masih dicoba

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 */
Sufa 30:d69cc27ac644 22 /* Kanan => */
Sufa 30:d69cc27ac644 23 /* Kiri => */
rahmadirizki18 6:68293bed71ea 24 /* */
Sufa 30:d69cc27ac644 25 /* Tombol silang => Power Screw Aktif */
Sufa 30:d69cc27ac644 26 /* Tombol segitiga => Aktif motor Launcher */
Sufa 30:d69cc27ac644 27 /* Tombol lingkaran => Aktif Pneumatik Launcher */
Sufa 30:d69cc27ac644 28 /* Tombol L1 => Pivot Kiri */
Sufa 30:d69cc27ac644 29 /* Tombol R1 => Pivot Kanan */
Sufa 30:d69cc27ac644 30 /* Tombol L3 => PWM Motor Belakang Dikurangi */
Sufa 30:d69cc27ac644 31 /* Tombol R3 => PWM Motor Belakang Ditambah */
Sufa 30:d69cc27ac644 32 /* Tombol L2 => PWM Motor Depan Dikurangi */
Sufa 30:d69cc27ac644 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 27:68efb1622985 57 double speed,speed2, maxSpeed = 0.8, minSpeed = 0;
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 27:68efb1622985 60 double p2,i2,d2;
be_bryan 26:256160a1a82d 61 double last_error = 0, current_error, sum_error = 0;
be_bryan 27:68efb1622985 62 double last_error2 = 0, current_error2, sum_error2 = 0;
be_bryan 27:68efb1622985 63 float rpm, target_rpm = 2.0;
be_bryan 27:68efb1622985 64 float rpm2, target_rpm2 = 4.0;
Sufa 30:d69cc27ac644 65 bool henti=false, hentis=false;
be_bryan 26:256160a1a82d 66
be_bryan 26:256160a1a82d 67 // Variable Bawah
Joshua23 25:054d3048dd03 68 float Vt;
calmantara186 16:90119f03c5d1 69 float k_enc = PI*D_ENCODER;
Joshua23 25:054d3048dd03 70 float k_robot = PI*D_ROBOT;
MarchioKevin 22:4632f58461e0 71 float speedT = 0.2;
be_bryan 27:68efb1622985 72 float vpid = 0;
be_bryan 27:68efb1622985 73 float pwmP = 0.5;
Sufa 29:7b372b0aaa61 74 float pwmT = -0.95;
MarchioKevin 22:4632f58461e0 75
calmantara186 16:90119f03c5d1 76
be_bryan 26:256160a1a82d 77 /* Deklarasi Encoder Base */
Joshua23 25:054d3048dd03 78 encoderKRAI encoderKiri(PC_4, PB_15, 2000, encoderKRAI::X2_ENCODING); //inA, inB, pin Indeks (NC = tak ada), resolusi, pembacaan
rahmadirizki18 15:98f0d56b14f0 79
be_bryan 26:256160a1a82d 80 /* Deklarasi Encoder Launcher */
Sufa 30:d69cc27ac644 81 encoderKRAI encoderAtas2( PC_10, PC_11, 28, encoderKRAI::X2_ENCODING);
Sufa 30:d69cc27ac644 82 encoderKRAI encoderAtas( PD_2, PC_12, 28, encoderKRAI::X2_ENCODING);
be_bryan 26:256160a1a82d 83
calmantara186 16:90119f03c5d1 84 /* Deklarasi Motor Base */
Sufa 29:7b372b0aaa61 85 Motor motor1(PB_9, PA_12, PC_5); // pwm, fwd, rev, Motor Depan
be_bryan 26:256160a1a82d 86 Motor motor2(PB_6, PB_1, PB_12); // pwm, fwd, rev, Motor Belakang
fanny868 0:9072e932503c 87
calmantara186 16:90119f03c5d1 88 /* Deklarasi Motor Launcher */
Sufa 29:7b372b0aaa61 89 Motor motorL1(PA_8,PC_2,PC_1); // pwm ,fwd, rev, Motor Launcher1
Sufa 29:7b372b0aaa61 90 Motor motorL3(PA_10, PC_3, PC_0); // pwm, fwd, rev, Motor Launcher2
Sufa 29:7b372b0aaa61 91 Motor motorP(PB_7, PA_14, PA_15); // pwm, fwd, rev, Motor Powerscrew
rahmadirizki18 5:3aa203218306 92
be_bryan 26:256160a1a82d 93 /* Deklarasi Penumatik Launcher */
Sufa 29:7b372b0aaa61 94 DigitalOut pneumatik(PB_3, PullUp);
be_bryan 26:256160a1a82d 95
be_bryan 27:68efb1622985 96 /*Dekalrasi Limit Switch */
Sufa 30:d69cc27ac644 97 DigitalIn limitA(PC_9, PullUp);
be_bryan 27:68efb1622985 98 DigitalIn limitT(PB_10, PullUp);
Sufa 30:d69cc27ac644 99 DigitalIn limitB(PC_8, PullUp);
be_bryan 27:68efb1622985 100
calmantara186 16:90119f03c5d1 101 /**
calmantara186 16:90119f03c5d1 102 * posX dan posY berdasarkan arah robot
calmantara186 16:90119f03c5d1 103 * encoder Depan & Belakang sejajar sumbu Y
be_bryan 27:68efb1622985 104 * encoder Kanan & Kirim sejajar sumbu X
calmantara186 16:90119f03c5d1 105 **/
rahmadirizki18 5:3aa203218306 106
calmantara186 16:90119f03c5d1 107 /* Variabel Encoder */
MarchioKevin 22:4632f58461e0 108 float errT, Tetha; // Variabel yang didapatkan encoder
rahmadirizki18 5:3aa203218306 109
calmantara186 16:90119f03c5d1 110 /* Fungsi dan Procedur Encoder */
calmantara186 16:90119f03c5d1 111 void setCenter(); // Fungsi reset agar robot di tengah
MarchioKevin 20:54dc93e7b016 112 float getTetha(); // Fungsi mendapatkan jarak Tetha
calmantara186 16:90119f03c5d1 113
calmantara186 16:90119f03c5d1 114 /* Inisialisasi Pin TX-RX Joystik dan PC */
Joshua23 25:054d3048dd03 115 joysticknucleo joystick(PA_0,PA_1);
rahmadirizki18 23:023b522977b2 116 Serial pc(USBTX,USBRX);
fanny868 0:9072e932503c 117
be_bryan 26:256160a1a82d 118 /* Deklarasi Variable Milis */
be_bryan 26:256160a1a82d 119 unsigned long int previousMillis = 0;
be_bryan 26:256160a1a82d 120 unsigned long int currentMillis;
be_bryan 27:68efb1622985 121 unsigned long int previousMillis2 = 0;
be_bryan 27:68efb1622985 122 unsigned long int currentMillis2;
be_bryan 26:256160a1a82d 123
calmantara186 16:90119f03c5d1 124 /* Variabel Stick */
fanny868 0:9072e932503c 125 char case_ger;
be_bryan 26:256160a1a82d 126 bool launcher = false;
be_bryan 27:68efb1622985 127 bool reload = false;
fanny868 0:9072e932503c 128
MarchioKevin 22:4632f58461e0 129 /****************************************************/
MarchioKevin 22:4632f58461e0 130 /* Deklarasi Fungsi dan Procedure */
MarchioKevin 22:4632f58461e0 131 /****************************************************/
be_bryan 26:256160a1a82d 132 void init_speed();
be_bryan 26:256160a1a82d 133 void speedKalibrasiMotor();
be_bryan 26:256160a1a82d 134 float pidBase(float Kp, float Ki, float Kd)
Joshua23 25:054d3048dd03 135 {
Joshua23 25:054d3048dd03 136 int errorP;
Joshua23 25:054d3048dd03 137 errorP = getTetha();
Joshua23 25:054d3048dd03 138 return (float)Kp*errorP;
Joshua23 25:054d3048dd03 139 }
Joshua23 25:054d3048dd03 140
calmantara186 16:90119f03c5d1 141 int case_gerak(){
rahmadirizki18 23:023b522977b2 142 /****************************************************
calmantara186 16:90119f03c5d1 143 ** Gerak Motor Base
calmantara186 16:90119f03c5d1 144 ** Case 1 : Pivot kanan
calmantara186 16:90119f03c5d1 145 ** Case 2 : Pivot Kiri
MarchioKevin 22:4632f58461e0 146 ** Case 3 : Kanan
MarchioKevin 22:4632f58461e0 147 ** Case 4 : Kiri
MarchioKevin 22:4632f58461e0 148 ** Case 5 : Break
calmantara186 16:90119f03c5d1 149 ****************************************************/
MarchioKevin 22:4632f58461e0 150
fanny868 0:9072e932503c 151 int casegerak;
calmantara186 16:90119f03c5d1 152 if (!joystick.L1 && joystick.R1) {
fanny868 0:9072e932503c 153 // Pivot Kanan
fanny868 0:9072e932503c 154 casegerak = 1;
calmantara186 16:90119f03c5d1 155 }
calmantara186 16:90119f03c5d1 156 else if (!joystick.R1 && joystick.L1) {
fanny868 0:9072e932503c 157 // Pivot Kiri
fanny868 0:9072e932503c 158 casegerak = 2;
calmantara186 16:90119f03c5d1 159 }
calmantara186 16:90119f03c5d1 160 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {
fanny868 0:9072e932503c 161 // Kanan
MarchioKevin 22:4632f58461e0 162 casegerak = 3;
be_bryan 27:68efb1622985 163 // pc.printf("kanan");
calmantara186 16:90119f03c5d1 164 }
calmantara186 16:90119f03c5d1 165 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {
fanny868 0:9072e932503c 166 // Kiri
rahmadirizki18 23:023b522977b2 167 casegerak = 4;
be_bryan 27:68efb1622985 168 // pc.printf("kiri");
calmantara186 16:90119f03c5d1 169 }
calmantara186 16:90119f03c5d1 170 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
MarchioKevin 22:4632f58461e0 171 // Break
MarchioKevin 22:4632f58461e0 172 casegerak = 5;
calmantara186 16:90119f03c5d1 173 }
fanny868 0:9072e932503c 174 return(casegerak);
fanny868 0:9072e932503c 175 }
fanny868 0:9072e932503c 176
calmantara186 16:90119f03c5d1 177 void aktuator(){
be_bryan 27:68efb1622985 178 /* Fungsi untuk menggerakkan Motor PowerScrew */
be_bryan 27:68efb1622985 179 // PowerScrew
be_bryan 27:68efb1622985 180 if (reload){
be_bryan 27:68efb1622985 181 if (!limitA){
be_bryan 27:68efb1622985 182 motorP.brake (1);
Sufa 30:d69cc27ac644 183 if (hentis != henti){
Sufa 30:d69cc27ac644 184 wait_ms(1500);
Sufa 30:d69cc27ac644 185 }
Sufa 30:d69cc27ac644 186 hentis = henti;
Joshua23 8:0711dea61312 187 }
be_bryan 27:68efb1622985 188 else
be_bryan 27:68efb1622985 189 {
Sufa 30:d69cc27ac644 190 henti = false;
be_bryan 27:68efb1622985 191 if (!limitT)
be_bryan 27:68efb1622985 192 {
be_bryan 27:68efb1622985 193 while (limitB)
be_bryan 27:68efb1622985 194 {
be_bryan 27:68efb1622985 195 motorP.speed(pwmT);
be_bryan 27:68efb1622985 196 }
be_bryan 27:68efb1622985 197 motorP.brake(1);
Sufa 29:7b372b0aaa61 198 reload = !reload;
Sufa 30:d69cc27ac644 199 }
Sufa 30:d69cc27ac644 200 else
Sufa 30:d69cc27ac644 201 {
Sufa 30:d69cc27ac644 202 motorP.speed(pwmP);
be_bryan 27:68efb1622985 203 }
be_bryan 27:68efb1622985 204 }
Sufa 30:d69cc27ac644 205 }
Sufa 30:d69cc27ac644 206 else
Sufa 30:d69cc27ac644 207 {
Sufa 30:d69cc27ac644 208 motorP.brake (1);
calmantara186 16:90119f03c5d1 209 }
be_bryan 28:2d0746dc2d7d 210
be_bryan 27:68efb1622985 211 /*Motor Atas*/
calmantara186 16:90119f03c5d1 212 if (launcher) {
be_bryan 26:256160a1a82d 213 startMillis();
be_bryan 28:2d0746dc2d7d 214 currentMillis = millis();
be_bryan 28:2d0746dc2d7d 215 currentMillis2 = millis();
be_bryan 28:2d0746dc2d7d 216
be_bryan 28:2d0746dc2d7d 217 /*PID Launcher Depan*/
be_bryan 26:256160a1a82d 218 if (currentMillis-previousMillis>=10)
be_bryan 26:256160a1a82d 219 {
be_bryan 27:68efb1622985 220 rpm = (float)encoderAtas.getPulses();
be_bryan 26:256160a1a82d 221 current_error = target_rpm - rpm;
be_bryan 26:256160a1a82d 222 sum_error = sum_error + current_error;
be_bryan 26:256160a1a82d 223 p = current_error*kpA;
be_bryan 26:256160a1a82d 224 d = (current_error-last_error)*kdA/10.0;
be_bryan 26:256160a1a82d 225 i = sum_error*kiA*10.0;
be_bryan 26:256160a1a82d 226 speed = p + d + i;
be_bryan 26:256160a1a82d 227 init_speed();
be_bryan 27:68efb1622985 228 motorL1.speed(speed);
be_bryan 26:256160a1a82d 229 last_error = current_error;
be_bryan 26:256160a1a82d 230 encoderAtas.reset();
be_bryan 26:256160a1a82d 231 //pc.printf("%.04lf\n",rpm);
be_bryan 26:256160a1a82d 232 previousMillis = currentMillis;
be_bryan 26:256160a1a82d 233 }
be_bryan 26:256160a1a82d 234 else
be_bryan 26:256160a1a82d 235 {
be_bryan 27:68efb1622985 236 encoderAtas.getPulses();
be_bryan 27:68efb1622985 237 }
be_bryan 27:68efb1622985 238 if (currentMillis2-previousMillis2>=10)
be_bryan 27:68efb1622985 239 {
be_bryan 27:68efb1622985 240 rpm2 = (float)encoderAtas2.getPulses();
be_bryan 27:68efb1622985 241 current_error2 = target_rpm2 - rpm2;
be_bryan 27:68efb1622985 242 sum_error2 = sum_error2 + current_error2;
be_bryan 27:68efb1622985 243 p2 = current_error2*kpA;
be_bryan 27:68efb1622985 244 d2 = (current_error2-last_error2)*kdA/10.0;
be_bryan 27:68efb1622985 245 i2 = sum_error2*kiA*10.0;
be_bryan 27:68efb1622985 246 speed2 = p2 + d2 + i2;
be_bryan 27:68efb1622985 247 init_speed();
be_bryan 27:68efb1622985 248 motorL3.speed(speed2);
be_bryan 27:68efb1622985 249 last_error2 = current_error2;
be_bryan 27:68efb1622985 250 encoderAtas2.reset();
be_bryan 27:68efb1622985 251 //pc.printf("%.04lf\n",rpm2);
be_bryan 27:68efb1622985 252 previousMillis2 = currentMillis2;
be_bryan 27:68efb1622985 253 }
be_bryan 27:68efb1622985 254 else
be_bryan 27:68efb1622985 255 {
be_bryan 27:68efb1622985 256 encoderAtas2.getPulses();
be_bryan 26:256160a1a82d 257 }
rahmadirizki18 6:68293bed71ea 258 }
Sufa 29:7b372b0aaa61 259 else
Sufa 29:7b372b0aaa61 260 {
Sufa 29:7b372b0aaa61 261 motorL1.brake(1);
Sufa 29:7b372b0aaa61 262 motorL3.brake(1);
Sufa 29:7b372b0aaa61 263 }
Sufa 29:7b372b0aaa61 264
be_bryan 28:2d0746dc2d7d 265
rahmadirizki18 6:68293bed71ea 266 // MOTOR Bawah
MarchioKevin 22:4632f58461e0 267 switch (case_ger) {
rahmadirizki18 17:e4229d77a5ab 268 case (1): {
rahmadirizki18 17:e4229d77a5ab 269 // Pivot Kanan
Joshua23 25:054d3048dd03 270 motor1.speed(PIVOT);
Joshua23 25:054d3048dd03 271 motor2.speed(PIVOT);
Joshua23 25:054d3048dd03 272 setCenter();
fanny868 0:9072e932503c 273 break;
rahmadirizki18 17:e4229d77a5ab 274 }
rahmadirizki18 17:e4229d77a5ab 275 case (2): {
rahmadirizki18 17:e4229d77a5ab 276 // Pivot Kiri
Joshua23 25:054d3048dd03 277 motor1.speed(-PIVOT);
Joshua23 25:054d3048dd03 278 motor2.speed(-PIVOT);
Joshua23 25:054d3048dd03 279 setCenter();
fanny868 0:9072e932503c 280 break;
MarchioKevin 22:4632f58461e0 281 }
MarchioKevin 22:4632f58461e0 282 case (3) : {
MarchioKevin 22:4632f58461e0 283 // Kanan
Joshua23 25:054d3048dd03 284 //motor1.speed(-VMAX-vpid);
Joshua23 25:054d3048dd03 285 //motor2.speed(0.2+vpid);
be_bryan 26:256160a1a82d 286 motor1.speed(-0.365+pidBase(0.09,0,0));
be_bryan 26:256160a1a82d 287 motor2.speed(0.46+pidBase(0.09,0,0));
rahmadirizki18 17:e4229d77a5ab 288 break;
MarchioKevin 22:4632f58461e0 289 }
MarchioKevin 22:4632f58461e0 290 case (4) : {
rahmadirizki18 17:e4229d77a5ab 291 // Kiri
Joshua23 25:054d3048dd03 292 //motor1.speed(VMAX-vpid);
Joshua23 25:054d3048dd03 293 //motor2.speed(-0.2+vpid);
be_bryan 26:256160a1a82d 294 motor1.speed(0.365+pidBase(0.09,0,0));//belakang
be_bryan 26:256160a1a82d 295 motor2.speed(-0.46+pidBase(0.09,0,0));//depan
fanny868 0:9072e932503c 296 break;
rahmadirizki18 17:e4229d77a5ab 297 }
rahmadirizki18 17:e4229d77a5ab 298 default : {
MarchioKevin 22:4632f58461e0 299 motor1.brake(1);
MarchioKevin 22:4632f58461e0 300 motor2.brake(1);
rahmadirizki18 17:e4229d77a5ab 301 }
MarchioKevin 22:4632f58461e0 302 } // End Switch
rahmadirizki18 5:3aa203218306 303 }
rahmadirizki18 5:3aa203218306 304
calmantara186 16:90119f03c5d1 305 void setCenter(){
calmantara186 16:90119f03c5d1 306 /* Fungsi untuk menentukan center dari robot */
rahmadirizki18 5:3aa203218306 307 encoderKiri.reset();
rahmadirizki18 5:3aa203218306 308 }
rahmadirizki18 5:3aa203218306 309
calmantara186 16:90119f03c5d1 310 float getTetha(){
calmantara186 16:90119f03c5d1 311 /* Fungsi untuk mendapatkan nilai tetha */
Joshua23 25:054d3048dd03 312 float busurKir, tetha;
MarchioKevin 22:4632f58461e0 313 busurKir = ((encoderKiri.getPulses())/(float)(2000.0)*k_enc);
Joshua23 25:054d3048dd03 314 tetha = busurKir/k_robot*360;
calmantara186 16:90119f03c5d1 315
Joshua23 25:054d3048dd03 316 return -(tetha);
calmantara186 16:90119f03c5d1 317 }
rahmadirizki18 5:3aa203218306 318
rahmadirizki18 24:b3e632cc4533 319 void speedKalibrasiMotor(){
calmantara186 16:90119f03c5d1 320 /* Fungsi untuk speed launcher */
be_bryan 27:68efb1622985 321 if (joystick.R3_click and target_rpm < 14){
be_bryan 27:68efb1622985 322 target_rpm = target_rpm + 1; // RPM++ Motor Belakang
calmantara186 16:90119f03c5d1 323 }
be_bryan 27:68efb1622985 324 if (joystick.L3_click and target_rpm > 1){
be_bryan 27:68efb1622985 325 target_rpm = target_rpm - 1; // RPM-- Motor Belakang
calmantara186 16:90119f03c5d1 326 }
be_bryan 27:68efb1622985 327 if (joystick.R2_click and target_rpm2 < 14){
be_bryan 27:68efb1622985 328 target_rpm2 = target_rpm2 + 1; // RPM++ Motor Depan
calmantara186 16:90119f03c5d1 329 }
be_bryan 27:68efb1622985 330 if (joystick.L2_click and target_rpm2 > 1 ){
be_bryan 27:68efb1622985 331 target_rpm2 = target_rpm2 - 1; // RPM-- Motor Depan
rahmadirizki18 24:b3e632cc4533 332 }
be_bryan 27:68efb1622985 333 // pc.printf("Rpm depan = %.4f\t Rpm belakang = %.4f\n", target_rpm, target_rpm2);
rahmadirizki18 7:d138c56dab20 334 }
be_bryan 26:256160a1a82d 335
be_bryan 26:256160a1a82d 336 void init_speed (){
be_bryan 26:256160a1a82d 337 if (speed>maxSpeed){
be_bryan 26:256160a1a82d 338 speed = maxSpeed;
be_bryan 26:256160a1a82d 339 }
be_bryan 26:256160a1a82d 340
be_bryan 26:256160a1a82d 341 if (speed<minSpeed){
be_bryan 26:256160a1a82d 342 speed = minSpeed;
be_bryan 26:256160a1a82d 343 }
be_bryan 27:68efb1622985 344 if (speed2>maxSpeed){
be_bryan 27:68efb1622985 345 speed2 = maxSpeed;
be_bryan 27:68efb1622985 346 }
be_bryan 27:68efb1622985 347
be_bryan 27:68efb1622985 348 if (speed2<minSpeed){
be_bryan 27:68efb1622985 349 speed2 = minSpeed;
be_bryan 27:68efb1622985 350 }
be_bryan 26:256160a1a82d 351 }
MarchioKevin 22:4632f58461e0 352 /*********************************************************/
MarchioKevin 22:4632f58461e0 353 /* Main Function */
MarchioKevin 22:4632f58461e0 354 /*********************************************************/
calmantara186 16:90119f03c5d1 355
calmantara186 16:90119f03c5d1 356 int main (void){
calmantara186 16:90119f03c5d1 357 /* Set baud rate - 115200 */
fanny868 0:9072e932503c 358 joystick.setup();
rahmadirizki18 23:023b522977b2 359 pc.baud(115200);
rahmadirizki18 6:68293bed71ea 360 wait_ms(1000);
rahmadirizki18 5:3aa203218306 361 setCenter();
rahmadirizki18 5:3aa203218306 362 wait_ms(500);
rahmadirizki18 23:023b522977b2 363 pc.printf("ready....");
calmantara186 16:90119f03c5d1 364
calmantara186 16:90119f03c5d1 365 /* Untuk mendapatkan serial dari Arduino */
fanny868 0:9072e932503c 366 while(1)
fanny868 0:9072e932503c 367 {
fanny868 0:9072e932503c 368 // Interrupt Serial
calmantara186 16:90119f03c5d1 369 joystick.idle();
Joshua23 25:054d3048dd03 370 //pc.printf("enco : %d \n",encoderKiri.getPulses());
be_bryan 26:256160a1a82d 371 if(joystick.readable()) {
fanny868 0:9072e932503c 372 // Panggil fungsi pembacaan joystik
fanny868 0:9072e932503c 373 joystick.baca_data();
fanny868 0:9072e932503c 374 // Panggil fungsi pengolahan data joystik
fanny868 0:9072e932503c 375 joystick.olah_data();
calmantara186 16:90119f03c5d1 376 // Masuk ke case gerak
fanny868 0:9072e932503c 377 case_ger = case_gerak();
rahmadirizki18 3:1287fccc11be 378 aktuator();
be_bryan 26:256160a1a82d 379 if (joystick.segitiga_click){
be_bryan 26:256160a1a82d 380 launcher = !launcher;
be_bryan 26:256160a1a82d 381 }
be_bryan 26:256160a1a82d 382 if (joystick.lingkaran_click){
Sufa 29:7b372b0aaa61 383 pneumatik = 0;
Sufa 30:d69cc27ac644 384 henti = true;
Sufa 30:d69cc27ac644 385 wait_ms(700);
be_bryan 26:256160a1a82d 386 pneumatik = 1;
be_bryan 26:256160a1a82d 387 }
be_bryan 27:68efb1622985 388 speedKalibrasiMotor();
be_bryan 28:2d0746dc2d7d 389 if (joystick.silang_click){
be_bryan 28:2d0746dc2d7d 390 reload = !reload;
be_bryan 28:2d0746dc2d7d 391 }
be_bryan 26:256160a1a82d 392 }
calmantara186 16:90119f03c5d1 393 else {
calmantara186 16:90119f03c5d1 394 joystick.idle();
be_bryan 26:256160a1a82d 395 motor1.brake(1);
be_bryan 26:256160a1a82d 396 motor2.brake(1);
MarchioKevin 21:da2f3d04468f 397 }
fanny868 0:9072e932503c 398 }
be_bryan 28:2d0746dc2d7d 399 }