![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Update 12 Maret 2017. Tambahan kombinasi tombol
Dependencies: DigitDisplay Motor PID Ping mbed millis
Fork of MainProgram_BaseBaru_20Feb_malam by
main.cpp@29:7b372b0aaa61, 2017-02-12 (annotated)
- Committer:
- Sufa
- Date:
- Sun Feb 12 08:07:04 2017 +0000
- Revision:
- 29:7b372b0aaa61
- Parent:
- 28:2d0746dc2d7d
- Child:
- 30:d69cc27ac644
terbaru, sudah assembly robot.
Who changed what in which revision?
User | Revision | Line number | New 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" |
be_bryan | 26:256160a1a82d | 44 | #include "millis.h" |
rahmadirizki18 | 5:3aa203218306 | 45 | |
calmantara186 | 16:90119f03c5d1 | 46 | /***********************************************/ |
calmantara186 | 16:90119f03c5d1 | 47 | /* Konstanta dan Variabel */ |
calmantara186 | 16:90119f03c5d1 | 48 | /***********************************************/ |
calmantara186 | 16:90119f03c5d1 | 49 | #define PI 3.14159265 |
be_bryan | 26:256160a1a82d | 50 | #define D_ENCODER 10 // Diameter Roda Encoder |
be_bryan | 26:256160a1a82d | 51 | #define D_ROBOT 80 // Diameter Roda Robot |
MarchioKevin | 22:4632f58461e0 | 52 | #define VMAX 0.3 // Kiri Kanan |
rahmadirizki18 | 17:e4229d77a5ab | 53 | #define PIVOT 0.4 // Pivka, Pivki |
rahmadirizki18 | 17:e4229d77a5ab | 54 | #define PERPINDAHAN 1 // Perpindahan ke kanan dan kiri |
rahmadirizki18 | 5:3aa203218306 | 55 | |
be_bryan | 26:256160a1a82d | 56 | // Variable Atas |
be_bryan | 27:68efb1622985 | 57 | double speed,speed2, maxSpeed = 0.8, minSpeed = 0; |
be_bryan | 26:256160a1a82d | 58 | double kpA=0.6757, kdA=0.6757, kiA=0.00006757; |
be_bryan | 26:256160a1a82d | 59 | double p,i,d; |
be_bryan | 27:68efb1622985 | 60 | double p2,i2,d2; |
be_bryan | 26:256160a1a82d | 61 | double last_error = 0, current_error, sum_error = 0; |
be_bryan | 27:68efb1622985 | 62 | double last_error2 = 0, current_error2, sum_error2 = 0; |
be_bryan | 27:68efb1622985 | 63 | float rpm, target_rpm = 2.0; |
be_bryan | 27:68efb1622985 | 64 | float rpm2, target_rpm2 = 4.0; |
be_bryan | 26:256160a1a82d | 65 | |
be_bryan | 26:256160a1a82d | 66 | // Variable Bawah |
Joshua23 | 25:054d3048dd03 | 67 | float Vt; |
calmantara186 | 16:90119f03c5d1 | 68 | float k_enc = PI*D_ENCODER; |
Joshua23 | 25:054d3048dd03 | 69 | float k_robot = PI*D_ROBOT; |
MarchioKevin | 22:4632f58461e0 | 70 | float speedT = 0.2; |
be_bryan | 27:68efb1622985 | 71 | float vpid = 0; |
be_bryan | 27:68efb1622985 | 72 | float pwmP = 0.5; |
Sufa | 29:7b372b0aaa61 | 73 | float pwmT = -0.95; |
MarchioKevin | 22:4632f58461e0 | 74 | |
calmantara186 | 16:90119f03c5d1 | 75 | |
be_bryan | 26:256160a1a82d | 76 | /* Deklarasi Encoder Base */ |
Joshua23 | 25:054d3048dd03 | 77 | encoderKRAI encoderKiri(PC_4, PB_15, 2000, encoderKRAI::X2_ENCODING); //inA, inB, pin Indeks (NC = tak ada), resolusi, pembacaan |
rahmadirizki18 | 15:98f0d56b14f0 | 78 | |
be_bryan | 26:256160a1a82d | 79 | /* Deklarasi Encoder Launcher */ |
Sufa | 29:7b372b0aaa61 | 80 | encoderKRAI encoderAtas( PC_10, PC_11, 28, encoderKRAI::X2_ENCODING); |
Sufa | 29:7b372b0aaa61 | 81 | encoderKRAI encoderAtas2( PD_2, PC_12, 28, encoderKRAI::X2_ENCODING); |
be_bryan | 26:256160a1a82d | 82 | |
calmantara186 | 16:90119f03c5d1 | 83 | /* Deklarasi Motor Base */ |
Sufa | 29:7b372b0aaa61 | 84 | Motor motor1(PB_9, PA_12, PC_5); // pwm, fwd, rev, Motor Depan |
be_bryan | 26:256160a1a82d | 85 | Motor motor2(PB_6, PB_1, PB_12); // pwm, fwd, rev, Motor Belakang |
fanny868 | 0:9072e932503c | 86 | |
calmantara186 | 16:90119f03c5d1 | 87 | /* Deklarasi Motor Launcher */ |
Sufa | 29:7b372b0aaa61 | 88 | Motor motorL1(PA_8,PC_2,PC_1); // pwm ,fwd, rev, Motor Launcher1 |
Sufa | 29:7b372b0aaa61 | 89 | Motor motorL3(PA_10, PC_3, PC_0); // pwm, fwd, rev, Motor Launcher2 |
Sufa | 29:7b372b0aaa61 | 90 | Motor motorP(PB_7, PA_14, PA_15); // pwm, fwd, rev, Motor Powerscrew |
rahmadirizki18 | 5:3aa203218306 | 91 | |
be_bryan | 26:256160a1a82d | 92 | /* Deklarasi Penumatik Launcher */ |
Sufa | 29:7b372b0aaa61 | 93 | DigitalOut pneumatik(PB_3, PullUp); |
be_bryan | 26:256160a1a82d | 94 | |
be_bryan | 27:68efb1622985 | 95 | /*Dekalrasi Limit Switch */ |
be_bryan | 27:68efb1622985 | 96 | DigitalIn limitA (PC_9, PullUp); |
be_bryan | 27:68efb1622985 | 97 | DigitalIn limitT(PB_10, PullUp); |
be_bryan | 27:68efb1622985 | 98 | DigitalIn limitB (PC_8, PullUp); |
be_bryan | 27:68efb1622985 | 99 | |
calmantara186 | 16:90119f03c5d1 | 100 | /** |
calmantara186 | 16:90119f03c5d1 | 101 | * posX dan posY berdasarkan arah robot |
calmantara186 | 16:90119f03c5d1 | 102 | * encoder Depan & Belakang sejajar sumbu Y |
be_bryan | 27:68efb1622985 | 103 | * encoder Kanan & Kirim sejajar sumbu X |
calmantara186 | 16:90119f03c5d1 | 104 | **/ |
rahmadirizki18 | 5:3aa203218306 | 105 | |
calmantara186 | 16:90119f03c5d1 | 106 | /* Variabel Encoder */ |
MarchioKevin | 22:4632f58461e0 | 107 | float errT, Tetha; // Variabel yang didapatkan encoder |
rahmadirizki18 | 5:3aa203218306 | 108 | |
calmantara186 | 16:90119f03c5d1 | 109 | /* Fungsi dan Procedur Encoder */ |
calmantara186 | 16:90119f03c5d1 | 110 | void setCenter(); // Fungsi reset agar robot di tengah |
MarchioKevin | 20:54dc93e7b016 | 111 | float getTetha(); // Fungsi mendapatkan jarak Tetha |
calmantara186 | 16:90119f03c5d1 | 112 | |
calmantara186 | 16:90119f03c5d1 | 113 | /* Inisialisasi Pin TX-RX Joystik dan PC */ |
Joshua23 | 25:054d3048dd03 | 114 | joysticknucleo joystick(PA_0,PA_1); |
rahmadirizki18 | 23:023b522977b2 | 115 | Serial pc(USBTX,USBRX); |
fanny868 | 0:9072e932503c | 116 | |
be_bryan | 26:256160a1a82d | 117 | /* Deklarasi Variable Milis */ |
be_bryan | 26:256160a1a82d | 118 | unsigned long int previousMillis = 0; |
be_bryan | 26:256160a1a82d | 119 | unsigned long int currentMillis; |
be_bryan | 27:68efb1622985 | 120 | unsigned long int previousMillis2 = 0; |
be_bryan | 27:68efb1622985 | 121 | unsigned long int currentMillis2; |
be_bryan | 26:256160a1a82d | 122 | |
calmantara186 | 16:90119f03c5d1 | 123 | /* Variabel Stick */ |
fanny868 | 0:9072e932503c | 124 | char case_ger; |
be_bryan | 26:256160a1a82d | 125 | bool launcher = false; |
be_bryan | 27:68efb1622985 | 126 | bool reload = false; |
fanny868 | 0:9072e932503c | 127 | |
MarchioKevin | 22:4632f58461e0 | 128 | /****************************************************/ |
MarchioKevin | 22:4632f58461e0 | 129 | /* Deklarasi Fungsi dan Procedure */ |
MarchioKevin | 22:4632f58461e0 | 130 | /****************************************************/ |
be_bryan | 26:256160a1a82d | 131 | void init_speed(); |
be_bryan | 26:256160a1a82d | 132 | void speedKalibrasiMotor(); |
be_bryan | 26:256160a1a82d | 133 | float pidBase(float Kp, float Ki, float Kd) |
Joshua23 | 25:054d3048dd03 | 134 | { |
Joshua23 | 25:054d3048dd03 | 135 | int errorP; |
Joshua23 | 25:054d3048dd03 | 136 | errorP = getTetha(); |
Joshua23 | 25:054d3048dd03 | 137 | return (float)Kp*errorP; |
Joshua23 | 25:054d3048dd03 | 138 | } |
Joshua23 | 25:054d3048dd03 | 139 | |
calmantara186 | 16:90119f03c5d1 | 140 | int case_gerak(){ |
rahmadirizki18 | 23:023b522977b2 | 141 | /**************************************************** |
calmantara186 | 16:90119f03c5d1 | 142 | ** Gerak Motor Base |
calmantara186 | 16:90119f03c5d1 | 143 | ** Case 1 : Pivot kanan |
calmantara186 | 16:90119f03c5d1 | 144 | ** Case 2 : Pivot Kiri |
MarchioKevin | 22:4632f58461e0 | 145 | ** Case 3 : Kanan |
MarchioKevin | 22:4632f58461e0 | 146 | ** Case 4 : Kiri |
MarchioKevin | 22:4632f58461e0 | 147 | ** Case 5 : Break |
calmantara186 | 16:90119f03c5d1 | 148 | ****************************************************/ |
MarchioKevin | 22:4632f58461e0 | 149 | |
fanny868 | 0:9072e932503c | 150 | int casegerak; |
calmantara186 | 16:90119f03c5d1 | 151 | if (!joystick.L1 && joystick.R1) { |
fanny868 | 0:9072e932503c | 152 | // Pivot Kanan |
fanny868 | 0:9072e932503c | 153 | casegerak = 1; |
calmantara186 | 16:90119f03c5d1 | 154 | } |
calmantara186 | 16:90119f03c5d1 | 155 | else if (!joystick.R1 && joystick.L1) { |
fanny868 | 0:9072e932503c | 156 | // Pivot Kiri |
fanny868 | 0:9072e932503c | 157 | casegerak = 2; |
calmantara186 | 16:90119f03c5d1 | 158 | } |
calmantara186 | 16:90119f03c5d1 | 159 | else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) { |
fanny868 | 0:9072e932503c | 160 | // Kanan |
MarchioKevin | 22:4632f58461e0 | 161 | casegerak = 3; |
be_bryan | 27:68efb1622985 | 162 | // pc.printf("kanan"); |
calmantara186 | 16:90119f03c5d1 | 163 | } |
calmantara186 | 16:90119f03c5d1 | 164 | else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) { |
fanny868 | 0:9072e932503c | 165 | // Kiri |
rahmadirizki18 | 23:023b522977b2 | 166 | casegerak = 4; |
be_bryan | 27:68efb1622985 | 167 | // pc.printf("kiri"); |
calmantara186 | 16:90119f03c5d1 | 168 | } |
calmantara186 | 16:90119f03c5d1 | 169 | else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) { |
MarchioKevin | 22:4632f58461e0 | 170 | // Break |
MarchioKevin | 22:4632f58461e0 | 171 | casegerak = 5; |
calmantara186 | 16:90119f03c5d1 | 172 | } |
fanny868 | 0:9072e932503c | 173 | return(casegerak); |
fanny868 | 0:9072e932503c | 174 | } |
fanny868 | 0:9072e932503c | 175 | |
calmantara186 | 16:90119f03c5d1 | 176 | void aktuator(){ |
be_bryan | 27:68efb1622985 | 177 | /* Fungsi untuk menggerakkan Motor PowerScrew */ |
be_bryan | 27:68efb1622985 | 178 | // PowerScrew |
be_bryan | 27:68efb1622985 | 179 | if (reload){ |
be_bryan | 27:68efb1622985 | 180 | if (!limitA){ |
be_bryan | 27:68efb1622985 | 181 | motorP.brake (1); |
Sufa | 29:7b372b0aaa61 | 182 | wait_ms(1000); |
Joshua23 | 8:0711dea61312 | 183 | } |
be_bryan | 27:68efb1622985 | 184 | else |
be_bryan | 27:68efb1622985 | 185 | { |
be_bryan | 27:68efb1622985 | 186 | motorP.speed(pwmP); |
be_bryan | 27:68efb1622985 | 187 | if (!limitT) |
be_bryan | 27:68efb1622985 | 188 | { |
be_bryan | 27:68efb1622985 | 189 | while (limitB) |
be_bryan | 27:68efb1622985 | 190 | { |
be_bryan | 27:68efb1622985 | 191 | motorP.speed(pwmT); |
be_bryan | 27:68efb1622985 | 192 | } |
be_bryan | 27:68efb1622985 | 193 | motorP.brake(1); |
Sufa | 29:7b372b0aaa61 | 194 | reload = !reload; |
be_bryan | 27:68efb1622985 | 195 | } |
be_bryan | 27:68efb1622985 | 196 | } |
calmantara186 | 16:90119f03c5d1 | 197 | } |
be_bryan | 28:2d0746dc2d7d | 198 | |
be_bryan | 27:68efb1622985 | 199 | /*Motor Atas*/ |
calmantara186 | 16:90119f03c5d1 | 200 | if (launcher) { |
be_bryan | 26:256160a1a82d | 201 | startMillis(); |
be_bryan | 28:2d0746dc2d7d | 202 | currentMillis = millis(); |
be_bryan | 28:2d0746dc2d7d | 203 | currentMillis2 = millis(); |
be_bryan | 28:2d0746dc2d7d | 204 | |
be_bryan | 28:2d0746dc2d7d | 205 | /*PID Launcher Depan*/ |
be_bryan | 26:256160a1a82d | 206 | if (currentMillis-previousMillis>=10) |
be_bryan | 26:256160a1a82d | 207 | { |
be_bryan | 27:68efb1622985 | 208 | rpm = (float)encoderAtas.getPulses(); |
be_bryan | 26:256160a1a82d | 209 | current_error = target_rpm - rpm; |
be_bryan | 26:256160a1a82d | 210 | sum_error = sum_error + current_error; |
be_bryan | 26:256160a1a82d | 211 | p = current_error*kpA; |
be_bryan | 26:256160a1a82d | 212 | d = (current_error-last_error)*kdA/10.0; |
be_bryan | 26:256160a1a82d | 213 | i = sum_error*kiA*10.0; |
be_bryan | 26:256160a1a82d | 214 | speed = p + d + i; |
be_bryan | 26:256160a1a82d | 215 | init_speed(); |
be_bryan | 27:68efb1622985 | 216 | motorL1.speed(speed); |
be_bryan | 26:256160a1a82d | 217 | last_error = current_error; |
be_bryan | 26:256160a1a82d | 218 | encoderAtas.reset(); |
be_bryan | 26:256160a1a82d | 219 | //pc.printf("%.04lf\n",rpm); |
be_bryan | 26:256160a1a82d | 220 | previousMillis = currentMillis; |
be_bryan | 26:256160a1a82d | 221 | } |
be_bryan | 26:256160a1a82d | 222 | else |
be_bryan | 26:256160a1a82d | 223 | { |
be_bryan | 27:68efb1622985 | 224 | encoderAtas.getPulses(); |
be_bryan | 27:68efb1622985 | 225 | } |
be_bryan | 27:68efb1622985 | 226 | if (currentMillis2-previousMillis2>=10) |
be_bryan | 27:68efb1622985 | 227 | { |
be_bryan | 27:68efb1622985 | 228 | rpm2 = (float)encoderAtas2.getPulses(); |
be_bryan | 27:68efb1622985 | 229 | current_error2 = target_rpm2 - rpm2; |
be_bryan | 27:68efb1622985 | 230 | sum_error2 = sum_error2 + current_error2; |
be_bryan | 27:68efb1622985 | 231 | p2 = current_error2*kpA; |
be_bryan | 27:68efb1622985 | 232 | d2 = (current_error2-last_error2)*kdA/10.0; |
be_bryan | 27:68efb1622985 | 233 | i2 = sum_error2*kiA*10.0; |
be_bryan | 27:68efb1622985 | 234 | speed2 = p2 + d2 + i2; |
be_bryan | 27:68efb1622985 | 235 | init_speed(); |
be_bryan | 27:68efb1622985 | 236 | motorL3.speed(speed2); |
be_bryan | 27:68efb1622985 | 237 | last_error2 = current_error2; |
be_bryan | 27:68efb1622985 | 238 | encoderAtas2.reset(); |
be_bryan | 27:68efb1622985 | 239 | //pc.printf("%.04lf\n",rpm2); |
be_bryan | 27:68efb1622985 | 240 | previousMillis2 = currentMillis2; |
be_bryan | 27:68efb1622985 | 241 | } |
be_bryan | 27:68efb1622985 | 242 | else |
be_bryan | 27:68efb1622985 | 243 | { |
be_bryan | 27:68efb1622985 | 244 | encoderAtas2.getPulses(); |
be_bryan | 26:256160a1a82d | 245 | } |
rahmadirizki18 | 6:68293bed71ea | 246 | } |
Sufa | 29:7b372b0aaa61 | 247 | else |
Sufa | 29:7b372b0aaa61 | 248 | { |
Sufa | 29:7b372b0aaa61 | 249 | motorL1.brake(1); |
Sufa | 29:7b372b0aaa61 | 250 | motorL3.brake(1); |
Sufa | 29:7b372b0aaa61 | 251 | } |
Sufa | 29:7b372b0aaa61 | 252 | |
be_bryan | 28:2d0746dc2d7d | 253 | |
rahmadirizki18 | 6:68293bed71ea | 254 | // MOTOR Bawah |
MarchioKevin | 22:4632f58461e0 | 255 | switch (case_ger) { |
rahmadirizki18 | 17:e4229d77a5ab | 256 | case (1): { |
rahmadirizki18 | 17:e4229d77a5ab | 257 | // Pivot Kanan |
Joshua23 | 25:054d3048dd03 | 258 | motor1.speed(PIVOT); |
Joshua23 | 25:054d3048dd03 | 259 | motor2.speed(PIVOT); |
Joshua23 | 25:054d3048dd03 | 260 | setCenter(); |
fanny868 | 0:9072e932503c | 261 | break; |
rahmadirizki18 | 17:e4229d77a5ab | 262 | } |
rahmadirizki18 | 17:e4229d77a5ab | 263 | case (2): { |
rahmadirizki18 | 17:e4229d77a5ab | 264 | // Pivot Kiri |
Joshua23 | 25:054d3048dd03 | 265 | motor1.speed(-PIVOT); |
Joshua23 | 25:054d3048dd03 | 266 | motor2.speed(-PIVOT); |
Joshua23 | 25:054d3048dd03 | 267 | setCenter(); |
fanny868 | 0:9072e932503c | 268 | break; |
MarchioKevin | 22:4632f58461e0 | 269 | } |
MarchioKevin | 22:4632f58461e0 | 270 | case (3) : { |
MarchioKevin | 22:4632f58461e0 | 271 | // Kanan |
Joshua23 | 25:054d3048dd03 | 272 | //motor1.speed(-VMAX-vpid); |
Joshua23 | 25:054d3048dd03 | 273 | //motor2.speed(0.2+vpid); |
be_bryan | 26:256160a1a82d | 274 | motor1.speed(-0.365+pidBase(0.09,0,0)); |
be_bryan | 26:256160a1a82d | 275 | motor2.speed(0.46+pidBase(0.09,0,0)); |
rahmadirizki18 | 17:e4229d77a5ab | 276 | break; |
MarchioKevin | 22:4632f58461e0 | 277 | } |
MarchioKevin | 22:4632f58461e0 | 278 | case (4) : { |
rahmadirizki18 | 17:e4229d77a5ab | 279 | // Kiri |
Joshua23 | 25:054d3048dd03 | 280 | //motor1.speed(VMAX-vpid); |
Joshua23 | 25:054d3048dd03 | 281 | //motor2.speed(-0.2+vpid); |
be_bryan | 26:256160a1a82d | 282 | motor1.speed(0.365+pidBase(0.09,0,0));//belakang |
be_bryan | 26:256160a1a82d | 283 | motor2.speed(-0.46+pidBase(0.09,0,0));//depan |
fanny868 | 0:9072e932503c | 284 | break; |
rahmadirizki18 | 17:e4229d77a5ab | 285 | } |
rahmadirizki18 | 17:e4229d77a5ab | 286 | default : { |
MarchioKevin | 22:4632f58461e0 | 287 | motor1.brake(1); |
MarchioKevin | 22:4632f58461e0 | 288 | motor2.brake(1); |
rahmadirizki18 | 17:e4229d77a5ab | 289 | } |
MarchioKevin | 22:4632f58461e0 | 290 | } // End Switch |
rahmadirizki18 | 5:3aa203218306 | 291 | } |
rahmadirizki18 | 5:3aa203218306 | 292 | |
calmantara186 | 16:90119f03c5d1 | 293 | void setCenter(){ |
calmantara186 | 16:90119f03c5d1 | 294 | /* Fungsi untuk menentukan center dari robot */ |
rahmadirizki18 | 5:3aa203218306 | 295 | encoderKiri.reset(); |
rahmadirizki18 | 5:3aa203218306 | 296 | } |
rahmadirizki18 | 5:3aa203218306 | 297 | |
calmantara186 | 16:90119f03c5d1 | 298 | float getTetha(){ |
calmantara186 | 16:90119f03c5d1 | 299 | /* Fungsi untuk mendapatkan nilai tetha */ |
Joshua23 | 25:054d3048dd03 | 300 | float busurKir, tetha; |
MarchioKevin | 22:4632f58461e0 | 301 | busurKir = ((encoderKiri.getPulses())/(float)(2000.0)*k_enc); |
Joshua23 | 25:054d3048dd03 | 302 | tetha = busurKir/k_robot*360; |
calmantara186 | 16:90119f03c5d1 | 303 | |
Joshua23 | 25:054d3048dd03 | 304 | return -(tetha); |
calmantara186 | 16:90119f03c5d1 | 305 | } |
rahmadirizki18 | 5:3aa203218306 | 306 | |
rahmadirizki18 | 24:b3e632cc4533 | 307 | void speedKalibrasiMotor(){ |
calmantara186 | 16:90119f03c5d1 | 308 | /* Fungsi untuk speed launcher */ |
be_bryan | 27:68efb1622985 | 309 | if (joystick.R3_click and target_rpm < 14){ |
be_bryan | 27:68efb1622985 | 310 | target_rpm = target_rpm + 1; // RPM++ Motor Belakang |
calmantara186 | 16:90119f03c5d1 | 311 | } |
be_bryan | 27:68efb1622985 | 312 | if (joystick.L3_click and target_rpm > 1){ |
be_bryan | 27:68efb1622985 | 313 | target_rpm = target_rpm - 1; // RPM-- Motor Belakang |
calmantara186 | 16:90119f03c5d1 | 314 | } |
be_bryan | 27:68efb1622985 | 315 | if (joystick.R2_click and target_rpm2 < 14){ |
be_bryan | 27:68efb1622985 | 316 | target_rpm2 = target_rpm2 + 1; // RPM++ Motor Depan |
calmantara186 | 16:90119f03c5d1 | 317 | } |
be_bryan | 27:68efb1622985 | 318 | if (joystick.L2_click and target_rpm2 > 1 ){ |
be_bryan | 27:68efb1622985 | 319 | target_rpm2 = target_rpm2 - 1; // RPM-- Motor Depan |
rahmadirizki18 | 24:b3e632cc4533 | 320 | } |
be_bryan | 27:68efb1622985 | 321 | // pc.printf("Rpm depan = %.4f\t Rpm belakang = %.4f\n", target_rpm, target_rpm2); |
rahmadirizki18 | 7:d138c56dab20 | 322 | } |
be_bryan | 26:256160a1a82d | 323 | |
be_bryan | 26:256160a1a82d | 324 | void init_speed (){ |
be_bryan | 26:256160a1a82d | 325 | if (speed>maxSpeed){ |
be_bryan | 26:256160a1a82d | 326 | speed = maxSpeed; |
be_bryan | 26:256160a1a82d | 327 | } |
be_bryan | 26:256160a1a82d | 328 | |
be_bryan | 26:256160a1a82d | 329 | if (speed<minSpeed){ |
be_bryan | 26:256160a1a82d | 330 | speed = minSpeed; |
be_bryan | 26:256160a1a82d | 331 | } |
be_bryan | 27:68efb1622985 | 332 | if (speed2>maxSpeed){ |
be_bryan | 27:68efb1622985 | 333 | speed2 = maxSpeed; |
be_bryan | 27:68efb1622985 | 334 | } |
be_bryan | 27:68efb1622985 | 335 | |
be_bryan | 27:68efb1622985 | 336 | if (speed2<minSpeed){ |
be_bryan | 27:68efb1622985 | 337 | speed2 = minSpeed; |
be_bryan | 27:68efb1622985 | 338 | } |
be_bryan | 26:256160a1a82d | 339 | } |
MarchioKevin | 22:4632f58461e0 | 340 | /*********************************************************/ |
MarchioKevin | 22:4632f58461e0 | 341 | /* Main Function */ |
MarchioKevin | 22:4632f58461e0 | 342 | /*********************************************************/ |
calmantara186 | 16:90119f03c5d1 | 343 | |
calmantara186 | 16:90119f03c5d1 | 344 | int main (void){ |
calmantara186 | 16:90119f03c5d1 | 345 | /* Set baud rate - 115200 */ |
fanny868 | 0:9072e932503c | 346 | joystick.setup(); |
rahmadirizki18 | 23:023b522977b2 | 347 | pc.baud(115200); |
rahmadirizki18 | 6:68293bed71ea | 348 | wait_ms(1000); |
rahmadirizki18 | 5:3aa203218306 | 349 | setCenter(); |
rahmadirizki18 | 5:3aa203218306 | 350 | wait_ms(500); |
rahmadirizki18 | 23:023b522977b2 | 351 | pc.printf("ready...."); |
calmantara186 | 16:90119f03c5d1 | 352 | |
calmantara186 | 16:90119f03c5d1 | 353 | /* Untuk mendapatkan serial dari Arduino */ |
fanny868 | 0:9072e932503c | 354 | while(1) |
fanny868 | 0:9072e932503c | 355 | { |
fanny868 | 0:9072e932503c | 356 | // Interrupt Serial |
calmantara186 | 16:90119f03c5d1 | 357 | joystick.idle(); |
Joshua23 | 25:054d3048dd03 | 358 | //pc.printf("enco : %d \n",encoderKiri.getPulses()); |
be_bryan | 26:256160a1a82d | 359 | if(joystick.readable()) { |
fanny868 | 0:9072e932503c | 360 | // Panggil fungsi pembacaan joystik |
fanny868 | 0:9072e932503c | 361 | joystick.baca_data(); |
fanny868 | 0:9072e932503c | 362 | // Panggil fungsi pengolahan data joystik |
fanny868 | 0:9072e932503c | 363 | joystick.olah_data(); |
calmantara186 | 16:90119f03c5d1 | 364 | // Masuk ke case gerak |
fanny868 | 0:9072e932503c | 365 | case_ger = case_gerak(); |
rahmadirizki18 | 3:1287fccc11be | 366 | aktuator(); |
be_bryan | 26:256160a1a82d | 367 | if (joystick.segitiga_click){ |
be_bryan | 26:256160a1a82d | 368 | launcher = !launcher; |
be_bryan | 26:256160a1a82d | 369 | } |
be_bryan | 26:256160a1a82d | 370 | if (joystick.lingkaran_click){ |
Sufa | 29:7b372b0aaa61 | 371 | pneumatik = 0; |
Sufa | 29:7b372b0aaa61 | 372 | wait_ms(1000); |
be_bryan | 26:256160a1a82d | 373 | pneumatik = 1; |
be_bryan | 26:256160a1a82d | 374 | } |
be_bryan | 27:68efb1622985 | 375 | speedKalibrasiMotor(); |
be_bryan | 28:2d0746dc2d7d | 376 | if (joystick.silang_click){ |
be_bryan | 28:2d0746dc2d7d | 377 | reload = !reload; |
be_bryan | 28:2d0746dc2d7d | 378 | } |
be_bryan | 26:256160a1a82d | 379 | } |
calmantara186 | 16:90119f03c5d1 | 380 | else { |
calmantara186 | 16:90119f03c5d1 | 381 | joystick.idle(); |
be_bryan | 26:256160a1a82d | 382 | motor1.brake(1); |
be_bryan | 26:256160a1a82d | 383 | motor2.brake(1); |
MarchioKevin | 21:da2f3d04468f | 384 | } |
fanny868 | 0:9072e932503c | 385 | } |
be_bryan | 28:2d0746dc2d7d | 386 | } |