KRAI 2017 / Mbed 2 deprecated MainProgram_BaseBaru_fix_omni_20April

paling baru

Dependencies:   DigitDisplay Motor PID Ping mbed millis

Fork of MainProgram_BaseBaru_fix_omni_11April by KRAI 2017

main.cpp@29:7b372b0aaa61, 2017-02-12 (annotated)

Committer:
Sufa
Date:
Sun Feb 12 08:07:04 2017 +0000
Revision:
29:7b372b0aaa61
Parent:
28:2d0746dc2d7d
Child:
30:d69cc27ac644
terbaru, sudah assembly robot.

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;
Sufa 29:7b372b0aaa61 73 float pwmT = -0.95;
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 */
Sufa 29:7b372b0aaa61 80 encoderKRAI encoderAtas( PC_10, PC_11, 28, encoderKRAI::X2_ENCODING);
Sufa 29:7b372b0aaa61 81 encoderKRAI encoderAtas2( PD_2, PC_12, 28, encoderKRAI::X2_ENCODING);
be_bryan 26:256160a1a82d 82
calmantara186 16:90119f03c5d1 83 /* Deklarasi Motor Base */
Sufa 29:7b372b0aaa61 84 Motor motor1(PB_9, PA_12, PC_5); // 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 */
Sufa 29:7b372b0aaa61 88 Motor motorL1(PA_8,PC_2,PC_1); // pwm ,fwd, rev, Motor Launcher1
Sufa 29:7b372b0aaa61 89 Motor motorL3(PA_10, PC_3, PC_0); // pwm, fwd, rev, Motor Launcher2
Sufa 29:7b372b0aaa61 90 Motor motorP(PB_7, PA_14, PA_15); // pwm, fwd, rev, Motor Powerscrew
rahmadirizki18 5:3aa203218306 91
be_bryan 26:256160a1a82d 92 /* Deklarasi Penumatik Launcher */
Sufa 29:7b372b0aaa61 93 DigitalOut pneumatik(PB_3, PullUp);
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);
Sufa 29:7b372b0aaa61 182 wait_ms(1000);
Joshua23 8:0711dea61312 183 }
be_bryan 27:68efb1622985 184 else
be_bryan 27:68efb1622985 185 {
be_bryan 27:68efb1622985 186 motorP.speed(pwmP);
be_bryan 27:68efb1622985 187 if (!limitT)
be_bryan 27:68efb1622985 188 {
be_bryan 27:68efb1622985 189 while (limitB)
be_bryan 27:68efb1622985 190 {
be_bryan 27:68efb1622985 191 motorP.speed(pwmT);
be_bryan 27:68efb1622985 192 }
be_bryan 27:68efb1622985 193 motorP.brake(1);
Sufa 29:7b372b0aaa61 194 reload = !reload;
be_bryan 27:68efb1622985 195 }
be_bryan 27:68efb1622985 196 }
calmantara186 16:90119f03c5d1 197 }
be_bryan 28:2d0746dc2d7d 198
be_bryan 27:68efb1622985 199 /*Motor Atas*/
calmantara186 16:90119f03c5d1 200 if (launcher) {
be_bryan 26:256160a1a82d 201 startMillis();
be_bryan 28:2d0746dc2d7d 202 currentMillis = millis();
be_bryan 28:2d0746dc2d7d 203 currentMillis2 = millis();
be_bryan 28:2d0746dc2d7d 204
be_bryan 28:2d0746dc2d7d 205 /*PID 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 }
Sufa 29:7b372b0aaa61 247 else
Sufa 29:7b372b0aaa61 248 {
Sufa 29:7b372b0aaa61 249 motorL1.brake(1);
Sufa 29:7b372b0aaa61 250 motorL3.brake(1);
Sufa 29:7b372b0aaa61 251 }
Sufa 29:7b372b0aaa61 252
be_bryan 28:2d0746dc2d7d 253
rahmadirizki18 6:68293bed71ea 254 // MOTOR Bawah
MarchioKevin 22:4632f58461e0 255 switch (case_ger) {
rahmadirizki18 17:e4229d77a5ab 256 case (1): {
rahmadirizki18 17:e4229d77a5ab 257 // Pivot Kanan
Joshua23 25:054d3048dd03 258 motor1.speed(PIVOT);
Joshua23 25:054d3048dd03 259 motor2.speed(PIVOT);
Joshua23 25:054d3048dd03 260 setCenter();
fanny868 0:9072e932503c 261 break;
rahmadirizki18 17:e4229d77a5ab 262 }
rahmadirizki18 17:e4229d77a5ab 263 case (2): {
rahmadirizki18 17:e4229d77a5ab 264 // Pivot Kiri
Joshua23 25:054d3048dd03 265 motor1.speed(-PIVOT);
Joshua23 25:054d3048dd03 266 motor2.speed(-PIVOT);
Joshua23 25:054d3048dd03 267 setCenter();
fanny868 0:9072e932503c 268 break;
MarchioKevin 22:4632f58461e0 269 }
MarchioKevin 22:4632f58461e0 270 case (3) : {
MarchioKevin 22:4632f58461e0 271 // Kanan
Joshua23 25:054d3048dd03 272 //motor1.speed(-VMAX-vpid);
Joshua23 25:054d3048dd03 273 //motor2.speed(0.2+vpid);
be_bryan 26:256160a1a82d 274 motor1.speed(-0.365+pidBase(0.09,0,0));
be_bryan 26:256160a1a82d 275 motor2.speed(0.46+pidBase(0.09,0,0));
rahmadirizki18 17:e4229d77a5ab 276 break;
MarchioKevin 22:4632f58461e0 277 }
MarchioKevin 22:4632f58461e0 278 case (4) : {
rahmadirizki18 17:e4229d77a5ab 279 // Kiri
Joshua23 25:054d3048dd03 280 //motor1.speed(VMAX-vpid);
Joshua23 25:054d3048dd03 281 //motor2.speed(-0.2+vpid);
be_bryan 26:256160a1a82d 282 motor1.speed(0.365+pidBase(0.09,0,0));//belakang
be_bryan 26:256160a1a82d 283 motor2.speed(-0.46+pidBase(0.09,0,0));//depan
fanny868 0:9072e932503c 284 break;
rahmadirizki18 17:e4229d77a5ab 285 }
rahmadirizki18 17:e4229d77a5ab 286 default : {
MarchioKevin 22:4632f58461e0 287 motor1.brake(1);
MarchioKevin 22:4632f58461e0 288 motor2.brake(1);
rahmadirizki18 17:e4229d77a5ab 289 }
MarchioKevin 22:4632f58461e0 290 } // End Switch
rahmadirizki18 5:3aa203218306 291 }
rahmadirizki18 5:3aa203218306 292
calmantara186 16:90119f03c5d1 293 void setCenter(){
calmantara186 16:90119f03c5d1 294 /* Fungsi untuk menentukan center dari robot */
rahmadirizki18 5:3aa203218306 295 encoderKiri.reset();
rahmadirizki18 5:3aa203218306 296 }
rahmadirizki18 5:3aa203218306 297
calmantara186 16:90119f03c5d1 298 float getTetha(){
calmantara186 16:90119f03c5d1 299 /* Fungsi untuk mendapatkan nilai tetha */
Joshua23 25:054d3048dd03 300 float busurKir, tetha;
MarchioKevin 22:4632f58461e0 301 busurKir = ((encoderKiri.getPulses())/(float)(2000.0)*k_enc);
Joshua23 25:054d3048dd03 302 tetha = busurKir/k_robot*360;
calmantara186 16:90119f03c5d1 303
Joshua23 25:054d3048dd03 304 return -(tetha);
calmantara186 16:90119f03c5d1 305 }
rahmadirizki18 5:3aa203218306 306
rahmadirizki18 24:b3e632cc4533 307 void speedKalibrasiMotor(){
calmantara186 16:90119f03c5d1 308 /* Fungsi untuk speed launcher */
be_bryan 27:68efb1622985 309 if (joystick.R3_click and target_rpm < 14){
be_bryan 27:68efb1622985 310 target_rpm = target_rpm + 1; // RPM++ Motor Belakang
calmantara186 16:90119f03c5d1 311 }
be_bryan 27:68efb1622985 312 if (joystick.L3_click and target_rpm > 1){
be_bryan 27:68efb1622985 313 target_rpm = target_rpm - 1; // RPM-- Motor Belakang
calmantara186 16:90119f03c5d1 314 }
be_bryan 27:68efb1622985 315 if (joystick.R2_click and target_rpm2 < 14){
be_bryan 27:68efb1622985 316 target_rpm2 = target_rpm2 + 1; // RPM++ Motor Depan
calmantara186 16:90119f03c5d1 317 }
be_bryan 27:68efb1622985 318 if (joystick.L2_click and target_rpm2 > 1 ){
be_bryan 27:68efb1622985 319 target_rpm2 = target_rpm2 - 1; // RPM-- Motor Depan
rahmadirizki18 24:b3e632cc4533 320 }
be_bryan 27:68efb1622985 321 // pc.printf("Rpm depan = %.4f\t Rpm belakang = %.4f\n", target_rpm, target_rpm2);
rahmadirizki18 7:d138c56dab20 322 }
be_bryan 26:256160a1a82d 323
be_bryan 26:256160a1a82d 324 void init_speed (){
be_bryan 26:256160a1a82d 325 if (speed>maxSpeed){
be_bryan 26:256160a1a82d 326 speed = maxSpeed;
be_bryan 26:256160a1a82d 327 }
be_bryan 26:256160a1a82d 328
be_bryan 26:256160a1a82d 329 if (speed<minSpeed){
be_bryan 26:256160a1a82d 330 speed = minSpeed;
be_bryan 26:256160a1a82d 331 }
be_bryan 27:68efb1622985 332 if (speed2>maxSpeed){
be_bryan 27:68efb1622985 333 speed2 = maxSpeed;
be_bryan 27:68efb1622985 334 }
be_bryan 27:68efb1622985 335
be_bryan 27:68efb1622985 336 if (speed2<minSpeed){
be_bryan 27:68efb1622985 337 speed2 = minSpeed;
be_bryan 27:68efb1622985 338 }
be_bryan 26:256160a1a82d 339 }
MarchioKevin 22:4632f58461e0 340 /*********************************************************/
MarchioKevin 22:4632f58461e0 341 /* Main Function */
MarchioKevin 22:4632f58461e0 342 /*********************************************************/
calmantara186 16:90119f03c5d1 343
calmantara186 16:90119f03c5d1 344 int main (void){
calmantara186 16:90119f03c5d1 345 /* Set baud rate - 115200 */
fanny868 0:9072e932503c 346 joystick.setup();
rahmadirizki18 23:023b522977b2 347 pc.baud(115200);
rahmadirizki18 6:68293bed71ea 348 wait_ms(1000);
rahmadirizki18 5:3aa203218306 349 setCenter();
rahmadirizki18 5:3aa203218306 350 wait_ms(500);
rahmadirizki18 23:023b522977b2 351 pc.printf("ready....");
calmantara186 16:90119f03c5d1 352
calmantara186 16:90119f03c5d1 353 /* Untuk mendapatkan serial dari Arduino */
fanny868 0:9072e932503c 354 while(1)
fanny868 0:9072e932503c 355 {
fanny868 0:9072e932503c 356 // Interrupt Serial
calmantara186 16:90119f03c5d1 357 joystick.idle();
Joshua23 25:054d3048dd03 358 //pc.printf("enco : %d \n",encoderKiri.getPulses());
be_bryan 26:256160a1a82d 359 if(joystick.readable()) {
fanny868 0:9072e932503c 360 // Panggil fungsi pembacaan joystik
fanny868 0:9072e932503c 361 joystick.baca_data();
fanny868 0:9072e932503c 362 // Panggil fungsi pengolahan data joystik
fanny868 0:9072e932503c 363 joystick.olah_data();
calmantara186 16:90119f03c5d1 364 // Masuk ke case gerak
fanny868 0:9072e932503c 365 case_ger = case_gerak();
rahmadirizki18 3:1287fccc11be 366 aktuator();
be_bryan 26:256160a1a82d 367 if (joystick.segitiga_click){
be_bryan 26:256160a1a82d 368 launcher = !launcher;
be_bryan 26:256160a1a82d 369 }
be_bryan 26:256160a1a82d 370 if (joystick.lingkaran_click){
Sufa 29:7b372b0aaa61 371 pneumatik = 0;
Sufa 29:7b372b0aaa61 372 wait_ms(1000);
be_bryan 26:256160a1a82d 373 pneumatik = 1;
be_bryan 26:256160a1a82d 374 }
be_bryan 27:68efb1622985 375 speedKalibrasiMotor();
be_bryan 28:2d0746dc2d7d 376 if (joystick.silang_click){
be_bryan 28:2d0746dc2d7d 377 reload = !reload;
be_bryan 28:2d0746dc2d7d 378 }
be_bryan 26:256160a1a82d 379 }
calmantara186 16:90119f03c5d1 380 else {
calmantara186 16:90119f03c5d1 381 joystick.idle();
be_bryan 26:256160a1a82d 382 motor1.brake(1);
be_bryan 26:256160a1a82d 383 motor2.brake(1);
MarchioKevin 21:da2f3d04468f 384 }
fanny868 0:9072e932503c 385 }
be_bryan 28:2d0746dc2d7d 386 }