Buat agip
Dependencies: Motor_1 encoderKRAI mbed millis
Fork of Robo_Taker_Nasional_2018 by
main.cpp@4:3c890389e256, 2018-03-03 (annotated)
- Committer:
- Fathoni17
- Date:
- Sat Mar 03 06:28:39 2018 +0000
- Revision:
- 4:3c890389e256
- Parent:
- 3:b1403fcdaeb1
Update kecepatan
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 |
MarchioKevin | 3:b1403fcdaeb1 | 21 | #define KD_W 125 |
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 | 0:22acd37ed695 | 48 | void printPulse(); |
Fathoni17 | 0:22acd37ed695 | 49 | void case_gerak(); |
MarchioKevin | 3:b1403fcdaeb1 | 50 | float compute_Alpha(float x_s, float y_s, float x, float y,float theta); |
Fathoni17 | 0:22acd37ed695 | 51 | |
Fathoni17 | 0:22acd37ed695 | 52 | // Variable-variable |
Fathoni17 | 0:22acd37ed695 | 53 | int joystick; |
Fathoni17 | 2:863436c840bf | 54 | float pulse_A=0; |
Fathoni17 | 2:863436c840bf | 55 | float pulse_B=0; |
Fathoni17 | 2:863436c840bf | 56 | float pulse_C=0; |
Fathoni17 | 0:22acd37ed695 | 57 | float Vr = 0; |
Fathoni17 | 0:22acd37ed695 | 58 | float Vw = 0; |
Fathoni17 | 0:22acd37ed695 | 59 | float a = 0; |
Fathoni17 | 2:863436c840bf | 60 | float w = 0; |
MarchioKevin | 3:b1403fcdaeb1 | 61 | float x =0; |
MarchioKevin | 3:b1403fcdaeb1 | 62 | float x_s = 0; |
MarchioKevin | 3:b1403fcdaeb1 | 63 | float y =0; |
MarchioKevin | 3:b1403fcdaeb1 | 64 | float y_s = 0; |
MarchioKevin | 3:b1403fcdaeb1 | 65 | float x_prev=0; |
MarchioKevin | 3:b1403fcdaeb1 | 66 | float y_prev=0; |
Fathoni17 | 2:863436c840bf | 67 | float theta_s = 0; |
Fathoni17 | 2:863436c840bf | 68 | float theta = 0; |
Fathoni17 | 2:863436c840bf | 69 | float theta_prev = 0; |
Fathoni17 | 2:863436c840bf | 70 | float theta_error_prev = 0; |
Fathoni17 | 2:863436c840bf | 71 | float sum_theta_error = 0; |
Fathoni17 | 2:863436c840bf | 72 | float theta_error; |
Fathoni17 | 2:863436c840bf | 73 | unsigned long last_mt_print, last_mt_pid, last_mt_rotasi; |
Fathoni17 | 2:863436c840bf | 74 | bool print_pulse = 0; |
MarchioKevin | 3:b1403fcdaeb1 | 75 | bool modeauto = 0; |
Fathoni17 | 0:22acd37ed695 | 76 | |
Fathoni17 | 0:22acd37ed695 | 77 | int main(){ |
Fathoni17 | 0:22acd37ed695 | 78 | encoder_A.reset(); |
Fathoni17 | 0:22acd37ed695 | 79 | encoder_B.reset(); |
Fathoni17 | 0:22acd37ed695 | 80 | encoder_C.reset(); |
Fathoni17 | 2:863436c840bf | 81 | pc.baud(115200); |
Fathoni17 | 0:22acd37ed695 | 82 | stick.setup(); |
Fathoni17 | 0:22acd37ed695 | 83 | stick.idle(); |
Fathoni17 | 0:22acd37ed695 | 84 | pneumatik = 0; |
Fathoni17 | 2:863436c840bf | 85 | startMillis(); |
Fathoni17 | 0:22acd37ed695 | 86 | |
Fathoni17 | 0:22acd37ed695 | 87 | while(1){ |
Fathoni17 | 0:22acd37ed695 | 88 | // do nothing |
Fathoni17 | 2:863436c840bf | 89 | if(stick.readable() ) { |
Fathoni17 | 0:22acd37ed695 | 90 | // Panggil fungsi pembacaan joystik |
Fathoni17 | 0:22acd37ed695 | 91 | stick.baca_data(); |
Fathoni17 | 0:22acd37ed695 | 92 | // Panggil fungsi pengolahan data joystik |
Fathoni17 | 0:22acd37ed695 | 93 | stick.olah_data(); |
Fathoni17 | 2:863436c840bf | 94 | // Ambil data joystick |
Fathoni17 | 2:863436c840bf | 95 | case_gerak(); |
Fathoni17 | 2:863436c840bf | 96 | |
Fathoni17 | 2:863436c840bf | 97 | gerakMotor(); |
Fathoni17 | 0:22acd37ed695 | 98 | |
MarchioKevin | 3:b1403fcdaeb1 | 99 | if ( (millis() - last_mt_pid > TS) && !(fabs(theta_s - (theta*RAD_TO_DEG))<TOLERANCET) && modeauto ){ |
Fathoni17 | 2:863436c840bf | 100 | hitungPID(theta_s); |
Fathoni17 | 2:863436c840bf | 101 | last_mt_pid = millis(); |
Fathoni17 | 1:735173a3b218 | 102 | } |
MarchioKevin | 3:b1403fcdaeb1 | 103 | if (fabs(theta_s - (theta*RAD_TO_DEG))<TOLERANCET || !modeauto){ |
Fathoni17 | 2:863436c840bf | 104 | pulse_A = encoder_A.getPulses()*PULSE_TO_JARAK; |
Fathoni17 | 2:863436c840bf | 105 | pulse_B = encoder_B.getPulses()*PULSE_TO_JARAK; |
Fathoni17 | 2:863436c840bf | 106 | pulse_C = encoder_C.getPulses()*PULSE_TO_JARAK; |
Fathoni17 | 2:863436c840bf | 107 | |
Fathoni17 | 2:863436c840bf | 108 | //Compute value |
Fathoni17 | 2:863436c840bf | 109 | theta = theta_prev + (pulse_A + pulse_C + pulse_B)/(3.0*L); |
MarchioKevin | 3:b1403fcdaeb1 | 110 | 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 | 111 | 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 | 112 | |
Fathoni17 | 2:863436c840bf | 113 | //Update value |
Fathoni17 | 2:863436c840bf | 114 | theta_prev = theta; |
MarchioKevin | 3:b1403fcdaeb1 | 115 | x_prev = x; |
MarchioKevin | 3:b1403fcdaeb1 | 116 | y_prev = y; |
Fathoni17 | 2:863436c840bf | 117 | |
Fathoni17 | 2:863436c840bf | 118 | encoder_A.reset(); |
Fathoni17 | 2:863436c840bf | 119 | encoder_B.reset(); |
Fathoni17 | 2:863436c840bf | 120 | encoder_C.reset(); |
MarchioKevin | 3:b1403fcdaeb1 | 121 | |
MarchioKevin | 3:b1403fcdaeb1 | 122 | if(modeauto) Vw = 0; |
Fathoni17 | 2:863436c840bf | 123 | } |
Fathoni17 | 2:863436c840bf | 124 | if (millis() - last_mt_print > TS+5){ |
Fathoni17 | 2:863436c840bf | 125 | if (print_pulse && DEBUG) |
Fathoni17 | 2:863436c840bf | 126 | printPulse(); |
Fathoni17 | 2:863436c840bf | 127 | last_mt_print = millis(); |
Fathoni17 | 2:863436c840bf | 128 | } |
Fathoni17 | 2:863436c840bf | 129 | } |
Fathoni17 | 1:735173a3b218 | 130 | } |
Fathoni17 | 0:22acd37ed695 | 131 | } |
Fathoni17 | 0:22acd37ed695 | 132 | |
Fathoni17 | 2:863436c840bf | 133 | void hitungPID(float theta_s){ |
Fathoni17 | 2:863436c840bf | 134 | pulse_A = encoder_A.getPulses()*PULSE_TO_JARAK; |
Fathoni17 | 2:863436c840bf | 135 | pulse_B = encoder_B.getPulses()*PULSE_TO_JARAK; |
Fathoni17 | 2:863436c840bf | 136 | pulse_C = encoder_C.getPulses()*PULSE_TO_JARAK; |
Fathoni17 | 2:863436c840bf | 137 | |
Fathoni17 | 2:863436c840bf | 138 | //Compute value |
MarchioKevin | 3:b1403fcdaeb1 | 139 | 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 | 140 | 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 | 141 | theta = theta_prev + (pulse_A + pulse_C + pulse_B)/(3.0*L); |
Fathoni17 | 2:863436c840bf | 142 | |
Fathoni17 | 2:863436c840bf | 143 | //Update value |
MarchioKevin | 3:b1403fcdaeb1 | 144 | x_prev = x; |
MarchioKevin | 3:b1403fcdaeb1 | 145 | y_prev = y; |
Fathoni17 | 2:863436c840bf | 146 | theta_prev = theta; |
Fathoni17 | 2:863436c840bf | 147 | |
Fathoni17 | 2:863436c840bf | 148 | encoder_A.reset(); |
Fathoni17 | 2:863436c840bf | 149 | encoder_B.reset(); |
Fathoni17 | 2:863436c840bf | 150 | encoder_C.reset(); |
Fathoni17 | 2:863436c840bf | 151 | |
Fathoni17 | 2:863436c840bf | 152 | //theta_s = theta_s/RAD_TO_DEG; |
Fathoni17 | 2:863436c840bf | 153 | //menghitung error jarak x,y terhaadap xs,ys |
Fathoni17 | 2:863436c840bf | 154 | theta_error = theta_s - (theta*RAD_TO_DEG); |
Fathoni17 | 2:863436c840bf | 155 | sum_theta_error += theta_error; |
Fathoni17 | 2:863436c840bf | 156 | |
Fathoni17 | 2:863436c840bf | 157 | //kalkulasi PID Theta |
Fathoni17 | 2:863436c840bf | 158 | w = KP_W*theta_error + KI_W*TS*sum_theta_error + KD_W*(theta_error - theta_error_prev)/TS; |
MarchioKevin | 3:b1403fcdaeb1 | 159 | Vw += (w*L/MAX_W_SPEED)*LIMITPWM; |
Fathoni17 | 2:863436c840bf | 160 | |
Fathoni17 | 2:863436c840bf | 161 | //update |
Fathoni17 | 2:863436c840bf | 162 | theta_error_prev = theta_error; |
Fathoni17 | 2:863436c840bf | 163 | //saturasi vw |
MarchioKevin | 3:b1403fcdaeb1 | 164 | if (Vw > 0.2){ |
MarchioKevin | 3:b1403fcdaeb1 | 165 | Vw = 0.2; |
Fathoni17 | 0:22acd37ed695 | 166 | } |
MarchioKevin | 3:b1403fcdaeb1 | 167 | else if ( Vw < -0.2){ |
MarchioKevin | 3:b1403fcdaeb1 | 168 | Vw = -0.2; |
Fathoni17 | 2:863436c840bf | 169 | } |
Fathoni17 | 0:22acd37ed695 | 170 | } |
Fathoni17 | 0:22acd37ed695 | 171 | |
Fathoni17 | 0:22acd37ed695 | 172 | void gerakMotor(){ |
Fathoni17 | 2:863436c840bf | 173 | if ((Vw == 0) && (Vr == 0)){ |
Fathoni17 | 2:863436c840bf | 174 | motor1.brake(BRAKE_HIGH); |
Fathoni17 | 2:863436c840bf | 175 | motor2.brake(BRAKE_HIGH); |
Fathoni17 | 2:863436c840bf | 176 | motor3.brake(BRAKE_HIGH); |
Fathoni17 | 2:863436c840bf | 177 | print_pulse = 0; |
Fathoni17 | 2:863436c840bf | 178 | } else { |
MarchioKevin | 3:b1403fcdaeb1 | 179 | //a = compute_Alpha(x_s, y_s, x, y, theta); |
Fathoni17 | 2:863436c840bf | 180 | motor1.speed((-1*Vr*cos(a) + Vw)); |
Fathoni17 | 2:863436c840bf | 181 | motor2.speed((Vr*(0.5*cos(a) + 0.866*sin(a)) + Vw)); |
Fathoni17 | 2:863436c840bf | 182 | motor3.speed((Vr*(0.5*cos(a) - 0.866*sin(a)) + Vw)); |
Fathoni17 | 2:863436c840bf | 183 | print_pulse = 1; |
Fathoni17 | 2:863436c840bf | 184 | } |
Fathoni17 | 0:22acd37ed695 | 185 | } |
Fathoni17 | 0:22acd37ed695 | 186 | |
Fathoni17 | 0:22acd37ed695 | 187 | void printPulse(){ |
MarchioKevin | 3:b1403fcdaeb1 | 188 | pc.printf("%.2f\t%.2f\n", theta*RAD_TO_DEG, theta_s); |
MarchioKevin | 3:b1403fcdaeb1 | 189 | } |
MarchioKevin | 3:b1403fcdaeb1 | 190 | |
MarchioKevin | 3:b1403fcdaeb1 | 191 | float compute_Alpha(float x_s, float y_s, float x, float y,float theta){ |
MarchioKevin | 3:b1403fcdaeb1 | 192 | //fungsi untuk menghitung alpha sebagai arah gerak robot |
MarchioKevin | 3:b1403fcdaeb1 | 193 | float temp = atan((y_s - y)/(x_s - x)) - theta; |
MarchioKevin | 3:b1403fcdaeb1 | 194 | |
MarchioKevin | 3:b1403fcdaeb1 | 195 | if (x_s < x) return temp + PI; |
MarchioKevin | 3:b1403fcdaeb1 | 196 | else return temp; |
Fathoni17 | 0:22acd37ed695 | 197 | } |
Fathoni17 | 0:22acd37ed695 | 198 | |
Fathoni17 | 0:22acd37ed695 | 199 | void case_gerak(){ |
Fathoni17 | 0:22acd37ed695 | 200 | // Rotasi |
MarchioKevin | 3:b1403fcdaeb1 | 201 | if(modeauto){ |
MarchioKevin | 3:b1403fcdaeb1 | 202 | if (!stick.L1 && stick.R1) // Pivot Kanan |
MarchioKevin | 3:b1403fcdaeb1 | 203 | theta_s += 0.15; |
MarchioKevin | 3:b1403fcdaeb1 | 204 | else if (!stick.R1 && stick.L1) // Pivot Kiri |
MarchioKevin | 3:b1403fcdaeb1 | 205 | theta_s -= 0.15; |
MarchioKevin | 3:b1403fcdaeb1 | 206 | |
MarchioKevin | 3:b1403fcdaeb1 | 207 | } |
MarchioKevin | 3:b1403fcdaeb1 | 208 | else if(!modeauto){ |
MarchioKevin | 3:b1403fcdaeb1 | 209 | if (!stick.L1 && stick.R1) // Pivot Kanan |
MarchioKevin | 3:b1403fcdaeb1 | 210 | Vw = 0.3; |
MarchioKevin | 3:b1403fcdaeb1 | 211 | else if (!stick.R1 && stick.L1) // Pivot Kiri |
MarchioKevin | 3:b1403fcdaeb1 | 212 | Vw = -0.3; |
MarchioKevin | 3:b1403fcdaeb1 | 213 | else |
MarchioKevin | 3:b1403fcdaeb1 | 214 | Vw = 0.0; |
MarchioKevin | 3:b1403fcdaeb1 | 215 | } |
MarchioKevin | 3:b1403fcdaeb1 | 216 | |
MarchioKevin | 3:b1403fcdaeb1 | 217 | if(stick.START_click){ |
MarchioKevin | 3:b1403fcdaeb1 | 218 | modeauto = !modeauto; |
MarchioKevin | 3:b1403fcdaeb1 | 219 | theta = 0.0; |
MarchioKevin | 3:b1403fcdaeb1 | 220 | theta_prev = 0.0; |
MarchioKevin | 3:b1403fcdaeb1 | 221 | theta_s = 0; |
MarchioKevin | 3:b1403fcdaeb1 | 222 | theta_error_prev = 0; |
MarchioKevin | 3:b1403fcdaeb1 | 223 | sum_theta_error = 0; |
MarchioKevin | 3:b1403fcdaeb1 | 224 | theta_error; |
MarchioKevin | 3:b1403fcdaeb1 | 225 | } |
Fathoni17 | 0:22acd37ed695 | 226 | |
Fathoni17 | 0:22acd37ed695 | 227 | // Linier |
MarchioKevin | 3:b1403fcdaeb1 | 228 | if ((stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(!stick.kiri)){ |
Fathoni17 | 0:22acd37ed695 | 229 | a = -90/RAD_TO_DEG; // Maju |
Fathoni17 | 4:3c890389e256 | 230 | Vr = 0.8; |
MarchioKevin | 3:b1403fcdaeb1 | 231 | // x_s = 0;// Maju |
MarchioKevin | 3:b1403fcdaeb1 | 232 | // y_s = 10000; |
MarchioKevin | 3:b1403fcdaeb1 | 233 | } |
MarchioKevin | 3:b1403fcdaeb1 | 234 | else if ((!stick.atas)&&(stick.bawah)&&(!stick.kanan)&&(!stick.kiri)){ |
Fathoni17 | 0:22acd37ed695 | 235 | a = 90/RAD_TO_DEG; // Mundur |
Fathoni17 | 4:3c890389e256 | 236 | Vr = 0.8; |
MarchioKevin | 3:b1403fcdaeb1 | 237 | // x_s = 0; // Mundur |
MarchioKevin | 3:b1403fcdaeb1 | 238 | // y_s = -10000; |
MarchioKevin | 3:b1403fcdaeb1 | 239 | } |
MarchioKevin | 3:b1403fcdaeb1 | 240 | else if ((stick.atas)&&(!stick.bawah)&&(!stick.kiri)&&(stick.kanan)){ |
Fathoni17 | 0:22acd37ed695 | 241 | a = -135/RAD_TO_DEG; // Serong Atas Kanan |
Fathoni17 | 4:3c890389e256 | 242 | Vr = 0.7; |
MarchioKevin | 3:b1403fcdaeb1 | 243 | // x_s = 50000; // Maju+Kanan |
MarchioKevin | 3:b1403fcdaeb1 | 244 | // y_s = 50000; |
MarchioKevin | 3:b1403fcdaeb1 | 245 | } |
MarchioKevin | 3:b1403fcdaeb1 | 246 | else if ((!stick.atas)&&(stick.bawah)&&(!stick.kiri)&&(stick.kanan)){ |
Fathoni17 | 0:22acd37ed695 | 247 | a = 135/RAD_TO_DEG; // Serong Bawah Kanan |
Fathoni17 | 4:3c890389e256 | 248 | Vr = 0.7; |
MarchioKevin | 3:b1403fcdaeb1 | 249 | // x_s = 50000; // Mundur+Kanan |
MarchioKevin | 3:b1403fcdaeb1 | 250 | // y_s = -50000; |
MarchioKevin | 3:b1403fcdaeb1 | 251 | } |
MarchioKevin | 3:b1403fcdaeb1 | 252 | else if ((stick.atas)&&(!stick.bawah)&&(stick.kiri)&&(!stick.kanan)){ |
Fathoni17 | 0:22acd37ed695 | 253 | a = -45/RAD_TO_DEG; // Serong Atas Kiri |
Fathoni17 | 4:3c890389e256 | 254 | Vr = 0.7; |
MarchioKevin | 3:b1403fcdaeb1 | 255 | // x_s = 50000; // Maju+Kiri |
MarchioKevin | 3:b1403fcdaeb1 | 256 | // y_s = -50000; |
MarchioKevin | 3:b1403fcdaeb1 | 257 | } |
MarchioKevin | 3:b1403fcdaeb1 | 258 | else if ((!stick.atas)&&(stick.bawah)&&(stick.kiri)&&(!stick.kanan)){ |
Fathoni17 | 0:22acd37ed695 | 259 | a = 45/RAD_TO_DEG; // Serong Bawah Kiri |
Fathoni17 | 4:3c890389e256 | 260 | Vr = 0.7; |
MarchioKevin | 3:b1403fcdaeb1 | 261 | // x_s = -50000; // Mundur+Kiri |
MarchioKevin | 3:b1403fcdaeb1 | 262 | // y_s = -50000; |
MarchioKevin | 3:b1403fcdaeb1 | 263 | } |
MarchioKevin | 3:b1403fcdaeb1 | 264 | else if ((!stick.atas)&&(!stick.bawah)&&(stick.kanan)&&(!stick.kiri)){ |
Fathoni17 | 0:22acd37ed695 | 265 | a = 180/RAD_TO_DEG; // Kanan |
Fathoni17 | 4:3c890389e256 | 266 | Vr = 0.5; |
MarchioKevin | 3:b1403fcdaeb1 | 267 | // x_s = 50000; // Kanan |
MarchioKevin | 3:b1403fcdaeb1 | 268 | // y_s = 0; |
MarchioKevin | 3:b1403fcdaeb1 | 269 | } |
MarchioKevin | 3:b1403fcdaeb1 | 270 | else if ((!stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(stick.kiri)){ |
Fathoni17 | 0:22acd37ed695 | 271 | a = 0/RAD_TO_DEG; // Kiri |
Fathoni17 | 4:3c890389e256 | 272 | Vr = 0.5; |
MarchioKevin | 3:b1403fcdaeb1 | 273 | // x_s = -50000; // Kiri |
MarchioKevin | 3:b1403fcdaeb1 | 274 | // y_s = 0; |
MarchioKevin | 3:b1403fcdaeb1 | 275 | } |
Fathoni17 | 0:22acd37ed695 | 276 | else { |
Fathoni17 | 0:22acd37ed695 | 277 | Vr = 0; |
Fathoni17 | 2:863436c840bf | 278 | a = 0; |
Fathoni17 | 0:22acd37ed695 | 279 | } |
Fathoni17 | 0:22acd37ed695 | 280 | |
Fathoni17 | 0:22acd37ed695 | 281 | if ((stick.silang_click)&&(!stick.kotak)&&(!stick.segitiga)&&(!stick.lingkaran)) |
Fathoni17 | 0:22acd37ed695 | 282 | pneumatik = !pneumatik; // Silang = Toggle pneumatik |
Fathoni17 | 0:22acd37ed695 | 283 | } |