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@22:4632f58461e0, 2017-01-28 (annotated)

Committer:
MarchioKevin
Date:
Sat Jan 28 10:19:36 2017 +0000
Revision:
22:4632f58461e0
Parent:
21:da2f3d04468f
Child:
23:023b522977b2
Base baruuuuu! PID Encoder kiri belum dioptimasi

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 */
MarchioKevin 22:4632f58461e0 67 Motor motor1(PB_7, PA_14, PA_15); // pwm, fwd, rev, Motor Depan
MarchioKevin 22:4632f58461e0 68 Motor motor2(PB_8, PA_13, PB_0); // pwm, fwd, rev, Motor Belakang
fanny868 0:9072e932503c 69
calmantara186 16:90119f03c5d1 70 /* Deklarasi Motor Launcher */
MarchioKevin 20:54dc93e7b016 71 Motor motorld(PA_8, PC_1, PC_2); // pwm, fwd, rev
MarchioKevin 20:54dc93e7b016 72 Motor motorlb(PA_9, PA_4, PC_15 ); // pwm, fwd, rev
rahmadirizki18 5:3aa203218306 73
calmantara186 16:90119f03c5d1 74 /* Deklarasi Servo Launcher */
rahmadirizki18 14:6d389e99981c 75 Servo servoS(PB_2);
rahmadirizki18 14:6d389e99981c 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 3:1287fccc11be 92 joysticknucleo joystick(PA_0,PA_1);
fanny868 0:9072e932503c 93
calmantara186 16:90119f03c5d1 94 /* Variabel Stick */
fanny868 0:9072e932503c 95 char case_ger;
MarchioKevin 22:4632f58461e0 96 bool launcher = false, servoGo = false;
fanny868 0:9072e932503c 97
MarchioKevin 22:4632f58461e0 98 /****************************************************/
MarchioKevin 22:4632f58461e0 99 /* Deklarasi Fungsi dan Procedure */
MarchioKevin 22:4632f58461e0 100 /****************************************************/
calmantara186 16:90119f03c5d1 101 int case_gerak(){
MarchioKevin 22:4632f58461e0 102 /****************************************************/
calmantara186 16:90119f03c5d1 103 ** Gerak Motor Base
calmantara186 16:90119f03c5d1 104 ** Case 1 : Pivot kanan
calmantara186 16:90119f03c5d1 105 ** Case 2 : Pivot Kiri
MarchioKevin 22:4632f58461e0 106 ** Case 3 : Kanan
MarchioKevin 22:4632f58461e0 107 ** Case 4 : Kiri
MarchioKevin 22:4632f58461e0 108 ** Case 5 : Break
calmantara186 16:90119f03c5d1 109 ****************************************************/
MarchioKevin 22:4632f58461e0 110
fanny868 0:9072e932503c 111 int casegerak;
calmantara186 16:90119f03c5d1 112 if (!joystick.L1 && joystick.R1) {
fanny868 0:9072e932503c 113 // Pivot Kanan
fanny868 0:9072e932503c 114 casegerak = 1;
calmantara186 16:90119f03c5d1 115 }
calmantara186 16:90119f03c5d1 116 else if (!joystick.R1 && joystick.L1) {
fanny868 0:9072e932503c 117 // Pivot Kiri
fanny868 0:9072e932503c 118 casegerak = 2;
calmantara186 16:90119f03c5d1 119 }
calmantara186 16:90119f03c5d1 120 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {
fanny868 0:9072e932503c 121 // Kanan
MarchioKevin 22:4632f58461e0 122 casegerak = 3;
calmantara186 16:90119f03c5d1 123 }
calmantara186 16:90119f03c5d1 124 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {
fanny868 0:9072e932503c 125 // Kiri
MarchioKevin 22:4632f58461e0 126 casegerak = 4;
calmantara186 16:90119f03c5d1 127 }
calmantara186 16:90119f03c5d1 128 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
MarchioKevin 22:4632f58461e0 129 // Break
MarchioKevin 22:4632f58461e0 130 casegerak = 5;
calmantara186 16:90119f03c5d1 131 }
fanny868 0:9072e932503c 132 return(casegerak);
fanny868 0:9072e932503c 133 }
fanny868 0:9072e932503c 134
calmantara186 16:90119f03c5d1 135 void aktuator(){
calmantara186 16:90119f03c5d1 136 /* Fungsi untuk menggerakkan servo */
calmantara186 16:90119f03c5d1 137 // Servo
calmantara186 16:90119f03c5d1 138 if (servoGo){
franshendri 12:e07c59c28c29 139 servoS.position(20);
franshendri 10:f0f0dc3904e0 140 wait_ms(500);
franshendri 12:e07c59c28c29 141 servoS.position(-28);
Joshua23 8:0711dea61312 142 wait_ms(500);
franshendri 12:e07c59c28c29 143 servoS.position(20);
Joshua23 8:0711dea61312 144 wait_ms(500);
franshendri 12:e07c59c28c29 145 for (int i = -0; i<=70; i++){
Joshua23 8:0711dea61312 146 servoB.position(i);
Joshua23 8:0711dea61312 147 wait_ms(10);
Joshua23 8:0711dea61312 148 }
Joshua23 8:0711dea61312 149 wait_ms(500);
Joshua23 8:0711dea61312 150 servoB.position(0);
calmantara186 16:90119f03c5d1 151 servoGo = false;
calmantara186 16:90119f03c5d1 152 }
calmantara186 16:90119f03c5d1 153 else{
franshendri 12:e07c59c28c29 154 servoS.position(20);
rahmadirizki18 6:68293bed71ea 155 servoB.position(0);
rahmadirizki18 7:d138c56dab20 156 }
rahmadirizki18 6:68293bed71ea 157
rahmadirizki18 6:68293bed71ea 158 // Motor Atas
calmantara186 16:90119f03c5d1 159 if (launcher) {
rahmadirizki18 7:d138c56dab20 160 motorld.speed(speedL);
franshendri 12:e07c59c28c29 161 motorlb.speed(speedB);
rahmadirizki18 6:68293bed71ea 162 }else{
rahmadirizki18 6:68293bed71ea 163 motorld.speed(0);
rahmadirizki18 6:68293bed71ea 164 motorlb.speed(0);
rahmadirizki18 6:68293bed71ea 165 }
rahmadirizki18 6:68293bed71ea 166
rahmadirizki18 6:68293bed71ea 167 // MOTOR Bawah
MarchioKevin 22:4632f58461e0 168 switch (case_ger) {
rahmadirizki18 17:e4229d77a5ab 169 case (1): {
rahmadirizki18 17:e4229d77a5ab 170 // Pivot Kanan
rahmadirizki18 17:e4229d77a5ab 171 motor1.speed(-PIVOT);
rahmadirizki18 17:e4229d77a5ab 172 motor2.speed(-PIVOT);
fanny868 0:9072e932503c 173 break;
rahmadirizki18 17:e4229d77a5ab 174 }
rahmadirizki18 17:e4229d77a5ab 175 case (2): {
rahmadirizki18 17:e4229d77a5ab 176 // Pivot Kiri
rahmadirizki18 17:e4229d77a5ab 177 motor1.speed(PIVOT);
rahmadirizki18 17:e4229d77a5ab 178 motor2.speed(PIVOT);
fanny868 0:9072e932503c 179 break;
MarchioKevin 22:4632f58461e0 180 }
MarchioKevin 22:4632f58461e0 181 case (3) : {
MarchioKevin 22:4632f58461e0 182 // Kanan
MarchioKevin 22:4632f58461e0 183 motor1.speed(-VMAX-vpid);
MarchioKevin 22:4632f58461e0 184 motor2.speed(VMAX+vpid);
rahmadirizki18 17:e4229d77a5ab 185 break;
MarchioKevin 22:4632f58461e0 186 }
MarchioKevin 22:4632f58461e0 187 case (4) : {
rahmadirizki18 17:e4229d77a5ab 188 // Kiri
MarchioKevin 22:4632f58461e0 189 motor1.speed(VMAX-vpid);
MarchioKevin 22:4632f58461e0 190 motor2.speed(-VMAX+vpid);
fanny868 0:9072e932503c 191 break;
rahmadirizki18 17:e4229d77a5ab 192 }
rahmadirizki18 17:e4229d77a5ab 193 default : {
MarchioKevin 22:4632f58461e0 194 motor1.brake(1);
MarchioKevin 22:4632f58461e0 195 motor2.brake(1);
rahmadirizki18 17:e4229d77a5ab 196 }
MarchioKevin 22:4632f58461e0 197 } // End Switch
rahmadirizki18 5:3aa203218306 198 }
rahmadirizki18 5:3aa203218306 199
calmantara186 16:90119f03c5d1 200 void setCenter(){
calmantara186 16:90119f03c5d1 201 /* Fungsi untuk menentukan center dari robot */
rahmadirizki18 5:3aa203218306 202 encoderKiri.reset();
rahmadirizki18 5:3aa203218306 203 }
rahmadirizki18 5:3aa203218306 204
calmantara186 16:90119f03c5d1 205 float getTetha(){
calmantara186 16:90119f03c5d1 206 /* Fungsi untuk mendapatkan nilai tetha */
MarchioKevin 22:4632f58461e0 207 float busurKir;
MarchioKevin 22:4632f58461e0 208 busurKir = ((encoderKiri.getPulses())/(float)(2000.0)*k_enc);
rahmadirizki18 5:3aa203218306 209
MarchioKevin 22:4632f58461e0 210 return -(busurKir);
fanny868 0:9072e932503c 211 }
fanny868 0:9072e932503c 212
MarchioKevin 22:4632f58461e0 213 void gotoXYT(float Ta){
MarchioKevin 22:4632f58461e0 214 /* Fungsi untuk bergerak ke target */
MarchioKevin 22:4632f58461e0 215 float vt;
calmantara186 16:90119f03c5d1 216
calmantara186 16:90119f03c5d1 217 errT = Ta-getTetha();
calmantara186 16:90119f03c5d1 218 Vt = kp_tetha*errT;
calmantara186 16:90119f03c5d1 219
MarchioKevin 22:4632f58461e0 220 if (vt>speed1)
MarchioKevin 22:4632f58461e0 221 { vt = speed1; }
MarchioKevin 22:4632f58461e0 222 else if (vt<-speed1)
MarchioKevin 22:4632f58461e0 223 { vt = -speed1; }
calmantara186 16:90119f03c5d1 224
MarchioKevin 22:4632f58461e0 225 if ((((errT > 0.05) || (errT<-0.05))){
MarchioKevin 22:4632f58461e0 226 vpid = vt;
calmantara186 16:90119f03c5d1 227 }
calmantara186 16:90119f03c5d1 228 else{
MarchioKevin 22:4632f58461e0 229 vpid = 0;
calmantara186 16:90119f03c5d1 230 }
calmantara186 16:90119f03c5d1 231 }
rahmadirizki18 5:3aa203218306 232
calmantara186 16:90119f03c5d1 233 void speedLauncher(){
calmantara186 16:90119f03c5d1 234 /* Fungsi untuk speed launcher */
franshendri 12:e07c59c28c29 235 if (joystick.R3_click and speedL < 0.8){
MarchioKevin 22:4632f58461e0 236 speedL = speedL + 0.01; // PWM++ Motor Belakang
calmantara186 16:90119f03c5d1 237 }
rahmadirizki18 7:d138c56dab20 238 if (joystick.L3_click and speedL > 0.1){
MarchioKevin 22:4632f58461e0 239 speedL = speedL - 0.01; // PWM-- Motor Belakang
calmantara186 16:90119f03c5d1 240 }
rahmadirizki18 13:8ab42383a2ca 241 if (joystick.R2_click and speedB < 0.8 ){
MarchioKevin 22:4632f58461e0 242 speedB = speedB + 0.01; // PWM++ Motor Depan
calmantara186 16:90119f03c5d1 243 }
rahmadirizki18 13:8ab42383a2ca 244 if (joystick.L2_click and speedB > 0.1 ){
MarchioKevin 22:4632f58461e0 245 speedB = speedB - 0.01; // PWM-- Motor Depan
calmantara186 16:90119f03c5d1 246 }
rahmadirizki18 7:d138c56dab20 247 }
franshendri 12:e07c59c28c29 248
MarchioKevin 22:4632f58461e0 249 /*********************************************************/
MarchioKevin 22:4632f58461e0 250 /* Main Function */
MarchioKevin 22:4632f58461e0 251 /*********************************************************/
calmantara186 16:90119f03c5d1 252
calmantara186 16:90119f03c5d1 253 int main (void){
calmantara186 16:90119f03c5d1 254 /* Set baud rate - 115200 */
fanny868 0:9072e932503c 255 joystick.setup();
rahmadirizki18 6:68293bed71ea 256 wait_ms(1000);
rahmadirizki18 5:3aa203218306 257 setCenter();
rahmadirizki18 5:3aa203218306 258 wait_ms(500);
rahmadirizki18 5:3aa203218306 259
calmantara186 16:90119f03c5d1 260 /* Posisi Awal */
rahmadirizki18 5:3aa203218306 261 Tetha = 0;
calmantara186 16:90119f03c5d1 262
calmantara186 16:90119f03c5d1 263 /* Untuk mendapatkan serial dari Arduino */
fanny868 0:9072e932503c 264 while(1)
fanny868 0:9072e932503c 265 {
fanny868 0:9072e932503c 266 // Interrupt Serial
calmantara186 16:90119f03c5d1 267 joystick.idle();
calmantara186 16:90119f03c5d1 268
calmantara186 16:90119f03c5d1 269 if(joystick.readable() ) {
fanny868 0:9072e932503c 270 // Panggil fungsi pembacaan joystik
fanny868 0:9072e932503c 271 joystick.baca_data();
calmantara186 16:90119f03c5d1 272
fanny868 0:9072e932503c 273 // Panggil fungsi pengolahan data joystik
fanny868 0:9072e932503c 274 joystick.olah_data();
calmantara186 16:90119f03c5d1 275
calmantara186 16:90119f03c5d1 276 // Masuk ke case gerak
fanny868 0:9072e932503c 277 case_ger = case_gerak();
rahmadirizki18 3:1287fccc11be 278 aktuator();
calmantara186 16:90119f03c5d1 279
rahmadirizki18 17:e4229d77a5ab 280 if (joystick.segitiga_click) launcher = !launcher;
rahmadirizki18 17:e4229d77a5ab 281 if (joystick.lingkaran_click) servoGo = true;
rahmadirizki18 7:d138c56dab20 282 speedLauncher();
calmantara186 16:90119f03c5d1 283 }
calmantara186 16:90119f03c5d1 284 else {
calmantara186 16:90119f03c5d1 285 joystick.idle();
MarchioKevin 21:da2f3d04468f 286 }
fanny868 0:9072e932503c 287 }
fanny868 0:9072e932503c 288 }