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@25:054d3048dd03, 2017-02-10 (annotated)

Committer:
Joshua23
Date:
Fri Feb 10 10:29:45 2017 +0000
Revision:
25:054d3048dd03
Parent:
24:b3e632cc4533
Child:
26:256160a1a82d
update closeloop encoder ; ; masih perlu tuning

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
Joshua23 25:054d3048dd03 49 #define D_ENCODER 10 // Diameter Roda Encoder
Joshua23 25:054d3048dd03 50 #define D_ROBOT 80 // 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
Joshua23 25:054d3048dd03 55 float Vt;
Joshua23 25:054d3048dd03 56
calmantara186 16:90119f03c5d1 57 float k_enc = PI*D_ENCODER;
Joshua23 25:054d3048dd03 58 float k_robot = PI*D_ROBOT;
rahmadirizki18 5:3aa203218306 59
MarchioKevin 22:4632f58461e0 60 float speedT = 0.2;
MarchioKevin 22:4632f58461e0 61 float speedB = 0.43;
MarchioKevin 22:4632f58461e0 62 float speedL = 0.4;
rahmadirizki18 5:3aa203218306 63
MarchioKevin 22:4632f58461e0 64 float vpid = 0;
MarchioKevin 22:4632f58461e0 65
MarchioKevin 22:4632f58461e0 66 float kpX = 0.5, kpY = 0.5, kp_tetha = 0.03;
calmantara186 16:90119f03c5d1 67
calmantara186 16:90119f03c5d1 68 /* Deklarasi encoder */
Joshua23 25:054d3048dd03 69 encoderKRAI encoderKiri(PC_4, PB_15, 2000, encoderKRAI::X2_ENCODING); //inA, inB, pin Indeks (NC = tak ada), resolusi, pembacaan
rahmadirizki18 15:98f0d56b14f0 70
calmantara186 16:90119f03c5d1 71 /* Deklarasi Motor Base */
Joshua23 25:054d3048dd03 72 Motor motor1(PB_9, PC_5, PA_12); // pwm, fwd, rev, Motor Depan
Joshua23 25:054d3048dd03 73 Motor motor2(PB_6, PB_1, PB_12); // pwm, fwd, rev, Motor Belakang
fanny868 0:9072e932503c 74
calmantara186 16:90119f03c5d1 75 /* Deklarasi Motor Launcher */
rahmadirizki18 23:023b522977b2 76 //Motor motorld(PA_8, PC_1, PC_2); // pwm, fwd, rev
rahmadirizki18 23:023b522977b2 77 //Motor motorlb(PA_0, PA_4, PC_15 ); // pwm, fwd, rev
rahmadirizki18 5:3aa203218306 78
calmantara186 16:90119f03c5d1 79 /* Deklarasi Servo Launcher */
rahmadirizki18 23:023b522977b2 80 //Servo servoS(PB_2);
rahmadirizki18 23:023b522977b2 81 //Servo servoB(PA_5);
rahmadirizki18 6:68293bed71ea 82
calmantara186 16:90119f03c5d1 83 /**
calmantara186 16:90119f03c5d1 84 * posX dan posY berdasarkan arah robot
calmantara186 16:90119f03c5d1 85 * encoder Depan & Belakang sejajar sumbu Y
calmantara186 16:90119f03c5d1 86 * encoder Kanan & Kiri sejajar sumbu X
calmantara186 16:90119f03c5d1 87 **/
rahmadirizki18 5:3aa203218306 88
calmantara186 16:90119f03c5d1 89 /* Variabel Encoder */
MarchioKevin 22:4632f58461e0 90 float errT, Tetha; // Variabel yang didapatkan encoder
rahmadirizki18 5:3aa203218306 91
calmantara186 16:90119f03c5d1 92 /* Fungsi dan Procedur Encoder */
calmantara186 16:90119f03c5d1 93 void setCenter(); // Fungsi reset agar robot di tengah
MarchioKevin 20:54dc93e7b016 94 float getTetha(); // Fungsi mendapatkan jarak Tetha
calmantara186 16:90119f03c5d1 95
calmantara186 16:90119f03c5d1 96 /* Inisialisasi Pin TX-RX Joystik dan PC */
Joshua23 25:054d3048dd03 97 joysticknucleo joystick(PA_0,PA_1);
rahmadirizki18 23:023b522977b2 98 Serial pc(USBTX,USBRX);
fanny868 0:9072e932503c 99
calmantara186 16:90119f03c5d1 100 /* Variabel Stick */
fanny868 0:9072e932503c 101 char case_ger;
MarchioKevin 22:4632f58461e0 102 bool launcher = false, servoGo = false;
fanny868 0:9072e932503c 103
MarchioKevin 22:4632f58461e0 104 /****************************************************/
MarchioKevin 22:4632f58461e0 105 /* Deklarasi Fungsi dan Procedure */
MarchioKevin 22:4632f58461e0 106 /****************************************************/
Joshua23 25:054d3048dd03 107
Joshua23 25:054d3048dd03 108 float pid(float Kp, float Ki, float Kd)
Joshua23 25:054d3048dd03 109 {
Joshua23 25:054d3048dd03 110 int errorP;
Joshua23 25:054d3048dd03 111
Joshua23 25:054d3048dd03 112 errorP = getTetha();
Joshua23 25:054d3048dd03 113
Joshua23 25:054d3048dd03 114 return (float)Kp*errorP;
Joshua23 25:054d3048dd03 115 }
Joshua23 25:054d3048dd03 116
calmantara186 16:90119f03c5d1 117 int case_gerak(){
rahmadirizki18 23:023b522977b2 118 /****************************************************
calmantara186 16:90119f03c5d1 119 ** Gerak Motor Base
calmantara186 16:90119f03c5d1 120 ** Case 1 : Pivot kanan
calmantara186 16:90119f03c5d1 121 ** Case 2 : Pivot Kiri
MarchioKevin 22:4632f58461e0 122 ** Case 3 : Kanan
MarchioKevin 22:4632f58461e0 123 ** Case 4 : Kiri
MarchioKevin 22:4632f58461e0 124 ** Case 5 : Break
calmantara186 16:90119f03c5d1 125 ****************************************************/
MarchioKevin 22:4632f58461e0 126
fanny868 0:9072e932503c 127 int casegerak;
calmantara186 16:90119f03c5d1 128 if (!joystick.L1 && joystick.R1) {
fanny868 0:9072e932503c 129 // Pivot Kanan
fanny868 0:9072e932503c 130 casegerak = 1;
calmantara186 16:90119f03c5d1 131 }
calmantara186 16:90119f03c5d1 132 else if (!joystick.R1 && joystick.L1) {
fanny868 0:9072e932503c 133 // Pivot Kiri
fanny868 0:9072e932503c 134 casegerak = 2;
calmantara186 16:90119f03c5d1 135 }
calmantara186 16:90119f03c5d1 136 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {
fanny868 0:9072e932503c 137 // Kanan
MarchioKevin 22:4632f58461e0 138 casegerak = 3;
rahmadirizki18 23:023b522977b2 139 pc.printf("kanan");
calmantara186 16:90119f03c5d1 140 }
calmantara186 16:90119f03c5d1 141 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {
fanny868 0:9072e932503c 142 // Kiri
rahmadirizki18 23:023b522977b2 143 casegerak = 4;
rahmadirizki18 23:023b522977b2 144 pc.printf("kiri");
calmantara186 16:90119f03c5d1 145 }
calmantara186 16:90119f03c5d1 146 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
MarchioKevin 22:4632f58461e0 147 // Break
MarchioKevin 22:4632f58461e0 148 casegerak = 5;
calmantara186 16:90119f03c5d1 149 }
fanny868 0:9072e932503c 150 return(casegerak);
fanny868 0:9072e932503c 151 }
fanny868 0:9072e932503c 152
calmantara186 16:90119f03c5d1 153 void aktuator(){
calmantara186 16:90119f03c5d1 154 /* Fungsi untuk menggerakkan servo */
calmantara186 16:90119f03c5d1 155 // Servo
rahmadirizki18 23:023b522977b2 156 /* if (servoGo){
franshendri 12:e07c59c28c29 157 servoS.position(20);
franshendri 10:f0f0dc3904e0 158 wait_ms(500);
franshendri 12:e07c59c28c29 159 servoS.position(-28);
Joshua23 8:0711dea61312 160 wait_ms(500);
franshendri 12:e07c59c28c29 161 servoS.position(20);
Joshua23 8:0711dea61312 162 wait_ms(500);
franshendri 12:e07c59c28c29 163 for (int i = -0; i<=70; i++){
Joshua23 8:0711dea61312 164 servoB.position(i);
Joshua23 8:0711dea61312 165 wait_ms(10);
Joshua23 8:0711dea61312 166 }
Joshua23 8:0711dea61312 167 wait_ms(500);
Joshua23 8:0711dea61312 168 servoB.position(0);
calmantara186 16:90119f03c5d1 169 servoGo = false;
calmantara186 16:90119f03c5d1 170 }
calmantara186 16:90119f03c5d1 171 else{
franshendri 12:e07c59c28c29 172 servoS.position(20);
rahmadirizki18 6:68293bed71ea 173 servoB.position(0);
rahmadirizki18 7:d138c56dab20 174 }
rahmadirizki18 6:68293bed71ea 175
rahmadirizki18 6:68293bed71ea 176 // Motor Atas
calmantara186 16:90119f03c5d1 177 if (launcher) {
rahmadirizki18 7:d138c56dab20 178 motorld.speed(speedL);
franshendri 12:e07c59c28c29 179 motorlb.speed(speedB);
rahmadirizki18 6:68293bed71ea 180 }else{
rahmadirizki18 6:68293bed71ea 181 motorld.speed(0);
rahmadirizki18 6:68293bed71ea 182 motorlb.speed(0);
rahmadirizki18 6:68293bed71ea 183 }
rahmadirizki18 23:023b522977b2 184 */
rahmadirizki18 6:68293bed71ea 185 // MOTOR Bawah
MarchioKevin 22:4632f58461e0 186 switch (case_ger) {
rahmadirizki18 17:e4229d77a5ab 187 case (1): {
rahmadirizki18 17:e4229d77a5ab 188 // Pivot Kanan
Joshua23 25:054d3048dd03 189 motor1.speed(PIVOT);
Joshua23 25:054d3048dd03 190 motor2.speed(PIVOT);
Joshua23 25:054d3048dd03 191 setCenter();
fanny868 0:9072e932503c 192 break;
rahmadirizki18 17:e4229d77a5ab 193 }
rahmadirizki18 17:e4229d77a5ab 194 case (2): {
rahmadirizki18 17:e4229d77a5ab 195 // Pivot Kiri
Joshua23 25:054d3048dd03 196 motor1.speed(-PIVOT);
Joshua23 25:054d3048dd03 197 motor2.speed(-PIVOT);
Joshua23 25:054d3048dd03 198 setCenter();
fanny868 0:9072e932503c 199 break;
MarchioKevin 22:4632f58461e0 200 }
MarchioKevin 22:4632f58461e0 201 case (3) : {
MarchioKevin 22:4632f58461e0 202 // Kanan
Joshua23 25:054d3048dd03 203 //motor1.speed(-VMAX-vpid);
Joshua23 25:054d3048dd03 204 //motor2.speed(0.2+vpid);
Joshua23 25:054d3048dd03 205 motor1.speed(-0.365+pid(0.09,0,0));
Joshua23 25:054d3048dd03 206 motor2.speed(0.46+pid(0.09,0,0));
rahmadirizki18 17:e4229d77a5ab 207 break;
MarchioKevin 22:4632f58461e0 208 }
MarchioKevin 22:4632f58461e0 209 case (4) : {
rahmadirizki18 17:e4229d77a5ab 210 // Kiri
Joshua23 25:054d3048dd03 211 //motor1.speed(VMAX-vpid);
Joshua23 25:054d3048dd03 212 //motor2.speed(-0.2+vpid);
Joshua23 25:054d3048dd03 213 motor1.speed(0.365+pid(0.09,0,0));//belakang
Joshua23 25:054d3048dd03 214 motor2.speed(-0.46+pid(0.09,0,0));//depan
fanny868 0:9072e932503c 215 break;
rahmadirizki18 17:e4229d77a5ab 216 }
rahmadirizki18 17:e4229d77a5ab 217 default : {
MarchioKevin 22:4632f58461e0 218 motor1.brake(1);
MarchioKevin 22:4632f58461e0 219 motor2.brake(1);
rahmadirizki18 17:e4229d77a5ab 220 }
MarchioKevin 22:4632f58461e0 221 } // End Switch
rahmadirizki18 5:3aa203218306 222 }
rahmadirizki18 5:3aa203218306 223
calmantara186 16:90119f03c5d1 224 void setCenter(){
calmantara186 16:90119f03c5d1 225 /* Fungsi untuk menentukan center dari robot */
rahmadirizki18 5:3aa203218306 226 encoderKiri.reset();
rahmadirizki18 5:3aa203218306 227 }
rahmadirizki18 5:3aa203218306 228
calmantara186 16:90119f03c5d1 229 float getTetha(){
calmantara186 16:90119f03c5d1 230 /* Fungsi untuk mendapatkan nilai tetha */
Joshua23 25:054d3048dd03 231 float busurKir, tetha;
MarchioKevin 22:4632f58461e0 232 busurKir = ((encoderKiri.getPulses())/(float)(2000.0)*k_enc);
Joshua23 25:054d3048dd03 233 tetha = busurKir/k_robot*360;
calmantara186 16:90119f03c5d1 234
Joshua23 25:054d3048dd03 235 return -(tetha);
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
calmantara186 16:90119f03c5d1 268 /* Untuk mendapatkan serial dari Arduino */
fanny868 0:9072e932503c 269 while(1)
fanny868 0:9072e932503c 270 {
fanny868 0:9072e932503c 271 // Interrupt Serial
calmantara186 16:90119f03c5d1 272 joystick.idle();
Joshua23 25:054d3048dd03 273 //pc.printf("enco : %d \n",encoderKiri.getPulses());
calmantara186 16:90119f03c5d1 274 if(joystick.readable() ) {
fanny868 0:9072e932503c 275 // Panggil fungsi pembacaan joystik
fanny868 0:9072e932503c 276 joystick.baca_data();
calmantara186 16:90119f03c5d1 277
fanny868 0:9072e932503c 278 // Panggil fungsi pengolahan data joystik
fanny868 0:9072e932503c 279 joystick.olah_data();
calmantara186 16:90119f03c5d1 280
calmantara186 16:90119f03c5d1 281 // Masuk ke case gerak
fanny868 0:9072e932503c 282 case_ger = case_gerak();
rahmadirizki18 3:1287fccc11be 283 aktuator();
calmantara186 16:90119f03c5d1 284
rahmadirizki18 17:e4229d77a5ab 285 if (joystick.segitiga_click) launcher = !launcher;
rahmadirizki18 17:e4229d77a5ab 286 if (joystick.lingkaran_click) servoGo = true;
rahmadirizki18 24:b3e632cc4533 287 speedKalibrasiMotor();
calmantara186 16:90119f03c5d1 288 }
calmantara186 16:90119f03c5d1 289 else {
calmantara186 16:90119f03c5d1 290 joystick.idle();
Joshua23 25:054d3048dd03 291 //motor1.brake(1);
Joshua23 25:054d3048dd03 292 //motor2.brake(1);
MarchioKevin 21:da2f3d04468f 293 }
fanny868 0:9072e932503c 294 }
fanny868 0:9072e932503c 295 }