KRAI 2017 / Mbed 2 deprecated MainProgram_BaseBaru_20Feb_terbaru

terbaru

Dependencies:   DigitDisplay Motor PID Ping mbed millis

Fork of MainProgram_BaseBaru_20Feb_malam by KRAI 2017

main.cpp@43:5e347df94a26, 2017-03-09 (annotated)

Committer:
Najib_irvani
Date:
Thu Mar 09 13:06:30 2017 +0000
Revision:
43:5e347df94a26
Parent:
42:6caf8bd5abbc 
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 /* */
Najib_irvani 43:5e347df94a26 4 /* Last Update : 8 Maret 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 5:3aa203218306 44 #include "encoderKRAI.h"
be_bryan 26:256160a1a82d 45 #include "millis.h"
gustavaditya 40:5b937cac959a 46 #include "Ping.h"
franshendri 42:6caf8bd5abbc 47 #include "DigitDisplay.h"
franshendri 42:6caf8bd5abbc 48
calmantara186 16:90119f03c5d1 49 /***********************************************/
calmantara186 16:90119f03c5d1 50 /* Konstanta dan Variabel */
calmantara186 16:90119f03c5d1 51 /***********************************************/
calmantara186 16:90119f03c5d1 52 #define PI 3.14159265
be_bryan 26:256160a1a82d 53 #define D_ENCODER 10 // Diameter Roda Encoder
be_bryan 26:256160a1a82d 54 #define D_ROBOT 80 // Diameter Roda Robot
rahmadirizki18 17:e4229d77a5ab 55 #define PERPINDAHAN 1 // Perpindahan ke kanan dan kiri
rahmadirizki18 5:3aa203218306 56
be_bryan 26:256160a1a82d 57 // Variable Atas
gustavaditya 38:3ef6754bd8d8 58 double speed, speed2;
gustavaditya 39:11358f3f61ff 59 const double maxSpeed = 0.95, minSpeed = 0.0;
Najib_irvani 43:5e347df94a26 60 const double kpA=0.6757, kdA=0.06757, kiA=0.00006757;
be_bryan 26:256160a1a82d 61 double p,i,d;
be_bryan 27:68efb1622985 62 double p2,i2,d2;
be_bryan 26:256160a1a82d 63 double last_error = 0, current_error, sum_error = 0;
be_bryan 27:68efb1622985 64 double last_error2 = 0, current_error2, sum_error2 = 0;
gustavaditya 38:3ef6754bd8d8 65 float rpm, rpm2;
gustavaditya 41:336a19289c2d 66 float target_rpm = 15.0, target_rpm2 = 17.0;
gustavaditya 41:336a19289c2d 67 const float maxRPM = 28, minRPM = 0; // Limit 25 atau 27
gustavaditya 38:3ef6754bd8d8 68
gustavaditya 41:336a19289c2d 69 const float pwmPowerUp = 0.8;
gustavaditya 38:3ef6754bd8d8 70 const float pwmPowerDown = -0.9;
gustavaditya 40:5b937cac959a 71
gustavaditya 40:5b937cac959a 72 float jarak_ping=0;
be_bryan 26:256160a1a82d 73
be_bryan 26:256160a1a82d 74 // Variable Bawah
Joshua23 25:054d3048dd03 75 float Vt;
gustavaditya 31:d5cbda07fd95 76 float keliling_enc = PI*D_ENCODER;
gustavaditya 31:d5cbda07fd95 77 float keliling_robot = PI*D_ROBOT;
gustavaditya 31:d5cbda07fd95 78 float speedT = 0.2;
gustavaditya 31:d5cbda07fd95 79 float vpid = 0;
Najib_irvani 43:5e347df94a26 80 float PIVOT = 0.2; // PWM Pivot Kanan, Pivot Kiri
Najib_irvani 43:5e347df94a26 81 float tuneDpn = 0.45; // Tunning PWM motor Depan
Najib_irvani 43:5e347df94a26 82 float tuneBlk = 0.37; // Tunning PWM motor belakang
gustavaditya 31:d5cbda07fd95 83
gustavaditya 31:d5cbda07fd95 84 /* Variabel Encoder Bawah */
gustavaditya 31:d5cbda07fd95 85 float errTetha, Tetha; // Variabel yang didapatkan encoder
gustavaditya 31:d5cbda07fd95 86
gustavaditya 31:d5cbda07fd95 87 /* Deklarasi Variable Millis */
gustavaditya 33:69d266bc3fe9 88 unsigned long int previousMillis = 0; // PID launcher
gustavaditya 31:d5cbda07fd95 89 unsigned long int currentMillis;
gustavaditya 33:69d266bc3fe9 90 unsigned long int previousMillis2 = 0; // PID launcher
gustavaditya 31:d5cbda07fd95 91 unsigned long int currentMillis2;
gustavaditya 33:69d266bc3fe9 92 unsigned long int previousMillis3 = 0; // Pneumatik
gustavaditya 40:5b937cac959a 93 unsigned long int previousMillis4 = 0; // Ping
franshendri 42:6caf8bd5abbc 94 unsigned long int previousMillis5 = 0; // Display
MarchioKevin 22:4632f58461e0 95
gustavaditya 31:d5cbda07fd95 96 /* Variabel Stick */
gustavaditya 31:d5cbda07fd95 97 //Logic untuk masuk aktuator
gustavaditya 31:d5cbda07fd95 98 int case_joy;
gustavaditya 31:d5cbda07fd95 99 bool isLauncher = false;
gustavaditya 31:d5cbda07fd95 100 bool isReload = false;
rizqicahyo 36:5963c9a49485 101 bool ReloadOn = false;
gustavaditya 31:d5cbda07fd95 102 bool flag_Pneu = false;
gustavaditya 31:d5cbda07fd95 103
gustavaditya 31:d5cbda07fd95 104 /*****************************************************/
gustavaditya 31:d5cbda07fd95 105 /* Definisi Prosedur, Fungsi dan Setting Pinout */
gustavaditya 31:d5cbda07fd95 106 /*****************************************************/
gustavaditya 31:d5cbda07fd95 107
gustavaditya 31:d5cbda07fd95 108 /* Fungsi dan Procedur Encoder */
gustavaditya 31:d5cbda07fd95 109 void init_speed(); //
gustavaditya 31:d5cbda07fd95 110 void aktuator(); // Pergerakan aktuator berdasarkan case joystick
gustavaditya 31:d5cbda07fd95 111 int case_joystick(); // Mendapatkan case dari joystick
gustavaditya 33:69d266bc3fe9 112 //void speedKalibrasiMotor(); // Pertambahan target RPM motor atas melalui joystick
gustavaditya 31:d5cbda07fd95 113 void setCenter(); // Prosedur reset encoder, posisi saat itu diset jadi titik (0,0)
gustavaditya 31:d5cbda07fd95 114 float getTetha(); // Fungsi mendapatkan error Tetha
gustavaditya 31:d5cbda07fd95 115
gustavaditya 31:d5cbda07fd95 116 /* Inisialisasi Pin TX-RX Joystik dan PC */
gustavaditya 31:d5cbda07fd95 117 joysticknucleo joystick(PA_0,PA_1);
gustavaditya 31:d5cbda07fd95 118 Serial pc(USBTX,USBRX);
calmantara186 16:90119f03c5d1 119
be_bryan 26:256160a1a82d 120 /* Deklarasi Encoder Base */
gustavaditya 31:d5cbda07fd95 121 encoderKRAI encoderBase(PC_4, PB_15, 2000, encoderKRAI::X2_ENCODING); //inA, inB, pin Indeks (NC = tak ada), 2xresolusi, mode pembacaan
rahmadirizki18 15:98f0d56b14f0 122
be_bryan 26:256160a1a82d 123 /* Deklarasi Encoder Launcher */
gustavaditya 37:67d54563af90 124 encoderKRAI encLauncherBlk( PC_10, PC_11, 28, encoderKRAI::X4_ENCODING);
gustavaditya 37:67d54563af90 125 encoderKRAI encLauncherDpn( PD_2, PC_12, 28, encoderKRAI::X4_ENCODING);
be_bryan 26:256160a1a82d 126
calmantara186 16:90119f03c5d1 127 /* Deklarasi Motor Base */
gustavaditya 31:d5cbda07fd95 128 Motor motorDpn(PB_9, PA_12, PC_5); // pwm, fwd, rev
gustavaditya 31:d5cbda07fd95 129 Motor motorBlk(PB_6, PB_1, PB_12); // pwm, fwd, rev
fanny868 0:9072e932503c 130
calmantara186 16:90119f03c5d1 131 /* Deklarasi Motor Launcher */
gustavaditya 31:d5cbda07fd95 132 Motor launcherDpn(PA_8,PC_2,PC_1); // pwm ,fwd, rev
gustavaditya 31:d5cbda07fd95 133 Motor launcherBlk(PA_10, PC_3, PC_0); // pwm, fwd, rev
gustavaditya 31:d5cbda07fd95 134 Motor powerScrew(PB_7, PA_14, PA_15); // pwm, fwd, rev
rahmadirizki18 5:3aa203218306 135
be_bryan 26:256160a1a82d 136 /* Deklarasi Penumatik Launcher */
Sufa 29:7b372b0aaa61 137 DigitalOut pneumatik(PB_3, PullUp);
be_bryan 26:256160a1a82d 138
be_bryan 27:68efb1622985 139 /*Dekalrasi Limit Switch */
gustavaditya 40:5b937cac959a 140 //DigitalIn infraAtas(PC_9, PullUp);
gustavaditya 31:d5cbda07fd95 141 DigitalIn limitTengah(PB_10, PullUp);
gustavaditya 31:d5cbda07fd95 142 DigitalIn limitBawah(PC_8, PullUp);
rahmadirizki18 5:3aa203218306 143
gustavaditya 40:5b937cac959a 144 /*deklarasi PING ultrasonic*/
gustavaditya 40:5b937cac959a 145 Ping pingAtas(PC_9);
fanny868 0:9072e932503c 146
franshendri 42:6caf8bd5abbc 147 /*Deklarasi Display*/
franshendri 42:6caf8bd5abbc 148 DigitDisplay display (D15, D4);
franshendri 42:6caf8bd5abbc 149
MarchioKevin 22:4632f58461e0 150 /****************************************************/
MarchioKevin 22:4632f58461e0 151 /* Deklarasi Fungsi dan Procedure */
MarchioKevin 22:4632f58461e0 152 /****************************************************/
gustavaditya 31:d5cbda07fd95 153 int case_joystick()
gustavaditya 31:d5cbda07fd95 154 {
gustavaditya 31:d5cbda07fd95 155 //---------------------------------------------------//
gustavaditya 31:d5cbda07fd95 156 // Gerak Motor Base //
gustavaditya 31:d5cbda07fd95 157 // Case 1 : Pivot kanan //
gustavaditya 31:d5cbda07fd95 158 // Case 2 : Pivot Kiri //
gustavaditya 31:d5cbda07fd95 159 // Case 3 : Kanan //
gustavaditya 31:d5cbda07fd95 160 // Case 4 : Kiri //
gustavaditya 31:d5cbda07fd95 161 // Case 5 : Break //
gustavaditya 31:d5cbda07fd95 162 //---------------------------------------------------//
gustavaditya 31:d5cbda07fd95 163
gustavaditya 31:d5cbda07fd95 164 int caseJoystick;
gustavaditya 31:d5cbda07fd95 165 if (!joystick.L1 && joystick.R1) {
gustavaditya 31:d5cbda07fd95 166 // Pivot Kanan
gustavaditya 31:d5cbda07fd95 167 caseJoystick = 1;
gustavaditya 31:d5cbda07fd95 168 }
gustavaditya 31:d5cbda07fd95 169 else if (!joystick.R1 && joystick.L1) {
gustavaditya 31:d5cbda07fd95 170 // Pivot Kiri
gustavaditya 31:d5cbda07fd95 171 caseJoystick = 2;
gustavaditya 31:d5cbda07fd95 172 }
gustavaditya 31:d5cbda07fd95 173 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {
gustavaditya 31:d5cbda07fd95 174 // Kanan
gustavaditya 31:d5cbda07fd95 175 caseJoystick = 3;
gustavaditya 33:69d266bc3fe9 176 //pc.printf("kanan");
gustavaditya 31:d5cbda07fd95 177 }
gustavaditya 31:d5cbda07fd95 178 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {
gustavaditya 31:d5cbda07fd95 179 // Kiri
gustavaditya 31:d5cbda07fd95 180 caseJoystick = 4;
gustavaditya 33:69d266bc3fe9 181 //pc.printf("kiri");
gustavaditya 31:d5cbda07fd95 182 }
gustavaditya 38:3ef6754bd8d8 183 else if (joystick.segitiga_click){
gustavaditya 31:d5cbda07fd95 184 // Motor Launcher
gustavaditya 31:d5cbda07fd95 185 caseJoystick = 5;
gustavaditya 31:d5cbda07fd95 186 }
gustavaditya 38:3ef6754bd8d8 187 else if (joystick.R2_click){
gustavaditya 31:d5cbda07fd95 188 // Target Pulse PID ++ Motor Depan
gustavaditya 31:d5cbda07fd95 189 caseJoystick = 6;
gustavaditya 31:d5cbda07fd95 190 }
gustavaditya 38:3ef6754bd8d8 191 else if (joystick.L2_click){
Najib_irvani 43:5e347df94a26 192 // Target Pulse PID -- Motor
gustavaditya 31:d5cbda07fd95 193 caseJoystick = 7;
gustavaditya 31:d5cbda07fd95 194 }
Najib_irvani 43:5e347df94a26 195 else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {
Najib_irvani 43:5e347df94a26 196 // kiri + pivot kiri
Najib_irvani 43:5e347df94a26 197 caseJoystick = 14;
Najib_irvani 43:5e347df94a26 198 }
Najib_irvani 43:5e347df94a26 199 else if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {
Najib_irvani 43:5e347df94a26 200 // kiri + pivot kanan
Najib_irvani 43:5e347df94a26 201 caseJoystick = 15;
gustavaditya 31:d5cbda07fd95 202 }
Najib_irvani 43:5e347df94a26 203 else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {
Najib_irvani 43:5e347df94a26 204 // kanan + pivot kiri
Najib_irvani 43:5e347df94a26 205 caseJoystick = 16;
Najib_irvani 43:5e347df94a26 206 }
Najib_irvani 43:5e347df94a26 207 else if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {
Najib_irvani 43:5e347df94a26 208 // kanan + pivot kanan
Najib_irvani 43:5e347df94a26 209 caseJoystick = 17;
Najib_irvani 43:5e347df94a26 210 }
gustavaditya 31:d5cbda07fd95 211 else if (joystick.silang_click){
gustavaditya 31:d5cbda07fd95 212 // Pnemuatik ON
gustavaditya 31:d5cbda07fd95 213 caseJoystick = 10;
gustavaditya 31:d5cbda07fd95 214 }
gustavaditya 31:d5cbda07fd95 215 else if ((joystick.atas)&&(!joystick.bawah)) {
gustavaditya 31:d5cbda07fd95 216 // Power Screw Up
gustavaditya 31:d5cbda07fd95 217 caseJoystick = 11;
gustavaditya 31:d5cbda07fd95 218 }
gustavaditya 31:d5cbda07fd95 219 else if ((!joystick.atas)&&(joystick.bawah)) {
gustavaditya 31:d5cbda07fd95 220 // Power Screw Down
gustavaditya 31:d5cbda07fd95 221 caseJoystick = 12;
gustavaditya 31:d5cbda07fd95 222 }
gustavaditya 31:d5cbda07fd95 223 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
gustavaditya 31:d5cbda07fd95 224 // Break
gustavaditya 31:d5cbda07fd95 225 caseJoystick = 13;
gustavaditya 31:d5cbda07fd95 226 }
gustavaditya 31:d5cbda07fd95 227
gustavaditya 31:d5cbda07fd95 228 return(caseJoystick);
gustavaditya 31:d5cbda07fd95 229 }
gustavaditya 31:d5cbda07fd95 230
gustavaditya 31:d5cbda07fd95 231 float getTetha(){
gustavaditya 31:d5cbda07fd95 232 // Fungsi untuk mendapatkan nilai tetha
gustavaditya 31:d5cbda07fd95 233 float busur, tetha;
gustavaditya 31:d5cbda07fd95 234 busur = ((encoderBase.getPulses())/(float)(2000.0)*keliling_enc);
gustavaditya 31:d5cbda07fd95 235 tetha = busur/keliling_robot*360;
gustavaditya 31:d5cbda07fd95 236
gustavaditya 31:d5cbda07fd95 237 return -(tetha);
gustavaditya 31:d5cbda07fd95 238 }
gustavaditya 31:d5cbda07fd95 239
be_bryan 26:256160a1a82d 240 float pidBase(float Kp, float Ki, float Kd)
Joshua23 25:054d3048dd03 241 {
Joshua23 25:054d3048dd03 242 int errorP;
Joshua23 25:054d3048dd03 243 errorP = getTetha();
gustavaditya 33:69d266bc3fe9 244 if (errorP<3.5 && errorP>(-3.5))
gustavaditya 33:69d266bc3fe9 245 errorP = 0;
Joshua23 25:054d3048dd03 246 return (float)Kp*errorP;
Joshua23 25:054d3048dd03 247 }
gustavaditya 31:d5cbda07fd95 248
gustavaditya 31:d5cbda07fd95 249 void setCenter(){
gustavaditya 31:d5cbda07fd95 250 // Fungsi untuk menentukan center dari robot
gustavaditya 31:d5cbda07fd95 251 encoderBase.reset();
fanny868 0:9072e932503c 252 }
fanny868 0:9072e932503c 253
gustavaditya 37:67d54563af90 254 void init_rpm (){
gustavaditya 38:3ef6754bd8d8 255 if (target_rpm>maxRPM-2){
gustavaditya 38:3ef6754bd8d8 256 target_rpm = maxRPM-2;
gustavaditya 37:67d54563af90 257 }
franshendri 42:6caf8bd5abbc 258 if (target_rpm<minRPM){
gustavaditya 37:67d54563af90 259 target_rpm = minRPM;
gustavaditya 37:67d54563af90 260 }
gustavaditya 37:67d54563af90 261 if (target_rpm2>maxRPM){
gustavaditya 37:67d54563af90 262 target_rpm2 = maxRPM;
gustavaditya 37:67d54563af90 263 }
franshendri 42:6caf8bd5abbc 264 if (target_rpm2<minRPM+2){
gustavaditya 38:3ef6754bd8d8 265 target_rpm2 = minRPM+2;
gustavaditya 37:67d54563af90 266 }
gustavaditya 37:67d54563af90 267 }
gustavaditya 37:67d54563af90 268
gustavaditya 31:d5cbda07fd95 269 void aktuator()
gustavaditya 31:d5cbda07fd95 270 {
gustavaditya 31:d5cbda07fd95 271 switch (case_joy) {
gustavaditya 31:d5cbda07fd95 272 case (1):
gustavaditya 31:d5cbda07fd95 273 {
gustavaditya 31:d5cbda07fd95 274 // Pivot Kanan
gustavaditya 31:d5cbda07fd95 275 motorDpn.speed(-PIVOT);
gustavaditya 31:d5cbda07fd95 276 motorBlk.speed(-PIVOT);
gustavaditya 31:d5cbda07fd95 277 setCenter();
gustavaditya 31:d5cbda07fd95 278 break;
gustavaditya 31:d5cbda07fd95 279 }
gustavaditya 31:d5cbda07fd95 280 case (2):
gustavaditya 31:d5cbda07fd95 281 {
gustavaditya 31:d5cbda07fd95 282 // Pivot Kiri
gustavaditya 31:d5cbda07fd95 283 motorDpn.speed(PIVOT);
gustavaditya 31:d5cbda07fd95 284 motorBlk.speed(PIVOT);
gustavaditya 31:d5cbda07fd95 285 setCenter();
gustavaditya 31:d5cbda07fd95 286 break;
gustavaditya 31:d5cbda07fd95 287 }
gustavaditya 31:d5cbda07fd95 288 case (3) :
gustavaditya 31:d5cbda07fd95 289 {
gustavaditya 31:d5cbda07fd95 290 // Kanan
Najib_irvani 43:5e347df94a26 291 //motorDpn.speed(-tuneDpn + pidBase(0.009,0,0));
Najib_irvani 43:5e347df94a26 292 //motorBlk.speed(tuneBlk + pidBase(0.009,0,0));
gustavaditya 33:69d266bc3fe9 293 //speedDpn = tuneDpn + pidBase(0.009,0,0)
gustavaditya 33:69d266bc3fe9 294 //speedBlk = tuneBlk + pidBase(0.009,0,0)
Najib_irvani 43:5e347df94a26 295 motorDpn.speed(-tuneDpn);
Najib_irvani 43:5e347df94a26 296 motorBlk.speed(tuneBlk);
gustavaditya 31:d5cbda07fd95 297 break;
gustavaditya 31:d5cbda07fd95 298 }
gustavaditya 31:d5cbda07fd95 299 case (4) :
gustavaditya 31:d5cbda07fd95 300 {
gustavaditya 31:d5cbda07fd95 301 // Kiri
Najib_irvani 43:5e347df94a26 302 //motorDpn.speed(tuneDpn + pidBase(0.009,0,0));
Najib_irvani 43:5e347df94a26 303 //motorBlk.speed(-tuneBlk + pidBase(0.009,0,0));
Najib_irvani 43:5e347df94a26 304 motorDpn.speed(tuneDpn+0.02);
Najib_irvani 43:5e347df94a26 305 motorBlk.speed(-tuneBlk);
gustavaditya 31:d5cbda07fd95 306 break;
gustavaditya 31:d5cbda07fd95 307 }
gustavaditya 31:d5cbda07fd95 308 case (5) :
gustavaditya 31:d5cbda07fd95 309 {
gustavaditya 31:d5cbda07fd95 310 // Aktifkan motor atas
gustavaditya 31:d5cbda07fd95 311 isLauncher = !isLauncher;
gustavaditya 31:d5cbda07fd95 312 break;
Joshua23 8:0711dea61312 313 }
gustavaditya 31:d5cbda07fd95 314 case (6) :
gustavaditya 31:d5cbda07fd95 315 {
gustavaditya 31:d5cbda07fd95 316 // Target Pulse PID ++ Motor Depan
gustavaditya 39:11358f3f61ff 317 target_rpm2 = target_rpm2+1.0;
gustavaditya 39:11358f3f61ff 318 target_rpm = target_rpm+1.0;
gustavaditya 37:67d54563af90 319 init_rpm();
gustavaditya 31:d5cbda07fd95 320 break;
gustavaditya 31:d5cbda07fd95 321 }
gustavaditya 31:d5cbda07fd95 322 case (7) :
gustavaditya 31:d5cbda07fd95 323 {
gustavaditya 31:d5cbda07fd95 324 // Target Pulse PID -- Motor Depan
gustavaditya 39:11358f3f61ff 325 target_rpm2 = target_rpm2-1.0;
gustavaditya 39:11358f3f61ff 326 target_rpm = target_rpm-1.0;
gustavaditya 37:67d54563af90 327 init_rpm();
gustavaditya 31:d5cbda07fd95 328 break;
gustavaditya 31:d5cbda07fd95 329 }
gustavaditya 38:3ef6754bd8d8 330 /*case (8) :
gustavaditya 31:d5cbda07fd95 331 {
gustavaditya 32:581d4a2373f0 332 // Target Pulse PID ++ Motor Belakang=
gustavaditya 38:3ef6754bd8d8 333 //init_rpm();
gustavaditya 31:d5cbda07fd95 334 break;
gustavaditya 31:d5cbda07fd95 335 }
gustavaditya 31:d5cbda07fd95 336 case (9) :
be_bryan 27:68efb1622985 337 {
gustavaditya 31:d5cbda07fd95 338 // Target Pulse PID -- Motor Belakang
gustavaditya 38:3ef6754bd8d8 339 //init_rpm();
gustavaditya 31:d5cbda07fd95 340 break;
gustavaditya 38:3ef6754bd8d8 341 }*/
gustavaditya 31:d5cbda07fd95 342 case (10) :
gustavaditya 31:d5cbda07fd95 343 {
gustavaditya 31:d5cbda07fd95 344 // Pneumatik
gustavaditya 31:d5cbda07fd95 345 pneumatik = 0;
gustavaditya 31:d5cbda07fd95 346 previousMillis3 = millis();
gustavaditya 31:d5cbda07fd95 347 flag_Pneu = true;
gustavaditya 31:d5cbda07fd95 348 break;
gustavaditya 31:d5cbda07fd95 349 }
gustavaditya 31:d5cbda07fd95 350 case (11) :
gustavaditya 31:d5cbda07fd95 351 {
gustavaditya 31:d5cbda07fd95 352 // Power Screw Up
rizqicahyo 36:5963c9a49485 353 //powerScrew.speed(pwmPowerUp);
gustavaditya 40:5b937cac959a 354 ReloadOn = !ReloadOn;
gustavaditya 40:5b937cac959a 355 //powerScrew.speed(pwmPowerUp);
gustavaditya 31:d5cbda07fd95 356 break;
gustavaditya 31:d5cbda07fd95 357 }
gustavaditya 31:d5cbda07fd95 358 case (12) :
gustavaditya 31:d5cbda07fd95 359 {
gustavaditya 31:d5cbda07fd95 360 // Power Screw Down
rizqicahyo 36:5963c9a49485 361 //powerScrew.speed(pwmPowerDown);
gustavaditya 31:d5cbda07fd95 362 break;
gustavaditya 31:d5cbda07fd95 363 }
Najib_irvani 43:5e347df94a26 364 case (14) :
Najib_irvani 43:5e347df94a26 365 {
Najib_irvani 43:5e347df94a26 366 // kiri + piv kiri
Najib_irvani 43:5e347df94a26 367 motorDpn.speed(tuneDpn+0.02);
Najib_irvani 43:5e347df94a26 368 motorBlk.brake(1);
Najib_irvani 43:5e347df94a26 369 break;
Najib_irvani 43:5e347df94a26 370 }
Najib_irvani 43:5e347df94a26 371 case (15) :
Najib_irvani 43:5e347df94a26 372 {
Najib_irvani 43:5e347df94a26 373 // kiri + pivkanan
Najib_irvani 43:5e347df94a26 374 motorDpn.brake(1);
Najib_irvani 43:5e347df94a26 375 motorBlk.speed(-tuneBlk);
Najib_irvani 43:5e347df94a26 376 break;
Najib_irvani 43:5e347df94a26 377 }
Najib_irvani 43:5e347df94a26 378 case (16) :
Najib_irvani 43:5e347df94a26 379 {
Najib_irvani 43:5e347df94a26 380 // kanan + pivkiri
Najib_irvani 43:5e347df94a26 381 motorDpn.brake(1);
Najib_irvani 43:5e347df94a26 382 motorBlk.speed(tuneBlk);
Najib_irvani 43:5e347df94a26 383 break;
Najib_irvani 43:5e347df94a26 384 }
Najib_irvani 43:5e347df94a26 385 case (17) :
Najib_irvani 43:5e347df94a26 386 {
Najib_irvani 43:5e347df94a26 387 // kanan + pivkanan
Najib_irvani 43:5e347df94a26 388 motorDpn.speed(-tuneDpn);
Najib_irvani 43:5e347df94a26 389 motorBlk.brake(1);
Najib_irvani 43:5e347df94a26 390 break;
Najib_irvani 43:5e347df94a26 391 }
gustavaditya 31:d5cbda07fd95 392 default :
gustavaditya 31:d5cbda07fd95 393 {
gustavaditya 31:d5cbda07fd95 394 motorDpn.brake(1);
gustavaditya 31:d5cbda07fd95 395 motorBlk.brake(1);
gustavaditya 40:5b937cac959a 396 /* if(isReload){
gustavaditya 39:11358f3f61ff 397 powerScrew.speed(pwmPowerDown);
gustavaditya 39:11358f3f61ff 398 if(!limitBawah){
gustavaditya 39:11358f3f61ff 399 isReload = false;
gustavaditya 39:11358f3f61ff 400 ReloadOn = false;
gustavaditya 39:11358f3f61ff 401 }
gustavaditya 39:11358f3f61ff 402 }
gustavaditya 39:11358f3f61ff 403 else if(!limitTengah){
gustavaditya 39:11358f3f61ff 404 isReload = true;
gustavaditya 39:11358f3f61ff 405 }
gustavaditya 39:11358f3f61ff 406 else{
gustavaditya 31:d5cbda07fd95 407 powerScrew.brake(1);
gustavaditya 40:5b937cac959a 408 }*/
gustavaditya 31:d5cbda07fd95 409 }
gustavaditya 31:d5cbda07fd95 410 } // End Switch
gustavaditya 31:d5cbda07fd95 411 }
gustavaditya 31:d5cbda07fd95 412
gustavaditya 40:5b937cac959a 413 void reloader()
rizqicahyo 36:5963c9a49485 414 {
rizqicahyo 36:5963c9a49485 415 if(ReloadOn){
rizqicahyo 36:5963c9a49485 416 if(isReload){
rizqicahyo 36:5963c9a49485 417 powerScrew.speed(pwmPowerDown);
rizqicahyo 36:5963c9a49485 418 if(!limitBawah){
rizqicahyo 36:5963c9a49485 419 isReload = false;
rizqicahyo 36:5963c9a49485 420 ReloadOn = false;
rizqicahyo 36:5963c9a49485 421 }
rizqicahyo 36:5963c9a49485 422 }
rizqicahyo 36:5963c9a49485 423 else if(!limitTengah){
rizqicahyo 36:5963c9a49485 424 isReload = true;
rizqicahyo 36:5963c9a49485 425 }
Najib_irvani 43:5e347df94a26 426 else if((jarak_ping > 3.8) && !flag_Pneu){
gustavaditya 40:5b937cac959a 427 powerScrew.speed(pwmPowerUp);
gustavaditya 40:5b937cac959a 428 }
Najib_irvani 43:5e347df94a26 429 else if((jarak_ping < 3.3 ) && !flag_Pneu) {
gustavaditya 41:336a19289c2d 430 powerScrew.speed(-0.1);
rizqicahyo 36:5963c9a49485 431 }
rizqicahyo 36:5963c9a49485 432 else{
gustavaditya 40:5b937cac959a 433 powerScrew.brake(1);
rizqicahyo 36:5963c9a49485 434 }
rizqicahyo 36:5963c9a49485 435 }
rizqicahyo 36:5963c9a49485 436 else{
rizqicahyo 36:5963c9a49485 437 powerScrew.brake(1);
rizqicahyo 36:5963c9a49485 438 }
gustavaditya 40:5b937cac959a 439 }
rizqicahyo 36:5963c9a49485 440
rizqicahyo 36:5963c9a49485 441
gustavaditya 31:d5cbda07fd95 442 void launcher()
gustavaditya 31:d5cbda07fd95 443 {
gustavaditya 31:d5cbda07fd95 444 if (isLauncher)
Sufa 30:d69cc27ac644 445 {
be_bryan 28:2d0746dc2d7d 446 currentMillis = millis();
be_bryan 28:2d0746dc2d7d 447 currentMillis2 = millis();
be_bryan 28:2d0746dc2d7d 448
gustavaditya 31:d5cbda07fd95 449 // PID Launcher Depan
gustavaditya 38:3ef6754bd8d8 450 if (currentMillis-previousMillis>=12.5)
be_bryan 26:256160a1a82d 451 {
gustavaditya 31:d5cbda07fd95 452 rpm = (float)encLauncherBlk.getPulses();
be_bryan 26:256160a1a82d 453 current_error = target_rpm - rpm;
be_bryan 26:256160a1a82d 454 sum_error = sum_error + current_error;
be_bryan 26:256160a1a82d 455 p = current_error*kpA;
gustavaditya 38:3ef6754bd8d8 456 d = (current_error-last_error)*kdA/12.5;
gustavaditya 38:3ef6754bd8d8 457 i = sum_error*kiA*12.5;
be_bryan 26:256160a1a82d 458 speed = p + d + i;
gustavaditya 38:3ef6754bd8d8 459 //init_speed();
gustavaditya 38:3ef6754bd8d8 460 if(speed > maxSpeed){
gustavaditya 38:3ef6754bd8d8 461 launcherBlk.speed(maxSpeed);
gustavaditya 38:3ef6754bd8d8 462 }
gustavaditya 39:11358f3f61ff 463 else if ( speed < minSpeed){
gustavaditya 39:11358f3f61ff 464 launcherBlk.speed(minSpeed);
gustavaditya 39:11358f3f61ff 465 }
gustavaditya 38:3ef6754bd8d8 466 else {
gustavaditya 38:3ef6754bd8d8 467 launcherBlk.speed(speed);
gustavaditya 38:3ef6754bd8d8 468 }
be_bryan 26:256160a1a82d 469 last_error = current_error;
gustavaditya 31:d5cbda07fd95 470 encLauncherBlk.reset();
be_bryan 26:256160a1a82d 471 //pc.printf("%.04lf\n",rpm);
be_bryan 26:256160a1a82d 472 previousMillis = currentMillis;
be_bryan 26:256160a1a82d 473 }
gustavaditya 38:3ef6754bd8d8 474 if (currentMillis2-previousMillis2>=12.5)
be_bryan 27:68efb1622985 475 {
gustavaditya 31:d5cbda07fd95 476 rpm2 = (float)encLauncherDpn.getPulses();
be_bryan 27:68efb1622985 477 current_error2 = target_rpm2 - rpm2;
be_bryan 27:68efb1622985 478 sum_error2 = sum_error2 + current_error2;
be_bryan 27:68efb1622985 479 p2 = current_error2*kpA;
gustavaditya 38:3ef6754bd8d8 480 d2 = (current_error2-last_error2)*kdA/12.5;
gustavaditya 38:3ef6754bd8d8 481 i2 = sum_error2*kiA*12.5;
be_bryan 27:68efb1622985 482 speed2 = p2 + d2 + i2;
gustavaditya 38:3ef6754bd8d8 483 //init_speed();
gustavaditya 38:3ef6754bd8d8 484 if (speed2 > maxSpeed){
gustavaditya 38:3ef6754bd8d8 485 launcherDpn.speed(maxSpeed);
gustavaditya 38:3ef6754bd8d8 486 }
gustavaditya 39:11358f3f61ff 487 else if ( speed < minSpeed){
gustavaditya 39:11358f3f61ff 488 launcherDpn.speed(minSpeed);
gustavaditya 39:11358f3f61ff 489 }
gustavaditya 38:3ef6754bd8d8 490 else{
gustavaditya 38:3ef6754bd8d8 491 launcherDpn.speed(speed2);
gustavaditya 38:3ef6754bd8d8 492 }
be_bryan 27:68efb1622985 493 last_error2 = current_error2;
gustavaditya 31:d5cbda07fd95 494 encLauncherDpn.reset();
be_bryan 27:68efb1622985 495 previousMillis2 = currentMillis2;
be_bryan 27:68efb1622985 496 }
rahmadirizki18 6:68293bed71ea 497 }
Sufa 29:7b372b0aaa61 498 else
Sufa 29:7b372b0aaa61 499 {
gustavaditya 31:d5cbda07fd95 500 launcherDpn.brake(1);
gustavaditya 31:d5cbda07fd95 501 launcherBlk.brake(1);
gustavaditya 31:d5cbda07fd95 502 }
rahmadirizki18 5:3aa203218306 503 }
gustavaditya 31:d5cbda07fd95 504
MarchioKevin 22:4632f58461e0 505 /*********************************************************/
MarchioKevin 22:4632f58461e0 506 /* Main Function */
MarchioKevin 22:4632f58461e0 507 /*********************************************************/
calmantara186 16:90119f03c5d1 508
gustavaditya 31:d5cbda07fd95 509 int main (void)
gustavaditya 31:d5cbda07fd95 510 {
gustavaditya 31:d5cbda07fd95 511 // Set baud rate - 115200
fanny868 0:9072e932503c 512 joystick.setup();
rahmadirizki18 23:023b522977b2 513 pc.baud(115200);
rahmadirizki18 6:68293bed71ea 514 wait_ms(1000);
gustavaditya 40:5b937cac959a 515
gustavaditya 40:5b937cac959a 516 // initializing encoder
gustavaditya 41:336a19289c2d 517 pneumatik =1;
gustavaditya 41:336a19289c2d 518
rahmadirizki18 5:3aa203218306 519 setCenter();
gustavaditya 41:336a19289c2d 520
rahmadirizki18 5:3aa203218306 521 wait_ms(500);
gustavaditya 40:5b937cac959a 522
gustavaditya 40:5b937cac959a 523 //initializing PING
gustavaditya 40:5b937cac959a 524 pingAtas.Send();
gustavaditya 40:5b937cac959a 525
rahmadirizki18 23:023b522977b2 526 pc.printf("ready....");
gustavaditya 35:69a47b4cb3fc 527 startMillis();
fanny868 0:9072e932503c 528 while(1)
gustavaditya 40:5b937cac959a 529 {
gustavaditya 40:5b937cac959a 530 // interupsi pembacaan PING setiap 30 ms
gustavaditya 41:336a19289c2d 531 if(millis() - previousMillis4 >= 5){ //30
gustavaditya 41:336a19289c2d 532 jarak_ping = (float)pingAtas.Read_cm()/2;
gustavaditya 40:5b937cac959a 533
gustavaditya 40:5b937cac959a 534 pingAtas.Send();
gustavaditya 40:5b937cac959a 535 previousMillis4 = millis();
gustavaditya 40:5b937cac959a 536 }
gustavaditya 40:5b937cac959a 537
fanny868 0:9072e932503c 538 // Interrupt Serial
calmantara186 16:90119f03c5d1 539 joystick.idle();
gustavaditya 31:d5cbda07fd95 540 if(joystick.readable())
gustavaditya 31:d5cbda07fd95 541 {
fanny868 0:9072e932503c 542 // Panggil fungsi pembacaan joystik
fanny868 0:9072e932503c 543 joystick.baca_data();
fanny868 0:9072e932503c 544 // Panggil fungsi pengolahan data joystik
fanny868 0:9072e932503c 545 joystick.olah_data();
gustavaditya 31:d5cbda07fd95 546 // Masuk ke case joystick
gustavaditya 31:d5cbda07fd95 547 case_joy = case_joystick();
rahmadirizki18 3:1287fccc11be 548 aktuator();
gustavaditya 31:d5cbda07fd95 549 launcher();
gustavaditya 40:5b937cac959a 550 reloader();
Najib_irvani 43:5e347df94a26 551 if ((millis()-previousMillis3 >= 320)&&(flag_Pneu)){
be_bryan 26:256160a1a82d 552 pneumatik = 1;
gustavaditya 31:d5cbda07fd95 553 flag_Pneu = false;
be_bryan 26:256160a1a82d 554 }
be_bryan 26:256160a1a82d 555 }
gustavaditya 31:d5cbda07fd95 556 else
gustavaditya 31:d5cbda07fd95 557 {
gustavaditya 31:d5cbda07fd95 558 joystick.idle();
MarchioKevin 21:da2f3d04468f 559 }
franshendri 42:6caf8bd5abbc 560
franshendri 42:6caf8bd5abbc 561 if(millis() - previousMillis5 >= 400){
Najib_irvani 43:5e347df94a26 562 //display.write(0,((int) target_rpm2-2) / 10);
Najib_irvani 43:5e347df94a26 563 //display.write(1,((int)target_rpm2-2) % 10);
Najib_irvani 43:5e347df94a26 564 //display.write(2, (int)target_rpm2 / 10);
Najib_irvani 43:5e347df94a26 565 //display.write(3, (int)target_rpm2 % 10);
Najib_irvani 43:5e347df94a26 566 //display.setColon(true);
Najib_irvani 43:5e347df94a26 567
Najib_irvani 43:5e347df94a26 568 display.write(0,((int) rpm2) / 10);
Najib_irvani 43:5e347df94a26 569 display.write(1,((int)rpm2) % 10);
franshendri 42:6caf8bd5abbc 570 display.write(2, (int)target_rpm2 / 10);
franshendri 42:6caf8bd5abbc 571 display.write(3, (int)target_rpm2 % 10);
franshendri 42:6caf8bd5abbc 572 display.setColon(true);
franshendri 42:6caf8bd5abbc 573
franshendri 42:6caf8bd5abbc 574 previousMillis5 = millis();
franshendri 42:6caf8bd5abbc 575 }
fanny868 0:9072e932503c 576 }
be_bryan 28:2d0746dc2d7d 577 }