KRAI 2017 / Mbed 2 deprecated MainProgram_BaseBaru_fix_omni_25Mar

di BSCA

Dependencies:   DigitDisplay Motor PID Ping mbed millis

Fork of MainProgram_BaseBaru_fix_omni12Feb by KRAI 2017

main.cpp@28:2d0746dc2d7d, 2017-02-10 (annotated)

Committer:
be_bryan
Date:
Fri Feb 10 19:18:55 2017 +0000
Revision:
28:2d0746dc2d7d
Parent:
27:68efb1622985
Child:
29:7b372b0aaa61
Update 11 Februari 2017 untuk Base Baru

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 28:2d0746dc2d7d 80 encoderKRAI encoderAtas( PB_13, PB_14, 28, encoderKRAI::X2_ENCODING);
be_bryan 28:2d0746dc2d7d 81 encoderKRAI encoderAtas2( PB_15, PC_4, 28, 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
calmantara186 16:90119f03c5d1 100 /**
calmantara186 16:90119f03c5d1 101 * posX dan posY berdasarkan arah robot
calmantara186 16:90119f03c5d1 102 * encoder Depan & Belakang sejajar sumbu Y
be_bryan 27:68efb1622985 103 * encoder Kanan & Kirim sejajar sumbu X
calmantara186 16:90119f03c5d1 104 **/
rahmadirizki18 5:3aa203218306 105
calmantara186 16:90119f03c5d1 106 /* Variabel Encoder */
MarchioKevin 22:4632f58461e0 107 float errT, Tetha; // Variabel yang didapatkan encoder
rahmadirizki18 5:3aa203218306 108
calmantara186 16:90119f03c5d1 109 /* Fungsi dan Procedur Encoder */
calmantara186 16:90119f03c5d1 110 void setCenter(); // Fungsi reset agar robot di tengah
MarchioKevin 20:54dc93e7b016 111 float getTetha(); // Fungsi mendapatkan jarak Tetha
calmantara186 16:90119f03c5d1 112
calmantara186 16:90119f03c5d1 113 /* Inisialisasi Pin TX-RX Joystik dan PC */
Joshua23 25:054d3048dd03 114 joysticknucleo joystick(PA_0,PA_1);
rahmadirizki18 23:023b522977b2 115 Serial pc(USBTX,USBRX);
fanny868 0:9072e932503c 116
be_bryan 26:256160a1a82d 117 /* Deklarasi Variable Milis */
be_bryan 26:256160a1a82d 118 unsigned long int previousMillis = 0;
be_bryan 26:256160a1a82d 119 unsigned long int currentMillis;
be_bryan 27:68efb1622985 120 unsigned long int previousMillis2 = 0;
be_bryan 27:68efb1622985 121 unsigned long int currentMillis2;
be_bryan 26:256160a1a82d 122
calmantara186 16:90119f03c5d1 123 /* Variabel Stick */
fanny868 0:9072e932503c 124 char case_ger;
be_bryan 26:256160a1a82d 125 bool launcher = false;
be_bryan 27:68efb1622985 126 bool reload = false;
fanny868 0:9072e932503c 127
MarchioKevin 22:4632f58461e0 128 /****************************************************/
MarchioKevin 22:4632f58461e0 129 /* Deklarasi Fungsi dan Procedure */
MarchioKevin 22:4632f58461e0 130 /****************************************************/
be_bryan 26:256160a1a82d 131 void init_speed();
be_bryan 26:256160a1a82d 132 void speedKalibrasiMotor();
be_bryan 26:256160a1a82d 133 float pidBase(float Kp, float Ki, float Kd)
Joshua23 25:054d3048dd03 134 {
Joshua23 25:054d3048dd03 135 int errorP;
Joshua23 25:054d3048dd03 136 errorP = getTetha();
Joshua23 25:054d3048dd03 137 return (float)Kp*errorP;
Joshua23 25:054d3048dd03 138 }
Joshua23 25:054d3048dd03 139
calmantara186 16:90119f03c5d1 140 int case_gerak(){
rahmadirizki18 23:023b522977b2 141 /****************************************************
calmantara186 16:90119f03c5d1 142 ** Gerak Motor Base
calmantara186 16:90119f03c5d1 143 ** Case 1 : Pivot kanan
calmantara186 16:90119f03c5d1 144 ** Case 2 : Pivot Kiri
MarchioKevin 22:4632f58461e0 145 ** Case 3 : Kanan
MarchioKevin 22:4632f58461e0 146 ** Case 4 : Kiri
MarchioKevin 22:4632f58461e0 147 ** Case 5 : Break
calmantara186 16:90119f03c5d1 148 ****************************************************/
MarchioKevin 22:4632f58461e0 149
fanny868 0:9072e932503c 150 int casegerak;
calmantara186 16:90119f03c5d1 151 if (!joystick.L1 && joystick.R1) {
fanny868 0:9072e932503c 152 // Pivot Kanan
fanny868 0:9072e932503c 153 casegerak = 1;
calmantara186 16:90119f03c5d1 154 }
calmantara186 16:90119f03c5d1 155 else if (!joystick.R1 && joystick.L1) {
fanny868 0:9072e932503c 156 // Pivot Kiri
fanny868 0:9072e932503c 157 casegerak = 2;
calmantara186 16:90119f03c5d1 158 }
calmantara186 16:90119f03c5d1 159 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {
fanny868 0:9072e932503c 160 // Kanan
MarchioKevin 22:4632f58461e0 161 casegerak = 3;
be_bryan 27:68efb1622985 162 // pc.printf("kanan");
calmantara186 16:90119f03c5d1 163 }
calmantara186 16:90119f03c5d1 164 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {
fanny868 0:9072e932503c 165 // Kiri
rahmadirizki18 23:023b522977b2 166 casegerak = 4;
be_bryan 27:68efb1622985 167 // pc.printf("kiri");
calmantara186 16:90119f03c5d1 168 }
calmantara186 16:90119f03c5d1 169 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
MarchioKevin 22:4632f58461e0 170 // Break
MarchioKevin 22:4632f58461e0 171 casegerak = 5;
calmantara186 16:90119f03c5d1 172 }
fanny868 0:9072e932503c 173 return(casegerak);
fanny868 0:9072e932503c 174 }
fanny868 0:9072e932503c 175
calmantara186 16:90119f03c5d1 176 void aktuator(){
be_bryan 27:68efb1622985 177 /* Fungsi untuk menggerakkan Motor PowerScrew */
be_bryan 27:68efb1622985 178 // PowerScrew
be_bryan 27:68efb1622985 179 if (reload){
be_bryan 27:68efb1622985 180 if (!limitA){
be_bryan 27:68efb1622985 181 motorP.brake (1);
Joshua23 8:0711dea61312 182 }
be_bryan 27:68efb1622985 183 else
be_bryan 27:68efb1622985 184 {
be_bryan 27:68efb1622985 185 motorP.speed(pwmP);
be_bryan 27:68efb1622985 186 if (!limitT)
be_bryan 27:68efb1622985 187 {
be_bryan 27:68efb1622985 188 while (limitB)
be_bryan 27:68efb1622985 189 {
be_bryan 27:68efb1622985 190 motorP.speed(pwmT);
be_bryan 27:68efb1622985 191 }
be_bryan 27:68efb1622985 192 motorP.brake(1);
be_bryan 27:68efb1622985 193 }
be_bryan 27:68efb1622985 194 }
be_bryan 27:68efb1622985 195 reload = !reload;
calmantara186 16:90119f03c5d1 196 }
be_bryan 28:2d0746dc2d7d 197
be_bryan 27:68efb1622985 198 /*Motor Atas*/
calmantara186 16:90119f03c5d1 199 if (launcher) {
be_bryan 26:256160a1a82d 200 startMillis();
be_bryan 28:2d0746dc2d7d 201 currentMillis = millis();
be_bryan 28:2d0746dc2d7d 202 currentMillis2 = millis();
be_bryan 28:2d0746dc2d7d 203
be_bryan 28:2d0746dc2d7d 204 /*PID Launcher Depan*/
be_bryan 26:256160a1a82d 205 if (currentMillis-previousMillis>=10)
be_bryan 26:256160a1a82d 206 {
be_bryan 27:68efb1622985 207 rpm = (float)encoderAtas.getPulses();
be_bryan 26:256160a1a82d 208 current_error = target_rpm - rpm;
be_bryan 26:256160a1a82d 209 sum_error = sum_error + current_error;
be_bryan 26:256160a1a82d 210 p = current_error*kpA;
be_bryan 26:256160a1a82d 211 d = (current_error-last_error)*kdA/10.0;
be_bryan 26:256160a1a82d 212 i = sum_error*kiA*10.0;
be_bryan 26:256160a1a82d 213 speed = p + d + i;
be_bryan 26:256160a1a82d 214 init_speed();
be_bryan 27:68efb1622985 215 motorL1.speed(speed);
be_bryan 26:256160a1a82d 216 last_error = current_error;
be_bryan 26:256160a1a82d 217 encoderAtas.reset();
be_bryan 26:256160a1a82d 218 //pc.printf("%.04lf\n",rpm);
be_bryan 26:256160a1a82d 219 previousMillis = currentMillis;
be_bryan 26:256160a1a82d 220 }
be_bryan 26:256160a1a82d 221 else
be_bryan 26:256160a1a82d 222 {
be_bryan 27:68efb1622985 223 encoderAtas.getPulses();
be_bryan 27:68efb1622985 224 }
be_bryan 27:68efb1622985 225 if (currentMillis2-previousMillis2>=10)
be_bryan 27:68efb1622985 226 {
be_bryan 27:68efb1622985 227 rpm2 = (float)encoderAtas2.getPulses();
be_bryan 27:68efb1622985 228 current_error2 = target_rpm2 - rpm2;
be_bryan 27:68efb1622985 229 sum_error2 = sum_error2 + current_error2;
be_bryan 27:68efb1622985 230 p2 = current_error2*kpA;
be_bryan 27:68efb1622985 231 d2 = (current_error2-last_error2)*kdA/10.0;
be_bryan 27:68efb1622985 232 i2 = sum_error2*kiA*10.0;
be_bryan 27:68efb1622985 233 speed2 = p2 + d2 + i2;
be_bryan 27:68efb1622985 234 init_speed();
be_bryan 27:68efb1622985 235 motorL3.speed(speed2);
be_bryan 27:68efb1622985 236 last_error2 = current_error2;
be_bryan 27:68efb1622985 237 encoderAtas2.reset();
be_bryan 27:68efb1622985 238 //pc.printf("%.04lf\n",rpm2);
be_bryan 27:68efb1622985 239 previousMillis2 = currentMillis2;
be_bryan 27:68efb1622985 240 }
be_bryan 27:68efb1622985 241 else
be_bryan 27:68efb1622985 242 {
be_bryan 27:68efb1622985 243 encoderAtas2.getPulses();
be_bryan 26:256160a1a82d 244 }
rahmadirizki18 6:68293bed71ea 245 }
be_bryan 28:2d0746dc2d7d 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 28:2d0746dc2d7d 369 if (joystick.silang_click){
be_bryan 28:2d0746dc2d7d 370 reload = !reload;
be_bryan 28:2d0746dc2d7d 371 }
be_bryan 26:256160a1a82d 372 }
calmantara186 16:90119f03c5d1 373 else {
calmantara186 16:90119f03c5d1 374 joystick.idle();
be_bryan 26:256160a1a82d 375 motor1.brake(1);
be_bryan 26:256160a1a82d 376 motor2.brake(1);
MarchioKevin 21:da2f3d04468f 377 }
fanny868 0:9072e932503c 378 }
be_bryan 28:2d0746dc2d7d 379 }