Bryan Purba / Mbed 2 deprecated DagonFly_Nasional_Mbed

convert_KeilToMbed

Dependencies:   DigitDisplay Motor PID mbed millis

Fork of DagonFly__RoadToJapan_15Mei_Ultimate by KRAI 2017

main.cpp@7:d138c56dab20, 2016-11-28 (annotated)

Committer:
rahmadirizki18
Date:
Mon Nov 28 10:31:15 2016 +0000
Revision:
7:d138c56dab20
Parent:
6:68293bed71ea
Child:
8:0711dea61312
servo blm dioptimisasi

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