KRAI 2017 / Mbed 2 deprecated Joystick_ManualBaseBaruV2_enc

code base baru closeloop

Dependencies:   Motor PID Joystick_OrdoV5 mbed

Fork of Joystick_ManualBaseBaruV1_2 by KRAI 2017

main.cpp@15:98f0d56b14f0, 2016-12-09 (annotated)

Committer:
rahmadirizki18
Date:
Fri Dec 09 12:03:44 2016 +0000
Revision:
15:98f0d56b14f0
Parent:
14:6d389e99981c
Child:
16:90119f03c5d1
update pin;

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 /* */
rahmadirizki18 5:3aa203218306 4 /* - Digunakan encoder autonics */
rahmadirizki18 5:3aa203218306 5 /* - Konfigurasi Motor dan Encoder sbb : */
rahmadirizki18 5:3aa203218306 6 /* _________________ */
rahmadirizki18 5:3aa203218306 7 /* | DEPAN | */
rahmadirizki18 5:3aa203218306 8 /* | 1. e .2 | Angka ==> Motor */
rahmadirizki18 5:3aa203218306 9 /* | ` ` | e ==> Encoder */
rahmadirizki18 5:3aa203218306 10 /* | e e | */
rahmadirizki18 5:3aa203218306 11 /* | . . | */
rahmadirizki18 5:3aa203218306 12 /* | 4` e `3 | */
rahmadirizki18 5:3aa203218306 13 /* |________________| */
rahmadirizki18 5:3aa203218306 14 /* */
rahmadirizki18 5:3aa203218306 15 /* SETTINGS (WAJIB!) : */
rahmadirizki18 5:3aa203218306 16 /* 1. Settings Pin Encoder, Resolusi, dan Tipe encoding di omniPos.h */
rahmadirizki18 5:3aa203218306 17 /* 2. Deklarasi penggunaan library omniPos pada bagian deklarasi encoder */
rahmadirizki18 5:3aa203218306 18 /* */
rahmadirizki18 5:3aa203218306 19 /****************************************************************************/
rahmadirizki18 6:68293bed71ea 20 /* */
rahmadirizki18 6:68293bed71ea 21 /* Joystick */
rahmadirizki18 6:68293bed71ea 22 /* kanan => posisi target x ditambah 0.01 */
rahmadirizki18 6:68293bed71ea 23 /* kiri => posisi target x dikurang 0.01 */
rahmadirizki18 6:68293bed71ea 24 /* atas => posisi target y ditambah 0.01 */
rahmadirizki18 6:68293bed71ea 25 /* bawah => posisi target y dikurang 0.01 */
rahmadirizki18 6:68293bed71ea 26 /* */
rahmadirizki18 6:68293bed71ea 27 /* Tombol silang => Kembali keposisi Awal */
rahmadirizki18 6:68293bed71ea 28 /* Tombol segitiga => Aktif motor Launcher */
rahmadirizki18 13:8ab42383a2ca 29 /* Tombol lingkaran=> Aktif servo Launcher */
rahmadirizki18 13:8ab42383a2ca 30 /* Tombol L3 => PWM Launcher dikurangin */
rahmadirizki18 13:8ab42383a2ca 31 /* Tombol R3 => PWM Launcher ditambahin */
rahmadirizki18 13:8ab42383a2ca 32 /* */
rahmadirizki18 13:8ab42383a2ca 33 /* */
rahmadirizki18 6:68293bed71ea 34 /* */
rahmadirizki18 6:68293bed71ea 35 /****************************************************************************/
rahmadirizki18 6:68293bed71ea 36
fanny868 0:9072e932503c 37
fanny868 0:9072e932503c 38 #include "mbed.h"
fanny868 0:9072e932503c 39 #include "JoystickPS3.h"
fanny868 0:9072e932503c 40 #include "Motor.h"
rahmadirizki18 6:68293bed71ea 41 #include "Servo.h"
fanny868 0:9072e932503c 42
fanny868 0:9072e932503c 43 //#define koefperlambatan 0.8
rahmadirizki18 5:3aa203218306 44 #include "encoderKRAI.h"
rahmadirizki18 5:3aa203218306 45
rahmadirizki18 5:3aa203218306 46 #define pi 22/7
rahmadirizki18 5:3aa203218306 47 #define diaEncoder 0.058
rahmadirizki18 5:3aa203218306 48 #define PPR 1000
rahmadirizki18 5:3aa203218306 49 #define diaRobot 0.64
Joshua23 9:5a50782510fb 50 #define pinservo1 PC_8
rahmadirizki18 7:d138c56dab20 51 //PC 9
Joshua23 9:5a50782510fb 52 #define pinservo2 PC_9
rahmadirizki18 5:3aa203218306 53
rahmadirizki18 5:3aa203218306 54 float K_enc = pi*diaEncoder;
rahmadirizki18 5:3aa203218306 55 float K_robot = pi*diaRobot;
rahmadirizki18 5:3aa203218306 56
rahmadirizki18 5:3aa203218306 57 float speed1=0.6;
rahmadirizki18 5:3aa203218306 58 float speed2=0.6;
rahmadirizki18 5:3aa203218306 59 float speed3=0.6;
rahmadirizki18 5:3aa203218306 60 float speed4=0.6;
franshendri 12:e07c59c28c29 61 float speedB=0.43 ;
franshendri 12:e07c59c28c29 62 float speedL=0.4;
rahmadirizki18 5:3aa203218306 63
rahmadirizki18 5:3aa203218306 64 float KpX=0.5, KpY=0.5, Kp_tetha=0.03;
rahmadirizki18 6:68293bed71ea 65
rahmadirizki18 6:68293bed71ea 66 Timer waktu;
rahmadirizki18 5:3aa203218306 67 //float jarakGlobalY;
rahmadirizki18 5:3aa203218306 68
rahmadirizki18 5:3aa203218306 69 // Deklarasi variabel PID
rahmadirizki18 5:3aa203218306 70 //PID PID(0.992,0.000,0.81,0.001); //(P,I,D, time sampling)
rahmadirizki18 15:98f0d56b14f0 71 /*
rahmadirizki18 5:3aa203218306 72 // Deklarasi variabel encoder
rahmadirizki18 6:68293bed71ea 73 encoderKRAI encoderDepan( PB_14, PB_13, 2000, encoderKRAI::X2_ENCODING); //inA, inB, pin Indeks (NC = tak ada), resolusi, pembacaan
rahmadirizki18 5:3aa203218306 74 encoderKRAI encoderBelakang( PC_4, PB_15, 2000, encoderKRAI::X2_ENCODING);
rahmadirizki18 5:3aa203218306 75 encoderKRAI encoderKanan( PD_2, PC_12, 720, encoderKRAI::X2_ENCODING);
rahmadirizki18 5:3aa203218306 76 encoderKRAI encoderKiri( PC_11, PC_10, 2000, encoderKRAI::X2_ENCODING);
rahmadirizki18 15:98f0d56b14f0 77 */
rahmadirizki18 15:98f0d56b14f0 78
rahmadirizki18 15:98f0d56b14f0 79 // Deklarasi variabel encoder
rahmadirizki18 15:98f0d56b14f0 80 encoderKRAI encoderDepan( PB_13, PB_14, 2000, encoderKRAI::X2_ENCODING); //inA, inB, pin Indeks (NC = tak ada), resolusi, pembacaan
rahmadirizki18 15:98f0d56b14f0 81 encoderKRAI encoderBelakang( PC_11, PC_10, 2000, encoderKRAI::X2_ENCODING);
rahmadirizki18 15:98f0d56b14f0 82 encoderKRAI encoderKanan( PC_12, PD_2, 720, encoderKRAI::X2_ENCODING);
rahmadirizki18 15:98f0d56b14f0 83 encoderKRAI encoderKiri( PC_4, PB_15, 2000, encoderKRAI::X2_ENCODING);
fanny868 0:9072e932503c 84
fanny868 0:9072e932503c 85 // Deklarasi variabel motor
rahmadirizki18 14:6d389e99981c 86 Motor motor1(PB_7, PA_14 , PA_15); // pwm, fwd, rev
rahmadirizki18 14:6d389e99981c 87 Motor motor2(PB_8, PA_13 ,PB_0); // pwm, fwd, rev
rahmadirizki18 15:98f0d56b14f0 88 Motor motor3(PB_9, PA_12 ,PC_5); // pwm, fwd, rev
rahmadirizki18 14:6d389e99981c 89 Motor motor4(PB_6, PB_1 ,PB_12); // pwm, fwd, rev
rahmadirizki18 5:3aa203218306 90
rahmadirizki18 6:68293bed71ea 91 //Motor Atas
rahmadirizki18 14:6d389e99981c 92 Motor motorld(PA_8, PC_1 , PC_2); // pwm, fwd, rev
rahmadirizki18 14:6d389e99981c 93 Motor motorlb(PA_9, PA_4, PC_15 ); // pwm, fwd, rev
rahmadirizki18 6:68293bed71ea 94
rahmadirizki18 6:68293bed71ea 95 //Servo Atas
rahmadirizki18 14:6d389e99981c 96 Servo servoS(PB_2);
rahmadirizki18 14:6d389e99981c 97 Servo servoB(PA_5);
rahmadirizki18 6:68293bed71ea 98
rahmadirizki18 5:3aa203218306 99 // Deklarasi variabel posisi robot
rahmadirizki18 5:3aa203218306 100 //robotPos posisi();
rahmadirizki18 5:3aa203218306 101
rahmadirizki18 5:3aa203218306 102 // Inisialisasi Pin TX-RX Joystik dan PC
rahmadirizki18 5:3aa203218306 103 //Serial pc(PA_0,PA_1);
rahmadirizki18 5:3aa203218306 104 //Serial pc(USBTX,USBRX);
rahmadirizki18 5:3aa203218306 105 // Deklarasi Variabel Global
rahmadirizki18 5:3aa203218306 106 /*
rahmadirizki18 5:3aa203218306 107 * posX dan posY berdasarkan arah robot
rahmadirizki18 5:3aa203218306 108 * encoder Depan & Belakang sejajar sumbu Y
rahmadirizki18 5:3aa203218306 109 * encoder Kanan & Kiri sejajar sumbu X
rahmadirizki18 5:3aa203218306 110 */
rahmadirizki18 5:3aa203218306 111 float jarak, posX, posY;
rahmadirizki18 5:3aa203218306 112 //float keliling = pi*diameterRoda;
rahmadirizki18 5:3aa203218306 113
rahmadirizki18 5:3aa203218306 114 void detect_encoder(float speed);
rahmadirizki18 5:3aa203218306 115 void setCenter();
rahmadirizki18 5:3aa203218306 116 float getY();
rahmadirizki18 5:3aa203218306 117 float getX();
rahmadirizki18 5:3aa203218306 118 float getTetha();
rahmadirizki18 5:3aa203218306 119
fanny868 0:9072e932503c 120 // Inisialisasi Pin TX-RX Joystik dan PC
rahmadirizki18 3:1287fccc11be 121 joysticknucleo joystick(PA_0,PA_1);
rahmadirizki18 3:1287fccc11be 122 Serial pc(USBTX,USBRX);
fanny868 0:9072e932503c 123
rahmadirizki18 5:3aa203218306 124 // Posisi target
rahmadirizki18 5:3aa203218306 125 float XT, YT, Tetha;
rahmadirizki18 5:3aa203218306 126
rahmadirizki18 5:3aa203218306 127 //encoder variable
rahmadirizki18 5:3aa203218306 128 float errX, errY, errT, Vt, Vx, Vy;
rahmadirizki18 5:3aa203218306 129 float V1, V2, V3, V4;
rahmadirizki18 5:3aa203218306 130
fanny868 0:9072e932503c 131 //bool perlambatan=0;
fanny868 0:9072e932503c 132 char case_ger;
rahmadirizki18 3:1287fccc11be 133 bool maju=false,mundur=false,pivka=false,pivki=false,kiri=false,kanan=false,saka=false,saki=false,sbka=false,sbki=false,cw1=false,ccw1=false,cw2=false,ccw2=false,cw3=false,ccw3=false,analog=false;
fanny868 0:9072e932503c 134 bool stop = true;
rahmadirizki18 6:68293bed71ea 135 bool Launcher = false,ServoGo = false;
rahmadirizki18 4:483c07cc22e1 136 float jLX,jLY;
rahmadirizki18 5:3aa203218306 137 double vcurr;
rahmadirizki18 4:483c07cc22e1 138 float x,y; // untuk analoghat kiri
rahmadirizki18 3:1287fccc11be 139 float errorx=0,errory=0;
rahmadirizki18 3:1287fccc11be 140
rahmadirizki18 4:483c07cc22e1 141 // Fungsi mapping 0-255 ke -128 sampai 127
rahmadirizki18 4:483c07cc22e1 142 float mapping(float a,float error)
rahmadirizki18 3:1287fccc11be 143 {
rahmadirizki18 4:483c07cc22e1 144 float hasil,b;
rahmadirizki18 4:483c07cc22e1 145 b = (float)((a-128)/128);
rahmadirizki18 4:483c07cc22e1 146 if (b>(error - 0.2) && b<(error + 0.2))
rahmadirizki18 4:483c07cc22e1 147 {
rahmadirizki18 4:483c07cc22e1 148 hasil = 0;
rahmadirizki18 4:483c07cc22e1 149 } else {
rahmadirizki18 4:483c07cc22e1 150 hasil = b;
rahmadirizki18 4:483c07cc22e1 151 }
rahmadirizki18 3:1287fccc11be 152 return (hasil);
rahmadirizki18 3:1287fccc11be 153 }
fanny868 0:9072e932503c 154
rahmadirizki18 4:483c07cc22e1 155 // Kalibrasi tombol analog kiri
rahmadirizki18 4:483c07cc22e1 156 void kalibrasi()
rahmadirizki18 4:483c07cc22e1 157 {
rahmadirizki18 4:483c07cc22e1 158 errorx = (jLX - 128)/128;
rahmadirizki18 4:483c07cc22e1 159 errory = (jLY - 128)/128;
rahmadirizki18 4:483c07cc22e1 160
rahmadirizki18 4:483c07cc22e1 161 }
rahmadirizki18 4:483c07cc22e1 162
rahmadirizki18 3:1287fccc11be 163 // simpan data analog
rahmadirizki18 3:1287fccc11be 164 void baca_analog()
rahmadirizki18 3:1287fccc11be 165 {
rahmadirizki18 3:1287fccc11be 166 jLX = joystick.LX;
rahmadirizki18 3:1287fccc11be 167 jLY = joystick.LY;
rahmadirizki18 3:1287fccc11be 168
rahmadirizki18 3:1287fccc11be 169 // Pembacaan nilai Y terbalik
rahmadirizki18 4:483c07cc22e1 170 x = mapping(jLX,errorx);
rahmadirizki18 4:483c07cc22e1 171 y = -mapping(jLY,errory);
rahmadirizki18 3:1287fccc11be 172 }
fanny868 0:9072e932503c 173
rahmadirizki18 3:1287fccc11be 174 // Gerak dari Motor base
fanny868 0:9072e932503c 175 int case_gerak()
fanny868 0:9072e932503c 176 {
fanny868 0:9072e932503c 177 int casegerak;
rahmadirizki18 3:1287fccc11be 178 baca_analog();
rahmadirizki18 3:1287fccc11be 179 if (!joystick.L1 && joystick.R1) {
fanny868 0:9072e932503c 180 // Pivot Kanan
fanny868 0:9072e932503c 181 casegerak = 1;
fanny868 0:9072e932503c 182 } else if (!joystick.R1 && joystick.L1) {
fanny868 0:9072e932503c 183 // Pivot Kiri
fanny868 0:9072e932503c 184 casegerak = 2;
rahmadirizki18 3:1287fccc11be 185 } else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
fanny868 0:9072e932503c 186 // Maju
rahmadirizki18 3:1287fccc11be 187 casegerak = 3;
rahmadirizki18 3:1287fccc11be 188 } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
fanny868 0:9072e932503c 189 // Mundur
fanny868 0:9072e932503c 190 casegerak = 4;
rahmadirizki18 3:1287fccc11be 191 } else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kiri)&&(joystick.kanan)) {
fanny868 0:9072e932503c 192 // Serong Atas Kanan
fanny868 0:9072e932503c 193 casegerak = 5;
rahmadirizki18 3:1287fccc11be 194 } else if((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kiri)&&(joystick.kanan)) {
fanny868 0:9072e932503c 195 // Serong Bawah Kanan
fanny868 0:9072e932503c 196 casegerak = 6;
rahmadirizki18 3:1287fccc11be 197 } else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(joystick.kiri)&&(!joystick.kanan)) {
fanny868 0:9072e932503c 198 // Serong Atas Kiri
fanny868 0:9072e932503c 199 casegerak = 7;
rahmadirizki18 3:1287fccc11be 200 } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(joystick.kiri)&&(!joystick.kanan)) {
fanny868 0:9072e932503c 201 // Serong Bawah Kiri
fanny868 0:9072e932503c 202 casegerak = 8;
rahmadirizki18 3:1287fccc11be 203 } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {
fanny868 0:9072e932503c 204 // Kanan
fanny868 0:9072e932503c 205 casegerak = 9;
rahmadirizki18 3:1287fccc11be 206 } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {
fanny868 0:9072e932503c 207 // Kiri
rahmadirizki18 3:1287fccc11be 208 casegerak = 10;
rahmadirizki18 3:1287fccc11be 209 } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
rahmadirizki18 3:1287fccc11be 210 // case gerak analog on off
rahmadirizki18 3:1287fccc11be 211 if (analog){
rahmadirizki18 3:1287fccc11be 212 casegerak = 11;
rahmadirizki18 3:1287fccc11be 213 } else {
rahmadirizki18 3:1287fccc11be 214 casegerak = 12;
rahmadirizki18 3:1287fccc11be 215 }
rahmadirizki18 3:1287fccc11be 216 }
fanny868 0:9072e932503c 217 return(casegerak);
fanny868 0:9072e932503c 218 }
fanny868 0:9072e932503c 219
fanny868 0:9072e932503c 220
fanny868 0:9072e932503c 221
fanny868 0:9072e932503c 222 /**
fanny868 0:9072e932503c 223
rahmadirizki18 3:1287fccc11be 224 ** Case 1 : Pivot kanan
fanny868 0:9072e932503c 225 ** Case 2 : Pivot Kiri
fanny868 0:9072e932503c 226 ** Case 3 : Maju
fanny868 0:9072e932503c 227 ** Case 4 : Mundur
fanny868 0:9072e932503c 228 ** Case 5 : Serong Atas Kanan
fanny868 0:9072e932503c 229 ** Case 6 : Serong Bawah Kanan
fanny868 0:9072e932503c 230 ** Case 7 : Serong Atas Kiri
fanny868 0:9072e932503c 231 ** Case 8 : Serong Bawah Kiri
fanny868 0:9072e932503c 232 ** Case 9 : Kanan
fanny868 0:9072e932503c 233 ** Case 10 : Kiri
rahmadirizki18 3:1287fccc11be 234 ** Case 11 : Analog
rahmadirizki18 3:1287fccc11be 235 ** Case 11 : break
fanny868 0:9072e932503c 236
fanny868 0:9072e932503c 237 **/
rahmadirizki18 3:1287fccc11be 238
rahmadirizki18 5:3aa203218306 239
rahmadirizki18 3:1287fccc11be 240 // Pergerakan dari base
fanny868 0:9072e932503c 241 void aktuator()
fanny868 0:9072e932503c 242 {
rahmadirizki18 6:68293bed71ea 243 //Servo
rahmadirizki18 7:d138c56dab20 244 if (ServoGo){
franshendri 12:e07c59c28c29 245 servoS.position(20);
franshendri 10:f0f0dc3904e0 246 wait_ms(500);
franshendri 12:e07c59c28c29 247 servoS.position(-28);
Joshua23 8:0711dea61312 248 wait_ms(500);
franshendri 12:e07c59c28c29 249 servoS.position(20);
Joshua23 8:0711dea61312 250 wait_ms(500);
franshendri 12:e07c59c28c29 251 for (int i = -0; i<=70; i++){
Joshua23 8:0711dea61312 252 servoB.position(i);
Joshua23 8:0711dea61312 253 wait_ms(10);
Joshua23 8:0711dea61312 254 }
Joshua23 8:0711dea61312 255 wait_ms(500);
Joshua23 8:0711dea61312 256 servoB.position(0);
rahmadirizki18 7:d138c56dab20 257 ServoGo = false;
rahmadirizki18 6:68293bed71ea 258
rahmadirizki18 6:68293bed71ea 259 }else{
franshendri 12:e07c59c28c29 260 servoS.position(20);
rahmadirizki18 6:68293bed71ea 261 servoB.position(0);
fanny868 0:9072e932503c 262
rahmadirizki18 7:d138c56dab20 263 }
rahmadirizki18 6:68293bed71ea 264
rahmadirizki18 6:68293bed71ea 265 // Motor Atas
rahmadirizki18 6:68293bed71ea 266 if (Launcher) {
rahmadirizki18 7:d138c56dab20 267 motorld.speed(speedL);
franshendri 12:e07c59c28c29 268 motorlb.speed(speedB);
rahmadirizki18 6:68293bed71ea 269 }else{
rahmadirizki18 6:68293bed71ea 270 motorld.speed(0);
rahmadirizki18 6:68293bed71ea 271 motorlb.speed(0);
rahmadirizki18 6:68293bed71ea 272 }
rahmadirizki18 6:68293bed71ea 273
rahmadirizki18 6:68293bed71ea 274 // MOTOR Bawah
fanny868 0:9072e932503c 275 switch (case_ger)
fanny868 0:9072e932503c 276 {
rahmadirizki18 3:1287fccc11be 277 case (1):
fanny868 0:9072e932503c 278 {
rahmadirizki18 15:98f0d56b14f0 279 Tetha = Tetha - 0.05;
fanny868 0:9072e932503c 280 pivka=true;
rahmadirizki18 3:1287fccc11be 281 maju=mundur=analog=kiri=kanan=saka=saki=sbka=sbki=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 282
rahmadirizki18 15:98f0d56b14f0 283 //pc.printf("pivKa Xt =%.2f x=%.2f YT=%.2f y=%.2f errx=%.2f erry=%.2f \n",XT,x,YT,y,errX,errY);
fanny868 0:9072e932503c 284
rahmadirizki18 5:3aa203218306 285
fanny868 0:9072e932503c 286 break;
fanny868 0:9072e932503c 287 }
fanny868 0:9072e932503c 288 case (2):
fanny868 0:9072e932503c 289 {
rahmadirizki18 15:98f0d56b14f0 290 Tetha = Tetha + 0.05;
fanny868 0:9072e932503c 291
fanny868 0:9072e932503c 292 pivki=true;
rahmadirizki18 3:1287fccc11be 293 maju=mundur=kiri=analog=kanan=saka=saki=sbka=sbki=pivka=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 294
rahmadirizki18 15:98f0d56b14f0 295 //pc.printf("pivKi Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
fanny868 0:9072e932503c 296
rahmadirizki18 5:3aa203218306 297
fanny868 0:9072e932503c 298 break;
fanny868 0:9072e932503c 299 }
fanny868 0:9072e932503c 300 case (3):
fanny868 0:9072e932503c 301 {
rahmadirizki18 15:98f0d56b14f0 302 YT = YT + 0.01;
fanny868 0:9072e932503c 303
fanny868 0:9072e932503c 304 maju=true;
rahmadirizki18 3:1287fccc11be 305 mundur=kiri=kanan=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 306
rahmadirizki18 15:98f0d56b14f0 307 //pc.printf("maju Xt =%.2f x=%.2f YT=%.2f y=%.2f errx=%.2f erry=%.2f \n",XT,x,YT,y,errX,errY);
fanny868 0:9072e932503c 308
fanny868 0:9072e932503c 309 break;
fanny868 0:9072e932503c 310 }
fanny868 0:9072e932503c 311 case (4):
fanny868 0:9072e932503c 312 {
rahmadirizki18 15:98f0d56b14f0 313 YT = YT - 0.01;
fanny868 0:9072e932503c 314
fanny868 0:9072e932503c 315 mundur=true;
rahmadirizki18 3:1287fccc11be 316 maju=kiri=kanan=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 317
rahmadirizki18 15:98f0d56b14f0 318 //pc.printf("mundur Xt =%.2f x=%.2f YT=%.2f y=%.2f errx=%.2f erry=%.2f \n",XT,x,YT,y,errX,errY);
fanny868 0:9072e932503c 319
fanny868 0:9072e932503c 320
fanny868 0:9072e932503c 321 break;
fanny868 0:9072e932503c 322 }
fanny868 0:9072e932503c 323 case (5) :
fanny868 0:9072e932503c 324 {
rahmadirizki18 15:98f0d56b14f0 325 XT = XT + 0.01;
rahmadirizki18 15:98f0d56b14f0 326 YT = YT + 0.01;
fanny868 0:9072e932503c 327
fanny868 0:9072e932503c 328 saka=true;
rahmadirizki18 3:1287fccc11be 329 maju=mundur=kiri=kanan=sbka=saki=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 330
rahmadirizki18 15:98f0d56b14f0 331 //pc.printf("saka Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
fanny868 0:9072e932503c 332
fanny868 0:9072e932503c 333
fanny868 0:9072e932503c 334 break;
fanny868 0:9072e932503c 335 }
fanny868 0:9072e932503c 336 case (6) :
fanny868 0:9072e932503c 337 {
rahmadirizki18 15:98f0d56b14f0 338 XT = XT + 0.01;
rahmadirizki18 15:98f0d56b14f0 339 YT = YT - 0.01;
rahmadirizki18 5:3aa203218306 340
fanny868 0:9072e932503c 341
fanny868 0:9072e932503c 342 sbka=true;
rahmadirizki18 3:1287fccc11be 343 maju=mundur=kiri=kanan=saka=saki=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 344
rahmadirizki18 15:98f0d56b14f0 345 //pc.printf("sbka Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
fanny868 0:9072e932503c 346
fanny868 0:9072e932503c 347
fanny868 0:9072e932503c 348 break;
fanny868 0:9072e932503c 349 }
fanny868 0:9072e932503c 350 case (7) :
fanny868 0:9072e932503c 351 {
rahmadirizki18 15:98f0d56b14f0 352 XT = XT - 0.01;
rahmadirizki18 15:98f0d56b14f0 353 YT = YT + 0.01;
rahmadirizki18 5:3aa203218306 354
fanny868 0:9072e932503c 355
fanny868 0:9072e932503c 356 saki=true;
rahmadirizki18 3:1287fccc11be 357 maju=kiri=kanan=saka=mundur=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 358
rahmadirizki18 15:98f0d56b14f0 359 //pc.printf("saki Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
fanny868 0:9072e932503c 360
fanny868 0:9072e932503c 361
fanny868 0:9072e932503c 362 break;
fanny868 0:9072e932503c 363 }
fanny868 0:9072e932503c 364 case (8) :
fanny868 0:9072e932503c 365 {
rahmadirizki18 15:98f0d56b14f0 366 XT = XT - 0.01;
rahmadirizki18 15:98f0d56b14f0 367 YT = YT - 0.01;
rahmadirizki18 5:3aa203218306 368
rahmadirizki18 15:98f0d56b14f0 369 //pc.printf("sbki Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
fanny868 0:9072e932503c 370
fanny868 0:9072e932503c 371
fanny868 0:9072e932503c 372 break;
fanny868 0:9072e932503c 373 }
fanny868 0:9072e932503c 374 case (9) :
fanny868 0:9072e932503c 375 {
rahmadirizki18 15:98f0d56b14f0 376 XT = XT + 0.01;
fanny868 0:9072e932503c 377 kanan=true;
rahmadirizki18 3:1287fccc11be 378 maju=kiri=mundur=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 379
rahmadirizki18 15:98f0d56b14f0 380 //pc.printf("Kanan Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
fanny868 0:9072e932503c 381
fanny868 0:9072e932503c 382 break;
fanny868 0:9072e932503c 383 }
fanny868 0:9072e932503c 384 case (10) :
fanny868 0:9072e932503c 385 {
rahmadirizki18 15:98f0d56b14f0 386 XT = XT - 0.01;
fanny868 0:9072e932503c 387
fanny868 0:9072e932503c 388 kiri=true;
rahmadirizki18 3:1287fccc11be 389 maju=kanan=mundur=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 390
rahmadirizki18 15:98f0d56b14f0 391 //pc.printf("Kiri Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
fanny868 0:9072e932503c 392
fanny868 0:9072e932503c 393
fanny868 0:9072e932503c 394 break;
fanny868 0:9072e932503c 395 }
rahmadirizki18 3:1287fccc11be 396 case (11):
rahmadirizki18 3:1287fccc11be 397 {
rahmadirizki18 4:483c07cc22e1 398
rahmadirizki18 15:98f0d56b14f0 399 XT = XT + 0.01*x;
rahmadirizki18 15:98f0d56b14f0 400 YT = YT + 0.01*y;
rahmadirizki18 3:1287fccc11be 401
rahmadirizki18 3:1287fccc11be 402 analog=true;
rahmadirizki18 3:1287fccc11be 403 maju=mundur=kiri=kanan=saka=saki=sbka=sbki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
rahmadirizki18 3:1287fccc11be 404
rahmadirizki18 15:98f0d56b14f0 405 //pc.printf("analog Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
rahmadirizki18 3:1287fccc11be 406
rahmadirizki18 3:1287fccc11be 407 break;
rahmadirizki18 3:1287fccc11be 408 }
rahmadirizki18 3:1287fccc11be 409 default :
fanny868 0:9072e932503c 410 {
rahmadirizki18 5:3aa203218306 411
rahmadirizki18 3:1287fccc11be 412 maju=mundur=kiri=kanan=saka=saki=sbka=sbki=analog=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
fanny868 0:9072e932503c 413 stop = true;
fanny868 0:9072e932503c 414
fanny868 0:9072e932503c 415 //s1 = 0;s2 =0; s3 =0; s4 =0;
fanny868 0:9072e932503c 416
rahmadirizki18 5:3aa203218306 417 // pc.printf("Stop V1=%.2f V2=%.2f V3=%.2f V4=%.2f\n",V1,V2,V3,V4);
rahmadirizki18 3:1287fccc11be 418 }
rahmadirizki18 5:3aa203218306 419 }
rahmadirizki18 5:3aa203218306 420 }
rahmadirizki18 5:3aa203218306 421
rahmadirizki18 5:3aa203218306 422 //Begin Encoder
rahmadirizki18 5:3aa203218306 423
rahmadirizki18 5:3aa203218306 424 void setCenter()
rahmadirizki18 5:3aa203218306 425 {
rahmadirizki18 5:3aa203218306 426 encoderDepan.reset();
rahmadirizki18 5:3aa203218306 427 encoderBelakang.reset();
rahmadirizki18 5:3aa203218306 428 encoderKanan.reset();
rahmadirizki18 5:3aa203218306 429 encoderKiri.reset();
rahmadirizki18 5:3aa203218306 430 }
rahmadirizki18 5:3aa203218306 431
rahmadirizki18 5:3aa203218306 432 float getX()
rahmadirizki18 5:3aa203218306 433 {
rahmadirizki18 5:3aa203218306 434 float jarakEncDpn, jarakEncBlk;
rahmadirizki18 5:3aa203218306 435 jarakEncDpn = (encoderDepan.getPulses())/(float)(2000.0)*K_enc;
rahmadirizki18 5:3aa203218306 436 jarakEncBlk = (encoderBelakang.getPulses())/(float)(2000.0)*K_enc;
rahmadirizki18 5:3aa203218306 437 return (jarakEncDpn-jarakEncBlk)/2;
rahmadirizki18 5:3aa203218306 438 }
rahmadirizki18 5:3aa203218306 439
rahmadirizki18 5:3aa203218306 440 float getY()
rahmadirizki18 5:3aa203218306 441 {
rahmadirizki18 5:3aa203218306 442 float jarakEncKir, jarakEncKan;
rahmadirizki18 5:3aa203218306 443 jarakEncKir = (encoderKiri.getPulses())/(float)(2000.0)*K_enc;
rahmadirizki18 5:3aa203218306 444 jarakEncKan = (encoderKanan.getPulses())/(float)(720.0)*K_enc;
rahmadirizki18 5:3aa203218306 445 return (jarakEncKir-jarakEncKan)/2;
rahmadirizki18 5:3aa203218306 446 }
rahmadirizki18 5:3aa203218306 447
rahmadirizki18 5:3aa203218306 448 float getTetha()
rahmadirizki18 5:3aa203218306 449 {
rahmadirizki18 5:3aa203218306 450 float busurDpn, busurBlk, busurKir, busurKan;
rahmadirizki18 5:3aa203218306 451 busurDpn = ((encoderDepan.getPulses())/(float)(2000.0)*K_enc)/K_robot*360.0;
rahmadirizki18 5:3aa203218306 452 busurBlk = ((encoderBelakang.getPulses())/(float)(2000.0)*K_enc)/K_robot*360.0;
rahmadirizki18 5:3aa203218306 453 busurKir = ((encoderKiri.getPulses())/(float)(2000.0)*K_enc)/K_robot*360.0;
rahmadirizki18 5:3aa203218306 454 busurKan = ((encoderKanan.getPulses())/(float)(720.0)*K_enc)/K_robot*360.0;
rahmadirizki18 5:3aa203218306 455
rahmadirizki18 5:3aa203218306 456 return -(busurDpn+busurBlk+busurKir+busurKan)/4;
fanny868 0:9072e932503c 457 }
fanny868 0:9072e932503c 458
rahmadirizki18 5:3aa203218306 459 void gotoXYT(float xa, float ya, float Ta)
rahmadirizki18 5:3aa203218306 460 {
rahmadirizki18 5:3aa203218306 461
rahmadirizki18 5:3aa203218306 462 errX = xa-getX();
rahmadirizki18 5:3aa203218306 463 Vx = KpX*errX;
rahmadirizki18 5:3aa203218306 464
rahmadirizki18 5:3aa203218306 465 errY = ya-getY();
rahmadirizki18 5:3aa203218306 466 Vy = KpY*errY;
rahmadirizki18 5:3aa203218306 467
rahmadirizki18 5:3aa203218306 468 errT = Ta-getTetha();
rahmadirizki18 5:3aa203218306 469 Vt = Kp_tetha*errT;
rahmadirizki18 5:3aa203218306 470
rahmadirizki18 5:3aa203218306 471 V1 = Vx+Vy-Vt;
rahmadirizki18 5:3aa203218306 472 V2 = Vx-Vy-Vt;
rahmadirizki18 5:3aa203218306 473 V3 = -Vx-Vy-Vt;
rahmadirizki18 5:3aa203218306 474 V4 = -Vx+Vy-Vt;
rahmadirizki18 5:3aa203218306 475
rahmadirizki18 5:3aa203218306 476 if (V1>speed1)
rahmadirizki18 5:3aa203218306 477 { V1 = speed1; }
rahmadirizki18 5:3aa203218306 478 else if (V1<-speed1)
rahmadirizki18 5:3aa203218306 479 { V1 = -speed1; }
rahmadirizki18 5:3aa203218306 480
rahmadirizki18 5:3aa203218306 481 if (V2>speed2)
rahmadirizki18 5:3aa203218306 482 { V2 = speed2; }
rahmadirizki18 5:3aa203218306 483 else if (V2<-speed2)
rahmadirizki18 5:3aa203218306 484 { V2 = -speed2; }
rahmadirizki18 5:3aa203218306 485
rahmadirizki18 5:3aa203218306 486 if (V3>speed3)
rahmadirizki18 5:3aa203218306 487 { V3 = speed3; }
rahmadirizki18 5:3aa203218306 488 else if (V3<-speed3)
rahmadirizki18 5:3aa203218306 489 { V3 = -speed3; }
rahmadirizki18 5:3aa203218306 490
rahmadirizki18 5:3aa203218306 491 if (V4>speed4)
rahmadirizki18 5:3aa203218306 492 { V4 = speed4; }
rahmadirizki18 5:3aa203218306 493 else if (V4<-speed4)
rahmadirizki18 5:3aa203218306 494 { V4 = -speed4; }
rahmadirizki18 5:3aa203218306 495
rahmadirizki18 5:3aa203218306 496 if (((errX > 0.05) || (errX<-0.05)) || ((errY > 0.05) || (errY<-0.05)) || ((errT > 0.05) || (errT<-0.05)))
rahmadirizki18 5:3aa203218306 497 {
rahmadirizki18 5:3aa203218306 498 motor1.speed(V1);
rahmadirizki18 5:3aa203218306 499 motor2.speed(V2);
rahmadirizki18 5:3aa203218306 500 motor3.speed(V3);
rahmadirizki18 5:3aa203218306 501 motor4.speed(V4);
rahmadirizki18 5:3aa203218306 502 // pc.printf("V1=%.2f \ ", V1);
rahmadirizki18 5:3aa203218306 503 }
rahmadirizki18 5:3aa203218306 504 else
rahmadirizki18 5:3aa203218306 505 {
rahmadirizki18 5:3aa203218306 506 motor1.brake(1);
rahmadirizki18 5:3aa203218306 507 motor2.brake(1);
rahmadirizki18 5:3aa203218306 508 motor3.brake(1);
rahmadirizki18 5:3aa203218306 509 motor4.brake(1);
rahmadirizki18 5:3aa203218306 510 //_ms(1000);
rahmadirizki18 5:3aa203218306 511 }
rahmadirizki18 5:3aa203218306 512 //pc.printf("%f\t%f\t%f ", errX*100,errY*100,errT);
rahmadirizki18 5:3aa203218306 513 // wait_ms(10);
rahmadirizki18 5:3aa203218306 514
rahmadirizki18 5:3aa203218306 515 }
rahmadirizki18 5:3aa203218306 516 //End Encoder
rahmadirizki18 5:3aa203218306 517
rahmadirizki18 7:d138c56dab20 518 void speedLauncher()
rahmadirizki18 7:d138c56dab20 519 {
franshendri 12:e07c59c28c29 520 if (joystick.R3_click and speedL < 0.8){
franshendri 12:e07c59c28c29 521 speedL = speedL + 0.01;}
rahmadirizki18 7:d138c56dab20 522 if (joystick.L3_click and speedL > 0.1){
franshendri 12:e07c59c28c29 523 speedL = speedL - 0.01;}
rahmadirizki18 13:8ab42383a2ca 524 if (joystick.R2_click and speedB < 0.8 ){
rahmadirizki18 13:8ab42383a2ca 525 speedB = speedB + 0.01;}
rahmadirizki18 13:8ab42383a2ca 526 if (joystick.L2_click and speedB > 0.1 ){
rahmadirizki18 13:8ab42383a2ca 527 speedB = speedB - 0.01;}
rahmadirizki18 15:98f0d56b14f0 528 //pc.printf("Pwm depan = %.3f\t Pwm belakang = %.3f\n", speedL, speedB);
rahmadirizki18 7:d138c56dab20 529 }
franshendri 12:e07c59c28c29 530
franshendri 12:e07c59c28c29 531
franshendri 12:e07c59c28c29 532
fanny868 0:9072e932503c 533 int main (void)
fanny868 0:9072e932503c 534 {
fanny868 0:9072e932503c 535 // Set baud rate - 115200
fanny868 0:9072e932503c 536 joystick.setup();
rahmadirizki18 3:1287fccc11be 537 pc.baud(115200);
rahmadirizki18 6:68293bed71ea 538 wait_ms(1000);
rahmadirizki18 5:3aa203218306 539 setCenter();
rahmadirizki18 5:3aa203218306 540 wait_ms(500);
rahmadirizki18 5:3aa203218306 541
rahmadirizki18 5:3aa203218306 542 //Posisi Awal
rahmadirizki18 5:3aa203218306 543 XT = 0;
rahmadirizki18 5:3aa203218306 544 YT = 0;
rahmadirizki18 5:3aa203218306 545 Tetha = 0;
rahmadirizki18 3:1287fccc11be 546 pc.printf("Ready...\n");
rahmadirizki18 4:483c07cc22e1 547 kalibrasi();
rahmadirizki18 6:68293bed71ea 548 waktu.start();
fanny868 0:9072e932503c 549 while(1)
fanny868 0:9072e932503c 550 {
fanny868 0:9072e932503c 551 // Interrupt Serial
fanny868 0:9072e932503c 552 joystick.idle();
fanny868 0:9072e932503c 553 if(joystick.readable() ) {
fanny868 0:9072e932503c 554 // Panggil fungsi pembacaan joystik
fanny868 0:9072e932503c 555 joystick.baca_data();
fanny868 0:9072e932503c 556 // Panggil fungsi pengolahan data joystik
fanny868 0:9072e932503c 557 joystick.olah_data();
franshendri 12:e07c59c28c29 558 //pc.printf("%3d %3d\n\r",joystick.R2, joystick.L2);
fanny868 0:9072e932503c 559 //pc.printf("%2x %2x %2x %2x %3d %3d %3d %3d %3d %3d\n\r",joystick.button, joystick.RL, joystick.button_click, joystick.RL_click, joystick.R2, joystick.L2, joystick.RX, joystick.RY, joystick.LX, joystick.LY);
fanny868 0:9072e932503c 560 case_ger = case_gerak();
rahmadirizki18 3:1287fccc11be 561 aktuator();
rahmadirizki18 6:68293bed71ea 562
franshendri 12:e07c59c28c29 563 //pc.printf("bacaS = %.2f\tbacaB = %.2f\n",servoS.read(), servoB.read());
rahmadirizki18 6:68293bed71ea 564
rahmadirizki18 3:1287fccc11be 565 //kalibrasi
rahmadirizki18 4:483c07cc22e1 566 if (joystick.START){
rahmadirizki18 4:483c07cc22e1 567 kalibrasi();}
rahmadirizki18 4:483c07cc22e1 568 // analog switch mode
rahmadirizki18 6:68293bed71ea 569 if (joystick.SELECT_click) {analog = !analog;}
rahmadirizki18 6:68293bed71ea 570 if (joystick.segitiga_click) {Launcher = !Launcher;}
rahmadirizki18 6:68293bed71ea 571 if (joystick.lingkaran_click){
rahmadirizki18 6:68293bed71ea 572 ServoGo = true;
rahmadirizki18 6:68293bed71ea 573 waktu.reset();
rahmadirizki18 6:68293bed71ea 574 }
rahmadirizki18 5:3aa203218306 575 if (joystick.silang) {
rahmadirizki18 5:3aa203218306 576 XT = 0;
rahmadirizki18 5:3aa203218306 577 YT = 0;
rahmadirizki18 5:3aa203218306 578 Tetha = 0;
rahmadirizki18 15:98f0d56b14f0 579 //pc.printf("x..\n");
rahmadirizki18 5:3aa203218306 580 }
rahmadirizki18 7:d138c56dab20 581 speedLauncher();
franshendri 12:e07c59c28c29 582
fanny868 0:9072e932503c 583 } else {
fanny868 0:9072e932503c 584 joystick.idle();
fanny868 0:9072e932503c 585
fanny868 0:9072e932503c 586 }
rahmadirizki18 5:3aa203218306 587 gotoXYT(XT,YT,Tetha);
rahmadirizki18 5:3aa203218306 588
fanny868 0:9072e932503c 589 }
fanny868 0:9072e932503c 590 }