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@12:9f56ff893897, 2018-05-26 (annotated)

Committer:
MarchioKevin
Date:
Sat May 26 08:19:01 2018 +0000
Revision:
12:9f56ff893897
Parent:
11:c682c965f781 
Waini;

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
MarchioKevin 12:9f56ff893897 28 #define PULSE_TO_JARAK 0.58148148 //0.73885605 //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
MarchioKevin 11:c682c965f781 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 8:8072ee4f1740 52 DigitalOut pneumatik[5]{PIN_PNEUMATIK_1, PIN_PNEUMATIK_2, PIN_PNEUMATIK_3, PIN_PNEUMATIK_4, PIN_PNEUMATIK_5};
Fathoni17 7:8d8eb4676356 53 //DigitalOut pneumatik2(PIN_PNEUMATIK_2);
Fathoni17 7:8d8eb4676356 54 //DigitalOut pneumatik3(PIN_PNEUMATIK_3);
Fathoni17 0:22acd37ed695 55
Fathoni17 0:22acd37ed695 56 // Encoder
Fathoni17 0:22acd37ed695 57 encoderKRAI encoder_A(PIN_A_CHANNEL_A, PIN_A_CHANNEL_B, 540, encoderKRAI::X4_ENCODING);
Fathoni17 0:22acd37ed695 58 encoderKRAI encoder_B(PIN_B_CHANNEL_A, PIN_B_CHANNEL_B, 540, encoderKRAI::X4_ENCODING);
Fathoni17 0:22acd37ed695 59 encoderKRAI encoder_C(PIN_C_CHANNEL_A, PIN_C_CHANNEL_B, 540, encoderKRAI::X4_ENCODING);
Fathoni17 0:22acd37ed695 60
Fathoni17 0:22acd37ed695 61 // Motor
Fathoni17 0:22acd37ed695 62 Motor motor1(PIN_PWM_A, PIN_FWD_A, PIN_REV_A);
Fathoni17 0:22acd37ed695 63 Motor motor2(PIN_PWM_B, PIN_FWD_B, PIN_REV_B);
Fathoni17 0:22acd37ed695 64 Motor motor3(PIN_PWM_C, PIN_FWD_C, PIN_REV_C);
Fathoni17 0:22acd37ed695 65
Fathoni17 6:bb7e29420efd 66 ////////////////////////////////////////////////////////////////////////////////
Fathoni17 6:bb7e29420efd 67 // Deklarasi Prosedure dan Fungsi //
Fathoni17 6:bb7e29420efd 68 ////////////////////////////////////////////////////////////////////////////////
Fathoni17 0:22acd37ed695 69 void gerakMotor();
Fathoni17 6:bb7e29420efd 70 void hitungPID(double theta_s);
Fathoni17 5:4a70c53d7f86 71 void hitungParameter();
Fathoni17 0:22acd37ed695 72 void printPulse();
Fathoni17 0:22acd37ed695 73 void case_gerak();
Fathoni17 0:22acd37ed695 74
Fathoni17 6:bb7e29420efd 75 ////////////////////////////////////////////////////////////////////////////////
Fathoni17 6:bb7e29420efd 76 // Variable-variable //
Fathoni17 6:bb7e29420efd 77 ////////////////////////////////////////////////////////////////////////////////
Fathoni17 0:22acd37ed695 78 int joystick;
Fathoni17 7:8d8eb4676356 79 int pn = 0;
Fathoni17 6:bb7e29420efd 80 double pulse_A=0, pulse_B=0, pulse_C=0;
Fathoni17 6:bb7e29420efd 81 double Vr = 0, Vr_max = 0;
Fathoni17 6:bb7e29420efd 82 double Vw = 0;
Fathoni17 6:bb7e29420efd 83 double a = 0;
Fathoni17 6:bb7e29420efd 84 double w = 0;
Fathoni17 6:bb7e29420efd 85 double x =0, x_s=0, y=0, y_s=0, x_prev=0, y_prev=0;
Fathoni17 6:bb7e29420efd 86 double theta=0, theta_s=0;
Fathoni17 6:bb7e29420efd 87 double theta_error = 0, theta_prev=0, theta_error_prev=0, sum_theta_error=0;
Fathoni17 6:bb7e29420efd 88 unsigned long last_mt_print, last_mt_pid, last_mt_rotasi;
Fathoni17 6:bb7e29420efd 89 unsigned long last_mt_aksel, last_mt_desel;
Fathoni17 6:bb7e29420efd 90 bool modeauto = 1;
Fathoni17 2:863436c840bf 91 bool print_pulse = 0;
Fathoni17 0:22acd37ed695 92
Fathoni17 6:bb7e29420efd 93 ////////////////////////////////////////////////////////////////////////////////
Fathoni17 6:bb7e29420efd 94 // Main Program //
Fathoni17 6:bb7e29420efd 95 ////////////////////////////////////////////////////////////////////////////////
Fathoni17 0:22acd37ed695 96 int main(){
Fathoni17 0:22acd37ed695 97 encoder_A.reset();
Fathoni17 0:22acd37ed695 98 encoder_B.reset();
Fathoni17 0:22acd37ed695 99 encoder_C.reset();
Fathoni17 7:8d8eb4676356 100 for (int i = 0; i<3; i++){
Fathoni17 7:8d8eb4676356 101 pneumatik[i] = 0;
Fathoni17 7:8d8eb4676356 102 }
Fathoni17 6:bb7e29420efd 103 startMillis();
Fathoni17 0:22acd37ed695 104 stick.setup();
Fathoni17 0:22acd37ed695 105 stick.idle();
Fathoni17 0:22acd37ed695 106
Fathoni17 0:22acd37ed695 107 while(1){
Fathoni17 2:863436c840bf 108 if(stick.readable() ) {
Fathoni17 0:22acd37ed695 109 // Panggil fungsi pembacaan joystik
Fathoni17 0:22acd37ed695 110 stick.baca_data();
Fathoni17 0:22acd37ed695 111 // Panggil fungsi pengolahan data joystik
Fathoni17 0:22acd37ed695 112 stick.olah_data();
Fathoni17 2:863436c840bf 113 // Ambil data joystick
Fathoni17 2:863436c840bf 114 case_gerak();
Fathoni17 6:bb7e29420efd 115
Fathoni17 6:bb7e29420efd 116 // Pengatur Gerak Motor
Fathoni17 2:863436c840bf 117 gerakMotor();
Fathoni17 6:bb7e29420efd 118
Fathoni17 6:bb7e29420efd 119 // Penghitungan Theta, X, dan Y
Fathoni17 5:4a70c53d7f86 120 if (millis() - last_mt_pid > TS){
Fathoni17 5:4a70c53d7f86 121 hitungParameter();
Fathoni17 5:4a70c53d7f86 122 last_mt_pid = millis();
Fathoni17 5:4a70c53d7f86 123 }
Fathoni17 6:bb7e29420efd 124
Fathoni17 6:bb7e29420efd 125 // Program PID
Fathoni17 5:4a70c53d7f86 126 if (!(fabs(theta_s - (theta*RAD_TO_DEG))<TOLERANCET) && modeauto ){
Fathoni17 2:863436c840bf 127 hitungPID(theta_s);
Fathoni17 1:735173a3b218 128 }
MarchioKevin 3:b1403fcdaeb1 129 if (fabs(theta_s - (theta*RAD_TO_DEG))<TOLERANCET || !modeauto){
MarchioKevin 3:b1403fcdaeb1 130 if(modeauto) Vw = 0;
Fathoni17 2:863436c840bf 131 }
Fathoni17 6:bb7e29420efd 132
Fathoni17 6:bb7e29420efd 133 // Fungsi Serial Print
Fathoni17 6:bb7e29420efd 134 if (millis() - last_mt_print > TS*10){
Fathoni17 2:863436c840bf 135 if (print_pulse && DEBUG)
Fathoni17 2:863436c840bf 136 printPulse();
Fathoni17 2:863436c840bf 137 last_mt_print = millis();
Fathoni17 2:863436c840bf 138 }
Fathoni17 2:863436c840bf 139 }
Fathoni17 1:735173a3b218 140 }
Fathoni17 0:22acd37ed695 141 }
Fathoni17 0:22acd37ed695 142
Fathoni17 6:bb7e29420efd 143
Fathoni17 6:bb7e29420efd 144 ////////////////////////////////////////////////////////////////////////////////
Fathoni17 6:bb7e29420efd 145 // Prosedure dan Fungsi //
Fathoni17 6:bb7e29420efd 146 ////////////////////////////////////////////////////////////////////////////////
Fathoni17 5:4a70c53d7f86 147 void hitungParameter(){
Fathoni17 2:863436c840bf 148 pulse_A = encoder_A.getPulses()*PULSE_TO_JARAK;
Fathoni17 2:863436c840bf 149 pulse_B = encoder_B.getPulses()*PULSE_TO_JARAK;
Fathoni17 2:863436c840bf 150 pulse_C = encoder_C.getPulses()*PULSE_TO_JARAK;
Fathoni17 2:863436c840bf 151
Fathoni17 2:863436c840bf 152 //Compute value
MarchioKevin 3:b1403fcdaeb1 153 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 154 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 155 theta = theta_prev + (pulse_A + pulse_C + pulse_B)/(3.0*L);
Fathoni17 2:863436c840bf 156
Fathoni17 2:863436c840bf 157 //Update value
MarchioKevin 3:b1403fcdaeb1 158 x_prev = x;
MarchioKevin 3:b1403fcdaeb1 159 y_prev = y;
Fathoni17 2:863436c840bf 160 theta_prev = theta;
Fathoni17 2:863436c840bf 161
Fathoni17 2:863436c840bf 162 encoder_A.reset();
Fathoni17 2:863436c840bf 163 encoder_B.reset();
Fathoni17 2:863436c840bf 164 encoder_C.reset();
Fathoni17 5:4a70c53d7f86 165 }
Fathoni17 6:bb7e29420efd 166
Fathoni17 6:bb7e29420efd 167 void hitungPID(double theta_s){
Fathoni17 2:863436c840bf 168 //menghitung error jarak x,y terhaadap xs,ys
Fathoni17 2:863436c840bf 169 theta_error = theta_s - (theta*RAD_TO_DEG);
Fathoni17 2:863436c840bf 170 sum_theta_error += theta_error;
Fathoni17 2:863436c840bf 171
Fathoni17 2:863436c840bf 172 //kalkulasi PID Theta
Fathoni17 2:863436c840bf 173 w = KP_W*theta_error + KI_W*TS*sum_theta_error + KD_W*(theta_error - theta_error_prev)/TS;
MarchioKevin 3:b1403fcdaeb1 174 Vw += (w*L/MAX_W_SPEED)*LIMITPWM;
Fathoni17 2:863436c840bf 175
Fathoni17 2:863436c840bf 176 //update
Fathoni17 2:863436c840bf 177 theta_error_prev = theta_error;
Fathoni17 6:bb7e29420efd 178
Fathoni17 2:863436c840bf 179 //saturasi vw
MarchioKevin 3:b1403fcdaeb1 180 if (Vw > 0.2){
MarchioKevin 3:b1403fcdaeb1 181 Vw = 0.2;
Fathoni17 0:22acd37ed695 182 }
MarchioKevin 3:b1403fcdaeb1 183 else if ( Vw < -0.2){
MarchioKevin 3:b1403fcdaeb1 184 Vw = -0.2;
Fathoni17 2:863436c840bf 185 }
Fathoni17 0:22acd37ed695 186 }
Fathoni17 0:22acd37ed695 187
Fathoni17 0:22acd37ed695 188 void gerakMotor(){
Fathoni17 5:4a70c53d7f86 189 if ((Vw == 0) && (Vr_max == 0)){
Fathoni17 7:8d8eb4676356 190 motor1.brake(BRAKE_HIGH);
Fathoni17 7:8d8eb4676356 191 motor2.brake(BRAKE_HIGH);
Fathoni17 7:8d8eb4676356 192 motor3.brake(BRAKE_HIGH);
Fathoni17 7:8d8eb4676356 193
Fathoni17 7:8d8eb4676356 194 // Pengaturan variable saat berhenti
Fathoni17 7:8d8eb4676356 195 print_pulse = 0;
Fathoni17 7:8d8eb4676356 196 Vr = 0;
Fathoni17 2:863436c840bf 197 } else {
Fathoni17 6:bb7e29420efd 198 // Akselerasi
Fathoni17 5:4a70c53d7f86 199 if ((millis() - last_mt_aksel > 150) && Vr < Vr_max){
Fathoni17 7:8d8eb4676356 200 if (Vr < 0.35)
Fathoni17 7:8d8eb4676356 201 Vr = 0.35;
Fathoni17 5:4a70c53d7f86 202 else
Fathoni17 8:8072ee4f1740 203 Vr += 0.07;
Fathoni17 5:4a70c53d7f86 204 last_mt_aksel = millis();
Fathoni17 5:4a70c53d7f86 205 }
Fathoni17 6:bb7e29420efd 206 // Limit
Fathoni17 5:4a70c53d7f86 207 if (Vr > Vr_max && Vr_max >= 0.000)
Fathoni17 5:4a70c53d7f86 208 Vr = Vr_max;
Fathoni17 6:bb7e29420efd 209
Fathoni17 6:bb7e29420efd 210 // Control Motor
Fathoni17 7:8d8eb4676356 211 motor1.speed((Vr*cos(a) + Vw));
Fathoni17 7:8d8eb4676356 212 motor2.speed((Vr*(-0.5*cos(a) - 0.866*sin(a)) + Vw));
Fathoni17 7:8d8eb4676356 213 motor3.speed((Vr*(-0.5*cos(a) + 0.866*sin(a)) + Vw));
Fathoni17 2:863436c840bf 214 print_pulse = 1;
Fathoni17 2:863436c840bf 215 }
Fathoni17 0:22acd37ed695 216 }
Fathoni17 0:22acd37ed695 217
Fathoni17 0:22acd37ed695 218 void printPulse(){
Fathoni17 6:bb7e29420efd 219 pc.printf("%.2f\t%.2f\n", theta*RAD_TO_DEG, theta_s);
Fathoni17 6:bb7e29420efd 220 //pc.printf("%.2d\t%.2d\t%.2d\n", pulse_A, pulse_B, pulse_C);
Fathoni17 0:22acd37ed695 221 }
Fathoni17 0:22acd37ed695 222
Fathoni17 0:22acd37ed695 223 void case_gerak(){
Fathoni17 6:bb7e29420efd 224 // Pengubah Mode
Fathoni17 6:bb7e29420efd 225 if(stick.START_click){
Fathoni17 6:bb7e29420efd 226 modeauto = !modeauto;
Fathoni17 6:bb7e29420efd 227 theta = 0.0;
Fathoni17 6:bb7e29420efd 228 theta_prev = 0.0;
Fathoni17 6:bb7e29420efd 229 theta_s = 0;
Fathoni17 6:bb7e29420efd 230 theta_error_prev = 0;
Fathoni17 6:bb7e29420efd 231 sum_theta_error = 0;
Fathoni17 6:bb7e29420efd 232 theta_error = 0;
Fathoni17 6:bb7e29420efd 233 }
Fathoni17 6:bb7e29420efd 234
Fathoni17 0:22acd37ed695 235 // Rotasi
MarchioKevin 3:b1403fcdaeb1 236 if(modeauto){
Fathoni17 5:4a70c53d7f86 237 if (!stick.L1 && stick.R1){ // Pivot Kanan
Fathoni17 5:4a70c53d7f86 238 theta = 0.0;
Fathoni17 5:4a70c53d7f86 239 theta_prev = 0.0;
Fathoni17 5:4a70c53d7f86 240 theta_s = 0;
Fathoni17 5:4a70c53d7f86 241 theta_error_prev = 0;
Fathoni17 5:4a70c53d7f86 242 sum_theta_error = 0;
Fathoni17 5:4a70c53d7f86 243 theta_error = 0;
Fathoni17 5:4a70c53d7f86 244 Vw = 0.2;
Fathoni17 5:4a70c53d7f86 245 }
Fathoni17 5:4a70c53d7f86 246 else if (!stick.R1 && stick.L1){ // Pivot Kiri
Fathoni17 5:4a70c53d7f86 247 theta = 0.0;
Fathoni17 5:4a70c53d7f86 248 theta_prev = 0.0;
Fathoni17 5:4a70c53d7f86 249 theta_s = 0;
Fathoni17 5:4a70c53d7f86 250 theta_error_prev = 0;
Fathoni17 5:4a70c53d7f86 251 sum_theta_error = 0;
Fathoni17 5:4a70c53d7f86 252 theta_error = 0;
Fathoni17 5:4a70c53d7f86 253 Vw = -0.2;
Fathoni17 5:4a70c53d7f86 254 }
Fathoni17 6:bb7e29420efd 255 } else if(!modeauto){
MarchioKevin 3:b1403fcdaeb1 256 if (!stick.L1 && stick.R1) // Pivot Kanan
MarchioKevin 3:b1403fcdaeb1 257 Vw = 0.3;
MarchioKevin 3:b1403fcdaeb1 258 else if (!stick.R1 && stick.L1) // Pivot Kiri
MarchioKevin 3:b1403fcdaeb1 259 Vw = -0.3;
MarchioKevin 3:b1403fcdaeb1 260 else
MarchioKevin 3:b1403fcdaeb1 261 Vw = 0.0;
MarchioKevin 3:b1403fcdaeb1 262 }
MarchioKevin 3:b1403fcdaeb1 263
Fathoni17 0:22acd37ed695 264 // Linier
MarchioKevin 11:c682c965f781 265 if ((stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(!stick.kiri)){
MarchioKevin 11:c682c965f781 266 a = 90/RAD_TO_DEG; // Maju
MarchioKevin 10:a478ba07ed33 267 Vr_max = 0.7;
MarchioKevin 11:c682c965f781 268 if (stick.R2){
MarchioKevin 11:c682c965f781 269 Vr_max = 0.9;
MarchioKevin 11:c682c965f781 270 }
MarchioKevin 10:a478ba07ed33 271 }
MarchioKevin 10:a478ba07ed33 272 else if ((!stick.atas)&&(stick.bawah)&&(!stick.kanan)&&(!stick.kiri)){
MarchioKevin 11:c682c965f781 273 a = -90/RAD_TO_DEG; // Mundur
MarchioKevin 10:a478ba07ed33 274 Vr_max = 0.7;
MarchioKevin 11:c682c965f781 275 if (stick.R2){
MarchioKevin 11:c682c965f781 276 Vr_max = 0.9;
MarchioKevin 11:c682c965f781 277 }
MarchioKevin 10:a478ba07ed33 278 }
MarchioKevin 10:a478ba07ed33 279 else if ((stick.atas)&&(!stick.bawah)&&(!stick.kiri)&&(stick.kanan)){
MarchioKevin 10:a478ba07ed33 280 a = -135/RAD_TO_DEG; // Serong Atas Kanan
MarchioKevin 10:a478ba07ed33 281 Vr_max = 0.7;
MarchioKevin 11:c682c965f781 282 if (stick.R2){
MarchioKevin 11:c682c965f781 283 Vr_max = 0.9;
MarchioKevin 11:c682c965f781 284 }
MarchioKevin 10:a478ba07ed33 285 }
MarchioKevin 10:a478ba07ed33 286 else if ((!stick.atas)&&(stick.bawah)&&(!stick.kiri)&&(stick.kanan)){
MarchioKevin 10:a478ba07ed33 287 a = 135/RAD_TO_DEG; // Serong Bawah Kanan
MarchioKevin 10:a478ba07ed33 288 Vr_max = 0.7;
MarchioKevin 11:c682c965f781 289 if (stick.R2){
MarchioKevin 11:c682c965f781 290 Vr_max = 0.9;
MarchioKevin 11:c682c965f781 291 }
MarchioKevin 10:a478ba07ed33 292 }
MarchioKevin 10:a478ba07ed33 293 else if ((stick.atas)&&(!stick.bawah)&&(stick.kiri)&&(!stick.kanan)){
MarchioKevin 10:a478ba07ed33 294 a = -45/RAD_TO_DEG; // Serong Atas Kiri
MarchioKevin 10:a478ba07ed33 295 Vr_max = 0.7;
MarchioKevin 11:c682c965f781 296 if (stick.R2){
MarchioKevin 11:c682c965f781 297 Vr_max = 0.9;
MarchioKevin 11:c682c965f781 298 }
MarchioKevin 10:a478ba07ed33 299 }
MarchioKevin 10:a478ba07ed33 300 else if ((!stick.atas)&&(stick.bawah)&&(stick.kiri)&&(!stick.kanan)){
MarchioKevin 10:a478ba07ed33 301 a = 45/RAD_TO_DEG; // Serong Bawah Kiri
MarchioKevin 10:a478ba07ed33 302 Vr_max = 0.7;
MarchioKevin 11:c682c965f781 303 if (stick.R2){
MarchioKevin 11:c682c965f781 304 Vr_max = 0.9;
MarchioKevin 11:c682c965f781 305 }
MarchioKevin 10:a478ba07ed33 306 }
MarchioKevin 10:a478ba07ed33 307 else if ((!stick.atas)&&(!stick.bawah)&&(stick.kanan)&&(!stick.kiri)){
MarchioKevin 10:a478ba07ed33 308 a = 180/RAD_TO_DEG; // Kanan
MarchioKevin 10:a478ba07ed33 309 Vr_max = 0.7;
MarchioKevin 11:c682c965f781 310 if (stick.R2){
MarchioKevin 11:c682c965f781 311 Vr_max = 0.9;
MarchioKevin 11:c682c965f781 312 }
MarchioKevin 10:a478ba07ed33 313 }
MarchioKevin 10:a478ba07ed33 314 else if ((!stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(stick.kiri)){
MarchioKevin 10:a478ba07ed33 315 a = 0/RAD_TO_DEG; // Kiri
MarchioKevin 10:a478ba07ed33 316 Vr_max = 0.7;
MarchioKevin 11:c682c965f781 317 if (stick.R2){
MarchioKevin 11:c682c965f781 318 Vr_max = 0.9;
MarchioKevin 11:c682c965f781 319 }
MarchioKevin 10:a478ba07ed33 320 }
MarchioKevin 10:a478ba07ed33 321 else {
MarchioKevin 10:a478ba07ed33 322 Vr_max = 0;
MarchioKevin 10:a478ba07ed33 323 }
Fathoni17 0:22acd37ed695 324
Fathoni17 6:bb7e29420efd 325 // Control Pneumatic
Fathoni17 7:8d8eb4676356 326 if ((stick.silang_click)&&(!stick.kotak)&&(!stick.segitiga)&&(!stick.lingkaran)){
Fathoni17 7:8d8eb4676356 327 pneumatik[pn] = 1;
Fathoni17 7:8d8eb4676356 328 pn++;
Fathoni17 8:8072ee4f1740 329 if (pn > 4) pn = 2;
Fathoni17 7:8d8eb4676356 330 }
Fathoni17 7:8d8eb4676356 331 if ((!stick.silang_click)&&(!stick.kotak)&&(!stick.segitiga)&&(stick.lingkaran)){
Fathoni17 8:8072ee4f1740 332 //pneumatik[pn] = 0;
Fathoni17 8:8072ee4f1740 333 //pn--;
Fathoni17 7:8d8eb4676356 334 if (pn < 0) pn = 0;
Fathoni17 7:8d8eb4676356 335 }
Fathoni17 7:8d8eb4676356 336 if ((!stick.silang_click)&&(!stick.kotak)&&(stick.segitiga)&&(!stick.lingkaran))
Fathoni17 8:8072ee4f1740 337 for (int i = 0; i<5; i++){
Fathoni17 7:8d8eb4676356 338 pneumatik[i] = 0;
Fathoni17 7:8d8eb4676356 339 pn = 0;
Fathoni17 7:8d8eb4676356 340 }
Fathoni17 7:8d8eb4676356 341 if ((!stick.silang_click)&&(stick.kotak)&&(!stick.segitiga)&&(!stick.lingkaran))
Fathoni17 8:8072ee4f1740 342 for (int i = 0; i<5; i++){
Fathoni17 7:8d8eb4676356 343 pneumatik[i] = 1;
Fathoni17 7:8d8eb4676356 344 pn = 0;
Fathoni17 7:8d8eb4676356 345 }
Fathoni17 6:bb7e29420efd 346
Fathoni17 0:22acd37ed695 347 }