taker KRAI 2018
Dependencies: mbed encoderKRTMI Motornew millis
main.cpp@0:dddc43348c25, 2019-02-24 (annotated)
- Committer:
- SalbiFaza
- Date:
- Sun Feb 24 09:47:02 2019 +0000
- Revision:
- 0:dddc43348c25
bismillah
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
SalbiFaza | 0:dddc43348c25 | 1 | |
SalbiFaza | 0:dddc43348c25 | 2 | //////////////////////////////////////////////////////////////////////////////// |
SalbiFaza | 0:dddc43348c25 | 3 | // Robo Taker (Semi-Otomatis) 2018 // |
SalbiFaza | 0:dddc43348c25 | 4 | // -------------------------------------------------------------------------- // |
SalbiFaza | 0:dddc43348c25 | 5 | // Gerakan Robot: // |
SalbiFaza | 0:dddc43348c25 | 6 | // - Arah -> Gerak // |
SalbiFaza | 0:dddc43348c25 | 7 | // - Silang -> Buka atau Tutup Gripper A // |
SalbiFaza | 0:dddc43348c25 | 8 | // - Lingkaran -> Buka atau Tutup Gripper B // |
SalbiFaza | 0:dddc43348c25 | 9 | // - Kotak -> Extend atau Shrink Slider A // |
SalbiFaza | 0:dddc43348c25 | 10 | // - Segitigas -> Extend atau Shrink Slider B // |
SalbiFaza | 0:dddc43348c25 | 11 | // - Start -> Ubah Mode // |
SalbiFaza | 0:dddc43348c25 | 12 | //////////////////////////////////////////////////////////////////////////////// |
SalbiFaza | 0:dddc43348c25 | 13 | #include "mbed.h" |
SalbiFaza | 0:dddc43348c25 | 14 | #include "Motor.h" |
SalbiFaza | 0:dddc43348c25 | 15 | #include "encoderKRTMI.h" |
SalbiFaza | 0:dddc43348c25 | 16 | #include "JoystickPS3.h" |
SalbiFaza | 0:dddc43348c25 | 17 | #include "pinList.h" |
SalbiFaza | 0:dddc43348c25 | 18 | #include "millis.h" |
SalbiFaza | 0:dddc43348c25 | 19 | |
SalbiFaza | 0:dddc43348c25 | 20 | //////////////////////////////////////////////////////////////////////////////// |
SalbiFaza | 0:dddc43348c25 | 21 | // Konstanta Program // |
SalbiFaza | 0:dddc43348c25 | 22 | //////////////////////////////////////////////////////////////////////////////// |
SalbiFaza | 0:dddc43348c25 | 23 | #define PI 3.141592653593 |
SalbiFaza | 0:dddc43348c25 | 24 | #define RAD_TO_DEG 57.2957795131 |
SalbiFaza | 0:dddc43348c25 | 25 | #define MAX_W_SPEED 15000 //max angular speed of robot |
SalbiFaza | 0:dddc43348c25 | 26 | #define TOLERANCET 0.8 //theta tolerance |
SalbiFaza | 0:dddc43348c25 | 27 | #define PULSE_TO_JARAK 0.7416027 //0.7656027 //0.581776 //kll roda / pulses |
SalbiFaza | 0:dddc43348c25 | 28 | #define L 298.0 //roda to center of robot |
SalbiFaza | 0:dddc43348c25 | 29 | #define TS 2.0 //time sampling |
SalbiFaza | 0:dddc43348c25 | 30 | #define LIMITPWM 0.4 //limit pwm motor |
SalbiFaza | 0:dddc43348c25 | 31 | |
SalbiFaza | 0:dddc43348c25 | 32 | //Konstanta PID Theta |
SalbiFaza | 0:dddc43348c25 | 33 | #define KP_W 1.8 |
SalbiFaza | 0:dddc43348c25 | 34 | #define KI_W 0 |
SalbiFaza | 0:dddc43348c25 | 35 | #define KD_W 140 |
SalbiFaza | 0:dddc43348c25 | 36 | |
SalbiFaza | 0:dddc43348c25 | 37 | #define MOTOR_LIMIT_MAX 1 |
SalbiFaza | 0:dddc43348c25 | 38 | #define MOTOR_LIMIT_MIN -1 |
SalbiFaza | 0:dddc43348c25 | 39 | |
SalbiFaza | 0:dddc43348c25 | 40 | // Set 1 untuk aktifkan fitur pc.print |
SalbiFaza | 0:dddc43348c25 | 41 | #define DEBUG 1 |
SalbiFaza | 0:dddc43348c25 | 42 | |
SalbiFaza | 0:dddc43348c25 | 43 | //////////////////////////////////////////////////////////////////////////////// |
SalbiFaza | 0:dddc43348c25 | 44 | // Object Program // |
SalbiFaza | 0:dddc43348c25 | 45 | //////////////////////////////////////////////////////////////////////////////// |
SalbiFaza | 0:dddc43348c25 | 46 | // Serial |
SalbiFaza | 0:dddc43348c25 | 47 | Serial pc(USBTX, USBRX, 115200); |
SalbiFaza | 0:dddc43348c25 | 48 | joysticknucleo stick(PIN_TX, PIN_RX); |
SalbiFaza | 0:dddc43348c25 | 49 | |
SalbiFaza | 0:dddc43348c25 | 50 | // Pneumatik |
SalbiFaza | 0:dddc43348c25 | 51 | DigitalOut pneumatik[5]{PIN_PNEUMATIK_1, PIN_PNEUMATIK_2, PIN_PNEUMATIK_5, PIN_PNEUMATIK_6, PIN_PNEUMATIK_9}; |
SalbiFaza | 0:dddc43348c25 | 52 | |
SalbiFaza | 0:dddc43348c25 | 53 | // Encoder |
SalbiFaza | 0:dddc43348c25 | 54 | encoderKRTMI encoder_A(PIN_A_CHANNEL_A, PIN_A_CHANNEL_B, 540, encoderKRTMI::X4_ENCODING); |
SalbiFaza | 0:dddc43348c25 | 55 | encoderKRTMI encoder_B(PIN_B_CHANNEL_A, PIN_B_CHANNEL_B, 540, encoderKRTMI::X4_ENCODING); |
SalbiFaza | 0:dddc43348c25 | 56 | encoderKRTMI encoder_C(PIN_C_CHANNEL_A, PIN_C_CHANNEL_B, 540, encoderKRTMI::X4_ENCODING); |
SalbiFaza | 0:dddc43348c25 | 57 | |
SalbiFaza | 0:dddc43348c25 | 58 | // Motor |
SalbiFaza | 0:dddc43348c25 | 59 | Motor motor1(PIN_PWM_A, PIN_FWD_A, PIN_REV_A); |
SalbiFaza | 0:dddc43348c25 | 60 | Motor motor2(PIN_PWM_B, PIN_FWD_B, PIN_REV_B); |
SalbiFaza | 0:dddc43348c25 | 61 | Motor motor3(PIN_PWM_C, PIN_FWD_C, PIN_REV_C); |
SalbiFaza | 0:dddc43348c25 | 62 | |
SalbiFaza | 0:dddc43348c25 | 63 | //////////////////////////////////////////////////////////////////////////////// |
SalbiFaza | 0:dddc43348c25 | 64 | // Deklarasi Prosedure dan Fungsi // |
SalbiFaza | 0:dddc43348c25 | 65 | //////////////////////////////////////////////////////////////////////////////// |
SalbiFaza | 0:dddc43348c25 | 66 | void gerakMotor(); |
SalbiFaza | 0:dddc43348c25 | 67 | void hitungPID(double theta_s); |
SalbiFaza | 0:dddc43348c25 | 68 | void hitungParameter(); |
SalbiFaza | 0:dddc43348c25 | 69 | void printPulse(); |
SalbiFaza | 0:dddc43348c25 | 70 | void case_gerak(); |
SalbiFaza | 0:dddc43348c25 | 71 | void motor_Stop(); |
SalbiFaza | 0:dddc43348c25 | 72 | void motor_ForceStop(); |
SalbiFaza | 0:dddc43348c25 | 73 | void gerak_Pneumatic(); |
SalbiFaza | 0:dddc43348c25 | 74 | void case_pneumatic(); |
SalbiFaza | 0:dddc43348c25 | 75 | |
SalbiFaza | 0:dddc43348c25 | 76 | //////////////////////////////////////////////////////////////////////////////// |
SalbiFaza | 0:dddc43348c25 | 77 | // Variable-variable // |
SalbiFaza | 0:dddc43348c25 | 78 | //////////////////////////////////////////////////////////////////////////////// |
SalbiFaza | 0:dddc43348c25 | 79 | int joystick; |
SalbiFaza | 0:dddc43348c25 | 80 | int pn = 0; |
SalbiFaza | 0:dddc43348c25 | 81 | int pn2 = 0; |
SalbiFaza | 0:dddc43348c25 | 82 | int pn3 = 0; |
SalbiFaza | 0:dddc43348c25 | 83 | int pn_state = 0; |
SalbiFaza | 0:dddc43348c25 | 84 | double pulse_A=0, pulse_B=0, pulse_C=0; |
SalbiFaza | 0:dddc43348c25 | 85 | double Vr = 0, Vr_max = 0; |
SalbiFaza | 0:dddc43348c25 | 86 | double Vw = 0; |
SalbiFaza | 0:dddc43348c25 | 87 | double a = 0; |
SalbiFaza | 0:dddc43348c25 | 88 | double w = 0; |
SalbiFaza | 0:dddc43348c25 | 89 | double x =0, x_s=0, y=0, y_s=0, x_prev=0, y_prev=0; |
SalbiFaza | 0:dddc43348c25 | 90 | double theta=0, theta_s=0; |
SalbiFaza | 0:dddc43348c25 | 91 | double theta_error = 0, theta_prev=0, theta_error_prev=0, sum_theta_error=0; |
SalbiFaza | 0:dddc43348c25 | 92 | unsigned long last_mt_print,last_mt_print2, last_mt_pid, last_mt_rotasi; |
SalbiFaza | 0:dddc43348c25 | 93 | unsigned long last_mt_aksel, last_mt_desel,last1; |
SalbiFaza | 0:dddc43348c25 | 94 | bool modeauto = 1; |
SalbiFaza | 0:dddc43348c25 | 95 | bool print_pulse = 0; |
SalbiFaza | 0:dddc43348c25 | 96 | int brake_state=0; |
SalbiFaza | 0:dddc43348c25 | 97 | double Vmax = 0.4; |
SalbiFaza | 0:dddc43348c25 | 98 | double Vw_max = 0.3; |
SalbiFaza | 0:dddc43348c25 | 99 | int force=0; |
SalbiFaza | 0:dddc43348c25 | 100 | int count = 0; |
SalbiFaza | 0:dddc43348c25 | 101 | int countX = 0; |
SalbiFaza | 0:dddc43348c25 | 102 | int countKotak = 0; |
SalbiFaza | 0:dddc43348c25 | 103 | int countCircle = 0; |
SalbiFaza | 0:dddc43348c25 | 104 | int countSegitiga=0; |
SalbiFaza | 0:dddc43348c25 | 105 | |
SalbiFaza | 0:dddc43348c25 | 106 | //////////////////////////////////////////////////////////////////////////////// |
SalbiFaza | 0:dddc43348c25 | 107 | // Main Program // |
SalbiFaza | 0:dddc43348c25 | 108 | //////////////////////////////////////////////////////////////////////////////// |
SalbiFaza | 0:dddc43348c25 | 109 | int main(){ |
SalbiFaza | 0:dddc43348c25 | 110 | stick.setup(); |
SalbiFaza | 0:dddc43348c25 | 111 | stick.idle(); |
SalbiFaza | 0:dddc43348c25 | 112 | |
SalbiFaza | 0:dddc43348c25 | 113 | while(1){ |
SalbiFaza | 0:dddc43348c25 | 114 | if(stick.readable() ) { |
SalbiFaza | 0:dddc43348c25 | 115 | // Panggil fungsi pembacaan joystik |
SalbiFaza | 0:dddc43348c25 | 116 | stick.baca_data(); |
SalbiFaza | 0:dddc43348c25 | 117 | // Panggil fungsi pengolahan data joystik |
SalbiFaza | 0:dddc43348c25 | 118 | stick.olah_data(); |
SalbiFaza | 0:dddc43348c25 | 119 | if ( (!stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(!stick.kiri)&&(!stick.L1)&&(!stick.R1) && (!stick.R2) && (!stick.L2) ){ |
SalbiFaza | 0:dddc43348c25 | 120 | |
SalbiFaza | 0:dddc43348c25 | 121 | pc.printf("tidak ada input\n"); |
SalbiFaza | 0:dddc43348c25 | 122 | } else { |
SalbiFaza | 0:dddc43348c25 | 123 | pc.printf("ada input\n"); |
SalbiFaza | 0:dddc43348c25 | 124 | } |
SalbiFaza | 0:dddc43348c25 | 125 | |
SalbiFaza | 0:dddc43348c25 | 126 | |
SalbiFaza | 0:dddc43348c25 | 127 | } |
SalbiFaza | 0:dddc43348c25 | 128 | |
SalbiFaza | 0:dddc43348c25 | 129 | }/* |
SalbiFaza | 0:dddc43348c25 | 130 | while(1){ |
SalbiFaza | 0:dddc43348c25 | 131 | //doing nothing |
SalbiFaza | 0:dddc43348c25 | 132 | |
SalbiFaza | 0:dddc43348c25 | 133 | pulse_A = encoder_C.getPulses(); |
SalbiFaza | 0:dddc43348c25 | 134 | pc.printf("%.2f \n", pulse_A); |
SalbiFaza | 0:dddc43348c25 | 135 | if (pulse_A>10000) |
SalbiFaza | 0:dddc43348c25 | 136 | motor3.speed(0.0); |
SalbiFaza | 0:dddc43348c25 | 137 | else |
SalbiFaza | 0:dddc43348c25 | 138 | motor3.speed(-0.9); |
SalbiFaza | 0:dddc43348c25 | 139 | Thread::wait(2); |
SalbiFaza | 0:dddc43348c25 | 140 | }*/ |
SalbiFaza | 0:dddc43348c25 | 141 | } |
SalbiFaza | 0:dddc43348c25 | 142 | |
SalbiFaza | 0:dddc43348c25 | 143 | //////////////////////////////////////////////////////////////////////////////// |
SalbiFaza | 0:dddc43348c25 | 144 | // Prosedure dan Fungsi // |
SalbiFaza | 0:dddc43348c25 | 145 | //////////////////////////////////////////////////////////////////////////////// |
SalbiFaza | 0:dddc43348c25 | 146 | void hitungParameter(){ |
SalbiFaza | 0:dddc43348c25 | 147 | pulse_A = encoder_A.getPulses()*PULSE_TO_JARAK; |
SalbiFaza | 0:dddc43348c25 | 148 | pulse_B = encoder_B.getPulses()*PULSE_TO_JARAK; |
SalbiFaza | 0:dddc43348c25 | 149 | pulse_C = encoder_C.getPulses()*PULSE_TO_JARAK; |
SalbiFaza | 0:dddc43348c25 | 150 | |
SalbiFaza | 0:dddc43348c25 | 151 | //Compute value |
SalbiFaza | 0:dddc43348c25 | 152 | x = x_prev + (2*pulse_A - pulse_C - pulse_B)/3*cos(theta_prev) - (-pulse_C+pulse_B)*0.5773*sin(theta_prev); |
SalbiFaza | 0:dddc43348c25 | 153 | y = y_prev + (2*pulse_A - pulse_C - pulse_B)/3*sin(theta_prev) + (-pulse_C+pulse_B)*0.5773*cos(theta_prev); |
SalbiFaza | 0:dddc43348c25 | 154 | theta = theta_prev + (pulse_A + pulse_C + pulse_B)/(3.0*L); |
SalbiFaza | 0:dddc43348c25 | 155 | |
SalbiFaza | 0:dddc43348c25 | 156 | //Update value |
SalbiFaza | 0:dddc43348c25 | 157 | x_prev = x; |
SalbiFaza | 0:dddc43348c25 | 158 | y_prev = y; |
SalbiFaza | 0:dddc43348c25 | 159 | theta_prev = theta; |
SalbiFaza | 0:dddc43348c25 | 160 | |
SalbiFaza | 0:dddc43348c25 | 161 | encoder_A.reset(); |
SalbiFaza | 0:dddc43348c25 | 162 | encoder_B.reset(); |
SalbiFaza | 0:dddc43348c25 | 163 | encoder_C.reset(); |
SalbiFaza | 0:dddc43348c25 | 164 | } |
SalbiFaza | 0:dddc43348c25 | 165 | |
SalbiFaza | 0:dddc43348c25 | 166 | void hitungPID(double theta_s){ |
SalbiFaza | 0:dddc43348c25 | 167 | //menghitung error theta |
SalbiFaza | 0:dddc43348c25 | 168 | theta_error = theta_s - (theta*RAD_TO_DEG); |
SalbiFaza | 0:dddc43348c25 | 169 | sum_theta_error += theta_error; |
SalbiFaza | 0:dddc43348c25 | 170 | |
SalbiFaza | 0:dddc43348c25 | 171 | //kalkulasi PID Theta |
SalbiFaza | 0:dddc43348c25 | 172 | w = KP_W*theta_error + KI_W*TS*sum_theta_error + KD_W*(theta_error - theta_error_prev)/TS; |
SalbiFaza | 0:dddc43348c25 | 173 | Vw += (w*L/MAX_W_SPEED)*LIMITPWM; |
SalbiFaza | 0:dddc43348c25 | 174 | |
SalbiFaza | 0:dddc43348c25 | 175 | //update |
SalbiFaza | 0:dddc43348c25 | 176 | theta_error_prev = theta_error; |
SalbiFaza | 0:dddc43348c25 | 177 | |
SalbiFaza | 0:dddc43348c25 | 178 | //Limit Kecepatan Vw (kec rotasi) |
SalbiFaza | 0:dddc43348c25 | 179 | if (Vw > Vw_max){ |
SalbiFaza | 0:dddc43348c25 | 180 | Vw = Vw_max; |
SalbiFaza | 0:dddc43348c25 | 181 | } |
SalbiFaza | 0:dddc43348c25 | 182 | else if ( Vw < -1*Vw_max){ |
SalbiFaza | 0:dddc43348c25 | 183 | Vw = -1*Vw_max; |
SalbiFaza | 0:dddc43348c25 | 184 | } |
SalbiFaza | 0:dddc43348c25 | 185 | } |
SalbiFaza | 0:dddc43348c25 | 186 | |
SalbiFaza | 0:dddc43348c25 | 187 | void gerakMotor(){ |
SalbiFaza | 0:dddc43348c25 | 188 | // Berhenti jika tidak maju, mundur, kiri, kanan, atau pivot |
SalbiFaza | 0:dddc43348c25 | 189 | if (brake_state == 1){ |
SalbiFaza | 0:dddc43348c25 | 190 | motor_ForceStop(); |
SalbiFaza | 0:dddc43348c25 | 191 | //motor_Stop(); |
SalbiFaza | 0:dddc43348c25 | 192 | } else { |
SalbiFaza | 0:dddc43348c25 | 193 | if (count>50){ |
SalbiFaza | 0:dddc43348c25 | 194 | if ((millis() - last_mt_aksel > 100) && Vr < Vr_max){ |
SalbiFaza | 0:dddc43348c25 | 195 | if (Vr < 0.39){ |
SalbiFaza | 0:dddc43348c25 | 196 | Vr = 0.39; |
SalbiFaza | 0:dddc43348c25 | 197 | } |
SalbiFaza | 0:dddc43348c25 | 198 | else if ( (Vr >= 0.39)&& (Vr<=0.45)){ |
SalbiFaza | 0:dddc43348c25 | 199 | Vr+= 0.05; |
SalbiFaza | 0:dddc43348c25 | 200 | } else if ((Vr>0.45) && (Vr<1.00) ){ |
SalbiFaza | 0:dddc43348c25 | 201 | Vr+=0.12; |
SalbiFaza | 0:dddc43348c25 | 202 | } else { |
SalbiFaza | 0:dddc43348c25 | 203 | Vr = 1.2; |
SalbiFaza | 0:dddc43348c25 | 204 | } |
SalbiFaza | 0:dddc43348c25 | 205 | last_mt_aksel = millis(); |
SalbiFaza | 0:dddc43348c25 | 206 | } |
SalbiFaza | 0:dddc43348c25 | 207 | } else { |
SalbiFaza | 0:dddc43348c25 | 208 | Vr=0.42; |
SalbiFaza | 0:dddc43348c25 | 209 | } |
SalbiFaza | 0:dddc43348c25 | 210 | |
SalbiFaza | 0:dddc43348c25 | 211 | // Limit |
SalbiFaza | 0:dddc43348c25 | 212 | if (Vr > Vr_max && Vr_max >= 0.000){ |
SalbiFaza | 0:dddc43348c25 | 213 | Vr = Vr_max; |
SalbiFaza | 0:dddc43348c25 | 214 | } |
SalbiFaza | 0:dddc43348c25 | 215 | |
SalbiFaza | 0:dddc43348c25 | 216 | // Control Motor |
SalbiFaza | 0:dddc43348c25 | 217 | motor1.speed((Vr*cos(a) - Vw)); |
SalbiFaza | 0:dddc43348c25 | 218 | motor2.speed((Vr*(-0.5*cos(a) - 0.866*sin(a)) - Vw)); |
SalbiFaza | 0:dddc43348c25 | 219 | motor3.speed((Vr*(-0.5*cos(a) + 0.866*sin(a)) - Vw)); |
SalbiFaza | 0:dddc43348c25 | 220 | print_pulse = 1; |
SalbiFaza | 0:dddc43348c25 | 221 | } |
SalbiFaza | 0:dddc43348c25 | 222 | |
SalbiFaza | 0:dddc43348c25 | 223 | } |
SalbiFaza | 0:dddc43348c25 | 224 | |
SalbiFaza | 0:dddc43348c25 | 225 | void printPulse(){ |
SalbiFaza | 0:dddc43348c25 | 226 | pc.printf("%d \n", pn); |
SalbiFaza | 0:dddc43348c25 | 227 | //pc.printf("%.2f\t%.2f\t%.2f\t%.2f\t%.2f\n", pulse_A, pulse_B, pulse_C, x, y); |
SalbiFaza | 0:dddc43348c25 | 228 | //pc.printf("%.2f\t%.2f\t%.2f\t%.2f\t%d\n",theta*RAD_TO_DEG, x, y, Vmax, modeauto); |
SalbiFaza | 0:dddc43348c25 | 229 | } |
SalbiFaza | 0:dddc43348c25 | 230 | |
SalbiFaza | 0:dddc43348c25 | 231 | void motor_Stop(){ |
SalbiFaza | 0:dddc43348c25 | 232 | //brake biasa |
SalbiFaza | 0:dddc43348c25 | 233 | motor1.speed(0); |
SalbiFaza | 0:dddc43348c25 | 234 | motor2.speed(0); |
SalbiFaza | 0:dddc43348c25 | 235 | motor2.speed(0); |
SalbiFaza | 0:dddc43348c25 | 236 | } |
SalbiFaza | 0:dddc43348c25 | 237 | |
SalbiFaza | 0:dddc43348c25 | 238 | void motor_ForceStop(){ |
SalbiFaza | 0:dddc43348c25 | 239 | //FORCEBRAKE |
SalbiFaza | 0:dddc43348c25 | 240 | motor1.brake(BRAKE_HIGH); |
SalbiFaza | 0:dddc43348c25 | 241 | motor2.brake(BRAKE_HIGH); |
SalbiFaza | 0:dddc43348c25 | 242 | motor3.brake(BRAKE_HIGH); |
SalbiFaza | 0:dddc43348c25 | 243 | } |
SalbiFaza | 0:dddc43348c25 | 244 | |
SalbiFaza | 0:dddc43348c25 | 245 | void case_gerak(){ |
SalbiFaza | 0:dddc43348c25 | 246 | if(stick.SELECT_click){ |
SalbiFaza | 0:dddc43348c25 | 247 | //reset semua variable |
SalbiFaza | 0:dddc43348c25 | 248 | pneumatik[0]=1; //gripA |
SalbiFaza | 0:dddc43348c25 | 249 | pneumatik[1]=1; //gripB |
SalbiFaza | 0:dddc43348c25 | 250 | pneumatik[2]=1; //sliderA |
SalbiFaza | 0:dddc43348c25 | 251 | pneumatik[3]=1; //sliderB |
SalbiFaza | 0:dddc43348c25 | 252 | pn=0; |
SalbiFaza | 0:dddc43348c25 | 253 | pn2=0; |
SalbiFaza | 0:dddc43348c25 | 254 | pn3=0; |
SalbiFaza | 0:dddc43348c25 | 255 | modeauto = 0; |
SalbiFaza | 0:dddc43348c25 | 256 | theta = 0.0; |
SalbiFaza | 0:dddc43348c25 | 257 | theta_prev = 0.0; |
SalbiFaza | 0:dddc43348c25 | 258 | theta_s = 0; |
SalbiFaza | 0:dddc43348c25 | 259 | theta_error_prev = 0; |
SalbiFaza | 0:dddc43348c25 | 260 | sum_theta_error = 0; |
SalbiFaza | 0:dddc43348c25 | 261 | theta_error = 0; |
SalbiFaza | 0:dddc43348c25 | 262 | } |
SalbiFaza | 0:dddc43348c25 | 263 | |
SalbiFaza | 0:dddc43348c25 | 264 | // Rotasi |
SalbiFaza | 0:dddc43348c25 | 265 | if (!stick.L1 && stick.R1){ // Pivot Kanan |
SalbiFaza | 0:dddc43348c25 | 266 | //theta = 0.0; |
SalbiFaza | 0:dddc43348c25 | 267 | //theta_prev = 0.0; |
SalbiFaza | 0:dddc43348c25 | 268 | theta_s = theta*RAD_TO_DEG; |
SalbiFaza | 0:dddc43348c25 | 269 | theta_error_prev = 0; |
SalbiFaza | 0:dddc43348c25 | 270 | sum_theta_error = 0; |
SalbiFaza | 0:dddc43348c25 | 271 | theta_error = 0; |
SalbiFaza | 0:dddc43348c25 | 272 | if (Vr_max==0) |
SalbiFaza | 0:dddc43348c25 | 273 | Vw = 0.25; |
SalbiFaza | 0:dddc43348c25 | 274 | else |
SalbiFaza | 0:dddc43348c25 | 275 | Vw = 0.17; |
SalbiFaza | 0:dddc43348c25 | 276 | } |
SalbiFaza | 0:dddc43348c25 | 277 | else if (!stick.R1 && stick.L1){ // Pivot Kiri |
SalbiFaza | 0:dddc43348c25 | 278 | //theta = 0.0; |
SalbiFaza | 0:dddc43348c25 | 279 | //theta_prev = 0.0; |
SalbiFaza | 0:dddc43348c25 | 280 | theta_s = theta*RAD_TO_DEG; |
SalbiFaza | 0:dddc43348c25 | 281 | theta_error_prev = 0; |
SalbiFaza | 0:dddc43348c25 | 282 | sum_theta_error = 0; |
SalbiFaza | 0:dddc43348c25 | 283 | theta_error = 0; |
SalbiFaza | 0:dddc43348c25 | 284 | if (Vr_max==0) |
SalbiFaza | 0:dddc43348c25 | 285 | Vw = -0.25; |
SalbiFaza | 0:dddc43348c25 | 286 | else |
SalbiFaza | 0:dddc43348c25 | 287 | Vw = -0.17; |
SalbiFaza | 0:dddc43348c25 | 288 | } |
SalbiFaza | 0:dddc43348c25 | 289 | |
SalbiFaza | 0:dddc43348c25 | 290 | |
SalbiFaza | 0:dddc43348c25 | 291 | if (stick.R2){ |
SalbiFaza | 0:dddc43348c25 | 292 | // Mode Sprint |
SalbiFaza | 0:dddc43348c25 | 293 | Vmax=1.25; |
SalbiFaza | 0:dddc43348c25 | 294 | } else |
SalbiFaza | 0:dddc43348c25 | 295 | Vmax=0.83; |
SalbiFaza | 0:dddc43348c25 | 296 | |
SalbiFaza | 0:dddc43348c25 | 297 | |
SalbiFaza | 0:dddc43348c25 | 298 | if ( (!stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(!stick.kiri)&&(!stick.L1)&&(!stick.R1) && (!stick.R2) && (!stick.L2) ){ |
SalbiFaza | 0:dddc43348c25 | 299 | count=0; |
SalbiFaza | 0:dddc43348c25 | 300 | brake_state=1; |
SalbiFaza | 0:dddc43348c25 | 301 | theta = 0.0; |
SalbiFaza | 0:dddc43348c25 | 302 | theta_prev = 0.0; |
SalbiFaza | 0:dddc43348c25 | 303 | theta_s = 0.0; |
SalbiFaza | 0:dddc43348c25 | 304 | theta_error_prev = 0; |
SalbiFaza | 0:dddc43348c25 | 305 | sum_theta_error = 0; |
SalbiFaza | 0:dddc43348c25 | 306 | theta_error = 0; |
SalbiFaza | 0:dddc43348c25 | 307 | }else{ |
SalbiFaza | 0:dddc43348c25 | 308 | if (count<200) |
SalbiFaza | 0:dddc43348c25 | 309 | count++; |
SalbiFaza | 0:dddc43348c25 | 310 | else |
SalbiFaza | 0:dddc43348c25 | 311 | count=500; |
SalbiFaza | 0:dddc43348c25 | 312 | brake_state=0; |
SalbiFaza | 0:dddc43348c25 | 313 | } |
SalbiFaza | 0:dddc43348c25 | 314 | // Linier |
SalbiFaza | 0:dddc43348c25 | 315 | |
SalbiFaza | 0:dddc43348c25 | 316 | if ((!stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(!stick.kiri)&&(stick.L2)){ |
SalbiFaza | 0:dddc43348c25 | 317 | //L2 : serong kiri bawah + pivot kiri (mundur saat kasih SC |
SalbiFaza | 0:dddc43348c25 | 318 | a = (-22/RAD_TO_DEG); // mundur saat setelah pengambilan |
SalbiFaza | 0:dddc43348c25 | 319 | Vr_max = 0.84; |
SalbiFaza | 0:dddc43348c25 | 320 | Vw=-0.19; |
SalbiFaza | 0:dddc43348c25 | 321 | } |
SalbiFaza | 0:dddc43348c25 | 322 | else if ((stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(!stick.kiri)&&(stick.L2)){ |
SalbiFaza | 0:dddc43348c25 | 323 | a = (5/RAD_TO_DEG); // L2+atas : mundur saat setelah pengambilan |
SalbiFaza | 0:dddc43348c25 | 324 | Vr_max = 0.87; |
SalbiFaza | 0:dddc43348c25 | 325 | Vw=-0.05; |
SalbiFaza | 0:dddc43348c25 | 326 | } |
SalbiFaza | 0:dddc43348c25 | 327 | else if ((stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(!stick.kiri)){ |
SalbiFaza | 0:dddc43348c25 | 328 | a = (90/RAD_TO_DEG); // Maju |
SalbiFaza | 0:dddc43348c25 | 329 | Vr_max = Vmax; |
SalbiFaza | 0:dddc43348c25 | 330 | } |
SalbiFaza | 0:dddc43348c25 | 331 | else if ((!stick.atas)&&(stick.bawah)&&(!stick.kanan)&&(!stick.kiri)){ |
SalbiFaza | 0:dddc43348c25 | 332 | a = (-96/RAD_TO_DEG); // Mundur |
SalbiFaza | 0:dddc43348c25 | 333 | Vr_max = Vmax; |
SalbiFaza | 0:dddc43348c25 | 334 | } |
SalbiFaza | 0:dddc43348c25 | 335 | else if ((stick.atas)&&(!stick.bawah)&&(!stick.kiri)&&(stick.kanan)){ |
SalbiFaza | 0:dddc43348c25 | 336 | a = (135/RAD_TO_DEG); // Serong Atas Kanan |
SalbiFaza | 0:dddc43348c25 | 337 | Vr_max = Vmax; |
SalbiFaza | 0:dddc43348c25 | 338 | } |
SalbiFaza | 0:dddc43348c25 | 339 | else if ((!stick.atas)&&(stick.bawah)&&(!stick.kiri)&&(stick.kanan)){ |
SalbiFaza | 0:dddc43348c25 | 340 | a = (-135/RAD_TO_DEG); // Serong Bawah Kanan |
SalbiFaza | 0:dddc43348c25 | 341 | Vr_max = Vmax; |
SalbiFaza | 0:dddc43348c25 | 342 | } |
SalbiFaza | 0:dddc43348c25 | 343 | else if ((stick.atas)&&(!stick.bawah)&&(stick.kiri)&&(!stick.kanan)){ |
SalbiFaza | 0:dddc43348c25 | 344 | a = (45/RAD_TO_DEG); // Serong Atas Kiri |
SalbiFaza | 0:dddc43348c25 | 345 | Vr_max = Vmax; |
SalbiFaza | 0:dddc43348c25 | 346 | } |
SalbiFaza | 0:dddc43348c25 | 347 | else if ((!stick.atas)&&(stick.bawah)&&(stick.kiri)&&(!stick.kanan)){ |
SalbiFaza | 0:dddc43348c25 | 348 | a = (-45/RAD_TO_DEG); // Serong Bawah Kiri |
SalbiFaza | 0:dddc43348c25 | 349 | Vr_max = Vmax; |
SalbiFaza | 0:dddc43348c25 | 350 | } |
SalbiFaza | 0:dddc43348c25 | 351 | else if ((!stick.atas)&&(!stick.bawah)&&(stick.kanan)&&(!stick.kiri)){ |
SalbiFaza | 0:dddc43348c25 | 352 | a = (180/RAD_TO_DEG); // Kanan |
SalbiFaza | 0:dddc43348c25 | 353 | Vr_max = 0.8; |
SalbiFaza | 0:dddc43348c25 | 354 | } |
SalbiFaza | 0:dddc43348c25 | 355 | else if ((!stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(stick.kiri)){ |
SalbiFaza | 0:dddc43348c25 | 356 | a = (0/RAD_TO_DEG); // Kiri |
SalbiFaza | 0:dddc43348c25 | 357 | Vr_max = 0.8; |
SalbiFaza | 0:dddc43348c25 | 358 | } |
SalbiFaza | 0:dddc43348c25 | 359 | else { |
SalbiFaza | 0:dddc43348c25 | 360 | Vr_max = 0; |
SalbiFaza | 0:dddc43348c25 | 361 | } |
SalbiFaza | 0:dddc43348c25 | 362 | //} |
SalbiFaza | 0:dddc43348c25 | 363 | } |
SalbiFaza | 0:dddc43348c25 | 364 | void case_pneumatic(){ |
SalbiFaza | 0:dddc43348c25 | 365 | // Control Pneumatic |
SalbiFaza | 0:dddc43348c25 | 366 | /** |
SalbiFaza | 0:dddc43348c25 | 367 | * pneumatik[0] = Gripper A |
SalbiFaza | 0:dddc43348c25 | 368 | * pneumatik[1] = Gripper B |
SalbiFaza | 0:dddc43348c25 | 369 | * pneumatik[3] = Slider A |
SalbiFaza | 0:dddc43348c25 | 370 | * pneumatik[4] = Slider B |
SalbiFaza | 0:dddc43348c25 | 371 | **/ |
SalbiFaza | 0:dddc43348c25 | 372 | if((!stick.silang) && (!stick.kotak) && (!stick.lingkaran) && (!stick.segitiga)) { |
SalbiFaza | 0:dddc43348c25 | 373 | // reset state pneumatik |
SalbiFaza | 0:dddc43348c25 | 374 | countX=0; |
SalbiFaza | 0:dddc43348c25 | 375 | countCircle=0; |
SalbiFaza | 0:dddc43348c25 | 376 | countSegitiga=0; |
SalbiFaza | 0:dddc43348c25 | 377 | countKotak=0; |
SalbiFaza | 0:dddc43348c25 | 378 | } |
SalbiFaza | 0:dddc43348c25 | 379 | if ((stick.silang && countX<50)) { |
SalbiFaza | 0:dddc43348c25 | 380 | //SILANG : sekuensial tiap tangan gripper |
SalbiFaza | 0:dddc43348c25 | 381 | countX++; |
SalbiFaza | 0:dddc43348c25 | 382 | } |
SalbiFaza | 0:dddc43348c25 | 383 | |
SalbiFaza | 0:dddc43348c25 | 384 | if (countX>0 && countX<100) { |
SalbiFaza | 0:dddc43348c25 | 385 | //state button X, lengan Kiri - lengakn Kanan |
SalbiFaza | 0:dddc43348c25 | 386 | countX=150; |
SalbiFaza | 0:dddc43348c25 | 387 | if (pn<4 && pn>=0) |
SalbiFaza | 0:dddc43348c25 | 388 | pn++; |
SalbiFaza | 0:dddc43348c25 | 389 | else |
SalbiFaza | 0:dddc43348c25 | 390 | pn=0; |
SalbiFaza | 0:dddc43348c25 | 391 | |
SalbiFaza | 0:dddc43348c25 | 392 | if(pn==0 || pn==22){ |
SalbiFaza | 0:dddc43348c25 | 393 | pneumatik[0]=1;//gripA |
SalbiFaza | 0:dddc43348c25 | 394 | pneumatik[1]=1;//gripB |
SalbiFaza | 0:dddc43348c25 | 395 | pneumatik[2]=1;//sliderA |
SalbiFaza | 0:dddc43348c25 | 396 | pneumatik[3]=1;//sliderB |
SalbiFaza | 0:dddc43348c25 | 397 | } else if (pn==1){ |
SalbiFaza | 0:dddc43348c25 | 398 | pneumatik[2] = 0; |
SalbiFaza | 0:dddc43348c25 | 399 | } else if (pn==2){ |
SalbiFaza | 0:dddc43348c25 | 400 | pneumatik[0] = 0; |
SalbiFaza | 0:dddc43348c25 | 401 | wait_ms(50); |
SalbiFaza | 0:dddc43348c25 | 402 | pneumatik[2] = 1; |
SalbiFaza | 0:dddc43348c25 | 403 | } else if (pn==3){ |
SalbiFaza | 0:dddc43348c25 | 404 | pneumatik[3]=0; |
SalbiFaza | 0:dddc43348c25 | 405 | } else if (pn==4){ |
SalbiFaza | 0:dddc43348c25 | 406 | pneumatik[1] = 0; |
SalbiFaza | 0:dddc43348c25 | 407 | wait_ms(50); |
SalbiFaza | 0:dddc43348c25 | 408 | pneumatik[3] = 1; |
SalbiFaza | 0:dddc43348c25 | 409 | } |
SalbiFaza | 0:dddc43348c25 | 410 | pn2=0; |
SalbiFaza | 0:dddc43348c25 | 411 | pn3=0; |
SalbiFaza | 0:dddc43348c25 | 412 | } |
SalbiFaza | 0:dddc43348c25 | 413 | |
SalbiFaza | 0:dddc43348c25 | 414 | if ((stick.kotak) && countKotak<50){ |
SalbiFaza | 0:dddc43348c25 | 415 | //KOTAK : maju 2 glider |
SalbiFaza | 0:dddc43348c25 | 416 | countKotak++; |
SalbiFaza | 0:dddc43348c25 | 417 | } |
SalbiFaza | 0:dddc43348c25 | 418 | |
SalbiFaza | 0:dddc43348c25 | 419 | if (countKotak>5 && countKotak<100){ |
SalbiFaza | 0:dddc43348c25 | 420 | //state button KOTAK |
SalbiFaza | 0:dddc43348c25 | 421 | countKotak=250; |
SalbiFaza | 0:dddc43348c25 | 422 | pn=0; |
SalbiFaza | 0:dddc43348c25 | 423 | pn3=0; |
SalbiFaza | 0:dddc43348c25 | 424 | if (pn2<2 && pn2>=0) |
SalbiFaza | 0:dddc43348c25 | 425 | pn2++; |
SalbiFaza | 0:dddc43348c25 | 426 | else |
SalbiFaza | 0:dddc43348c25 | 427 | pn2=0; |
SalbiFaza | 0:dddc43348c25 | 428 | |
SalbiFaza | 0:dddc43348c25 | 429 | if(pn2==0){ |
SalbiFaza | 0:dddc43348c25 | 430 | pneumatik[0]=1; |
SalbiFaza | 0:dddc43348c25 | 431 | pneumatik[1]=1; |
SalbiFaza | 0:dddc43348c25 | 432 | pneumatik[2]=1; |
SalbiFaza | 0:dddc43348c25 | 433 | pneumatik[3]=1; |
SalbiFaza | 0:dddc43348c25 | 434 | } else if(pn2==1){ |
SalbiFaza | 0:dddc43348c25 | 435 | pneumatik[2]=0; |
SalbiFaza | 0:dddc43348c25 | 436 | pneumatik[3]=0; |
SalbiFaza | 0:dddc43348c25 | 437 | } else if(pn2==2){ |
SalbiFaza | 0:dddc43348c25 | 438 | pneumatik[0]=0; |
SalbiFaza | 0:dddc43348c25 | 439 | pneumatik[1]=0; |
SalbiFaza | 0:dddc43348c25 | 440 | wait_ms(50); |
SalbiFaza | 0:dddc43348c25 | 441 | pneumatik[2]=1; |
SalbiFaza | 0:dddc43348c25 | 442 | pneumatik[3]=1; |
SalbiFaza | 0:dddc43348c25 | 443 | } |
SalbiFaza | 0:dddc43348c25 | 444 | |
SalbiFaza | 0:dddc43348c25 | 445 | } |
SalbiFaza | 0:dddc43348c25 | 446 | |
SalbiFaza | 0:dddc43348c25 | 447 | if ((stick.segitiga) && countSegitiga<50) |
SalbiFaza | 0:dddc43348c25 | 448 | //SEGITIGA : buka tutup kedua gripper |
SalbiFaza | 0:dddc43348c25 | 449 | countSegitiga++; |
SalbiFaza | 0:dddc43348c25 | 450 | if (countSegitiga>4 && countSegitiga<100) { |
SalbiFaza | 0:dddc43348c25 | 451 | //state button SEGITIGA |
SalbiFaza | 0:dddc43348c25 | 452 | countSegitiga=250; |
SalbiFaza | 0:dddc43348c25 | 453 | if ( (pneumatik[0]==0 && pneumatik[1]==1) || (pneumatik[0]==1 && pneumatik[1]==0) ){ |
SalbiFaza | 0:dddc43348c25 | 454 | pneumatik[0]=1; |
SalbiFaza | 0:dddc43348c25 | 455 | pneumatik[1]=1; |
SalbiFaza | 0:dddc43348c25 | 456 | pneumatik[2]=1; |
SalbiFaza | 0:dddc43348c25 | 457 | pneumatik[3]=1; |
SalbiFaza | 0:dddc43348c25 | 458 | } else { |
SalbiFaza | 0:dddc43348c25 | 459 | pneumatik[0]=!pneumatik[0]; |
SalbiFaza | 0:dddc43348c25 | 460 | pneumatik[1]=!pneumatik[1]; |
SalbiFaza | 0:dddc43348c25 | 461 | pneumatik[2]=1; |
SalbiFaza | 0:dddc43348c25 | 462 | pneumatik[3]=1; |
SalbiFaza | 0:dddc43348c25 | 463 | } |
SalbiFaza | 0:dddc43348c25 | 464 | pn=0; |
SalbiFaza | 0:dddc43348c25 | 465 | pn2=0; |
SalbiFaza | 0:dddc43348c25 | 466 | pn3=0; |
SalbiFaza | 0:dddc43348c25 | 467 | } |
SalbiFaza | 0:dddc43348c25 | 468 | |
SalbiFaza | 0:dddc43348c25 | 469 | if ((stick.lingkaran) && countCircle<50){ |
SalbiFaza | 0:dddc43348c25 | 470 | //KOTAK : maju 2 glider , 2 kali kotak 1 kali lingkaran |
SalbiFaza | 0:dddc43348c25 | 471 | countCircle++; |
SalbiFaza | 0:dddc43348c25 | 472 | } |
SalbiFaza | 0:dddc43348c25 | 473 | |
SalbiFaza | 0:dddc43348c25 | 474 | if (countCircle>4 && countCircle<100){ |
SalbiFaza | 0:dddc43348c25 | 475 | //LINGKARAN : lengan kanan - lengan kiri |
SalbiFaza | 0:dddc43348c25 | 476 | countCircle=250; |
SalbiFaza | 0:dddc43348c25 | 477 | pn=0; |
SalbiFaza | 0:dddc43348c25 | 478 | pn2=0; |
SalbiFaza | 0:dddc43348c25 | 479 | if (pn3<4 && pn3>=0) |
SalbiFaza | 0:dddc43348c25 | 480 | pn3++; |
SalbiFaza | 0:dddc43348c25 | 481 | else |
SalbiFaza | 0:dddc43348c25 | 482 | pn3=0; |
SalbiFaza | 0:dddc43348c25 | 483 | if(pn3==0 || pn3==22){ |
SalbiFaza | 0:dddc43348c25 | 484 | pneumatik[0]=1;//gripA |
SalbiFaza | 0:dddc43348c25 | 485 | pneumatik[1]=1;//gripB |
SalbiFaza | 0:dddc43348c25 | 486 | pneumatik[2]=1;//sliderA |
SalbiFaza | 0:dddc43348c25 | 487 | pneumatik[3]=1;//sliderB |
SalbiFaza | 0:dddc43348c25 | 488 | } else if (pn3==1){ |
SalbiFaza | 0:dddc43348c25 | 489 | pneumatik[3] = 0; |
SalbiFaza | 0:dddc43348c25 | 490 | } else if (pn3==2){ |
SalbiFaza | 0:dddc43348c25 | 491 | pneumatik[1] = 0; |
SalbiFaza | 0:dddc43348c25 | 492 | wait_ms(50); |
SalbiFaza | 0:dddc43348c25 | 493 | pneumatik[3] = 1; |
SalbiFaza | 0:dddc43348c25 | 494 | } else if (pn3==3){ |
SalbiFaza | 0:dddc43348c25 | 495 | pneumatik[2]=0; |
SalbiFaza | 0:dddc43348c25 | 496 | } else if (pn3==4){ |
SalbiFaza | 0:dddc43348c25 | 497 | pneumatik[0] = 0; |
SalbiFaza | 0:dddc43348c25 | 498 | wait_ms(50); |
SalbiFaza | 0:dddc43348c25 | 499 | pneumatik[2] = 1; |
SalbiFaza | 0:dddc43348c25 | 500 | } |
SalbiFaza | 0:dddc43348c25 | 501 | pn2=0; |
SalbiFaza | 0:dddc43348c25 | 502 | } |
SalbiFaza | 0:dddc43348c25 | 503 | |
SalbiFaza | 0:dddc43348c25 | 504 | } |