KRAI 2017 / Mbed 2 deprecated RELOADER_NASIONAL_V2

Reloader Siap. Bulat = naik lifter. Kotak = turun Lifter. Slider otomatis. Silang = pneumatik.

Dependencies:   Motor mbed millis

main.cpp@1:26fbc9316523, 2017-06-12 (annotated)

Committer:
gustavaditya
Date:
Mon Jun 12 09:22:28 2017 +0000
Revision:
1:26fbc9316523
Parent:
0:e708f9673603 
Reloader Siap

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gustavaditya 0:e708f9673603 1 #include "Motor.h"
gustavaditya 0:e708f9673603 2 #include "millis.h"
gustavaditya 0:e708f9673603 3 #include "mbed.h"
gustavaditya 0:e708f9673603 4 #include "JoystickPS3.h"
gustavaditya 0:e708f9673603 5
gustavaditya 0:e708f9673603 6 bool isReload = false;
gustavaditya 0:e708f9673603 7 bool ReloadOn = false;
gustavaditya 0:e708f9673603 8 bool flag_Pneu = false;
gustavaditya 0:e708f9673603 9 bool readySlideFromLeft = false;
gustavaditya 0:e708f9673603 10 bool readySlideFromMiddle = false;
gustavaditya 0:e708f9673603 11 bool getBack = false;
gustavaditya 0:e708f9673603 12 bool isUp = false, isDown = false, sliderOn = false;
gustavaditya 0:e708f9673603 13 bool ready = true;
gustavaditya 0:e708f9673603 14 bool init_slider = true;
gustavaditya 1:26fbc9316523 15 bool init_lifter = true;
gustavaditya 1:26fbc9316523 16 bool sliderReady = false;
gustavaditya 1:26fbc9316523 17 bool flag_tengah = true;
gustavaditya 1:26fbc9316523 18 bool delay = true;
gustavaditya 0:e708f9673603 19
gustavaditya 0:e708f9673603 20 static volatile uint32_t previousMillis3 = 0; // Pneumatik
gustavaditya 0:e708f9673603 21 static volatile uint32_t previousMillis6 = 0; // pneu
gustavaditya 1:26fbc9316523 22 static volatile uint32_t prevMillis = 0; // delay
gustavaditya 0:e708f9673603 23
gustavaditya 0:e708f9673603 24 int caseJoystick, case_joy;
gustavaditya 0:e708f9673603 25
gustavaditya 1:26fbc9316523 26 float lempar = -0.8, lempar2 = -0.8, balik = 0.6;
gustavaditya 0:e708f9673603 27
gustavaditya 0:e708f9673603 28 DigitalOut pneumatik(PA_4, PullUp);
gustavaditya 0:e708f9673603 29 DigitalIn limitAtasLifter(PB_3, PullUp); // Vertikal Atas: Lifter
gustavaditya 0:e708f9673603 30 DigitalIn limitAtasSlider(PB_2, PullUp); // Vertikal Atas: Saucer
gustavaditya 0:e708f9673603 31 DigitalIn limitBawah(PB_10, PullUp); // Vertikal Bawah
gustavaditya 0:e708f9673603 32 DigitalIn limitKiri(PA_5, PullUp); // Horizontal Kiri
gustavaditya 0:e708f9673603 33 DigitalIn limitTengah(PC_9, PullUp);// Horizontal Tengah
gustavaditya 0:e708f9673603 34 DigitalIn limitKanan(PC_8, PullUp); // Horizontal Kanan (Frisbee keluar)
gustavaditya 0:e708f9673603 35
gustavaditya 1:26fbc9316523 36 Motor lifter(PC_7, PC_13, PC_14); // pwm, fwd, rev
gustavaditya 0:e708f9673603 37 Motor slider(PA_10, PC_3, PC_0);
gustavaditya 0:e708f9673603 38
gustavaditya 0:e708f9673603 39 /* Inisialisasi Pin TX-RX Joystik dan PC */
gustavaditya 0:e708f9673603 40 joysticknucleo joystick(PA_0,PA_1);
gustavaditya 0:e708f9673603 41 Serial pc(USBTX,USBRX);
gustavaditya 0:e708f9673603 42
gustavaditya 0:e708f9673603 43 int case_joystick()
gustavaditya 0:e708f9673603 44 {
gustavaditya 0:e708f9673603 45 if (joystick.silang_click){
gustavaditya 0:e708f9673603 46 // Pnemuatik ON
gustavaditya 0:e708f9673603 47 caseJoystick = 1;
gustavaditya 0:e708f9673603 48 }
gustavaditya 0:e708f9673603 49 else if ((joystick.lingkaran_click)&&(!joystick.kotak_click)) {
gustavaditya 0:e708f9673603 50 // Lifter Up
gustavaditya 0:e708f9673603 51 caseJoystick = 2;
gustavaditya 1:26fbc9316523 52 pc.printf("LINGKARAN\n");
gustavaditya 0:e708f9673603 53 }
gustavaditya 0:e708f9673603 54 else if ((joystick.kotak_click)&&(!joystick.lingkaran_click)) {
gustavaditya 0:e708f9673603 55 // Lifter Down
gustavaditya 0:e708f9673603 56 caseJoystick = 3;
gustavaditya 1:26fbc9316523 57 pc.printf("KOTAK\n");
gustavaditya 1:26fbc9316523 58 }
gustavaditya 1:26fbc9316523 59 else{
gustavaditya 1:26fbc9316523 60 caseJoystick = 0;
gustavaditya 1:26fbc9316523 61 //pc.printf("DO NOTHING\n");
gustavaditya 0:e708f9673603 62 }
gustavaditya 0:e708f9673603 63
gustavaditya 0:e708f9673603 64 return caseJoystick;
gustavaditya 0:e708f9673603 65 }
gustavaditya 0:e708f9673603 66
gustavaditya 0:e708f9673603 67 void aktuator()
gustavaditya 0:e708f9673603 68 {
gustavaditya 0:e708f9673603 69 switch (case_joy) {
gustavaditya 0:e708f9673603 70 case (1):
gustavaditya 0:e708f9673603 71 // Pneumatik
gustavaditya 0:e708f9673603 72 if (ready)
gustavaditya 0:e708f9673603 73 {
gustavaditya 0:e708f9673603 74 pneumatik = 0;
gustavaditya 0:e708f9673603 75 previousMillis3 = millis();
gustavaditya 0:e708f9673603 76 flag_Pneu = true;
gustavaditya 0:e708f9673603 77 ready = false;
gustavaditya 0:e708f9673603 78 previousMillis6 = millis();
gustavaditya 0:e708f9673603 79 }
gustavaditya 0:e708f9673603 80 break;
gustavaditya 0:e708f9673603 81 case (2):
gustavaditya 0:e708f9673603 82 // Lifter Up
gustavaditya 0:e708f9673603 83 ReloadOn = !ReloadOn;
gustavaditya 1:26fbc9316523 84 //isUp = 1;
gustavaditya 0:e708f9673603 85 isDown = false;
gustavaditya 1:26fbc9316523 86 pc.printf("%d\n",isUp);
gustavaditya 1:26fbc9316523 87
gustavaditya 0:e708f9673603 88 break;
gustavaditya 0:e708f9673603 89 case (3):
gustavaditya 0:e708f9673603 90 // Lifter Down
gustavaditya 0:e708f9673603 91 ReloadOn = !ReloadOn;
gustavaditya 1:26fbc9316523 92 //isUp = 0;
gustavaditya 0:e708f9673603 93 isDown = true;
gustavaditya 0:e708f9673603 94 break;
gustavaditya 0:e708f9673603 95 }
gustavaditya 0:e708f9673603 96 }
gustavaditya 0:e708f9673603 97
gustavaditya 1:26fbc9316523 98 void sliderMove()
gustavaditya 1:26fbc9316523 99 {
gustavaditya 1:26fbc9316523 100 if (readySlideFromLeft)
gustavaditya 1:26fbc9316523 101 {
gustavaditya 1:26fbc9316523 102 slider.speed(lempar2);
gustavaditya 1:26fbc9316523 103 if(!limitTengah && flag_tengah)
gustavaditya 1:26fbc9316523 104 {
gustavaditya 1:26fbc9316523 105 slider.brake(1);
gustavaditya 1:26fbc9316523 106 readySlideFromLeft = false;
gustavaditya 1:26fbc9316523 107 readySlideFromMiddle = false;
gustavaditya 1:26fbc9316523 108 getBack = false;
gustavaditya 1:26fbc9316523 109 flag_tengah = false;
gustavaditya 1:26fbc9316523 110 }
gustavaditya 1:26fbc9316523 111 else { flag_tengah = true;}
gustavaditya 1:26fbc9316523 112 }
gustavaditya 1:26fbc9316523 113 else if (readySlideFromMiddle)
gustavaditya 1:26fbc9316523 114 {
gustavaditya 1:26fbc9316523 115 slider.speed(lempar);
gustavaditya 1:26fbc9316523 116 if(!limitKanan)
gustavaditya 1:26fbc9316523 117 {
gustavaditya 1:26fbc9316523 118 readySlideFromMiddle = false;
gustavaditya 1:26fbc9316523 119 getBack = true;
gustavaditya 1:26fbc9316523 120 }
gustavaditya 1:26fbc9316523 121 }
gustavaditya 1:26fbc9316523 122 else if (getBack)
gustavaditya 1:26fbc9316523 123 {
gustavaditya 1:26fbc9316523 124 slider.speed(balik);
gustavaditya 1:26fbc9316523 125 if(!limitKiri)
gustavaditya 1:26fbc9316523 126 {
gustavaditya 1:26fbc9316523 127 slider.brake(1);
gustavaditya 1:26fbc9316523 128 readySlideFromLeft = false;
gustavaditya 1:26fbc9316523 129 readySlideFromMiddle = false;
gustavaditya 1:26fbc9316523 130 getBack = false;
gustavaditya 1:26fbc9316523 131 sliderReady = false;
gustavaditya 1:26fbc9316523 132 }
gustavaditya 1:26fbc9316523 133 }
gustavaditya 1:26fbc9316523 134 else
gustavaditya 1:26fbc9316523 135 {
gustavaditya 1:26fbc9316523 136 slider.brake(1);
gustavaditya 1:26fbc9316523 137 }
gustavaditya 1:26fbc9316523 138 }
gustavaditya 1:26fbc9316523 139
gustavaditya 1:26fbc9316523 140 void lifterMove()
gustavaditya 0:e708f9673603 141 {
gustavaditya 0:e708f9673603 142 if(ReloadOn)
gustavaditya 0:e708f9673603 143 {
gustavaditya 1:26fbc9316523 144 if(isDown)
gustavaditya 0:e708f9673603 145 {
gustavaditya 0:e708f9673603 146 lifter.speed(-1.0);
gustavaditya 0:e708f9673603 147 if(!limitBawah)
gustavaditya 0:e708f9673603 148 {
gustavaditya 1:26fbc9316523 149 ReloadOn = false;
gustavaditya 1:26fbc9316523 150 isDown = false;
gustavaditya 0:e708f9673603 151 }
gustavaditya 0:e708f9673603 152 }
gustavaditya 1:26fbc9316523 153 else if (!limitAtasLifter)
gustavaditya 1:26fbc9316523 154 {
gustavaditya 1:26fbc9316523 155 isDown = true;
gustavaditya 1:26fbc9316523 156 }
gustavaditya 1:26fbc9316523 157 else if(sliderReady)
gustavaditya 1:26fbc9316523 158 {
gustavaditya 1:26fbc9316523 159 lifter.brake(1);
gustavaditya 1:26fbc9316523 160 if (!delay)
gustavaditya 1:26fbc9316523 161 {
gustavaditya 1:26fbc9316523 162 sliderMove();
gustavaditya 1:26fbc9316523 163 }
gustavaditya 1:26fbc9316523 164 }
gustavaditya 1:26fbc9316523 165 else if(!limitAtasSlider)
gustavaditya 1:26fbc9316523 166 {
gustavaditya 1:26fbc9316523 167 sliderReady = true;
gustavaditya 1:26fbc9316523 168 readySlideFromLeft = true;
gustavaditya 1:26fbc9316523 169 delay = true;
gustavaditya 1:26fbc9316523 170 prevMillis = millis();
gustavaditya 1:26fbc9316523 171 }
gustavaditya 1:26fbc9316523 172 else
gustavaditya 1:26fbc9316523 173 {
gustavaditya 1:26fbc9316523 174 lifter.speed(1.0);
gustavaditya 1:26fbc9316523 175 pc.printf("NAIK\n");
gustavaditya 1:26fbc9316523 176 }
gustavaditya 0:e708f9673603 177 }
gustavaditya 0:e708f9673603 178 else
gustavaditya 0:e708f9673603 179 {
gustavaditya 0:e708f9673603 180 lifter.brake(1);
gustavaditya 1:26fbc9316523 181 }
gustavaditya 0:e708f9673603 182 }
gustavaditya 0:e708f9673603 183
gustavaditya 0:e708f9673603 184 int main(void)
gustavaditya 0:e708f9673603 185 {
gustavaditya 1:26fbc9316523 186 while(init_slider)
gustavaditya 1:26fbc9316523 187 {
gustavaditya 1:26fbc9316523 188 slider.speed(balik);
gustavaditya 1:26fbc9316523 189 pc.printf("init_slider\n");
gustavaditya 1:26fbc9316523 190 if(!limitKiri)
gustavaditya 1:26fbc9316523 191 {
gustavaditya 1:26fbc9316523 192 init_slider = false;
gustavaditya 1:26fbc9316523 193 slider.brake(1);
gustavaditya 1:26fbc9316523 194 pc.printf("init slider selesai\n");
gustavaditya 1:26fbc9316523 195 }
gustavaditya 1:26fbc9316523 196 }
gustavaditya 1:26fbc9316523 197 while(init_lifter)
gustavaditya 1:26fbc9316523 198 {
gustavaditya 1:26fbc9316523 199 lifter.speed(-1.0);
gustavaditya 1:26fbc9316523 200 pc.printf("init_lifter\n");
gustavaditya 1:26fbc9316523 201 slider.brake(1);
gustavaditya 1:26fbc9316523 202 if(!limitBawah)
gustavaditya 1:26fbc9316523 203 {
gustavaditya 1:26fbc9316523 204 init_lifter = false;
gustavaditya 1:26fbc9316523 205 lifter.brake(1);
gustavaditya 1:26fbc9316523 206 pc.printf("init lifter selesai\n");
gustavaditya 1:26fbc9316523 207 }
gustavaditya 1:26fbc9316523 208 }
gustavaditya 0:e708f9673603 209 joystick.setup();
gustavaditya 0:e708f9673603 210 pc.baud(115200);
gustavaditya 0:e708f9673603 211 wait_ms(1000);
gustavaditya 0:e708f9673603 212 startMillis();
gustavaditya 0:e708f9673603 213 while(1)
gustavaditya 0:e708f9673603 214 {
gustavaditya 0:e708f9673603 215 //COBA ROTASI
gustavaditya 1:26fbc9316523 216 //pc.printf("MASUK PROGRAM UTAMA\n");
gustavaditya 1:26fbc9316523 217 joystick.idle();
gustavaditya 0:e708f9673603 218 if(joystick.readable())
gustavaditya 0:e708f9673603 219 {
gustavaditya 0:e708f9673603 220 // Panggil fungsi pembacaan joystik
gustavaditya 0:e708f9673603 221 joystick.baca_data();
gustavaditya 0:e708f9673603 222 // Panggil fungsi pengolahan data joystik
gustavaditya 0:e708f9673603 223 joystick.olah_data();
gustavaditya 0:e708f9673603 224 // Masuk ke case joystick
gustavaditya 0:e708f9673603 225 case_joy = case_joystick();
gustavaditya 1:26fbc9316523 226 //pc.printf("CASE JOYSTICK\n");
gustavaditya 0:e708f9673603 227 aktuator();
gustavaditya 1:26fbc9316523 228 lifterMove();
gustavaditya 0:e708f9673603 229
gustavaditya 0:e708f9673603 230 if ((millis()-previousMillis3 >= 230)&&(flag_Pneu)){
gustavaditya 0:e708f9673603 231 pneumatik = 1;
gustavaditya 0:e708f9673603 232 flag_Pneu = false;
gustavaditya 0:e708f9673603 233 ready = true;
gustavaditya 0:e708f9673603 234 readySlideFromMiddle = true;
gustavaditya 1:26fbc9316523 235 pc.printf("PNEUMATIK ON");
gustavaditya 1:26fbc9316523 236 prevMillis = millis();
gustavaditya 0:e708f9673603 237 //wait_ms(1000);
gustavaditya 0:e708f9673603 238 }
gustavaditya 1:26fbc9316523 239
gustavaditya 1:26fbc9316523 240 if((millis()-prevMillis>=500) && delay)
gustavaditya 1:26fbc9316523 241 {
gustavaditya 1:26fbc9316523 242 delay = false;
gustavaditya 1:26fbc9316523 243 //prevMillis = millis();
gustavaditya 1:26fbc9316523 244 }
gustavaditya 0:e708f9673603 245 }
gustavaditya 0:e708f9673603 246 else
gustavaditya 0:e708f9673603 247 {
gustavaditya 0:e708f9673603 248 joystick.idle();
gustavaditya 0:e708f9673603 249 }
gustavaditya 0:e708f9673603 250 }
gustavaditya 0:e708f9673603 251 }