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