Marchio Kevin / Mbed 2 deprecated Robo_Taker_Nasional_2018

Buat agip

Dependencies:   Motor_1 encoderKRAI mbed millis

Fork of Robo_Taker_Nasional_2018 by KRAI 2018

main.cpp@6:bb7e29420efd, 2018-03-08 (annotated)

Committer:
Fathoni17
Date:
Thu Mar 08 09:24:57 2018 +0000
Revision:
6:bb7e29420efd
Parent:
5:4a70c53d7f86
Child:
7:8d8eb4676356
Akselerasi+Deselerasi OK; Rotasi Close Loop; Default mode Close Loop; (?) mode open loop tidak dipake

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Fathoni17 6:bb7e29420efd 1 ////////////////////////////////////////////////////////////////////////////////
Fathoni17 6:bb7e29420efd 2 // Robo Taker 2018 //
Fathoni17 6:bb7e29420efd 3 // -------------------------------------------------------------------------- //
Fathoni17 6:bb7e29420efd 4 // Todo List: //
Fathoni17 6:bb7e29420efd 5 // - Perhalus Gerakan //
Fathoni17 6:bb7e29420efd 6 // - Pasang Mode Joystick Kabel/BT //
Fathoni17 6:bb7e29420efd 7 // - Program Pneumatic //
Fathoni17 6:bb7e29420efd 8 // - Tata code yang ada, Lengkapi dengan Komentar (Penjelasan) //
Fathoni17 6:bb7e29420efd 9 // Gerakan Robot: //
Fathoni17 6:bb7e29420efd 10 // - Arah -> Gerak //
Fathoni17 6:bb7e29420efd 11 // - Silang -> Pneumatic //
Fathoni17 6:bb7e29420efd 12 // - Start -> Ubah Mode //
Fathoni17 6:bb7e29420efd 13 ////////////////////////////////////////////////////////////////////////////////
Fathoni17 0:22acd37ed695 14 #include "mbed.h"
Fathoni17 0:22acd37ed695 15 #include "Motor.h"
Fathoni17 0:22acd37ed695 16 #include "encoderKRAI.h"
Fathoni17 0:22acd37ed695 17 #include "JoystickPS3.h"
Fathoni17 0:22acd37ed695 18 #include "pinList.h"
Fathoni17 2:863436c840bf 19 #include "millis.h"
Fathoni17 0:22acd37ed695 20
Fathoni17 6:bb7e29420efd 21 ////////////////////////////////////////////////////////////////////////////////
Fathoni17 6:bb7e29420efd 22 // Konstanta Program //
Fathoni17 6:bb7e29420efd 23 ////////////////////////////////////////////////////////////////////////////////
Fathoni17 0:22acd37ed695 24 #define PI 3.141592653593
Fathoni17 0:22acd37ed695 25 #define RAD_TO_DEG 57.2957795131
Fathoni17 2:863436c840bf 26 #define MAX_W_SPEED 15000 //max angular speed of robot
MarchioKevin 3:b1403fcdaeb1 27 #define TOLERANCET 0.8 //theta tolerance
Fathoni17 2:863436c840bf 28 #define PULSE_TO_JARAK 0.581776 //kll roda / pulses
Fathoni17 2:863436c840bf 29 #define L 298.0 //roda to center of robot
Fathoni17 2:863436c840bf 30 #define TS 2.0 //time sampling
Fathoni17 2:863436c840bf 31 #define LIMITPWM 0.4 //limit pwm motor
Fathoni17 2:863436c840bf 32
Fathoni17 6:bb7e29420efd 33 //Konstanta PID Theta
MarchioKevin 3:b1403fcdaeb1 34 #define KP_W 1.0
MarchioKevin 3:b1403fcdaeb1 35 #define KI_W 0.0065
Fathoni17 5:4a70c53d7f86 36 #define KD_W 175
Fathoni17 0:22acd37ed695 37
Fathoni17 0:22acd37ed695 38 #define MOTOR_LIMIT_MAX 1
Fathoni17 0:22acd37ed695 39 #define MOTOR_LIMIT_MIN -1
Fathoni17 0:22acd37ed695 40
Fathoni17 6:bb7e29420efd 41 // Set 1 untuk aktifkan fitur pc.print
Fathoni17 0:22acd37ed695 42 #define DEBUG 1
Fathoni17 0:22acd37ed695 43
Fathoni17 6:bb7e29420efd 44 ////////////////////////////////////////////////////////////////////////////////
Fathoni17 6:bb7e29420efd 45 // Object Program //
Fathoni17 6:bb7e29420efd 46 ////////////////////////////////////////////////////////////////////////////////
Fathoni17 0:22acd37ed695 47 // Serial
Fathoni17 6:bb7e29420efd 48 Serial pc(USBTX, USBRX, 115200);
Fathoni17 0:22acd37ed695 49 joysticknucleo stick(PIN_TX, PIN_RX);
Fathoni17 0:22acd37ed695 50
Fathoni17 0:22acd37ed695 51 // Pneumatik
Fathoni17 0:22acd37ed695 52 DigitalOut pneumatik(PIN_PNEUMATIK);
Fathoni17 0:22acd37ed695 53
Fathoni17 0:22acd37ed695 54 // Encoder
Fathoni17 0:22acd37ed695 55 encoderKRAI encoder_A(PIN_A_CHANNEL_A, PIN_A_CHANNEL_B, 540, encoderKRAI::X4_ENCODING);
Fathoni17 0:22acd37ed695 56 encoderKRAI encoder_B(PIN_B_CHANNEL_A, PIN_B_CHANNEL_B, 540, encoderKRAI::X4_ENCODING);
Fathoni17 0:22acd37ed695 57 encoderKRAI encoder_C(PIN_C_CHANNEL_A, PIN_C_CHANNEL_B, 540, encoderKRAI::X4_ENCODING);
Fathoni17 0:22acd37ed695 58
Fathoni17 0:22acd37ed695 59 // Motor
Fathoni17 0:22acd37ed695 60 Motor motor1(PIN_PWM_A, PIN_FWD_A, PIN_REV_A);
Fathoni17 0:22acd37ed695 61 Motor motor2(PIN_PWM_B, PIN_FWD_B, PIN_REV_B);
Fathoni17 0:22acd37ed695 62 Motor motor3(PIN_PWM_C, PIN_FWD_C, PIN_REV_C);
Fathoni17 0:22acd37ed695 63
Fathoni17 6:bb7e29420efd 64 ////////////////////////////////////////////////////////////////////////////////
Fathoni17 6:bb7e29420efd 65 // Deklarasi Prosedure dan Fungsi //
Fathoni17 6:bb7e29420efd 66 ////////////////////////////////////////////////////////////////////////////////
Fathoni17 0:22acd37ed695 67 void gerakMotor();
Fathoni17 6:bb7e29420efd 68 void hitungPID(double theta_s);
Fathoni17 5:4a70c53d7f86 69 void hitungParameter();
Fathoni17 0:22acd37ed695 70 void printPulse();
Fathoni17 0:22acd37ed695 71 void case_gerak();
Fathoni17 0:22acd37ed695 72
Fathoni17 6:bb7e29420efd 73 ////////////////////////////////////////////////////////////////////////////////
Fathoni17 6:bb7e29420efd 74 // Variable-variable //
Fathoni17 6:bb7e29420efd 75 ////////////////////////////////////////////////////////////////////////////////
Fathoni17 0:22acd37ed695 76 int joystick;
Fathoni17 6:bb7e29420efd 77 double pulse_A=0, pulse_B=0, pulse_C=0;
Fathoni17 6:bb7e29420efd 78 double Vr = 0, Vr_max = 0;
Fathoni17 6:bb7e29420efd 79 double Vw = 0;
Fathoni17 6:bb7e29420efd 80 double a = 0;
Fathoni17 6:bb7e29420efd 81 double w = 0;
Fathoni17 6:bb7e29420efd 82 double x =0, x_s=0, y=0, y_s=0, x_prev=0, y_prev=0;
Fathoni17 6:bb7e29420efd 83 double theta=0, theta_s=0;
Fathoni17 6:bb7e29420efd 84 double theta_error = 0, theta_prev=0, theta_error_prev=0, sum_theta_error=0;
Fathoni17 6:bb7e29420efd 85 unsigned long last_mt_print, last_mt_pid, last_mt_rotasi;
Fathoni17 6:bb7e29420efd 86 unsigned long last_mt_aksel, last_mt_desel;
Fathoni17 6:bb7e29420efd 87 bool modeauto = 1;
Fathoni17 2:863436c840bf 88 bool print_pulse = 0;
Fathoni17 0:22acd37ed695 89
Fathoni17 6:bb7e29420efd 90 ////////////////////////////////////////////////////////////////////////////////
Fathoni17 6:bb7e29420efd 91 // Main Program //
Fathoni17 6:bb7e29420efd 92 ////////////////////////////////////////////////////////////////////////////////
Fathoni17 0:22acd37ed695 93 int main(){
Fathoni17 0:22acd37ed695 94 encoder_A.reset();
Fathoni17 0:22acd37ed695 95 encoder_B.reset();
Fathoni17 0:22acd37ed695 96 encoder_C.reset();
Fathoni17 6:bb7e29420efd 97 pneumatik = 0;
Fathoni17 6:bb7e29420efd 98 startMillis();
Fathoni17 0:22acd37ed695 99 stick.setup();
Fathoni17 0:22acd37ed695 100 stick.idle();
Fathoni17 0:22acd37ed695 101
Fathoni17 0:22acd37ed695 102 while(1){
Fathoni17 2:863436c840bf 103 if(stick.readable() ) {
Fathoni17 0:22acd37ed695 104 // Panggil fungsi pembacaan joystik
Fathoni17 0:22acd37ed695 105 stick.baca_data();
Fathoni17 0:22acd37ed695 106 // Panggil fungsi pengolahan data joystik
Fathoni17 0:22acd37ed695 107 stick.olah_data();
Fathoni17 2:863436c840bf 108 // Ambil data joystick
Fathoni17 2:863436c840bf 109 case_gerak();
Fathoni17 6:bb7e29420efd 110
Fathoni17 6:bb7e29420efd 111 // Pengatur Gerak Motor
Fathoni17 2:863436c840bf 112 gerakMotor();
Fathoni17 6:bb7e29420efd 113
Fathoni17 6:bb7e29420efd 114 // Penghitungan Theta, X, dan Y
Fathoni17 5:4a70c53d7f86 115 if (millis() - last_mt_pid > TS){
Fathoni17 5:4a70c53d7f86 116 hitungParameter();
Fathoni17 5:4a70c53d7f86 117 last_mt_pid = millis();
Fathoni17 5:4a70c53d7f86 118 }
Fathoni17 6:bb7e29420efd 119
Fathoni17 6:bb7e29420efd 120 // Program PID
Fathoni17 5:4a70c53d7f86 121 if (!(fabs(theta_s - (theta*RAD_TO_DEG))<TOLERANCET) && modeauto ){
Fathoni17 2:863436c840bf 122 hitungPID(theta_s);
Fathoni17 1:735173a3b218 123 }
MarchioKevin 3:b1403fcdaeb1 124 if (fabs(theta_s - (theta*RAD_TO_DEG))<TOLERANCET || !modeauto){
MarchioKevin 3:b1403fcdaeb1 125 if(modeauto) Vw = 0;
Fathoni17 2:863436c840bf 126 }
Fathoni17 6:bb7e29420efd 127
Fathoni17 6:bb7e29420efd 128 // Fungsi Serial Print
Fathoni17 6:bb7e29420efd 129 if (millis() - last_mt_print > TS*10){
Fathoni17 2:863436c840bf 130 if (print_pulse && DEBUG)
Fathoni17 2:863436c840bf 131 printPulse();
Fathoni17 2:863436c840bf 132 last_mt_print = millis();
Fathoni17 2:863436c840bf 133 }
Fathoni17 2:863436c840bf 134 }
Fathoni17 1:735173a3b218 135 }
Fathoni17 0:22acd37ed695 136 }
Fathoni17 0:22acd37ed695 137
Fathoni17 6:bb7e29420efd 138
Fathoni17 6:bb7e29420efd 139 ////////////////////////////////////////////////////////////////////////////////
Fathoni17 6:bb7e29420efd 140 // Prosedure dan Fungsi //
Fathoni17 6:bb7e29420efd 141 ////////////////////////////////////////////////////////////////////////////////
Fathoni17 5:4a70c53d7f86 142 void hitungParameter(){
Fathoni17 2:863436c840bf 143 pulse_A = encoder_A.getPulses()*PULSE_TO_JARAK;
Fathoni17 2:863436c840bf 144 pulse_B = encoder_B.getPulses()*PULSE_TO_JARAK;
Fathoni17 2:863436c840bf 145 pulse_C = encoder_C.getPulses()*PULSE_TO_JARAK;
Fathoni17 2:863436c840bf 146
Fathoni17 2:863436c840bf 147 //Compute value
MarchioKevin 3:b1403fcdaeb1 148 x = x_prev + (2*pulse_A - pulse_C - pulse_B)/3*cos(theta_prev) - (-pulse_C+pulse_B)*0.5773*sin(theta_prev);
MarchioKevin 3:b1403fcdaeb1 149 y = y_prev + (2*pulse_A - pulse_C - pulse_B)/3*sin(theta_prev) + (-pulse_C+pulse_B)*0.5773*cos(theta_prev);
Fathoni17 2:863436c840bf 150 theta = theta_prev + (pulse_A + pulse_C + pulse_B)/(3.0*L);
Fathoni17 2:863436c840bf 151
Fathoni17 2:863436c840bf 152 //Update value
MarchioKevin 3:b1403fcdaeb1 153 x_prev = x;
MarchioKevin 3:b1403fcdaeb1 154 y_prev = y;
Fathoni17 2:863436c840bf 155 theta_prev = theta;
Fathoni17 2:863436c840bf 156
Fathoni17 2:863436c840bf 157 encoder_A.reset();
Fathoni17 2:863436c840bf 158 encoder_B.reset();
Fathoni17 2:863436c840bf 159 encoder_C.reset();
Fathoni17 5:4a70c53d7f86 160 }
Fathoni17 6:bb7e29420efd 161
Fathoni17 6:bb7e29420efd 162 void hitungPID(double theta_s){
Fathoni17 2:863436c840bf 163 //menghitung error jarak x,y terhaadap xs,ys
Fathoni17 2:863436c840bf 164 theta_error = theta_s - (theta*RAD_TO_DEG);
Fathoni17 2:863436c840bf 165 sum_theta_error += theta_error;
Fathoni17 2:863436c840bf 166
Fathoni17 2:863436c840bf 167 //kalkulasi PID Theta
Fathoni17 2:863436c840bf 168 w = KP_W*theta_error + KI_W*TS*sum_theta_error + KD_W*(theta_error - theta_error_prev)/TS;
MarchioKevin 3:b1403fcdaeb1 169 Vw += (w*L/MAX_W_SPEED)*LIMITPWM;
Fathoni17 2:863436c840bf 170
Fathoni17 2:863436c840bf 171 //update
Fathoni17 2:863436c840bf 172 theta_error_prev = theta_error;
Fathoni17 6:bb7e29420efd 173
Fathoni17 2:863436c840bf 174 //saturasi vw
MarchioKevin 3:b1403fcdaeb1 175 if (Vw > 0.2){
MarchioKevin 3:b1403fcdaeb1 176 Vw = 0.2;
Fathoni17 0:22acd37ed695 177 }
MarchioKevin 3:b1403fcdaeb1 178 else if ( Vw < -0.2){
MarchioKevin 3:b1403fcdaeb1 179 Vw = -0.2;
Fathoni17 2:863436c840bf 180 }
Fathoni17 0:22acd37ed695 181 }
Fathoni17 0:22acd37ed695 182
Fathoni17 0:22acd37ed695 183 void gerakMotor(){
Fathoni17 5:4a70c53d7f86 184 if ((Vw == 0) && (Vr_max == 0)){
Fathoni17 6:bb7e29420efd 185 // Deselerasi
Fathoni17 5:4a70c53d7f86 186 if (Vr >= 0.05){
Fathoni17 5:4a70c53d7f86 187 if (millis() - last_mt_desel > 70){
Fathoni17 5:4a70c53d7f86 188 Vr -= 0.1;
Fathoni17 5:4a70c53d7f86 189 last_mt_desel = millis();
Fathoni17 5:4a70c53d7f86 190 }
Fathoni17 6:bb7e29420efd 191 } // Rem
Fathoni17 6:bb7e29420efd 192 else {
Fathoni17 5:4a70c53d7f86 193 motor1.brake(BRAKE_HIGH);
Fathoni17 5:4a70c53d7f86 194 motor2.brake(BRAKE_HIGH);
Fathoni17 5:4a70c53d7f86 195 motor3.brake(BRAKE_HIGH);
Fathoni17 6:bb7e29420efd 196
Fathoni17 6:bb7e29420efd 197 // Pengaturan variable saat berhenti
Fathoni17 5:4a70c53d7f86 198 print_pulse = 0;
Fathoni17 5:4a70c53d7f86 199 Vr = 0;
Fathoni17 5:4a70c53d7f86 200 }
Fathoni17 2:863436c840bf 201 } else {
Fathoni17 6:bb7e29420efd 202 // Akselerasi
Fathoni17 5:4a70c53d7f86 203 if ((millis() - last_mt_aksel > 150) && Vr < Vr_max){
Fathoni17 5:4a70c53d7f86 204 if (Vr < 0.275)
Fathoni17 5:4a70c53d7f86 205 Vr = 0.275;
Fathoni17 5:4a70c53d7f86 206 else
Fathoni17 5:4a70c53d7f86 207 Vr += 0.05;
Fathoni17 5:4a70c53d7f86 208 last_mt_aksel = millis();
Fathoni17 5:4a70c53d7f86 209 }
Fathoni17 6:bb7e29420efd 210 // Limit
Fathoni17 5:4a70c53d7f86 211 if (Vr > Vr_max && Vr_max >= 0.000)
Fathoni17 5:4a70c53d7f86 212 Vr = Vr_max;
Fathoni17 6:bb7e29420efd 213
Fathoni17 6:bb7e29420efd 214 // Control Motor
Fathoni17 2:863436c840bf 215 motor1.speed((-1*Vr*cos(a) + Vw));
Fathoni17 2:863436c840bf 216 motor2.speed((Vr*(0.5*cos(a) + 0.866*sin(a)) + Vw));
Fathoni17 2:863436c840bf 217 motor3.speed((Vr*(0.5*cos(a) - 0.866*sin(a)) + Vw));
Fathoni17 2:863436c840bf 218 print_pulse = 1;
Fathoni17 2:863436c840bf 219 }
Fathoni17 0:22acd37ed695 220 }
Fathoni17 0:22acd37ed695 221
Fathoni17 0:22acd37ed695 222 void printPulse(){
Fathoni17 6:bb7e29420efd 223 pc.printf("%.2f\t%.2f\n", theta*RAD_TO_DEG, theta_s);
Fathoni17 6:bb7e29420efd 224 //pc.printf("%.2d\t%.2d\t%.2d\n", pulse_A, pulse_B, pulse_C);
Fathoni17 0:22acd37ed695 225 }
Fathoni17 0:22acd37ed695 226
Fathoni17 0:22acd37ed695 227 void case_gerak(){
Fathoni17 6:bb7e29420efd 228 // Pengubah Mode
Fathoni17 6:bb7e29420efd 229 if(stick.START_click){
Fathoni17 6:bb7e29420efd 230 modeauto = !modeauto;
Fathoni17 6:bb7e29420efd 231 theta = 0.0;
Fathoni17 6:bb7e29420efd 232 theta_prev = 0.0;
Fathoni17 6:bb7e29420efd 233 theta_s = 0;
Fathoni17 6:bb7e29420efd 234 theta_error_prev = 0;
Fathoni17 6:bb7e29420efd 235 sum_theta_error = 0;
Fathoni17 6:bb7e29420efd 236 theta_error = 0;
Fathoni17 6:bb7e29420efd 237 }
Fathoni17 6:bb7e29420efd 238
Fathoni17 0:22acd37ed695 239 // Rotasi
MarchioKevin 3:b1403fcdaeb1 240 if(modeauto){
Fathoni17 5:4a70c53d7f86 241 if (!stick.L1 && stick.R1){ // Pivot Kanan
Fathoni17 5:4a70c53d7f86 242 theta = 0.0;
Fathoni17 5:4a70c53d7f86 243 theta_prev = 0.0;
Fathoni17 5:4a70c53d7f86 244 theta_s = 0;
Fathoni17 5:4a70c53d7f86 245 theta_error_prev = 0;
Fathoni17 5:4a70c53d7f86 246 sum_theta_error = 0;
Fathoni17 5:4a70c53d7f86 247 theta_error = 0;
Fathoni17 5:4a70c53d7f86 248 Vw = 0.2;
Fathoni17 5:4a70c53d7f86 249 }
Fathoni17 5:4a70c53d7f86 250 else if (!stick.R1 && stick.L1){ // Pivot Kiri
Fathoni17 5:4a70c53d7f86 251 theta = 0.0;
Fathoni17 5:4a70c53d7f86 252 theta_prev = 0.0;
Fathoni17 5:4a70c53d7f86 253 theta_s = 0;
Fathoni17 5:4a70c53d7f86 254 theta_error_prev = 0;
Fathoni17 5:4a70c53d7f86 255 sum_theta_error = 0;
Fathoni17 5:4a70c53d7f86 256 theta_error = 0;
Fathoni17 5:4a70c53d7f86 257 Vw = -0.2;
Fathoni17 5:4a70c53d7f86 258 }
Fathoni17 6:bb7e29420efd 259 } else if(!modeauto){
MarchioKevin 3:b1403fcdaeb1 260 if (!stick.L1 && stick.R1) // Pivot Kanan
MarchioKevin 3:b1403fcdaeb1 261 Vw = 0.3;
MarchioKevin 3:b1403fcdaeb1 262 else if (!stick.R1 && stick.L1) // Pivot Kiri
MarchioKevin 3:b1403fcdaeb1 263 Vw = -0.3;
MarchioKevin 3:b1403fcdaeb1 264 else
MarchioKevin 3:b1403fcdaeb1 265 Vw = 0.0;
MarchioKevin 3:b1403fcdaeb1 266 }
MarchioKevin 3:b1403fcdaeb1 267
Fathoni17 0:22acd37ed695 268 // Linier
MarchioKevin 3:b1403fcdaeb1 269 if ((stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(!stick.kiri)){
Fathoni17 0:22acd37ed695 270 a = -90/RAD_TO_DEG; // Maju
Fathoni17 5:4a70c53d7f86 271 Vr_max = 0.8;
MarchioKevin 3:b1403fcdaeb1 272 }
MarchioKevin 3:b1403fcdaeb1 273 else if ((!stick.atas)&&(stick.bawah)&&(!stick.kanan)&&(!stick.kiri)){
Fathoni17 0:22acd37ed695 274 a = 90/RAD_TO_DEG; // Mundur
Fathoni17 5:4a70c53d7f86 275 Vr_max = 0.8;
MarchioKevin 3:b1403fcdaeb1 276 }
MarchioKevin 3:b1403fcdaeb1 277 else if ((stick.atas)&&(!stick.bawah)&&(!stick.kiri)&&(stick.kanan)){
Fathoni17 0:22acd37ed695 278 a = -135/RAD_TO_DEG; // Serong Atas Kanan
Fathoni17 5:4a70c53d7f86 279 Vr_max = 0.7;
MarchioKevin 3:b1403fcdaeb1 280 }
MarchioKevin 3:b1403fcdaeb1 281 else if ((!stick.atas)&&(stick.bawah)&&(!stick.kiri)&&(stick.kanan)){
Fathoni17 0:22acd37ed695 282 a = 135/RAD_TO_DEG; // Serong Bawah Kanan
Fathoni17 5:4a70c53d7f86 283 Vr_max = 0.7;
MarchioKevin 3:b1403fcdaeb1 284 }
MarchioKevin 3:b1403fcdaeb1 285 else if ((stick.atas)&&(!stick.bawah)&&(stick.kiri)&&(!stick.kanan)){
Fathoni17 0:22acd37ed695 286 a = -45/RAD_TO_DEG; // Serong Atas Kiri
Fathoni17 5:4a70c53d7f86 287 Vr_max = 0.7;
MarchioKevin 3:b1403fcdaeb1 288 }
MarchioKevin 3:b1403fcdaeb1 289 else if ((!stick.atas)&&(stick.bawah)&&(stick.kiri)&&(!stick.kanan)){
Fathoni17 0:22acd37ed695 290 a = 45/RAD_TO_DEG; // Serong Bawah Kiri
Fathoni17 5:4a70c53d7f86 291 Vr_max = 0.7;
MarchioKevin 3:b1403fcdaeb1 292 }
MarchioKevin 3:b1403fcdaeb1 293 else if ((!stick.atas)&&(!stick.bawah)&&(stick.kanan)&&(!stick.kiri)){
Fathoni17 0:22acd37ed695 294 a = 180/RAD_TO_DEG; // Kanan
Fathoni17 5:4a70c53d7f86 295 Vr_max = 0.5;
MarchioKevin 3:b1403fcdaeb1 296 }
MarchioKevin 3:b1403fcdaeb1 297 else if ((!stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(stick.kiri)){
Fathoni17 0:22acd37ed695 298 a = 0/RAD_TO_DEG; // Kiri
Fathoni17 5:4a70c53d7f86 299 Vr_max = 0.5;
MarchioKevin 3:b1403fcdaeb1 300 }
Fathoni17 0:22acd37ed695 301 else {
Fathoni17 5:4a70c53d7f86 302 Vr_max = 0;
Fathoni17 0:22acd37ed695 303 }
Fathoni17 0:22acd37ed695 304
Fathoni17 6:bb7e29420efd 305 // Control Pneumatic
Fathoni17 0:22acd37ed695 306 if ((stick.silang_click)&&(!stick.kotak)&&(!stick.segitiga)&&(!stick.lingkaran))
Fathoni17 6:bb7e29420efd 307 pneumatik = !pneumatik;
Fathoni17 6:bb7e29420efd 308 if ((!stick.silang_click)&&(!stick.kotak)&&(!stick.segitiga)&&(stick.lingkaran))
Fathoni17 6:bb7e29420efd 309 pneumatik_2 = !pneumatik_2;
Fathoni17 6:bb7e29420efd 310
Fathoni17 0:22acd37ed695 311 }