KRAI 2017 / Mbed 2 deprecated MainProgram_BaseBaru_fix_omni_5April_23-04

Pakai akselerasi

Dependencies:   DigitDisplay Motor PID Ping mbed millis

Fork of MainProgram_BaseBaru_fix_omni_5April_fix by KRAI 2017

main.cpp@41:336a19289c2d, 2017-02-18 (annotated)

Committer:
gustavaditya
Date:
Sat Feb 18 11:33:00 2017 +0000
Revision:
41:336a19289c2d
Parent:
40:5b937cac959a
Child:
42:6caf8bd5abbc
Update 19 Februari 2017 Pk. 18.32

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