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@36:5963c9a49485, 2017-02-14 (annotated)

Committer:
rizqicahyo
Date:
Tue Feb 14 16:20:35 2017 +0000
Revision:
36:5963c9a49485
Parent:
35:69a47b4cb3fc
Child:
37:67d54563af90
update otomatis reloader

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 /* */
gustavaditya 33:69d266bc3fe9 4 /* Last Update : 14 Februari 2017 */
gustavaditya 31:d5cbda07fd95 5 /* */
rahmadirizki18 5:3aa203218306 6 /* - Digunakan encoder autonics */
rahmadirizki18 5:3aa203218306 7 /* - Konfigurasi Motor dan Encoder sbb : */
MarchioKevin 22:4632f58461e0 8 /* ______________________ */
MarchioKevin 22:4632f58461e0 9 /* / \ Rode Depan Belakang: */
MarchioKevin 22:4632f58461e0 10 /* / 2 (Belakang) \ Omniwheel */
MarchioKevin 22:4632f58461e0 11 /* | | */
MarchioKevin 22:4632f58461e0 12 /* | 3 (Encoder) 4 | Roda Kiri Kanan: */
MarchioKevin 22:4632f58461e0 13 /* | | Fixed Wheel */
MarchioKevin 22:4632f58461e0 14 /* \ 1 (Depan) / */
MarchioKevin 22:4632f58461e0 15 /* \______________________/ Putaran berlawanan arah */
MarchioKevin 22:4632f58461e0 16 /* jarum jam positif */
rahmadirizki18 5:3aa203218306 17 /* SETTINGS (WAJIB!) : */
rahmadirizki18 5:3aa203218306 18 /* 1. Settings Pin Encoder, Resolusi, dan Tipe encoding di omniPos.h */
rahmadirizki18 5:3aa203218306 19 /* 2. Deklarasi penggunaan library omniPos pada bagian deklarasi encoder */
rahmadirizki18 5:3aa203218306 20 /* */
rahmadirizki18 5:3aa203218306 21 /****************************************************************************/
rahmadirizki18 6:68293bed71ea 22 /* */
MarchioKevin 20:54dc93e7b016 23 /* Joystick */
Sufa 30:d69cc27ac644 24 /* Kanan => */
Sufa 30:d69cc27ac644 25 /* Kiri => */
rahmadirizki18 6:68293bed71ea 26 /* */
Sufa 30:d69cc27ac644 27 /* Tombol silang => Power Screw Aktif */
Sufa 30:d69cc27ac644 28 /* Tombol segitiga => Aktif motor Launcher */
Sufa 30:d69cc27ac644 29 /* Tombol lingkaran => Aktif Pneumatik Launcher */
Sufa 30:d69cc27ac644 30 /* Tombol L1 => Pivot Kiri */
Sufa 30:d69cc27ac644 31 /* Tombol R1 => Pivot Kanan */
Sufa 30:d69cc27ac644 32 /* Tombol L3 => PWM Motor Belakang Dikurangi */
Sufa 30:d69cc27ac644 33 /* Tombol R3 => PWM Motor Belakang Ditambah */
Sufa 30:d69cc27ac644 34 /* Tombol L2 => PWM Motor Depan Dikurangi */
Sufa 30:d69cc27ac644 35 /* Tombol R2 => PWM Motor Depan Ditambah */
rahmadirizki18 13:8ab42383a2ca 36 /* */
calmantara186 16:90119f03c5d1 37 /* Bismillahirahmanirrahim */
MarchioKevin 20:54dc93e7b016 38 /* Jagalah Kebersihan Kodingan */
rahmadirizki18 6:68293bed71ea 39 /****************************************************************************/
rahmadirizki18 6:68293bed71ea 40
fanny868 0:9072e932503c 41 #include "mbed.h"
fanny868 0:9072e932503c 42 #include "JoystickPS3.h"
fanny868 0:9072e932503c 43 #include "Motor.h"
rahmadirizki18 6:68293bed71ea 44 #include "Servo.h"
rahmadirizki18 5:3aa203218306 45 #include "encoderKRAI.h"
be_bryan 26:256160a1a82d 46 #include "millis.h"
rahmadirizki18 5:3aa203218306 47
calmantara186 16:90119f03c5d1 48 /***********************************************/
calmantara186 16:90119f03c5d1 49 /* Konstanta dan Variabel */
calmantara186 16:90119f03c5d1 50 /***********************************************/
calmantara186 16:90119f03c5d1 51 #define PI 3.14159265
be_bryan 26:256160a1a82d 52 #define D_ENCODER 10 // Diameter Roda Encoder
be_bryan 26:256160a1a82d 53 #define D_ROBOT 80 // Diameter Roda Robot
rahmadirizki18 17:e4229d77a5ab 54 #define PERPINDAHAN 1 // Perpindahan ke kanan dan kiri
rahmadirizki18 5:3aa203218306 55
be_bryan 26:256160a1a82d 56 // Variable Atas
gustavaditya 32:581d4a2373f0 57 double speed, speed2, maxSpeed = 0.95, 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;
gustavaditya 31:d5cbda07fd95 63 float rpm, target_rpm = 8.0;
gustavaditya 34:1cfd9b1c7d27 64 float rpm2, target_rpm2 = 10.0;
rizqicahyo 36:5963c9a49485 65 float pwmPowerUp = 0.75;
gustavaditya 33:69d266bc3fe9 66 float pwmPowerDown = -0.80;
Sufa 30:d69cc27ac644 67 bool henti=false, hentis=false;
be_bryan 26:256160a1a82d 68
be_bryan 26:256160a1a82d 69 // Variable Bawah
Joshua23 25:054d3048dd03 70 float Vt;
gustavaditya 31:d5cbda07fd95 71 float keliling_enc = PI*D_ENCODER;
gustavaditya 31:d5cbda07fd95 72 float keliling_robot = PI*D_ROBOT;
gustavaditya 31:d5cbda07fd95 73 float speedT = 0.2;
gustavaditya 31:d5cbda07fd95 74 float vpid = 0;
gustavaditya 33:69d266bc3fe9 75 float PIVOT = 0.27; // PWM Pivot Kanan, Pivot Kiri
gustavaditya 33:69d266bc3fe9 76 float tuneDpn = 0.35; // Tunning PWM motor Depan
gustavaditya 33:69d266bc3fe9 77 float tuneBlk = 0.3; // Tunning PWM motor belakang
gustavaditya 31:d5cbda07fd95 78
gustavaditya 31:d5cbda07fd95 79 /* Variabel Encoder Bawah */
gustavaditya 31:d5cbda07fd95 80 float errTetha, Tetha; // Variabel yang didapatkan encoder
gustavaditya 31:d5cbda07fd95 81
gustavaditya 31:d5cbda07fd95 82 /* Deklarasi Variable Millis */
gustavaditya 33:69d266bc3fe9 83 unsigned long int previousMillis = 0; // PID launcher
gustavaditya 31:d5cbda07fd95 84 unsigned long int currentMillis;
gustavaditya 33:69d266bc3fe9 85 unsigned long int previousMillis2 = 0; // PID launcher
gustavaditya 31:d5cbda07fd95 86 unsigned long int currentMillis2;
gustavaditya 33:69d266bc3fe9 87 unsigned long int previousMillis3 = 0; // Pneumatik
MarchioKevin 22:4632f58461e0 88
gustavaditya 31:d5cbda07fd95 89 /* Variabel Stick */
gustavaditya 31:d5cbda07fd95 90 //Logic untuk masuk aktuator
gustavaditya 31:d5cbda07fd95 91 int case_joy;
gustavaditya 31:d5cbda07fd95 92 bool isLauncher = false;
gustavaditya 31:d5cbda07fd95 93 bool isReload = false;
rizqicahyo 36:5963c9a49485 94 bool ReloadOn = false;
gustavaditya 31:d5cbda07fd95 95 int flagLauncher = 1;
gustavaditya 31:d5cbda07fd95 96 bool flag_Pneu = false;
gustavaditya 31:d5cbda07fd95 97
gustavaditya 31:d5cbda07fd95 98 /*****************************************************/
gustavaditya 31:d5cbda07fd95 99 /* Definisi Prosedur, Fungsi dan Setting Pinout */
gustavaditya 31:d5cbda07fd95 100 /*****************************************************/
gustavaditya 31:d5cbda07fd95 101
gustavaditya 31:d5cbda07fd95 102 /* Fungsi dan Procedur Encoder */
gustavaditya 31:d5cbda07fd95 103 void init_speed(); //
gustavaditya 31:d5cbda07fd95 104 void aktuator(); // Pergerakan aktuator berdasarkan case joystick
gustavaditya 31:d5cbda07fd95 105 int case_joystick(); // Mendapatkan case dari joystick
gustavaditya 33:69d266bc3fe9 106 //void speedKalibrasiMotor(); // Pertambahan target RPM motor atas melalui joystick
gustavaditya 31:d5cbda07fd95 107 void setCenter(); // Prosedur reset encoder, posisi saat itu diset jadi titik (0,0)
gustavaditya 31:d5cbda07fd95 108 float getTetha(); // Fungsi mendapatkan error Tetha
gustavaditya 31:d5cbda07fd95 109
gustavaditya 31:d5cbda07fd95 110 /* Inisialisasi Pin TX-RX Joystik dan PC */
gustavaditya 31:d5cbda07fd95 111 joysticknucleo joystick(PA_0,PA_1);
gustavaditya 31:d5cbda07fd95 112 Serial pc(USBTX,USBRX);
calmantara186 16:90119f03c5d1 113
be_bryan 26:256160a1a82d 114 /* Deklarasi Encoder Base */
gustavaditya 31:d5cbda07fd95 115 encoderKRAI encoderBase(PC_4, PB_15, 2000, encoderKRAI::X2_ENCODING); //inA, inB, pin Indeks (NC = tak ada), 2xresolusi, mode pembacaan
rahmadirizki18 15:98f0d56b14f0 116
be_bryan 26:256160a1a82d 117 /* Deklarasi Encoder Launcher */
gustavaditya 31:d5cbda07fd95 118 encoderKRAI encLauncherBlk( PC_10, PC_11, 28, encoderKRAI::X2_ENCODING);
gustavaditya 31:d5cbda07fd95 119 encoderKRAI encLauncherDpn( PD_2, PC_12, 28, encoderKRAI::X2_ENCODING);
be_bryan 26:256160a1a82d 120
calmantara186 16:90119f03c5d1 121 /* Deklarasi Motor Base */
gustavaditya 31:d5cbda07fd95 122 Motor motorDpn(PB_9, PA_12, PC_5); // pwm, fwd, rev
gustavaditya 31:d5cbda07fd95 123 Motor motorBlk(PB_6, PB_1, PB_12); // pwm, fwd, rev
fanny868 0:9072e932503c 124
calmantara186 16:90119f03c5d1 125 /* Deklarasi Motor Launcher */
gustavaditya 31:d5cbda07fd95 126 Motor launcherDpn(PA_8,PC_2,PC_1); // pwm ,fwd, rev
gustavaditya 31:d5cbda07fd95 127 Motor launcherBlk(PA_10, PC_3, PC_0); // pwm, fwd, rev
gustavaditya 31:d5cbda07fd95 128 Motor powerScrew(PB_7, PA_14, PA_15); // pwm, fwd, rev
rahmadirizki18 5:3aa203218306 129
be_bryan 26:256160a1a82d 130 /* Deklarasi Penumatik Launcher */
Sufa 29:7b372b0aaa61 131 DigitalOut pneumatik(PB_3, PullUp);
be_bryan 26:256160a1a82d 132
be_bryan 27:68efb1622985 133 /*Dekalrasi Limit Switch */
gustavaditya 34:1cfd9b1c7d27 134 DigitalIn infraAtas(PC_9, PullUp);
gustavaditya 31:d5cbda07fd95 135 DigitalIn limitTengah(PB_10, PullUp);
gustavaditya 31:d5cbda07fd95 136 DigitalIn limitBawah(PC_8, PullUp);
rahmadirizki18 5:3aa203218306 137
fanny868 0:9072e932503c 138
MarchioKevin 22:4632f58461e0 139 /****************************************************/
MarchioKevin 22:4632f58461e0 140 /* Deklarasi Fungsi dan Procedure */
MarchioKevin 22:4632f58461e0 141 /****************************************************/
gustavaditya 31:d5cbda07fd95 142 int case_joystick()
gustavaditya 31:d5cbda07fd95 143 {
gustavaditya 31:d5cbda07fd95 144 //---------------------------------------------------//
gustavaditya 31:d5cbda07fd95 145 // Gerak Motor Base //
gustavaditya 31:d5cbda07fd95 146 // Case 1 : Pivot kanan //
gustavaditya 31:d5cbda07fd95 147 // Case 2 : Pivot Kiri //
gustavaditya 31:d5cbda07fd95 148 // Case 3 : Kanan //
gustavaditya 31:d5cbda07fd95 149 // Case 4 : Kiri //
gustavaditya 31:d5cbda07fd95 150 // Case 5 : Break //
gustavaditya 31:d5cbda07fd95 151 //---------------------------------------------------//
gustavaditya 31:d5cbda07fd95 152
gustavaditya 31:d5cbda07fd95 153 int caseJoystick;
gustavaditya 31:d5cbda07fd95 154 if (!joystick.L1 && joystick.R1) {
gustavaditya 31:d5cbda07fd95 155 // Pivot Kanan
gustavaditya 31:d5cbda07fd95 156 caseJoystick = 1;
gustavaditya 31:d5cbda07fd95 157 }
gustavaditya 31:d5cbda07fd95 158 else if (!joystick.R1 && joystick.L1) {
gustavaditya 31:d5cbda07fd95 159 // Pivot Kiri
gustavaditya 31:d5cbda07fd95 160 caseJoystick = 2;
gustavaditya 31:d5cbda07fd95 161 }
gustavaditya 31:d5cbda07fd95 162 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {
gustavaditya 31:d5cbda07fd95 163 // Kanan
gustavaditya 31:d5cbda07fd95 164 caseJoystick = 3;
gustavaditya 33:69d266bc3fe9 165 //pc.printf("kanan");
gustavaditya 31:d5cbda07fd95 166 }
gustavaditya 31:d5cbda07fd95 167 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {
gustavaditya 31:d5cbda07fd95 168 // Kiri
gustavaditya 31:d5cbda07fd95 169 caseJoystick = 4;
gustavaditya 33:69d266bc3fe9 170 //pc.printf("kiri");
gustavaditya 31:d5cbda07fd95 171 }
gustavaditya 31:d5cbda07fd95 172 else if (joystick.segitiga_click && flagLauncher){
gustavaditya 31:d5cbda07fd95 173 // Motor Launcher
gustavaditya 31:d5cbda07fd95 174 caseJoystick = 5;
gustavaditya 31:d5cbda07fd95 175 }
gustavaditya 32:581d4a2373f0 176 else if (joystick.R2_click && (target_rpm2 < 20)){
gustavaditya 31:d5cbda07fd95 177 // Target Pulse PID ++ Motor Depan
gustavaditya 31:d5cbda07fd95 178 caseJoystick = 6;
gustavaditya 31:d5cbda07fd95 179 }
gustavaditya 31:d5cbda07fd95 180 else if (joystick.L2_click && (target_rpm2 > 1)){
gustavaditya 31:d5cbda07fd95 181 // Target Pulse PID -- Motor Depan
gustavaditya 31:d5cbda07fd95 182 caseJoystick = 7;
gustavaditya 31:d5cbda07fd95 183 }
gustavaditya 32:581d4a2373f0 184 else if (joystick.R3_click && (target_rpm < 20 )){
gustavaditya 31:d5cbda07fd95 185 // Target Pulse PID ++ Motor Belakang
gustavaditya 31:d5cbda07fd95 186 caseJoystick = 8;
gustavaditya 31:d5cbda07fd95 187 }
gustavaditya 31:d5cbda07fd95 188 else if (joystick.L3_click && (target_rpm > 1)){
gustavaditya 31:d5cbda07fd95 189 // Target Pulse PID -- Motor Belakang
gustavaditya 31:d5cbda07fd95 190 caseJoystick = 9;
gustavaditya 31:d5cbda07fd95 191 }
gustavaditya 31:d5cbda07fd95 192 else if (joystick.silang_click){
gustavaditya 31:d5cbda07fd95 193 // Pnemuatik ON
gustavaditya 31:d5cbda07fd95 194 caseJoystick = 10;
gustavaditya 31:d5cbda07fd95 195 }
gustavaditya 31:d5cbda07fd95 196 else if ((joystick.atas)&&(!joystick.bawah)) {
gustavaditya 31:d5cbda07fd95 197 // Power Screw Up
gustavaditya 31:d5cbda07fd95 198 caseJoystick = 11;
gustavaditya 31:d5cbda07fd95 199 }
gustavaditya 31:d5cbda07fd95 200 else if ((!joystick.atas)&&(joystick.bawah)) {
gustavaditya 31:d5cbda07fd95 201 // Power Screw Down
gustavaditya 31:d5cbda07fd95 202 caseJoystick = 12;
gustavaditya 31:d5cbda07fd95 203 }
gustavaditya 31:d5cbda07fd95 204 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
gustavaditya 31:d5cbda07fd95 205 // Break
gustavaditya 31:d5cbda07fd95 206 caseJoystick = 13;
gustavaditya 31:d5cbda07fd95 207 }
gustavaditya 31:d5cbda07fd95 208
gustavaditya 31:d5cbda07fd95 209 return(caseJoystick);
gustavaditya 31:d5cbda07fd95 210 }
gustavaditya 31:d5cbda07fd95 211
gustavaditya 31:d5cbda07fd95 212 float getTetha(){
gustavaditya 31:d5cbda07fd95 213 // Fungsi untuk mendapatkan nilai tetha
gustavaditya 31:d5cbda07fd95 214 float busur, tetha;
gustavaditya 31:d5cbda07fd95 215 busur = ((encoderBase.getPulses())/(float)(2000.0)*keliling_enc);
gustavaditya 31:d5cbda07fd95 216 tetha = busur/keliling_robot*360;
gustavaditya 31:d5cbda07fd95 217
gustavaditya 31:d5cbda07fd95 218 return -(tetha);
gustavaditya 31:d5cbda07fd95 219 }
gustavaditya 31:d5cbda07fd95 220
be_bryan 26:256160a1a82d 221 float pidBase(float Kp, float Ki, float Kd)
Joshua23 25:054d3048dd03 222 {
Joshua23 25:054d3048dd03 223 int errorP;
Joshua23 25:054d3048dd03 224 errorP = getTetha();
gustavaditya 33:69d266bc3fe9 225 if (errorP<3.5 && errorP>(-3.5))
gustavaditya 33:69d266bc3fe9 226 errorP = 0;
Joshua23 25:054d3048dd03 227 return (float)Kp*errorP;
Joshua23 25:054d3048dd03 228 }
Joshua23 25:054d3048dd03 229
gustavaditya 31:d5cbda07fd95 230 void init_speed (){
gustavaditya 31:d5cbda07fd95 231 if (speed>maxSpeed){
gustavaditya 31:d5cbda07fd95 232 speed = maxSpeed;
gustavaditya 31:d5cbda07fd95 233 }
gustavaditya 31:d5cbda07fd95 234 if (speed<minSpeed){
gustavaditya 31:d5cbda07fd95 235 speed = minSpeed;
calmantara186 16:90119f03c5d1 236 }
gustavaditya 31:d5cbda07fd95 237 if (speed2>maxSpeed){
gustavaditya 31:d5cbda07fd95 238 speed2 = maxSpeed;
gustavaditya 31:d5cbda07fd95 239 }
gustavaditya 31:d5cbda07fd95 240 if (speed2<minSpeed){
gustavaditya 31:d5cbda07fd95 241 speed2 = minSpeed;
gustavaditya 31:d5cbda07fd95 242 }
gustavaditya 31:d5cbda07fd95 243 }
gustavaditya 31:d5cbda07fd95 244
gustavaditya 31:d5cbda07fd95 245 void setCenter(){
gustavaditya 31:d5cbda07fd95 246 // Fungsi untuk menentukan center dari robot
gustavaditya 31:d5cbda07fd95 247 encoderBase.reset();
fanny868 0:9072e932503c 248 }
fanny868 0:9072e932503c 249
gustavaditya 31:d5cbda07fd95 250 void aktuator()
gustavaditya 31:d5cbda07fd95 251 {
gustavaditya 31:d5cbda07fd95 252 switch (case_joy) {
gustavaditya 31:d5cbda07fd95 253 case (1):
gustavaditya 31:d5cbda07fd95 254 {
gustavaditya 31:d5cbda07fd95 255 // Pivot Kanan
gustavaditya 31:d5cbda07fd95 256 motorDpn.speed(-PIVOT);
gustavaditya 31:d5cbda07fd95 257 motorBlk.speed(-PIVOT);
gustavaditya 31:d5cbda07fd95 258 setCenter();
gustavaditya 31:d5cbda07fd95 259 break;
gustavaditya 31:d5cbda07fd95 260 }
gustavaditya 31:d5cbda07fd95 261 case (2):
gustavaditya 31:d5cbda07fd95 262 {
gustavaditya 31:d5cbda07fd95 263 // Pivot Kiri
gustavaditya 31:d5cbda07fd95 264 motorDpn.speed(PIVOT);
gustavaditya 31:d5cbda07fd95 265 motorBlk.speed(PIVOT);
gustavaditya 31:d5cbda07fd95 266 setCenter();
gustavaditya 31:d5cbda07fd95 267 break;
gustavaditya 31:d5cbda07fd95 268 }
gustavaditya 31:d5cbda07fd95 269 case (3) :
gustavaditya 31:d5cbda07fd95 270 {
gustavaditya 31:d5cbda07fd95 271 // Kanan
gustavaditya 33:69d266bc3fe9 272 motorDpn.speed(-tuneDpn + pidBase(0.009,0,0));
gustavaditya 33:69d266bc3fe9 273 motorBlk.speed(tuneBlk + pidBase(0.009,0,0));
gustavaditya 33:69d266bc3fe9 274 //speedDpn = tuneDpn + pidBase(0.009,0,0)
gustavaditya 33:69d266bc3fe9 275 //speedBlk = tuneBlk + pidBase(0.009,0,0)
gustavaditya 33:69d266bc3fe9 276 //motorDpn.speed(-tuneDpn);
gustavaditya 33:69d266bc3fe9 277 //motorBlk.speed(tuneBlk);
gustavaditya 31:d5cbda07fd95 278 break;
gustavaditya 31:d5cbda07fd95 279 }
gustavaditya 31:d5cbda07fd95 280 case (4) :
gustavaditya 31:d5cbda07fd95 281 {
gustavaditya 31:d5cbda07fd95 282 // Kiri
gustavaditya 33:69d266bc3fe9 283 motorDpn.speed(tuneDpn + pidBase(0.009,0,0));
gustavaditya 33:69d266bc3fe9 284 motorBlk.speed(-tuneBlk + pidBase(0.009,0,0));
gustavaditya 33:69d266bc3fe9 285 //motorDpn.speed(tuneDpn);
gustavaditya 33:69d266bc3fe9 286 //motorBlk.speed(-tuneBlk);
gustavaditya 31:d5cbda07fd95 287 break;
gustavaditya 31:d5cbda07fd95 288 }
gustavaditya 31:d5cbda07fd95 289 case (5) :
gustavaditya 31:d5cbda07fd95 290 {
gustavaditya 31:d5cbda07fd95 291 // Aktifkan motor atas
gustavaditya 31:d5cbda07fd95 292 isLauncher = !isLauncher;
gustavaditya 31:d5cbda07fd95 293 break;
Joshua23 8:0711dea61312 294 }
gustavaditya 31:d5cbda07fd95 295 case (6) :
gustavaditya 31:d5cbda07fd95 296 {
gustavaditya 31:d5cbda07fd95 297 // Target Pulse PID ++ Motor Depan
gustavaditya 31:d5cbda07fd95 298 target_rpm2 = target_rpm2++;
gustavaditya 31:d5cbda07fd95 299 break;
gustavaditya 31:d5cbda07fd95 300 }
gustavaditya 31:d5cbda07fd95 301 case (7) :
gustavaditya 31:d5cbda07fd95 302 {
gustavaditya 31:d5cbda07fd95 303 // Target Pulse PID -- Motor Depan
gustavaditya 31:d5cbda07fd95 304 target_rpm2 = target_rpm2--;
gustavaditya 31:d5cbda07fd95 305 break;
gustavaditya 31:d5cbda07fd95 306 }
gustavaditya 31:d5cbda07fd95 307 case (8) :
gustavaditya 31:d5cbda07fd95 308 {
gustavaditya 32:581d4a2373f0 309 // Target Pulse PID ++ Motor Belakang=
gustavaditya 31:d5cbda07fd95 310 target_rpm = target_rpm++;
gustavaditya 31:d5cbda07fd95 311 break;
gustavaditya 31:d5cbda07fd95 312 }
gustavaditya 31:d5cbda07fd95 313 case (9) :
be_bryan 27:68efb1622985 314 {
gustavaditya 31:d5cbda07fd95 315 // Target Pulse PID -- Motor Belakang
gustavaditya 31:d5cbda07fd95 316 target_rpm = target_rpm--;
gustavaditya 31:d5cbda07fd95 317 break;
gustavaditya 31:d5cbda07fd95 318 }
gustavaditya 31:d5cbda07fd95 319 case (10) :
gustavaditya 31:d5cbda07fd95 320 {
gustavaditya 31:d5cbda07fd95 321 // Pneumatik
gustavaditya 31:d5cbda07fd95 322 pneumatik = 0;
gustavaditya 31:d5cbda07fd95 323 previousMillis3 = millis();
gustavaditya 31:d5cbda07fd95 324 flag_Pneu = true;
gustavaditya 31:d5cbda07fd95 325 break;
gustavaditya 31:d5cbda07fd95 326 }
gustavaditya 31:d5cbda07fd95 327 case (11) :
gustavaditya 31:d5cbda07fd95 328 {
gustavaditya 31:d5cbda07fd95 329 // Power Screw Up
rizqicahyo 36:5963c9a49485 330 //powerScrew.speed(pwmPowerUp);
rizqicahyo 36:5963c9a49485 331 ReloadOn = !ReloadOn;
gustavaditya 31:d5cbda07fd95 332 break;
gustavaditya 31:d5cbda07fd95 333 }
gustavaditya 31:d5cbda07fd95 334 case (12) :
gustavaditya 31:d5cbda07fd95 335 {
gustavaditya 31:d5cbda07fd95 336 // Power Screw Down
rizqicahyo 36:5963c9a49485 337 //powerScrew.speed(pwmPowerDown);
gustavaditya 31:d5cbda07fd95 338 break;
gustavaditya 31:d5cbda07fd95 339 }
gustavaditya 31:d5cbda07fd95 340 default :
gustavaditya 31:d5cbda07fd95 341 {
gustavaditya 31:d5cbda07fd95 342 motorDpn.brake(1);
gustavaditya 31:d5cbda07fd95 343 motorBlk.brake(1);
rizqicahyo 36:5963c9a49485 344 // aktuator();
rizqicahyo 36:5963c9a49485 345 /* if (!infraAtas)
be_bryan 27:68efb1622985 346 {
gustavaditya 31:d5cbda07fd95 347 powerScrew.brake(1);
gustavaditya 31:d5cbda07fd95 348 }
gustavaditya 31:d5cbda07fd95 349 if (!limitTengah)
gustavaditya 31:d5cbda07fd95 350 {
gustavaditya 31:d5cbda07fd95 351 case_joy = 12;
gustavaditya 31:d5cbda07fd95 352 aktuator();
gustavaditya 31:d5cbda07fd95 353 }
gustavaditya 31:d5cbda07fd95 354 if (!limitBawah)
Sufa 30:d69cc27ac644 355 {
gustavaditya 31:d5cbda07fd95 356 case_joy = 11;
gustavaditya 31:d5cbda07fd95 357 aktuator();
rizqicahyo 36:5963c9a49485 358 }*/
gustavaditya 31:d5cbda07fd95 359 }
gustavaditya 31:d5cbda07fd95 360 } // End Switch
gustavaditya 31:d5cbda07fd95 361 }
gustavaditya 31:d5cbda07fd95 362
rizqicahyo 36:5963c9a49485 363 void reloader()
rizqicahyo 36:5963c9a49485 364 {
rizqicahyo 36:5963c9a49485 365 if(ReloadOn){
rizqicahyo 36:5963c9a49485 366 if(isReload){
rizqicahyo 36:5963c9a49485 367 powerScrew.speed(pwmPowerDown);
rizqicahyo 36:5963c9a49485 368 if(!limitBawah){
rizqicahyo 36:5963c9a49485 369 isReload = false;
rizqicahyo 36:5963c9a49485 370 ReloadOn = false;
rizqicahyo 36:5963c9a49485 371 }
rizqicahyo 36:5963c9a49485 372 }
rizqicahyo 36:5963c9a49485 373 else if(!limitTengah){
rizqicahyo 36:5963c9a49485 374 isReload = true;
rizqicahyo 36:5963c9a49485 375 }
rizqicahyo 36:5963c9a49485 376 else if(!infraAtas){
rizqicahyo 36:5963c9a49485 377 powerScrew.brake(1);
rizqicahyo 36:5963c9a49485 378 }
rizqicahyo 36:5963c9a49485 379 else{
rizqicahyo 36:5963c9a49485 380 powerScrew.speed(pwmPowerUp);
rizqicahyo 36:5963c9a49485 381 }
rizqicahyo 36:5963c9a49485 382 }
rizqicahyo 36:5963c9a49485 383 else{
rizqicahyo 36:5963c9a49485 384 powerScrew.brake(1);
rizqicahyo 36:5963c9a49485 385 }
rizqicahyo 36:5963c9a49485 386 }
rizqicahyo 36:5963c9a49485 387
rizqicahyo 36:5963c9a49485 388
gustavaditya 31:d5cbda07fd95 389 void launcher()
gustavaditya 31:d5cbda07fd95 390 {
gustavaditya 31:d5cbda07fd95 391 if (isLauncher)
Sufa 30:d69cc27ac644 392 {
be_bryan 28:2d0746dc2d7d 393 currentMillis = millis();
be_bryan 28:2d0746dc2d7d 394 currentMillis2 = millis();
be_bryan 28:2d0746dc2d7d 395
gustavaditya 31:d5cbda07fd95 396 // PID Launcher Depan
be_bryan 26:256160a1a82d 397 if (currentMillis-previousMillis>=10)
be_bryan 26:256160a1a82d 398 {
gustavaditya 31:d5cbda07fd95 399 rpm = (float)encLauncherBlk.getPulses();
be_bryan 26:256160a1a82d 400 current_error = target_rpm - rpm;
be_bryan 26:256160a1a82d 401 sum_error = sum_error + current_error;
be_bryan 26:256160a1a82d 402 p = current_error*kpA;
be_bryan 26:256160a1a82d 403 d = (current_error-last_error)*kdA/10.0;
be_bryan 26:256160a1a82d 404 i = sum_error*kiA*10.0;
be_bryan 26:256160a1a82d 405 speed = p + d + i;
be_bryan 26:256160a1a82d 406 init_speed();
gustavaditya 31:d5cbda07fd95 407 launcherBlk.speed(speed);
be_bryan 26:256160a1a82d 408 last_error = current_error;
gustavaditya 31:d5cbda07fd95 409 encLauncherBlk.reset();
be_bryan 26:256160a1a82d 410 //pc.printf("%.04lf\n",rpm);
be_bryan 26:256160a1a82d 411 previousMillis = currentMillis;
be_bryan 26:256160a1a82d 412 }
be_bryan 27:68efb1622985 413 if (currentMillis2-previousMillis2>=10)
be_bryan 27:68efb1622985 414 {
gustavaditya 31:d5cbda07fd95 415 rpm2 = (float)encLauncherDpn.getPulses();
be_bryan 27:68efb1622985 416 current_error2 = target_rpm2 - rpm2;
be_bryan 27:68efb1622985 417 sum_error2 = sum_error2 + current_error2;
be_bryan 27:68efb1622985 418 p2 = current_error2*kpA;
be_bryan 27:68efb1622985 419 d2 = (current_error2-last_error2)*kdA/10.0;
be_bryan 27:68efb1622985 420 i2 = sum_error2*kiA*10.0;
be_bryan 27:68efb1622985 421 speed2 = p2 + d2 + i2;
be_bryan 27:68efb1622985 422 init_speed();
gustavaditya 31:d5cbda07fd95 423 launcherDpn.speed(speed2);
be_bryan 27:68efb1622985 424 last_error2 = current_error2;
gustavaditya 31:d5cbda07fd95 425 encLauncherDpn.reset();
be_bryan 27:68efb1622985 426 previousMillis2 = currentMillis2;
be_bryan 27:68efb1622985 427 }
rahmadirizki18 6:68293bed71ea 428 }
Sufa 29:7b372b0aaa61 429 else
Sufa 29:7b372b0aaa61 430 {
gustavaditya 31:d5cbda07fd95 431 launcherDpn.brake(1);
gustavaditya 31:d5cbda07fd95 432 launcherBlk.brake(1);
gustavaditya 31:d5cbda07fd95 433 }
rahmadirizki18 5:3aa203218306 434 }
gustavaditya 31:d5cbda07fd95 435
MarchioKevin 22:4632f58461e0 436 /*********************************************************/
MarchioKevin 22:4632f58461e0 437 /* Main Function */
MarchioKevin 22:4632f58461e0 438 /*********************************************************/
calmantara186 16:90119f03c5d1 439
gustavaditya 31:d5cbda07fd95 440 int main (void)
gustavaditya 31:d5cbda07fd95 441 {
gustavaditya 31:d5cbda07fd95 442 // Set baud rate - 115200
fanny868 0:9072e932503c 443 joystick.setup();
rahmadirizki18 23:023b522977b2 444 pc.baud(115200);
rahmadirizki18 6:68293bed71ea 445 wait_ms(1000);
rahmadirizki18 5:3aa203218306 446 setCenter();
rahmadirizki18 5:3aa203218306 447 wait_ms(500);
rahmadirizki18 23:023b522977b2 448 pc.printf("ready....");
gustavaditya 35:69a47b4cb3fc 449 startMillis();
fanny868 0:9072e932503c 450 while(1)
fanny868 0:9072e932503c 451 {
fanny868 0:9072e932503c 452 // Interrupt Serial
calmantara186 16:90119f03c5d1 453 joystick.idle();
gustavaditya 31:d5cbda07fd95 454 if(joystick.readable())
gustavaditya 31:d5cbda07fd95 455 {
fanny868 0:9072e932503c 456 // Panggil fungsi pembacaan joystik
fanny868 0:9072e932503c 457 joystick.baca_data();
fanny868 0:9072e932503c 458 // Panggil fungsi pengolahan data joystik
fanny868 0:9072e932503c 459 joystick.olah_data();
gustavaditya 31:d5cbda07fd95 460 // Masuk ke case joystick
gustavaditya 31:d5cbda07fd95 461 case_joy = case_joystick();
rahmadirizki18 3:1287fccc11be 462 aktuator();
gustavaditya 31:d5cbda07fd95 463 launcher();
rizqicahyo 36:5963c9a49485 464 reloader();
rizqicahyo 36:5963c9a49485 465 if ((millis()-previousMillis3 >= 350)&&(flag_Pneu)){
be_bryan 26:256160a1a82d 466 pneumatik = 1;
gustavaditya 31:d5cbda07fd95 467 flag_Pneu = false;
be_bryan 26:256160a1a82d 468 }
be_bryan 26:256160a1a82d 469 }
gustavaditya 31:d5cbda07fd95 470 else
gustavaditya 31:d5cbda07fd95 471 {
gustavaditya 31:d5cbda07fd95 472 joystick.idle();
MarchioKevin 21:da2f3d04468f 473 }
fanny868 0:9072e932503c 474 }
be_bryan 28:2d0746dc2d7d 475 }