KRTMI 2019 / Mbed 2 deprecated Taker_KRAI

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?

UserRevisionLine numberNew 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 }