KRAI ITB GARUDAGO / Mbed 2 deprecated TAKER_KABEL

Kode Taker 2018

Dependencies:   mbed PS_PAD encoderKRAI Motor_1 millis

main.cpp@0:8c6f29487664, 2018-07-03 (annotated)

Committer:
gatulz
Date:
Tue Jul 03 05:45:18 2018 +0000
Revision:
0:8c6f29487664
Child:
1:1dc7c9cb4f8c
TAKER DENGAN JOYSTICK KABEL

Who changed what in which revision?

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