![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Kode Taker 2018
Dependencies: mbed PS_PAD encoderKRAI Motor_1 millis
main.cpp@1:1dc7c9cb4f8c, 2018-07-03 (annotated)
- Committer:
- gatulz
- Date:
- Tue Jul 03 13:52:06 2018 +0000
- Revision:
- 1:1dc7c9cb4f8c
- Parent:
- 0:8c6f29487664
tambah pad lingkaran.
Who changed what in which revision?
User | Revision | Line number | New 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 | 1:1dc7c9cb4f8c | 431 | pn3=0; |
gatulz | 1:1dc7c9cb4f8c | 432 | countX=375; |
gatulz | 0:8c6f29487664 | 433 | if (pn<5 && pn>=0) |
gatulz | 0:8c6f29487664 | 434 | pn++; |
gatulz | 0:8c6f29487664 | 435 | else |
gatulz | 0:8c6f29487664 | 436 | pn=0; |
gatulz | 0:8c6f29487664 | 437 | |
gatulz | 0:8c6f29487664 | 438 | if(pn==0 || pn==22){ |
gatulz | 0:8c6f29487664 | 439 | pneumatik[0]=1;//gripA |
gatulz | 0:8c6f29487664 | 440 | pneumatik[1]=1;//gripB |
gatulz | 0:8c6f29487664 | 441 | pneumatik[2]=1;//sliderA |
gatulz | 0:8c6f29487664 | 442 | pneumatik[3]=1;//sliderB |
gatulz | 0:8c6f29487664 | 443 | } else if (pn==1){ |
gatulz | 0:8c6f29487664 | 444 | pneumatik[2] = 0; |
gatulz | 0:8c6f29487664 | 445 | } else if (pn==2){ |
gatulz | 0:8c6f29487664 | 446 | pneumatik[0] = 0; |
gatulz | 0:8c6f29487664 | 447 | wait_ms(70); |
gatulz | 0:8c6f29487664 | 448 | pneumatik[2] = 1; |
gatulz | 0:8c6f29487664 | 449 | } else if (pn==3){ |
gatulz | 0:8c6f29487664 | 450 | pneumatik[0]=1; |
gatulz | 0:8c6f29487664 | 451 | pneumatik[2]=1; |
gatulz | 0:8c6f29487664 | 452 | } else if(pn==4){ |
gatulz | 0:8c6f29487664 | 453 | pneumatik[3]=0; |
gatulz | 0:8c6f29487664 | 454 | } else if (pn==5){ |
gatulz | 0:8c6f29487664 | 455 | pneumatik[1] = 0; |
gatulz | 0:8c6f29487664 | 456 | wait_ms(20); |
gatulz | 0:8c6f29487664 | 457 | pneumatik[3] = 1; |
gatulz | 0:8c6f29487664 | 458 | } |
gatulz | 0:8c6f29487664 | 459 | |
gatulz | 0:8c6f29487664 | 460 | } |
gatulz | 0:8c6f29487664 | 461 | |
gatulz | 0:8c6f29487664 | 462 | if (ps2.read(PS_PAD::PAD_SQUARE)==1){ |
gatulz | 0:8c6f29487664 | 463 | //KOTAK : maju 2 glider , 2 kali kotak 1 kali lingkaran |
gatulz | 0:8c6f29487664 | 464 | countKotak++; |
gatulz | 0:8c6f29487664 | 465 | } |
gatulz | 0:8c6f29487664 | 466 | |
gatulz | 0:8c6f29487664 | 467 | if (countKotak>5 && countKotak<300){ |
gatulz | 0:8c6f29487664 | 468 | pn=0; |
gatulz | 1:1dc7c9cb4f8c | 469 | pn3=0; |
gatulz | 1:1dc7c9cb4f8c | 470 | countKotak=575; |
gatulz | 0:8c6f29487664 | 471 | if (pn2<5 && pn2>=0) |
gatulz | 0:8c6f29487664 | 472 | pn2++; |
gatulz | 0:8c6f29487664 | 473 | else |
gatulz | 0:8c6f29487664 | 474 | pn2=0; |
gatulz | 0:8c6f29487664 | 475 | |
gatulz | 0:8c6f29487664 | 476 | if(pn2==0){ |
gatulz | 0:8c6f29487664 | 477 | pneumatik[0]=1; |
gatulz | 0:8c6f29487664 | 478 | pneumatik[1]=1; |
gatulz | 0:8c6f29487664 | 479 | pneumatik[2]=1; |
gatulz | 0:8c6f29487664 | 480 | pneumatik[3]=1; |
gatulz | 0:8c6f29487664 | 481 | } else if(pn2==1){ |
gatulz | 0:8c6f29487664 | 482 | pneumatik[2]=0; |
gatulz | 0:8c6f29487664 | 483 | pneumatik[3]=0; |
gatulz | 0:8c6f29487664 | 484 | } else if(pn2==2){ |
gatulz | 0:8c6f29487664 | 485 | pneumatik[0]=0; |
gatulz | 0:8c6f29487664 | 486 | pneumatik[1]=0; |
gatulz | 0:8c6f29487664 | 487 | wait_ms(50); |
gatulz | 0:8c6f29487664 | 488 | pneumatik[0]=0; |
gatulz | 0:8c6f29487664 | 489 | pneumatik[1]=0; |
gatulz | 0:8c6f29487664 | 490 | pneumatik[2]=1; |
gatulz | 0:8c6f29487664 | 491 | pneumatik[3]=1; |
gatulz | 0:8c6f29487664 | 492 | } else if (pn2==3){ |
gatulz | 0:8c6f29487664 | 493 | pneumatik[0]=0; |
gatulz | 0:8c6f29487664 | 494 | pneumatik[1]=0; |
gatulz | 0:8c6f29487664 | 495 | } |
gatulz | 0:8c6f29487664 | 496 | |
gatulz | 0:8c6f29487664 | 497 | } |
gatulz | 0:8c6f29487664 | 498 | |
gatulz | 0:8c6f29487664 | 499 | if (ps2.read(PS_PAD::PAD_TRIANGLE)==1 && countSegitiga<400) |
gatulz | 0:8c6f29487664 | 500 | countSegitiga++; |
gatulz | 1:1dc7c9cb4f8c | 501 | if (countSegitiga>=5 && countSegitiga<300){ |
gatulz | 0:8c6f29487664 | 502 | //SEGITIGA : buka tutup kedua gripper |
gatulz | 1:1dc7c9cb4f8c | 503 | countSegitiga=575; |
gatulz | 0:8c6f29487664 | 504 | pneumatik[0]=!pneumatik[0]; |
gatulz | 0:8c6f29487664 | 505 | pneumatik[1]=!pneumatik[1]; |
gatulz | 0:8c6f29487664 | 506 | pneumatik[2]=1; |
gatulz | 0:8c6f29487664 | 507 | pneumatik[3]=1; |
gatulz | 0:8c6f29487664 | 508 | pn=0; |
gatulz | 0:8c6f29487664 | 509 | pn2=0; |
gatulz | 1:1dc7c9cb4f8c | 510 | pn3=0; |
gatulz | 0:8c6f29487664 | 511 | } |
gatulz | 0:8c6f29487664 | 512 | |
gatulz | 0:8c6f29487664 | 513 | |
gatulz | 0:8c6f29487664 | 514 | if (ps2.read(PS_PAD::PAD_CIRCLE)==1 && countCircle<400) |
gatulz | 0:8c6f29487664 | 515 | countCircle++; |
gatulz | 1:1dc7c9cb4f8c | 516 | if (countCircle>=5 && countCircle<300){ |
gatulz | 0:8c6f29487664 | 517 | //LINGKARAN : glider AB mundur, gripper AB buka, (kombinasikan dg kotak utk passing golde sc) |
gatulz | 1:1dc7c9cb4f8c | 518 | countCircle=575; |
gatulz | 1:1dc7c9cb4f8c | 519 | if (pn3<2 && pn3>=0) |
gatulz | 1:1dc7c9cb4f8c | 520 | pn3++; |
gatulz | 1:1dc7c9cb4f8c | 521 | else |
gatulz | 1:1dc7c9cb4f8c | 522 | pn3=0; |
gatulz | 1:1dc7c9cb4f8c | 523 | |
gatulz | 1:1dc7c9cb4f8c | 524 | if(pn3==0){ |
gatulz | 1:1dc7c9cb4f8c | 525 | pneumatik[0]=1; |
gatulz | 1:1dc7c9cb4f8c | 526 | pneumatik[1]=1; |
gatulz | 1:1dc7c9cb4f8c | 527 | pneumatik[2]=1; |
gatulz | 1:1dc7c9cb4f8c | 528 | pneumatik[3]=1; |
gatulz | 1:1dc7c9cb4f8c | 529 | } else if(pn3==1){ |
gatulz | 1:1dc7c9cb4f8c | 530 | pneumatik[3]=0; |
gatulz | 1:1dc7c9cb4f8c | 531 | } else if(pn3==2){ |
gatulz | 1:1dc7c9cb4f8c | 532 | pneumatik[1]=0; |
gatulz | 1:1dc7c9cb4f8c | 533 | wait_ms(50); |
gatulz | 1:1dc7c9cb4f8c | 534 | pneumatik[1]=0; |
gatulz | 1:1dc7c9cb4f8c | 535 | pneumatik[3]=1; |
gatulz | 1:1dc7c9cb4f8c | 536 | } |
gatulz | 0:8c6f29487664 | 537 | pn=0; |
gatulz | 0:8c6f29487664 | 538 | pn2=0; |
gatulz | 0:8c6f29487664 | 539 | } |
gatulz | 0:8c6f29487664 | 540 | |
gatulz | 0:8c6f29487664 | 541 | } |