5 april ganti kecepatan power screw

Dependencies:   DigitDisplay Motor PID Ping mbed millis

Fork of MainProgram_BaseBaru_fix_omni12Feb by KRAI 2017

Committer:
fanny868
Date:
Sat Jun 27 07:37:28 2015 +0000
Revision:
2:ac21a024ac80
Parent:
0:9072e932503c
Child:
12:e07c59c28c29
.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
fanny868 0:9072e932503c 1 /*********************************************************************************************/
fanny868 0:9072e932503c 2 /** **/
fanny868 0:9072e932503c 3 /** PROGRAM KOMUNIKASI DATA JOYSTIK PS3 **/
fanny868 0:9072e932503c 4 /** **/
fanny868 0:9072e932503c 5 /** Joystik PS3 -> Arduino -> STM32Nucleo **/
fanny868 0:9072e932503c 6 /** **/
fanny868 0:9072e932503c 7 /** Fanny Achmad Hindrarta **/
fanny868 0:9072e932503c 8 /** EL'12 - 13212076 **/
fanny868 0:9072e932503c 9 /** **/
fanny868 2:ac21a024ac80 10 /** Last Update : 01 Februar1 2015, 20.30 **/
fanny868 0:9072e932503c 11 /*********************************************************************************************/
fanny868 0:9072e932503c 12
fanny868 0:9072e932503c 13 #include <PS3BT.h>
fanny868 0:9072e932503c 14 #include <usbhub.h>
fanny868 2:ac21a024ac80 15 //#include <PS3USB.h>
fanny868 0:9072e932503c 16 #include "Arduino.h"
fanny868 0:9072e932503c 17 // Satisfy IDE, which only needs to see the include statment in the ino.
fanny868 0:9072e932503c 18 #ifdef dobogusinclude
fanny868 0:9072e932503c 19 #include <spi4teensy3.h>
fanny868 0:9072e932503c 20 #endif
fanny868 0:9072e932503c 21
fanny868 2:ac21a024ac80 22
fanny868 0:9072e932503c 23 USB Usb;
fanny868 2:ac21a024ac80 24
fanny868 2:ac21a024ac80 25 // Bluetooth
fanny868 2:ac21a024ac80 26 // You can create the instance of the class in two ways
fanny868 0:9072e932503c 27 BTD Btd(&Usb); // You have to create the Bluetooth Dongle instance like so
fanny868 2:ac21a024ac80 28 //You can create the instance of the class in two ways
fanny868 0:9072e932503c 29 PS3BT PS3(&Btd); // This will just create the instance
fanny868 0:9072e932503c 30 //PS3BT PS3(&Btd, 0x00, 0x15, 0x83, 0x3D, 0x0A, 0x57); // This will also store the bluetooth address - this can be obtained from the dongle when running the sketch
fanny868 2:ac21a024ac80 31 /*
fanny868 2:ac21a024ac80 32 // USB
fanny868 2:ac21a024ac80 33 // You can create the instance of the class in two ways
fanny868 2:ac21a024ac80 34 PS3USB PS3(&Usb); // This will just create the instance
fanny868 2:ac21a024ac80 35 //PS3USB PS3(&Btd, 0x00, 0x15, 0x83, 0x3D, 0x0A, 0x57); // This will also store the bluetooth address - this can be obtained from the dongle when running the sketch
fanny868 2:ac21a024ac80 36 */
fanny868 0:9072e932503c 37
fanny868 0:9072e932503c 38 boolean printAngle;
fanny868 0:9072e932503c 39 uint8_t state = 0;
fanny868 0:9072e932503c 40
fanny868 0:9072e932503c 41 // Deklarasi tombol
fanny868 0:9072e932503c 42 boolean kiri_click=0, kanan_click=0, atas_click=0, bawah_click=0;
fanny868 0:9072e932503c 43 boolean segitiga_click=0, lingkaran_click=0, kotak_click=0, silang_click=0;
fanny868 0:9072e932503c 44 boolean L1_click=0, R1_click=0, L3_click=0, R3_click=0;
fanny868 0:9072e932503c 45 boolean START_click=0, SELECT_click=0, PS_click=0;
fanny868 0:9072e932503c 46
fanny868 0:9072e932503c 47 // Deklarasi variabel tombol analog
fanny868 0:9072e932503c 48 unsigned char LX, LY, RX, RY, aL2, aR2;
fanny868 0:9072e932503c 49
fanny868 0:9072e932503c 50 // Deklarasi varibel data yang dikirim
fanny868 0:9072e932503c 51 unsigned char button;
fanny868 0:9072e932503c 52 unsigned char RL;
fanny868 0:9072e932503c 53 unsigned int button_click;
fanny868 0:9072e932503c 54 unsigned int RL_click;
fanny868 0:9072e932503c 55
fanny868 0:9072e932503c 56 void setup_joystik()
fanny868 0:9072e932503c 57 {
fanny868 2:ac21a024ac80 58 while (!Serial) // Wait for serial port to connect - used on Leonardo, Teensy and other boards with built-in USB CDC serial connection
fanny868 2:ac21a024ac80 59 {
fanny868 2:ac21a024ac80 60 Serial.write(0x88);
fanny868 2:ac21a024ac80 61 Serial.write(0x09);
fanny868 2:ac21a024ac80 62 }
fanny868 0:9072e932503c 63 if (Usb.Init() == -1) {
fanny868 2:ac21a024ac80 64 //Serial.print(F("\r\nOSC did not start"));
fanny868 2:ac21a024ac80 65 while (1) //halt
fanny868 2:ac21a024ac80 66 {
fanny868 2:ac21a024ac80 67 Serial.write(0x88);
fanny868 2:ac21a024ac80 68 Serial.write(0x09);
fanny868 2:ac21a024ac80 69 }
fanny868 0:9072e932503c 70 }
fanny868 2:ac21a024ac80 71 //Serial.print(F("\r\nPS3 USB Library Started"));
fanny868 0:9072e932503c 72 }
fanny868 0:9072e932503c 73
fanny868 0:9072e932503c 74 /*********************************************************************************************/
fanny868 0:9072e932503c 75 /** **/
fanny868 0:9072e932503c 76 /** FUNGSI PENGIRIMAN DATA **/
fanny868 0:9072e932503c 77 /** - Data yang akan dikirim adalah paket data 8-bit dengan urutan sebagai berikut **/
fanny868 0:9072e932503c 78 /** |------|------|--------|----|--------------|----------|----|----|----|----|----|----| **/
fanny868 0:9072e932503c 79 /** | 0x88 | 0x08 | button | RL | button_click | RL_click | R2 | L2 | RX | RY | LX | LY | **/
fanny868 0:9072e932503c 80 /** |------|------|--------|----|--------------|----------|----|----|----|----|----|----| **/
fanny868 0:9072e932503c 81 /** **/
fanny868 0:9072e932503c 82 /*********************************************************************************************/
fanny868 0:9072e932503c 83
fanny868 0:9072e932503c 84 void kirimdatajoystik()
fanny868 0:9072e932503c 85 {
fanny868 0:9072e932503c 86
fanny868 0:9072e932503c 87 Serial.write(0x88);
fanny868 0:9072e932503c 88 Serial.write(0x08);
fanny868 0:9072e932503c 89 Serial.write(button);
fanny868 0:9072e932503c 90 Serial.write(RL);
fanny868 0:9072e932503c 91 Serial.write(button_click);
fanny868 0:9072e932503c 92 Serial.write(RL_click);
fanny868 0:9072e932503c 93 Serial.write(aR2);
fanny868 0:9072e932503c 94 Serial.write(aL2);
fanny868 0:9072e932503c 95 Serial.write(RX);
fanny868 0:9072e932503c 96 Serial.write(RY);
fanny868 0:9072e932503c 97 Serial.write(LX);
fanny868 0:9072e932503c 98 Serial.write(LY);
fanny868 2:ac21a024ac80 99
fanny868 0:9072e932503c 100 // Debug
fanny868 0:9072e932503c 101
fanny868 0:9072e932503c 102 /*
fanny868 0:9072e932503c 103 Serial.print(millis());
fanny868 0:9072e932503c 104 Serial.print("\t");
fanny868 0:9072e932503c 105 Serial.print(button);
fanny868 0:9072e932503c 106 Serial.print("\t");
fanny868 0:9072e932503c 107 Serial.print(RL);
fanny868 0:9072e932503c 108 Serial.print("\t");
fanny868 0:9072e932503c 109 Serial.print(button_click);
fanny868 0:9072e932503c 110 Serial.print("\t");
fanny868 0:9072e932503c 111 Serial.print(RL_click);
fanny868 0:9072e932503c 112 Serial.print("\t");
fanny868 0:9072e932503c 113 Serial.print(aR2);
fanny868 0:9072e932503c 114 Serial.print("\t");
fanny868 0:9072e932503c 115 Serial.print(aL2);
fanny868 0:9072e932503c 116 Serial.print("\t");
fanny868 0:9072e932503c 117 Serial.print(RX);
fanny868 0:9072e932503c 118 Serial.print("\t");
fanny868 0:9072e932503c 119 Serial.print(RY);
fanny868 0:9072e932503c 120 Serial.print("\t");
fanny868 0:9072e932503c 121 Serial.print(LX);
fanny868 0:9072e932503c 122 Serial.print("\t");
fanny868 0:9072e932503c 123 Serial.println(LY);
fanny868 0:9072e932503c 124 */
fanny868 0:9072e932503c 125 }
fanny868 0:9072e932503c 126
fanny868 0:9072e932503c 127 /*****************************************************************************************/
fanny868 0:9072e932503c 128 /** SETUP REGISTER dan INISIALISASI **/
fanny868 0:9072e932503c 129 /** - Setup Joystik **/
fanny868 0:9072e932503c 130 /** - Baud Rate Serial 115200, 8-bit, 1 stop, 0 parity **/
fanny868 0:9072e932503c 131 /*****************************************************************************************/
fanny868 0:9072e932503c 132
fanny868 0:9072e932503c 133 void setup() {
fanny868 0:9072e932503c 134 Serial.begin(115200);
fanny868 0:9072e932503c 135
fanny868 0:9072e932503c 136 setup_joystik();
fanny868 0:9072e932503c 137 }
fanny868 0:9072e932503c 138
fanny868 0:9072e932503c 139
fanny868 0:9072e932503c 140 /*****************************************************************************************/
fanny868 0:9072e932503c 141 /** **/
fanny868 0:9072e932503c 142 /** FUNGSI PEMBACAAN DATA JOYSTIK **/
fanny868 0:9072e932503c 143 /** - Data dari Joystik dikirim melalui Bluetooth. **/
fanny868 0:9072e932503c 144 /** Lihat Library "PS3BT.h" **/
fanny868 0:9072e932503c 145 /** - Data tombol dari joystik adalah data 1-bit. Pengiriman data melalui USART **/
fanny868 0:9072e932503c 146 /** adalah 8-bit. Agar pengiriman data efisien, maka data 1-bit digabungkan **/
fanny868 0:9072e932503c 147 /** agar terbentuk data 8-bit **/
fanny868 0:9072e932503c 148 /** - Penggabungan data segitiga, longkaran, silang, kotak, arah atas, arah kanan, **/
fanny868 0:9072e932503c 149 /** arah bawah, arah kiri disimpan dalam variabel "button" **/
fanny868 0:9072e932503c 150 /** - Urutan data pada variabel "button" dan "button_click" **/
fanny868 0:9072e932503c 151 /** adalah sebagai berikut **/
fanny868 0:9072e932503c 152 /** **/
fanny868 0:9072e932503c 153 /** |------|-------|-------|------|-------|--------|-----------|----------| **/
fanny868 0:9072e932503c 154 /** Bit Ke | 7 | 6 | 5 | 4 | 3 | 2 | 1 | 0 | **/
fanny868 0:9072e932503c 155 /** |------|-------|-------|------|-------|--------|-----------|----------| **/
fanny868 0:9072e932503c 156 /** Data | kiri | bawah | kanan | atas | kotak | silang | lingkaran | segitiga | **/
fanny868 0:9072e932503c 157 /** |------|-------|-------|------|-------|--------|-----------|----------| **/
fanny868 0:9072e932503c 158 /** **/
fanny868 0:9072e932503c 159 /** - Penggabungan data R1, R2, L1, L2, R3, L3, START, dan SELECT disimpan dalam **/
fanny868 0:9072e932503c 160 /** variabel "RL" **/
fanny868 0:9072e932503c 161 /** - Urutan data pada variabel "RL" dan "RL_click" adalah **/
fanny868 0:9072e932503c 162 /** sebagai berikut **/
fanny868 0:9072e932503c 163 /** **/
fanny868 0:9072e932503c 164 /** |----|--------|-------|----|----|----|----| **/
fanny868 0:9072e932503c 165 /** Bit Ke | 6 | 5 | 4 | 3 | 2 | 1 | 0 | **/
fanny868 0:9072e932503c 166 /** |----|--------|-------|----|----|----|----| **/
fanny868 0:9072e932503c 167 /** Data | PS | SELECT | START | L3 | L1 | R3 | R1 | **/
fanny868 0:9072e932503c 168 /** |----|--------|-------|----|----|----|----| **/
fanny868 0:9072e932503c 169 /** **/
fanny868 0:9072e932503c 170 /*****************************************************************************************/
fanny868 0:9072e932503c 171
fanny868 0:9072e932503c 172 void loop() {
fanny868 0:9072e932503c 173 button = 0;
fanny868 0:9072e932503c 174 RL = 0;
fanny868 2:ac21a024ac80 175 button_click=0;
fanny868 2:ac21a024ac80 176 RL_click=0;
fanny868 0:9072e932503c 177 Usb.Task();
fanny868 0:9072e932503c 178
fanny868 0:9072e932503c 179 // Pembacaan data joystik dilakukan jika PS3 tersambung ke Arduino
fanny868 0:9072e932503c 180 if (PS3.PS3Connected || PS3.PS3NavigationConnected) {
fanny868 0:9072e932503c 181 // Pembacaan dan penggabungan data segitiga, lingkaran, silang, kotak, atas, kanan, bawah, kiri
fanny868 0:9072e932503c 182 // Data bernilai '1' jika tombol ditekan
fanny868 0:9072e932503c 183 if(PS3.getButtonPress(TRIANGLE)){
fanny868 0:9072e932503c 184 button = button + (0x1 << 0);
fanny868 0:9072e932503c 185 }
fanny868 0:9072e932503c 186 if(PS3.getButtonPress(CIRCLE)){
fanny868 0:9072e932503c 187 button = button + (0x1 << 1);
fanny868 0:9072e932503c 188 }
fanny868 0:9072e932503c 189 if(PS3.getButtonPress(CROSS)){
fanny868 0:9072e932503c 190 button = button + (0x1 << 2);
fanny868 0:9072e932503c 191 }
fanny868 0:9072e932503c 192 if(PS3.getButtonPress(SQUARE)){
fanny868 0:9072e932503c 193 button = button + (0x1 << 3);
fanny868 0:9072e932503c 194 }
fanny868 0:9072e932503c 195 if(PS3.getButtonPress(UP)){
fanny868 0:9072e932503c 196 button = button + (0x1 << 4);
fanny868 0:9072e932503c 197 }
fanny868 0:9072e932503c 198 if(PS3.getButtonPress(RIGHT)){
fanny868 0:9072e932503c 199 button = button + (0x1 << 5);
fanny868 0:9072e932503c 200 }
fanny868 0:9072e932503c 201 if(PS3.getButtonPress(DOWN)){
fanny868 0:9072e932503c 202 button = button + (0x1 << 6);
fanny868 0:9072e932503c 203 }
fanny868 0:9072e932503c 204 if(PS3.getButtonPress(LEFT)){
fanny868 0:9072e932503c 205 button = button + (0x1 << 7);
fanny868 0:9072e932503c 206 }
fanny868 0:9072e932503c 207
fanny868 0:9072e932503c 208 // Pembacaan dan penggabungan data R1, R3, L1, L3, START, SELECT, dan PS
fanny868 0:9072e932503c 209 // Data bernilai '1' jika tombol ditekan
fanny868 0:9072e932503c 210 if(PS3.getButtonPress(R1)){
fanny868 0:9072e932503c 211 RL = RL + (0x1 << 0);
fanny868 0:9072e932503c 212 }
fanny868 0:9072e932503c 213 if(PS3.getButtonPress(R3)){
fanny868 0:9072e932503c 214 RL = RL + (0x1 << 1);
fanny868 0:9072e932503c 215 }
fanny868 0:9072e932503c 216 if(PS3.getButtonPress(L1)){
fanny868 0:9072e932503c 217 RL = RL + (0x1 << 2);
fanny868 0:9072e932503c 218 }
fanny868 0:9072e932503c 219 if(PS3.getButtonPress(L3)){
fanny868 0:9072e932503c 220 RL = RL + (0x1 << 3);
fanny868 0:9072e932503c 221 }
fanny868 0:9072e932503c 222 if(PS3.getButtonPress(START)){
fanny868 0:9072e932503c 223 RL = RL + (0x1 << 4);
fanny868 0:9072e932503c 224 }
fanny868 0:9072e932503c 225 if(PS3.getButtonPress(SELECT)){
fanny868 0:9072e932503c 226 RL = RL + (0x1 << 5);
fanny868 0:9072e932503c 227 }
fanny868 0:9072e932503c 228 if(PS3.getButtonPress(PS)){
fanny868 0:9072e932503c 229 RL = RL + (0x1 << 6);
fanny868 0:9072e932503c 230 }
fanny868 0:9072e932503c 231
fanny868 0:9072e932503c 232 // Pembacaan dan penggabungan data segitiga, lingkaran, silang, kotak, atas, kanan, bawah, kiri
fanny868 0:9072e932503c 233 // Data bernilai '1' hanya saat tombol pertama kali ditekan
fanny868 0:9072e932503c 234 if(PS3.getButtonClick(TRIANGLE)){
fanny868 2:ac21a024ac80 235 button_click = button_click + (0x1 << 0);
fanny868 0:9072e932503c 236 }
fanny868 0:9072e932503c 237 if(PS3.getButtonClick(CIRCLE)){
fanny868 2:ac21a024ac80 238 button_click = button_click + (0x1 << 1);
fanny868 0:9072e932503c 239 }
fanny868 0:9072e932503c 240 if(PS3.getButtonClick(CROSS)){
fanny868 2:ac21a024ac80 241 button_click = button_click + (0x1 << 2);
fanny868 0:9072e932503c 242 }
fanny868 0:9072e932503c 243 if(PS3.getButtonClick(SQUARE)){
fanny868 2:ac21a024ac80 244 button_click = button_click + (0x1 << 3);
fanny868 0:9072e932503c 245 }
fanny868 0:9072e932503c 246 if(PS3.getButtonClick(UP)){
fanny868 2:ac21a024ac80 247 button_click = button_click + (0x1 << 4);
fanny868 0:9072e932503c 248 }
fanny868 0:9072e932503c 249 if(PS3.getButtonClick(RIGHT)){
fanny868 2:ac21a024ac80 250 button_click = button_click + (0x1 << 5);
fanny868 0:9072e932503c 251 }
fanny868 0:9072e932503c 252 if(PS3.getButtonClick(DOWN)){
fanny868 2:ac21a024ac80 253 button_click = button_click + (0x1 << 6);
fanny868 0:9072e932503c 254 }
fanny868 0:9072e932503c 255 if(PS3.getButtonClick(LEFT)){
fanny868 2:ac21a024ac80 256 button_click = button_click + (0x1 << 7);
fanny868 0:9072e932503c 257 }
fanny868 0:9072e932503c 258
fanny868 0:9072e932503c 259 if(PS3.getButtonClick(R1)){
fanny868 2:ac21a024ac80 260 RL_click = RL_click + (0x1 << 0);
fanny868 0:9072e932503c 261 }
fanny868 0:9072e932503c 262 if(PS3.getButtonClick(R3)){
fanny868 2:ac21a024ac80 263 RL_click = RL_click + (0x1 << 1);
fanny868 0:9072e932503c 264 }
fanny868 0:9072e932503c 265 if(PS3.getButtonClick(L1)){
fanny868 2:ac21a024ac80 266 RL_click = RL_click + (0x1 << 2);
fanny868 0:9072e932503c 267 }
fanny868 0:9072e932503c 268 if(PS3.getButtonClick(L3)){
fanny868 2:ac21a024ac80 269 RL_click = RL_click + (0x1 << 3);
fanny868 0:9072e932503c 270 }
fanny868 0:9072e932503c 271 if(PS3.getButtonClick(START)){
fanny868 2:ac21a024ac80 272 RL_click = RL_click + (0x1 << 4);
fanny868 0:9072e932503c 273 }
fanny868 0:9072e932503c 274 if(PS3.getButtonClick(SELECT)){
fanny868 2:ac21a024ac80 275 RL_click = RL_click + (0x1 << 5);
fanny868 0:9072e932503c 276 }
fanny868 0:9072e932503c 277 if(PS3.getButtonClick(PS)){
fanny868 2:ac21a024ac80 278 RL_click = RL_click + (0x1 << 6);
fanny868 0:9072e932503c 279 }
fanny868 0:9072e932503c 280
fanny868 0:9072e932503c 281 // Tombol Analog
fanny868 0:9072e932503c 282 LX = PS3.getAnalogHat(LeftHatX);
fanny868 0:9072e932503c 283 LY = PS3.getAnalogHat(LeftHatY);
fanny868 0:9072e932503c 284 RX = PS3.getAnalogHat(RightHatX);
fanny868 0:9072e932503c 285 RY = PS3.getAnalogHat(RightHatY);
fanny868 0:9072e932503c 286
fanny868 0:9072e932503c 287 aL2 = PS3.getAnalogButton(L2);
fanny868 0:9072e932503c 288 aR2 = PS3.getAnalogButton(R2);
fanny868 2:ac21a024ac80 289
fanny868 2:ac21a024ac80 290 kirimdatajoystik();
fanny868 0:9072e932503c 291 } else {
fanny868 0:9072e932503c 292 // PS3 Disconnected
fanny868 0:9072e932503c 293 Serial.write(0x88);
fanny868 0:9072e932503c 294 Serial.write(0x09);
fanny868 0:9072e932503c 295 }
fanny868 2:ac21a024ac80 296 delay(3);
fanny868 2:ac21a024ac80 297 }