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@42:6caf8bd5abbc, 2017-02-20 (annotated)

Committer:
franshendri
Date:
Mon Feb 20 13:29:52 2017 +0000
Revision:
42:6caf8bd5abbc
Parent:
41:336a19289c2d
Child:
43:3807a95aa284
ffdgj

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"
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;
gustavaditya 38:3ef6754bd8d8 60 const double kpA=0.6757, kdA=0.6757, 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;
gustavaditya 33:69d266bc3fe9 80 float PIVOT = 0.27; // PWM Pivot Kanan, Pivot Kiri
gustavaditya 33:69d266bc3fe9 81 float tuneDpn = 0.35; // Tunning PWM motor Depan
gustavaditya 33:69d266bc3fe9 82 float tuneBlk = 0.3; // 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){
gustavaditya 31:d5cbda07fd95 192 // Target Pulse PID -- Motor Depan
gustavaditya 31:d5cbda07fd95 193 caseJoystick = 7;
gustavaditya 31:d5cbda07fd95 194 }
gustavaditya 38:3ef6754bd8d8 195 /*else if (joystick.R3_click){
gustavaditya 31:d5cbda07fd95 196 // Target Pulse PID ++ Motor Belakang
gustavaditya 31:d5cbda07fd95 197 caseJoystick = 8;
gustavaditya 31:d5cbda07fd95 198 }
gustavaditya 38:3ef6754bd8d8 199 else if (joystick.L3_click){
gustavaditya 31:d5cbda07fd95 200 // Target Pulse PID -- Motor Belakang
gustavaditya 31:d5cbda07fd95 201 caseJoystick = 9;
gustavaditya 38:3ef6754bd8d8 202 }*/
gustavaditya 31:d5cbda07fd95 203 else if (joystick.silang_click){
gustavaditya 31:d5cbda07fd95 204 // Pnemuatik ON
gustavaditya 31:d5cbda07fd95 205 caseJoystick = 10;
gustavaditya 31:d5cbda07fd95 206 }
gustavaditya 31:d5cbda07fd95 207 else if ((joystick.atas)&&(!joystick.bawah)) {
gustavaditya 31:d5cbda07fd95 208 // Power Screw Up
gustavaditya 31:d5cbda07fd95 209 caseJoystick = 11;
gustavaditya 31:d5cbda07fd95 210 }
gustavaditya 31:d5cbda07fd95 211 else if ((!joystick.atas)&&(joystick.bawah)) {
gustavaditya 31:d5cbda07fd95 212 // Power Screw Down
gustavaditya 31:d5cbda07fd95 213 caseJoystick = 12;
gustavaditya 31:d5cbda07fd95 214 }
gustavaditya 31:d5cbda07fd95 215 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
gustavaditya 31:d5cbda07fd95 216 // Break
gustavaditya 31:d5cbda07fd95 217 caseJoystick = 13;
gustavaditya 31:d5cbda07fd95 218 }
gustavaditya 31:d5cbda07fd95 219
gustavaditya 31:d5cbda07fd95 220 return(caseJoystick);
gustavaditya 31:d5cbda07fd95 221 }
gustavaditya 31:d5cbda07fd95 222
gustavaditya 31:d5cbda07fd95 223 float getTetha(){
gustavaditya 31:d5cbda07fd95 224 // Fungsi untuk mendapatkan nilai tetha
gustavaditya 31:d5cbda07fd95 225 float busur, tetha;
gustavaditya 31:d5cbda07fd95 226 busur = ((encoderBase.getPulses())/(float)(2000.0)*keliling_enc);
gustavaditya 31:d5cbda07fd95 227 tetha = busur/keliling_robot*360;
gustavaditya 31:d5cbda07fd95 228
gustavaditya 31:d5cbda07fd95 229 return -(tetha);
gustavaditya 31:d5cbda07fd95 230 }
gustavaditya 31:d5cbda07fd95 231
be_bryan 26:256160a1a82d 232 float pidBase(float Kp, float Ki, float Kd)
Joshua23 25:054d3048dd03 233 {
Joshua23 25:054d3048dd03 234 int errorP;
Joshua23 25:054d3048dd03 235 errorP = getTetha();
gustavaditya 33:69d266bc3fe9 236 if (errorP<3.5 && errorP>(-3.5))
gustavaditya 33:69d266bc3fe9 237 errorP = 0;
Joshua23 25:054d3048dd03 238 return (float)Kp*errorP;
Joshua23 25:054d3048dd03 239 }
gustavaditya 31:d5cbda07fd95 240
gustavaditya 31:d5cbda07fd95 241 void setCenter(){
gustavaditya 31:d5cbda07fd95 242 // Fungsi untuk menentukan center dari robot
gustavaditya 31:d5cbda07fd95 243 encoderBase.reset();
fanny868 0:9072e932503c 244 }
fanny868 0:9072e932503c 245
gustavaditya 37:67d54563af90 246 void init_rpm (){
gustavaditya 38:3ef6754bd8d8 247 if (target_rpm>maxRPM-2){
gustavaditya 38:3ef6754bd8d8 248 target_rpm = maxRPM-2;
gustavaditya 37:67d54563af90 249 }
franshendri 42:6caf8bd5abbc 250 if (target_rpm<minRPM){
gustavaditya 37:67d54563af90 251 target_rpm = minRPM;
gustavaditya 37:67d54563af90 252 }
gustavaditya 37:67d54563af90 253 if (target_rpm2>maxRPM){
gustavaditya 37:67d54563af90 254 target_rpm2 = maxRPM;
gustavaditya 37:67d54563af90 255 }
franshendri 42:6caf8bd5abbc 256 if (target_rpm2<minRPM+2){
gustavaditya 38:3ef6754bd8d8 257 target_rpm2 = minRPM+2;
gustavaditya 37:67d54563af90 258 }
gustavaditya 37:67d54563af90 259 }
gustavaditya 37:67d54563af90 260
gustavaditya 31:d5cbda07fd95 261 void aktuator()
gustavaditya 31:d5cbda07fd95 262 {
gustavaditya 31:d5cbda07fd95 263 switch (case_joy) {
gustavaditya 31:d5cbda07fd95 264 case (1):
gustavaditya 31:d5cbda07fd95 265 {
gustavaditya 31:d5cbda07fd95 266 // Pivot Kanan
gustavaditya 31:d5cbda07fd95 267 motorDpn.speed(-PIVOT);
gustavaditya 31:d5cbda07fd95 268 motorBlk.speed(-PIVOT);
gustavaditya 31:d5cbda07fd95 269 setCenter();
gustavaditya 31:d5cbda07fd95 270 break;
gustavaditya 31:d5cbda07fd95 271 }
gustavaditya 31:d5cbda07fd95 272 case (2):
gustavaditya 31:d5cbda07fd95 273 {
gustavaditya 31:d5cbda07fd95 274 // Pivot Kiri
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 (3) :
gustavaditya 31:d5cbda07fd95 281 {
gustavaditya 31:d5cbda07fd95 282 // Kanan
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 //speedDpn = tuneDpn + pidBase(0.009,0,0)
gustavaditya 33:69d266bc3fe9 286 //speedBlk = tuneBlk + pidBase(0.009,0,0)
gustavaditya 33:69d266bc3fe9 287 //motorDpn.speed(-tuneDpn);
gustavaditya 33:69d266bc3fe9 288 //motorBlk.speed(tuneBlk);
gustavaditya 31:d5cbda07fd95 289 break;
gustavaditya 31:d5cbda07fd95 290 }
gustavaditya 31:d5cbda07fd95 291 case (4) :
gustavaditya 31:d5cbda07fd95 292 {
gustavaditya 31:d5cbda07fd95 293 // Kiri
gustavaditya 33:69d266bc3fe9 294 motorDpn.speed(tuneDpn + pidBase(0.009,0,0));
gustavaditya 33:69d266bc3fe9 295 motorBlk.speed(-tuneBlk + pidBase(0.009,0,0));
gustavaditya 33:69d266bc3fe9 296 //motorDpn.speed(tuneDpn);
gustavaditya 33:69d266bc3fe9 297 //motorBlk.speed(-tuneBlk);
gustavaditya 31:d5cbda07fd95 298 break;
gustavaditya 31:d5cbda07fd95 299 }
gustavaditya 31:d5cbda07fd95 300 case (5) :
gustavaditya 31:d5cbda07fd95 301 {
gustavaditya 31:d5cbda07fd95 302 // Aktifkan motor atas
gustavaditya 31:d5cbda07fd95 303 isLauncher = !isLauncher;
gustavaditya 31:d5cbda07fd95 304 break;
Joshua23 8:0711dea61312 305 }
gustavaditya 31:d5cbda07fd95 306 case (6) :
gustavaditya 31:d5cbda07fd95 307 {
gustavaditya 31:d5cbda07fd95 308 // Target Pulse PID ++ Motor Depan
gustavaditya 39:11358f3f61ff 309 target_rpm2 = target_rpm2+1.0;
gustavaditya 39:11358f3f61ff 310 target_rpm = target_rpm+1.0;
gustavaditya 37:67d54563af90 311 init_rpm();
gustavaditya 31:d5cbda07fd95 312 break;
gustavaditya 31:d5cbda07fd95 313 }
gustavaditya 31:d5cbda07fd95 314 case (7) :
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 38:3ef6754bd8d8 322 /*case (8) :
gustavaditya 31:d5cbda07fd95 323 {
gustavaditya 32:581d4a2373f0 324 // Target Pulse PID ++ Motor Belakang=
gustavaditya 38:3ef6754bd8d8 325 //init_rpm();
gustavaditya 31:d5cbda07fd95 326 break;
gustavaditya 31:d5cbda07fd95 327 }
gustavaditya 31:d5cbda07fd95 328 case (9) :
be_bryan 27:68efb1622985 329 {
gustavaditya 31:d5cbda07fd95 330 // Target Pulse PID -- Motor Belakang
gustavaditya 38:3ef6754bd8d8 331 //init_rpm();
gustavaditya 31:d5cbda07fd95 332 break;
gustavaditya 38:3ef6754bd8d8 333 }*/
gustavaditya 31:d5cbda07fd95 334 case (10) :
gustavaditya 31:d5cbda07fd95 335 {
gustavaditya 31:d5cbda07fd95 336 // Pneumatik
gustavaditya 31:d5cbda07fd95 337 pneumatik = 0;
gustavaditya 31:d5cbda07fd95 338 previousMillis3 = millis();
gustavaditya 31:d5cbda07fd95 339 flag_Pneu = true;
gustavaditya 31:d5cbda07fd95 340 break;
gustavaditya 31:d5cbda07fd95 341 }
gustavaditya 31:d5cbda07fd95 342 case (11) :
gustavaditya 31:d5cbda07fd95 343 {
gustavaditya 31:d5cbda07fd95 344 // Power Screw Up
rizqicahyo 36:5963c9a49485 345 //powerScrew.speed(pwmPowerUp);
gustavaditya 40:5b937cac959a 346 ReloadOn = !ReloadOn;
gustavaditya 40:5b937cac959a 347 //powerScrew.speed(pwmPowerUp);
gustavaditya 31:d5cbda07fd95 348 break;
gustavaditya 31:d5cbda07fd95 349 }
gustavaditya 31:d5cbda07fd95 350 case (12) :
gustavaditya 31:d5cbda07fd95 351 {
gustavaditya 31:d5cbda07fd95 352 // Power Screw Down
rizqicahyo 36:5963c9a49485 353 //powerScrew.speed(pwmPowerDown);
gustavaditya 31:d5cbda07fd95 354 break;
gustavaditya 31:d5cbda07fd95 355 }
gustavaditya 31:d5cbda07fd95 356 default :
gustavaditya 31:d5cbda07fd95 357 {
gustavaditya 31:d5cbda07fd95 358 motorDpn.brake(1);
gustavaditya 31:d5cbda07fd95 359 motorBlk.brake(1);
gustavaditya 40:5b937cac959a 360 /* if(isReload){
gustavaditya 39:11358f3f61ff 361 powerScrew.speed(pwmPowerDown);
gustavaditya 39:11358f3f61ff 362 if(!limitBawah){
gustavaditya 39:11358f3f61ff 363 isReload = false;
gustavaditya 39:11358f3f61ff 364 ReloadOn = false;
gustavaditya 39:11358f3f61ff 365 }
gustavaditya 39:11358f3f61ff 366 }
gustavaditya 39:11358f3f61ff 367 else if(!limitTengah){
gustavaditya 39:11358f3f61ff 368 isReload = true;
gustavaditya 39:11358f3f61ff 369 }
gustavaditya 39:11358f3f61ff 370 else{
gustavaditya 31:d5cbda07fd95 371 powerScrew.brake(1);
gustavaditya 40:5b937cac959a 372 }*/
gustavaditya 31:d5cbda07fd95 373 }
gustavaditya 31:d5cbda07fd95 374 } // End Switch
gustavaditya 31:d5cbda07fd95 375 }
gustavaditya 31:d5cbda07fd95 376
gustavaditya 40:5b937cac959a 377 void reloader()
rizqicahyo 36:5963c9a49485 378 {
rizqicahyo 36:5963c9a49485 379 if(ReloadOn){
rizqicahyo 36:5963c9a49485 380 if(isReload){
rizqicahyo 36:5963c9a49485 381 powerScrew.speed(pwmPowerDown);
rizqicahyo 36:5963c9a49485 382 if(!limitBawah){
rizqicahyo 36:5963c9a49485 383 isReload = false;
rizqicahyo 36:5963c9a49485 384 ReloadOn = false;
rizqicahyo 36:5963c9a49485 385 }
rizqicahyo 36:5963c9a49485 386 }
rizqicahyo 36:5963c9a49485 387 else if(!limitTengah){
rizqicahyo 36:5963c9a49485 388 isReload = true;
rizqicahyo 36:5963c9a49485 389 }
gustavaditya 41:336a19289c2d 390 else if((jarak_ping > 4) && !flag_Pneu){
gustavaditya 40:5b937cac959a 391 powerScrew.speed(pwmPowerUp);
gustavaditya 40:5b937cac959a 392 }
gustavaditya 41:336a19289c2d 393 else if((jarak_ping < 3.5 ) && !flag_Pneu) {
gustavaditya 41:336a19289c2d 394 powerScrew.speed(-0.1);
rizqicahyo 36:5963c9a49485 395 }
rizqicahyo 36:5963c9a49485 396 else{
gustavaditya 40:5b937cac959a 397 powerScrew.brake(1);
rizqicahyo 36:5963c9a49485 398 }
rizqicahyo 36:5963c9a49485 399 }
rizqicahyo 36:5963c9a49485 400 else{
rizqicahyo 36:5963c9a49485 401 powerScrew.brake(1);
rizqicahyo 36:5963c9a49485 402 }
gustavaditya 40:5b937cac959a 403 }
rizqicahyo 36:5963c9a49485 404
rizqicahyo 36:5963c9a49485 405
gustavaditya 31:d5cbda07fd95 406 void launcher()
gustavaditya 31:d5cbda07fd95 407 {
gustavaditya 31:d5cbda07fd95 408 if (isLauncher)
Sufa 30:d69cc27ac644 409 {
be_bryan 28:2d0746dc2d7d 410 currentMillis = millis();
be_bryan 28:2d0746dc2d7d 411 currentMillis2 = millis();
be_bryan 28:2d0746dc2d7d 412
gustavaditya 31:d5cbda07fd95 413 // PID Launcher Depan
gustavaditya 38:3ef6754bd8d8 414 if (currentMillis-previousMillis>=12.5)
be_bryan 26:256160a1a82d 415 {
gustavaditya 31:d5cbda07fd95 416 rpm = (float)encLauncherBlk.getPulses();
be_bryan 26:256160a1a82d 417 current_error = target_rpm - rpm;
be_bryan 26:256160a1a82d 418 sum_error = sum_error + current_error;
be_bryan 26:256160a1a82d 419 p = current_error*kpA;
gustavaditya 38:3ef6754bd8d8 420 d = (current_error-last_error)*kdA/12.5;
gustavaditya 38:3ef6754bd8d8 421 i = sum_error*kiA*12.5;
be_bryan 26:256160a1a82d 422 speed = p + d + i;
gustavaditya 38:3ef6754bd8d8 423 //init_speed();
gustavaditya 38:3ef6754bd8d8 424 if(speed > maxSpeed){
gustavaditya 38:3ef6754bd8d8 425 launcherBlk.speed(maxSpeed);
gustavaditya 38:3ef6754bd8d8 426 }
gustavaditya 39:11358f3f61ff 427 else if ( speed < minSpeed){
gustavaditya 39:11358f3f61ff 428 launcherBlk.speed(minSpeed);
gustavaditya 39:11358f3f61ff 429 }
gustavaditya 38:3ef6754bd8d8 430 else {
gustavaditya 38:3ef6754bd8d8 431 launcherBlk.speed(speed);
gustavaditya 38:3ef6754bd8d8 432 }
be_bryan 26:256160a1a82d 433 last_error = current_error;
gustavaditya 31:d5cbda07fd95 434 encLauncherBlk.reset();
be_bryan 26:256160a1a82d 435 //pc.printf("%.04lf\n",rpm);
be_bryan 26:256160a1a82d 436 previousMillis = currentMillis;
be_bryan 26:256160a1a82d 437 }
gustavaditya 38:3ef6754bd8d8 438 if (currentMillis2-previousMillis2>=12.5)
be_bryan 27:68efb1622985 439 {
gustavaditya 31:d5cbda07fd95 440 rpm2 = (float)encLauncherDpn.getPulses();
be_bryan 27:68efb1622985 441 current_error2 = target_rpm2 - rpm2;
be_bryan 27:68efb1622985 442 sum_error2 = sum_error2 + current_error2;
be_bryan 27:68efb1622985 443 p2 = current_error2*kpA;
gustavaditya 38:3ef6754bd8d8 444 d2 = (current_error2-last_error2)*kdA/12.5;
gustavaditya 38:3ef6754bd8d8 445 i2 = sum_error2*kiA*12.5;
be_bryan 27:68efb1622985 446 speed2 = p2 + d2 + i2;
gustavaditya 38:3ef6754bd8d8 447 //init_speed();
gustavaditya 38:3ef6754bd8d8 448 if (speed2 > maxSpeed){
gustavaditya 38:3ef6754bd8d8 449 launcherDpn.speed(maxSpeed);
gustavaditya 38:3ef6754bd8d8 450 }
gustavaditya 39:11358f3f61ff 451 else if ( speed < minSpeed){
gustavaditya 39:11358f3f61ff 452 launcherDpn.speed(minSpeed);
gustavaditya 39:11358f3f61ff 453 }
gustavaditya 38:3ef6754bd8d8 454 else{
gustavaditya 38:3ef6754bd8d8 455 launcherDpn.speed(speed2);
gustavaditya 38:3ef6754bd8d8 456 }
be_bryan 27:68efb1622985 457 last_error2 = current_error2;
gustavaditya 31:d5cbda07fd95 458 encLauncherDpn.reset();
be_bryan 27:68efb1622985 459 previousMillis2 = currentMillis2;
be_bryan 27:68efb1622985 460 }
rahmadirizki18 6:68293bed71ea 461 }
Sufa 29:7b372b0aaa61 462 else
Sufa 29:7b372b0aaa61 463 {
gustavaditya 31:d5cbda07fd95 464 launcherDpn.brake(1);
gustavaditya 31:d5cbda07fd95 465 launcherBlk.brake(1);
gustavaditya 31:d5cbda07fd95 466 }
rahmadirizki18 5:3aa203218306 467 }
gustavaditya 31:d5cbda07fd95 468
MarchioKevin 22:4632f58461e0 469 /*********************************************************/
MarchioKevin 22:4632f58461e0 470 /* Main Function */
MarchioKevin 22:4632f58461e0 471 /*********************************************************/
calmantara186 16:90119f03c5d1 472
gustavaditya 31:d5cbda07fd95 473 int main (void)
gustavaditya 31:d5cbda07fd95 474 {
gustavaditya 31:d5cbda07fd95 475 // Set baud rate - 115200
fanny868 0:9072e932503c 476 joystick.setup();
rahmadirizki18 23:023b522977b2 477 pc.baud(115200);
rahmadirizki18 6:68293bed71ea 478 wait_ms(1000);
gustavaditya 40:5b937cac959a 479
gustavaditya 40:5b937cac959a 480 // initializing encoder
gustavaditya 41:336a19289c2d 481 pneumatik =1;
gustavaditya 41:336a19289c2d 482
rahmadirizki18 5:3aa203218306 483 setCenter();
gustavaditya 41:336a19289c2d 484
rahmadirizki18 5:3aa203218306 485 wait_ms(500);
gustavaditya 40:5b937cac959a 486
gustavaditya 40:5b937cac959a 487 //initializing PING
gustavaditya 40:5b937cac959a 488 pingAtas.Send();
gustavaditya 40:5b937cac959a 489
rahmadirizki18 23:023b522977b2 490 pc.printf("ready....");
gustavaditya 35:69a47b4cb3fc 491 startMillis();
fanny868 0:9072e932503c 492 while(1)
gustavaditya 40:5b937cac959a 493 {
gustavaditya 40:5b937cac959a 494 // interupsi pembacaan PING setiap 30 ms
gustavaditya 41:336a19289c2d 495 if(millis() - previousMillis4 >= 5){ //30
gustavaditya 41:336a19289c2d 496 jarak_ping = (float)pingAtas.Read_cm()/2;
gustavaditya 40:5b937cac959a 497
gustavaditya 40:5b937cac959a 498 pingAtas.Send();
gustavaditya 40:5b937cac959a 499 previousMillis4 = millis();
gustavaditya 40:5b937cac959a 500 }
gustavaditya 40:5b937cac959a 501
fanny868 0:9072e932503c 502 // Interrupt Serial
calmantara186 16:90119f03c5d1 503 joystick.idle();
gustavaditya 31:d5cbda07fd95 504 if(joystick.readable())
gustavaditya 31:d5cbda07fd95 505 {
fanny868 0:9072e932503c 506 // Panggil fungsi pembacaan joystik
fanny868 0:9072e932503c 507 joystick.baca_data();
fanny868 0:9072e932503c 508 // Panggil fungsi pengolahan data joystik
fanny868 0:9072e932503c 509 joystick.olah_data();
gustavaditya 31:d5cbda07fd95 510 // Masuk ke case joystick
gustavaditya 31:d5cbda07fd95 511 case_joy = case_joystick();
rahmadirizki18 3:1287fccc11be 512 aktuator();
gustavaditya 31:d5cbda07fd95 513 launcher();
gustavaditya 40:5b937cac959a 514 reloader();
gustavaditya 41:336a19289c2d 515 if ((millis()-previousMillis3 >= 370)&&(flag_Pneu)){
be_bryan 26:256160a1a82d 516 pneumatik = 1;
gustavaditya 31:d5cbda07fd95 517 flag_Pneu = false;
be_bryan 26:256160a1a82d 518 }
be_bryan 26:256160a1a82d 519 }
gustavaditya 31:d5cbda07fd95 520 else
gustavaditya 31:d5cbda07fd95 521 {
gustavaditya 31:d5cbda07fd95 522 joystick.idle();
MarchioKevin 21:da2f3d04468f 523 }
franshendri 42:6caf8bd5abbc 524
franshendri 42:6caf8bd5abbc 525 if(millis() - previousMillis5 >= 400){
franshendri 42:6caf8bd5abbc 526 display.write(0,((int) target_rpm2-2) / 10);
franshendri 42:6caf8bd5abbc 527 display.write(1,((int)target_rpm2-2) % 10);
franshendri 42:6caf8bd5abbc 528 display.write(2, (int)target_rpm2 / 10);
franshendri 42:6caf8bd5abbc 529 display.write(3, (int)target_rpm2 % 10);
franshendri 42:6caf8bd5abbc 530 display.setColon(true);
franshendri 42:6caf8bd5abbc 531
franshendri 42:6caf8bd5abbc 532 previousMillis5 = millis();
franshendri 42:6caf8bd5abbc 533 }
fanny868 0:9072e932503c 534 }
be_bryan 28:2d0746dc2d7d 535 }