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@23:023b522977b2, 2017-02-01 (annotated)

Committer:
rahmadirizki18
Date:
Wed Feb 01 15:00:08 2017 +0000
Revision:
23:023b522977b2
Parent:
22:4632f58461e0
Child:
24:b3e632cc4533
Penyesuaian 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 : */
MarchioKevin 22:4632f58461e0 6 /* ______________________ */
MarchioKevin 22:4632f58461e0 7 /* / \ Rode Depan Belakang: */
MarchioKevin 22:4632f58461e0 8 /* / 2 (Belakang) \ Omniwheel */
MarchioKevin 22:4632f58461e0 9 /* | | */
MarchioKevin 22:4632f58461e0 10 /* | 3 (Encoder) 4 | Roda Kiri Kanan: */
MarchioKevin 22:4632f58461e0 11 /* | | Fixed Wheel */
MarchioKevin 22:4632f58461e0 12 /* \ 1 (Depan) / */
MarchioKevin 22:4632f58461e0 13 /* \______________________/ Putaran berlawanan arah */
MarchioKevin 22:4632f58461e0 14 /* jarum jam positif */
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 /* */
MarchioKevin 20:54dc93e7b016 21 /* Joystick */
MarchioKevin 20:54dc93e7b016 22 /* Kanan => Posisi target x ditambah 'perpindahan' */
MarchioKevin 20:54dc93e7b016 23 /* Kiri => Posisi target x dikurang 'perpindahan' */
rahmadirizki18 6:68293bed71ea 24 /* */
rahmadirizki18 6:68293bed71ea 25 /* Tombol silang => Kembali keposisi Awal */
rahmadirizki18 6:68293bed71ea 26 /* Tombol segitiga => Aktif motor Launcher */
rahmadirizki18 13:8ab42383a2ca 27 /* Tombol lingkaran=> Aktif servo Launcher */
MarchioKevin 22:4632f58461e0 28 /* Tombol L1 => Pivot Kiri */
MarchioKevin 22:4632f58461e0 29 /* Tombol R1 => Pivot Kanan */
MarchioKevin 22:4632f58461e0 30 /* Tombol L3 => PWM Launcher Belakang dikurangin */
MarchioKevin 22:4632f58461e0 31 /* Tombol R3 => PWM Launcher Belakang ditambahin */
rahmadirizki18 13:8ab42383a2ca 32 /* */
calmantara186 16:90119f03c5d1 33 /* Bismillahirahmanirrahim */
MarchioKevin 20:54dc93e7b016 34 /* Jagalah Kebersihan Kodingan */
rahmadirizki18 6:68293bed71ea 35 /****************************************************************************/
rahmadirizki18 6:68293bed71ea 36
fanny868 0:9072e932503c 37 #include "mbed.h"
fanny868 0:9072e932503c 38 #include "JoystickPS3.h"
fanny868 0:9072e932503c 39 #include "Motor.h"
rahmadirizki18 6:68293bed71ea 40 #include "Servo.h"
rahmadirizki18 5:3aa203218306 41 #include "encoderKRAI.h"
rahmadirizki18 5:3aa203218306 42
calmantara186 16:90119f03c5d1 43 /***********************************************/
calmantara186 16:90119f03c5d1 44 /* Konstanta dan Variabel */
calmantara186 16:90119f03c5d1 45 /***********************************************/
calmantara186 16:90119f03c5d1 46 #define PI 3.14159265
MarchioKevin 22:4632f58461e0 47 #define D_ENCODER 0.997 // Diameter Roda Encoder
MarchioKevin 22:4632f58461e0 48 #define D_ROBOT // Diameter Roda Robot
MarchioKevin 22:4632f58461e0 49 #define VMAX 0.3 // Kiri Kanan
rahmadirizki18 17:e4229d77a5ab 50 #define PIVOT 0.4 // Pivka, Pivki
rahmadirizki18 17:e4229d77a5ab 51 #define PERPINDAHAN 1 // Perpindahan ke kanan dan kiri
rahmadirizki18 5:3aa203218306 52
calmantara186 16:90119f03c5d1 53 float k_enc = PI*D_ENCODER;
rahmadirizki18 5:3aa203218306 54
MarchioKevin 22:4632f58461e0 55 float speedT = 0.2;
MarchioKevin 22:4632f58461e0 56 float speedB = 0.43;
MarchioKevin 22:4632f58461e0 57 float speedL = 0.4;
rahmadirizki18 5:3aa203218306 58
MarchioKevin 22:4632f58461e0 59 float vpid = 0;
MarchioKevin 22:4632f58461e0 60
MarchioKevin 22:4632f58461e0 61 float kpX = 0.5, kpY = 0.5, kp_tetha = 0.03;
calmantara186 16:90119f03c5d1 62
calmantara186 16:90119f03c5d1 63 /* Deklarasi encoder */
MarchioKevin 21:da2f3d04468f 64 encoderKRAI encoderKiri( PC_4, PB_15, 2000, encoderKRAI::X2_ENCODING); //inA, inB, pin Indeks (NC = tak ada), resolusi, pembacaan
rahmadirizki18 15:98f0d56b14f0 65
calmantara186 16:90119f03c5d1 66 /* Deklarasi Motor Base */
rahmadirizki18 23:023b522977b2 67 Motor motor1(PB_9, PA_12, PC_5); // pwm, fwd, rev, Motor Depan
rahmadirizki18 23:023b522977b2 68 Motor motor2(PB_6, PB_12, PA_7); // pwm, fwd, rev, Motor Belakang
fanny868 0:9072e932503c 69
calmantara186 16:90119f03c5d1 70 /* Deklarasi Motor Launcher */
rahmadirizki18 23:023b522977b2 71 //Motor motorld(PA_8, PC_1, PC_2); // pwm, fwd, rev
rahmadirizki18 23:023b522977b2 72 //Motor motorlb(PA_0, PA_4, PC_15 ); // pwm, fwd, rev
rahmadirizki18 5:3aa203218306 73
calmantara186 16:90119f03c5d1 74 /* Deklarasi Servo Launcher */
rahmadirizki18 23:023b522977b2 75 //Servo servoS(PB_2);
rahmadirizki18 23:023b522977b2 76 //Servo servoB(PA_5);
rahmadirizki18 6:68293bed71ea 77
calmantara186 16:90119f03c5d1 78 /**
calmantara186 16:90119f03c5d1 79 * posX dan posY berdasarkan arah robot
calmantara186 16:90119f03c5d1 80 * encoder Depan & Belakang sejajar sumbu Y
calmantara186 16:90119f03c5d1 81 * encoder Kanan & Kiri sejajar sumbu X
calmantara186 16:90119f03c5d1 82 **/
rahmadirizki18 5:3aa203218306 83
calmantara186 16:90119f03c5d1 84 /* Variabel Encoder */
MarchioKevin 22:4632f58461e0 85 float errT, Tetha; // Variabel yang didapatkan encoder
rahmadirizki18 5:3aa203218306 86
calmantara186 16:90119f03c5d1 87 /* Fungsi dan Procedur Encoder */
calmantara186 16:90119f03c5d1 88 void setCenter(); // Fungsi reset agar robot di tengah
MarchioKevin 20:54dc93e7b016 89 float getTetha(); // Fungsi mendapatkan jarak Tetha
calmantara186 16:90119f03c5d1 90
calmantara186 16:90119f03c5d1 91 /* Inisialisasi Pin TX-RX Joystik dan PC */
rahmadirizki18 23:023b522977b2 92 joysticknucleo joystick(PA_9,PA_10);
rahmadirizki18 23:023b522977b2 93 Serial pc(USBTX,USBRX);
fanny868 0:9072e932503c 94
calmantara186 16:90119f03c5d1 95 /* Variabel Stick */
fanny868 0:9072e932503c 96 char case_ger;
MarchioKevin 22:4632f58461e0 97 bool launcher = false, servoGo = false;
fanny868 0:9072e932503c 98
MarchioKevin 22:4632f58461e0 99 /****************************************************/
MarchioKevin 22:4632f58461e0 100 /* Deklarasi Fungsi dan Procedure */
MarchioKevin 22:4632f58461e0 101 /****************************************************/
calmantara186 16:90119f03c5d1 102 int case_gerak(){
rahmadirizki18 23:023b522977b2 103 /****************************************************
calmantara186 16:90119f03c5d1 104 ** Gerak Motor Base
calmantara186 16:90119f03c5d1 105 ** Case 1 : Pivot kanan
calmantara186 16:90119f03c5d1 106 ** Case 2 : Pivot Kiri
MarchioKevin 22:4632f58461e0 107 ** Case 3 : Kanan
MarchioKevin 22:4632f58461e0 108 ** Case 4 : Kiri
MarchioKevin 22:4632f58461e0 109 ** Case 5 : Break
calmantara186 16:90119f03c5d1 110 ****************************************************/
MarchioKevin 22:4632f58461e0 111
fanny868 0:9072e932503c 112 int casegerak;
calmantara186 16:90119f03c5d1 113 if (!joystick.L1 && joystick.R1) {
fanny868 0:9072e932503c 114 // Pivot Kanan
fanny868 0:9072e932503c 115 casegerak = 1;
calmantara186 16:90119f03c5d1 116 }
calmantara186 16:90119f03c5d1 117 else if (!joystick.R1 && joystick.L1) {
fanny868 0:9072e932503c 118 // Pivot Kiri
fanny868 0:9072e932503c 119 casegerak = 2;
calmantara186 16:90119f03c5d1 120 }
calmantara186 16:90119f03c5d1 121 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {
fanny868 0:9072e932503c 122 // Kanan
MarchioKevin 22:4632f58461e0 123 casegerak = 3;
rahmadirizki18 23:023b522977b2 124 pc.printf("kanan");
calmantara186 16:90119f03c5d1 125 }
calmantara186 16:90119f03c5d1 126 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {
fanny868 0:9072e932503c 127 // Kiri
rahmadirizki18 23:023b522977b2 128 casegerak = 4;
rahmadirizki18 23:023b522977b2 129 pc.printf("kiri");
calmantara186 16:90119f03c5d1 130 }
calmantara186 16:90119f03c5d1 131 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
MarchioKevin 22:4632f58461e0 132 // Break
MarchioKevin 22:4632f58461e0 133 casegerak = 5;
calmantara186 16:90119f03c5d1 134 }
fanny868 0:9072e932503c 135 return(casegerak);
fanny868 0:9072e932503c 136 }
fanny868 0:9072e932503c 137
calmantara186 16:90119f03c5d1 138 void aktuator(){
calmantara186 16:90119f03c5d1 139 /* Fungsi untuk menggerakkan servo */
calmantara186 16:90119f03c5d1 140 // Servo
rahmadirizki18 23:023b522977b2 141 /* if (servoGo){
franshendri 12:e07c59c28c29 142 servoS.position(20);
franshendri 10:f0f0dc3904e0 143 wait_ms(500);
franshendri 12:e07c59c28c29 144 servoS.position(-28);
Joshua23 8:0711dea61312 145 wait_ms(500);
franshendri 12:e07c59c28c29 146 servoS.position(20);
Joshua23 8:0711dea61312 147 wait_ms(500);
franshendri 12:e07c59c28c29 148 for (int i = -0; i<=70; i++){
Joshua23 8:0711dea61312 149 servoB.position(i);
Joshua23 8:0711dea61312 150 wait_ms(10);
Joshua23 8:0711dea61312 151 }
Joshua23 8:0711dea61312 152 wait_ms(500);
Joshua23 8:0711dea61312 153 servoB.position(0);
calmantara186 16:90119f03c5d1 154 servoGo = false;
calmantara186 16:90119f03c5d1 155 }
calmantara186 16:90119f03c5d1 156 else{
franshendri 12:e07c59c28c29 157 servoS.position(20);
rahmadirizki18 6:68293bed71ea 158 servoB.position(0);
rahmadirizki18 7:d138c56dab20 159 }
rahmadirizki18 6:68293bed71ea 160
rahmadirizki18 6:68293bed71ea 161 // Motor Atas
calmantara186 16:90119f03c5d1 162 if (launcher) {
rahmadirizki18 7:d138c56dab20 163 motorld.speed(speedL);
franshendri 12:e07c59c28c29 164 motorlb.speed(speedB);
rahmadirizki18 6:68293bed71ea 165 }else{
rahmadirizki18 6:68293bed71ea 166 motorld.speed(0);
rahmadirizki18 6:68293bed71ea 167 motorlb.speed(0);
rahmadirizki18 6:68293bed71ea 168 }
rahmadirizki18 23:023b522977b2 169 */
rahmadirizki18 6:68293bed71ea 170 // MOTOR Bawah
MarchioKevin 22:4632f58461e0 171 switch (case_ger) {
rahmadirizki18 17:e4229d77a5ab 172 case (1): {
rahmadirizki18 17:e4229d77a5ab 173 // Pivot Kanan
rahmadirizki18 17:e4229d77a5ab 174 motor1.speed(-PIVOT);
rahmadirizki18 17:e4229d77a5ab 175 motor2.speed(-PIVOT);
fanny868 0:9072e932503c 176 break;
rahmadirizki18 17:e4229d77a5ab 177 }
rahmadirizki18 17:e4229d77a5ab 178 case (2): {
rahmadirizki18 17:e4229d77a5ab 179 // Pivot Kiri
rahmadirizki18 17:e4229d77a5ab 180 motor1.speed(PIVOT);
rahmadirizki18 17:e4229d77a5ab 181 motor2.speed(PIVOT);
fanny868 0:9072e932503c 182 break;
MarchioKevin 22:4632f58461e0 183 }
MarchioKevin 22:4632f58461e0 184 case (3) : {
MarchioKevin 22:4632f58461e0 185 // Kanan
MarchioKevin 22:4632f58461e0 186 motor1.speed(-VMAX-vpid);
MarchioKevin 22:4632f58461e0 187 motor2.speed(VMAX+vpid);
rahmadirizki18 17:e4229d77a5ab 188 break;
MarchioKevin 22:4632f58461e0 189 }
MarchioKevin 22:4632f58461e0 190 case (4) : {
rahmadirizki18 17:e4229d77a5ab 191 // Kiri
MarchioKevin 22:4632f58461e0 192 motor1.speed(VMAX-vpid);
MarchioKevin 22:4632f58461e0 193 motor2.speed(-VMAX+vpid);
fanny868 0:9072e932503c 194 break;
rahmadirizki18 17:e4229d77a5ab 195 }
rahmadirizki18 17:e4229d77a5ab 196 default : {
MarchioKevin 22:4632f58461e0 197 motor1.brake(1);
MarchioKevin 22:4632f58461e0 198 motor2.brake(1);
rahmadirizki18 17:e4229d77a5ab 199 }
MarchioKevin 22:4632f58461e0 200 } // End Switch
rahmadirizki18 5:3aa203218306 201 }
rahmadirizki18 5:3aa203218306 202
calmantara186 16:90119f03c5d1 203 void setCenter(){
calmantara186 16:90119f03c5d1 204 /* Fungsi untuk menentukan center dari robot */
rahmadirizki18 5:3aa203218306 205 encoderKiri.reset();
rahmadirizki18 5:3aa203218306 206 }
rahmadirizki18 5:3aa203218306 207
calmantara186 16:90119f03c5d1 208 float getTetha(){
calmantara186 16:90119f03c5d1 209 /* Fungsi untuk mendapatkan nilai tetha */
MarchioKevin 22:4632f58461e0 210 float busurKir;
MarchioKevin 22:4632f58461e0 211 busurKir = ((encoderKiri.getPulses())/(float)(2000.0)*k_enc);
rahmadirizki18 5:3aa203218306 212
MarchioKevin 22:4632f58461e0 213 return -(busurKir);
fanny868 0:9072e932503c 214 }
fanny868 0:9072e932503c 215
MarchioKevin 22:4632f58461e0 216 void gotoXYT(float Ta){
MarchioKevin 22:4632f58461e0 217 /* Fungsi untuk bergerak ke target */
MarchioKevin 22:4632f58461e0 218 float vt;
calmantara186 16:90119f03c5d1 219
calmantara186 16:90119f03c5d1 220 errT = Ta-getTetha();
rahmadirizki18 23:023b522977b2 221 vt = kp_tetha*errT;
calmantara186 16:90119f03c5d1 222
rahmadirizki18 23:023b522977b2 223 if (vt>speedT)
rahmadirizki18 23:023b522977b2 224 { vt = speedT; }
rahmadirizki18 23:023b522977b2 225 else if (vt<-speedT)
rahmadirizki18 23:023b522977b2 226 { vt = -speedT; }
calmantara186 16:90119f03c5d1 227
rahmadirizki18 23:023b522977b2 228 if (((errT > 0.05) || (errT<-0.05))){
MarchioKevin 22:4632f58461e0 229 vpid = vt;
calmantara186 16:90119f03c5d1 230 }
calmantara186 16:90119f03c5d1 231 else{
MarchioKevin 22:4632f58461e0 232 vpid = 0;
calmantara186 16:90119f03c5d1 233 }
calmantara186 16:90119f03c5d1 234 }
rahmadirizki18 5:3aa203218306 235
calmantara186 16:90119f03c5d1 236 void speedLauncher(){
calmantara186 16:90119f03c5d1 237 /* Fungsi untuk speed launcher */
franshendri 12:e07c59c28c29 238 if (joystick.R3_click and speedL < 0.8){
MarchioKevin 22:4632f58461e0 239 speedL = speedL + 0.01; // PWM++ Motor Belakang
calmantara186 16:90119f03c5d1 240 }
rahmadirizki18 7:d138c56dab20 241 if (joystick.L3_click and speedL > 0.1){
MarchioKevin 22:4632f58461e0 242 speedL = speedL - 0.01; // PWM-- Motor Belakang
calmantara186 16:90119f03c5d1 243 }
rahmadirizki18 13:8ab42383a2ca 244 if (joystick.R2_click and speedB < 0.8 ){
MarchioKevin 22:4632f58461e0 245 speedB = speedB + 0.01; // PWM++ Motor Depan
calmantara186 16:90119f03c5d1 246 }
rahmadirizki18 13:8ab42383a2ca 247 if (joystick.L2_click and speedB > 0.1 ){
MarchioKevin 22:4632f58461e0 248 speedB = speedB - 0.01; // PWM-- Motor Depan
calmantara186 16:90119f03c5d1 249 }
rahmadirizki18 7:d138c56dab20 250 }
franshendri 12:e07c59c28c29 251
MarchioKevin 22:4632f58461e0 252 /*********************************************************/
MarchioKevin 22:4632f58461e0 253 /* Main Function */
MarchioKevin 22:4632f58461e0 254 /*********************************************************/
calmantara186 16:90119f03c5d1 255
calmantara186 16:90119f03c5d1 256 int main (void){
calmantara186 16:90119f03c5d1 257 /* Set baud rate - 115200 */
fanny868 0:9072e932503c 258 joystick.setup();
rahmadirizki18 23:023b522977b2 259 pc.baud(115200);
rahmadirizki18 6:68293bed71ea 260 wait_ms(1000);
rahmadirizki18 5:3aa203218306 261 setCenter();
rahmadirizki18 5:3aa203218306 262 wait_ms(500);
rahmadirizki18 23:023b522977b2 263 pc.printf("ready....");
calmantara186 16:90119f03c5d1 264 /* Posisi Awal */
rahmadirizki18 5:3aa203218306 265 Tetha = 0;
calmantara186 16:90119f03c5d1 266
calmantara186 16:90119f03c5d1 267 /* Untuk mendapatkan serial dari Arduino */
fanny868 0:9072e932503c 268 while(1)
fanny868 0:9072e932503c 269 {
fanny868 0:9072e932503c 270 // Interrupt Serial
calmantara186 16:90119f03c5d1 271 joystick.idle();
calmantara186 16:90119f03c5d1 272
calmantara186 16:90119f03c5d1 273 if(joystick.readable() ) {
fanny868 0:9072e932503c 274 // Panggil fungsi pembacaan joystik
fanny868 0:9072e932503c 275 joystick.baca_data();
calmantara186 16:90119f03c5d1 276
fanny868 0:9072e932503c 277 // Panggil fungsi pengolahan data joystik
fanny868 0:9072e932503c 278 joystick.olah_data();
calmantara186 16:90119f03c5d1 279
calmantara186 16:90119f03c5d1 280 // Masuk ke case gerak
fanny868 0:9072e932503c 281 case_ger = case_gerak();
rahmadirizki18 3:1287fccc11be 282 aktuator();
calmantara186 16:90119f03c5d1 283
rahmadirizki18 17:e4229d77a5ab 284 if (joystick.segitiga_click) launcher = !launcher;
rahmadirizki18 17:e4229d77a5ab 285 if (joystick.lingkaran_click) servoGo = true;
rahmadirizki18 7:d138c56dab20 286 speedLauncher();
calmantara186 16:90119f03c5d1 287 }
calmantara186 16:90119f03c5d1 288 else {
calmantara186 16:90119f03c5d1 289 joystick.idle();
MarchioKevin 21:da2f3d04468f 290 }
fanny868 0:9072e932503c 291 }
fanny868 0:9072e932503c 292 }