KRAI 2017 / Mbed 2 deprecated MainProgram_BaseBaru_17feb

Update 18 Februari 2017, PID laucnher dan Base sudah diperbarui

Dependencies:   Motor PID Joystick_OrdoV5 mbed millis

Fork of MainProgram_BaseBaru_otomatis-reloader by KRAI 2017

main.cpp@27:68efb1622985, 2017-02-10 (annotated)

Committer:
be_bryan
Date:
Fri Feb 10 17:59:57 2017 +0000
Revision:
27:68efb1622985
Parent:
26:256160a1a82d
Child:
28:2d0746dc2d7d
Rancha Main Terbaru

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