KRAI 2017 / Mbed 2 deprecated MainProgram_BaseBaru_fix_omni_20April

paling baru

Dependencies:   DigitDisplay Motor PID Ping mbed millis

Fork of MainProgram_BaseBaru_fix_omni_11April by KRAI 2017

main.cpp@24:b3e632cc4533, 2017-02-02 (annotated)

Committer:
rahmadirizki18
Date:
Thu Feb 02 11:12:43 2017 +0000
Revision:
24:b3e632cc4533
Parent:
23:023b522977b2
Child:
25:054d3048dd03
Tuning PWM Motor

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