KRAI ITB GARUDAGO
Kode Taker 2018
Dependencies: mbed PS_PAD encoderKRAI Motor_1 millis
Diff: main.cpp
- Revision:
- 0:8c6f29487664
- Child:
- 1:1dc7c9cb4f8c
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue Jul 03 05:45:18 2018 +0000
@@ -0,0 +1,524 @@
+
+////////////////////////////////////////////////////////////////////////////////
+// Robo Taker (Semi-Otomatis) 2018 //
+// -------------------------------------------------------------------------- //
+// Gerakan Robot: //
+// - Arah -> Gerak //
+// - Silang -> Buka atau Tutup Gripper A //
+// - Lingkaran -> Buka atau Tutup Gripper B //
+// - Kotak -> Extend atau Shrink Slider A //
+// - Segitigas -> Extend atau Shrink Slider B //
+// - Start -> Ubah Mode //
+////////////////////////////////////////////////////////////////////////////////
+#include "mbed.h"
+#include "Motor.h"
+#include "encoderKRAI.h"
+#include "JoystickPS3.h"
+#include "PS_PAD.h"
+#include "pinList.h"
+#include "millis.h"
+
+////////////////////////////////////////////////////////////////////////////////
+// Konstanta Program //
+////////////////////////////////////////////////////////////////////////////////
+#define PI 3.141592653593
+#define RAD_TO_DEG 57.2957795131
+#define MAX_W_SPEED 15000 //max angular speed of robot
+#define TOLERANCET 0.8 //theta tolerance
+#define PULSE_TO_JARAK 0.7416027 //0.7656027 //0.581776 //kll roda / pulses
+#define L 298.0 //roda to center of robot
+#define TS 2.0 //time sampling
+#define LIMITPWM 0.4 //limit pwm motor
+
+//Konstanta PID Theta
+#define KP_W 1.8
+#define KI_W 0
+#define KD_W 140
+
+#define MOTOR_LIMIT_MAX 1
+#define MOTOR_LIMIT_MIN -1
+
+// Set 1 untuk aktifkan fitur pc.print
+#define DEBUG 1
+
+////////////////////////////////////////////////////////////////////////////////
+// Object Program //
+////////////////////////////////////////////////////////////////////////////////
+// Serial
+Serial pc(USBTX, USBRX, 115200);
+joysticknucleo stick(PIN_TX, PIN_RX);
+
+//joystick PS2
+PS_PAD ps2(PB_15,PB_14, PB_10, PB_12); //(mosi, miso, sck, ss)
+//PS_PAD ps2(PB_15,PB_14,PB_13, PC_4); //PB_13 udah dipake pin forward motor-_-
+
+// Pneumatik
+DigitalOut pneumatik[5]{PIN_PNEUMATIK_1, PIN_PNEUMATIK_2, PIN_PNEUMATIK_5, PIN_PNEUMATIK_6, PIN_PNEUMATIK_9};
+
+// Encoder
+encoderKRAI encoder_A(PIN_A_CHANNEL_A, PIN_A_CHANNEL_B, 540, encoderKRAI::X4_ENCODING);
+encoderKRAI encoder_B(PIN_B_CHANNEL_A, PIN_B_CHANNEL_B, 540, encoderKRAI::X4_ENCODING);
+encoderKRAI encoder_C(PIN_C_CHANNEL_A, PIN_C_CHANNEL_B, 540, encoderKRAI::X4_ENCODING);
+
+// Motor
+Motor motor1(PIN_PWM_A, PIN_FWD_A, PIN_REV_A);
+Motor motor2(PIN_PWM_B, PIN_FWD_B, PIN_REV_B);
+Motor motor3(PIN_PWM_C, PIN_FWD_C, PIN_REV_C);
+
+////////////////////////////////////////////////////////////////////////////////
+// Deklarasi Prosedure dan Fungsi //
+////////////////////////////////////////////////////////////////////////////////
+void gerakMotor();
+void hitungPID(double theta_s);
+void hitungParameter();
+void printPulse();
+void case_gerak();
+void motor_Stop();
+void motor_ForceStop();
+void gerak_Pneumatic();
+void case_pneumatic();
+
+////////////////////////////////////////////////////////////////////////////////
+// Variable-variable //
+////////////////////////////////////////////////////////////////////////////////
+int joystick;
+int pn = 0;
+int pn2 = 0;
+int pn3 = 0;
+int pn_state = 0;
+double pulse_A=0, pulse_B=0, pulse_C=0;
+double Vr = 0, Vr_max = 0;
+double Vw = 0;
+double a = 0;
+double w = 0;
+double x =0, x_s=0, y=0, y_s=0, x_prev=0, y_prev=0;
+double theta=0, theta_s=0;
+double theta_error = 0, theta_prev=0, theta_error_prev=0, sum_theta_error=0;
+unsigned long last_mt_print,last_mt_print2, last_mt_pid, last_mt_rotasi;
+unsigned long last_mt_aksel, last_mt_desel,last1;
+bool modeauto = 1;
+bool print_pulse = 0;
+int brake_state=0;
+double Vmax = 0.4;
+double Vw_max = 0.3;
+int force=0;
+int count = 0;
+int countX = 0;
+int countKotak = 0;
+int countSegitiga = 0;
+int countCircle = 0;
+int dudu;
+
+////////////////////////////////////////////////////////////////////////////////
+// Main Program //
+////////////////////////////////////////////////////////////////////////////////
+int main(){
+ encoder_A.reset();
+ encoder_B.reset();
+ encoder_C.reset();
+
+ pneumatik[0]=0; //gripA
+ pneumatik[1]=0; //gripB
+ pneumatik[2]=1; //sliderA
+ pneumatik[3]=1; //sliderB
+
+ startMillis();
+ //stick.setup();
+ //stick.idle();
+
+ ps2.init();
+
+
+ while(1){
+ ps2.poll();
+ if (millis() - last_mt_pid > TS){
+ last_mt_pid = millis();
+ hitungParameter();
+ if (!(fabs(theta_s - (theta*RAD_TO_DEG))<TOLERANCET) ){
+ hitungPID(theta_s);
+ }
+ //if (fabs(theta_s - (theta*RAD_TO_DEG))<TOLERANCET ){
+// Vw = 0;
+// }
+ if (DEBUG==1)
+ printPulse();
+ }
+ //gerakMotor();
+
+ //if (millis() - last_mt_print2 > TS){
+// hitungParameter();
+// // printPulse();
+// pc.printf("%.2f\t%.2f\t%.2f\t%d\n",theta*RAD_TO_DEG, theta_s, a,modeauto);
+// last_mt_print2 = millis();
+// }
+ //if(stick.readable() ) {
+ // Panggil fungsi pembacaan joystik
+ //stick.baca_data();
+ // Panggil fungsi pengolahan data joystik
+ //stick.olah_data();
+ // Ambil data joystick
+ case_gerak();
+ case_pneumatic();
+
+ // Pengatur Gerak Motor
+ gerakMotor();
+
+ // Program PID
+ if (fabs(theta_s - (theta*RAD_TO_DEG))<TOLERANCET ){
+ Vw = 0;
+ }
+
+ // Fungsi Serial Print
+ if (millis() - last_mt_print > TS*10){
+ if (print_pulse && DEBUG)
+ printPulse();
+ last_mt_print = millis();
+ }
+ //}
+ }
+}
+
+
+////////////////////////////////////////////////////////////////////////////////
+// Prosedure dan Fungsi //
+////////////////////////////////////////////////////////////////////////////////
+void hitungParameter(){
+ pulse_A = encoder_A.getPulses()*PULSE_TO_JARAK;
+ pulse_B = encoder_B.getPulses()*PULSE_TO_JARAK;
+ pulse_C = encoder_C.getPulses()*PULSE_TO_JARAK;
+
+ //Compute value
+ x = x_prev + (2*pulse_A - pulse_C - pulse_B)/3*cos(theta_prev) - (-pulse_C+pulse_B)*0.5773*sin(theta_prev);
+ y = y_prev + (2*pulse_A - pulse_C - pulse_B)/3*sin(theta_prev) + (-pulse_C+pulse_B)*0.5773*cos(theta_prev);
+ theta = theta_prev + (pulse_A + pulse_C + pulse_B)/(3.0*L);
+ //theta2 = theta_prev2 + (pulse_A + pulse_C + pulse_B)/(3.0*L);
+
+ //Update value
+ x_prev = x;
+ y_prev = y;
+ theta_prev = theta;
+ //theta_prev2 = theta2;
+
+ encoder_A.reset();
+ encoder_B.reset();
+ encoder_C.reset();
+}
+
+void hitungPID(double theta_s){
+ //menghitung error jarak x,y terhaadap xs,ys
+ theta_error = theta_s - (theta*RAD_TO_DEG);
+
+ sum_theta_error += theta_error;
+
+ //kalkulasi PID Theta
+ w = KP_W*theta_error + KI_W*TS*sum_theta_error + KD_W*(theta_error - theta_error_prev)/TS;
+ Vw += (w*L/MAX_W_SPEED)*LIMITPWM;
+
+ //update
+ theta_error_prev = theta_error;
+
+ //Limit vw
+ if (Vw > Vw_max){
+ Vw = Vw_max;
+ }
+ else if ( Vw < -1*Vw_max){
+ Vw = -1*Vw_max;
+ }
+}
+
+void gerakMotor(){
+ // Berhent jika tidak maju, mundur, kiri, kanan, atau pivot
+ if (brake_state == 1){
+ motor_ForceStop();
+ //motor_Stop();
+ } else {
+ if (count>50){
+ if ((millis() - last_mt_aksel > 60) && Vr < Vr_max){
+ if (Vr < 0.35){
+ Vr = 0.35;
+ }
+ else if ( (Vr >= 0.35)&& (Vr<=0.45)){
+ Vr+= 0.025;
+ } else if ((Vr>0.45) && (Vr<0.7) ){
+ Vr+=0.1;
+ } else if ((Vr>=0.7) && (Vr<1.00)) {
+ Vr+=0.15;
+ } else
+ Vr = 1.00;
+ last_mt_aksel = millis();
+ }
+ } else {
+ Vr=0.4;
+ }
+
+ // Limit
+ if (Vr > Vr_max && Vr_max >= 0.000){
+ Vr = Vr_max;
+ }
+
+ // Control Motor
+ motor1.speed((Vr*cos(a) - Vw));
+ motor2.speed((Vr*(-0.5*cos(a) - 0.866*sin(a)) - Vw));
+ motor3.speed((Vr*(-0.5*cos(a) + 0.866*sin(a)) - Vw));
+ print_pulse = 1;
+ }
+
+}
+
+void printPulse(){
+ 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)));
+ //pc.printf("%d \n", pn);
+ //pc.printf("%.2f\t%.2f\t%.2f\t%.2f\t%.2f\n", pulse_A, pulse_B, pulse_C, x, y);
+ //pc.printf("%.2f\t%.2f\t%.2f\t%.2f\t%d\n",theta*RAD_TO_DEG, x, y, Vmax, modeauto);
+}
+
+void motor_Stop(){
+ motor1.speed(0);
+ motor2.speed(0);
+ motor2.speed(0);
+}
+
+void motor_ForceStop(){
+ motor1.brake(BRAKE_HIGH);
+ motor2.brake(BRAKE_HIGH);
+ motor3.brake(BRAKE_HIGH);
+}
+
+void gerak_Pneumatic(){
+ if (pn_state==0){
+ pneumatik[0] = 1;
+ pneumatik[1] = 1;
+ pneumatik[2] = 1;
+ pneumatik[3] = 1;
+ } else if (pn_state==1){
+ pneumatik[0] = 1;
+ pneumatik[1] = 1;
+ pneumatik[2] = 0;
+ pneumatik[3] = 1;
+ } else if (pn_state==2){
+ pneumatik[0] = 0;
+ pneumatik[1] = 1;
+ wait_ms(100);
+ pneumatik[2] = 1;
+ pneumatik[3] = 1;
+ } else if (pn_state==3){
+ pneumatik[0] = 0;
+ pneumatik[1] = 1;
+ pneumatik[2] = 1;
+ pneumatik[3] = 0;
+ } else if (pn_state==4){
+ pneumatik[0] = 0;
+ pneumatik[1] = 0;
+ wait_ms(100);
+ pneumatik[2] = 1;
+ pneumatik[3] = 1;
+ }
+}
+void case_gerak(){
+ if(ps2.read(PS_PAD::PAD_SELECT)==1){
+ pneumatik[0]=1; //gripA
+ pneumatik[1]=1; //gripB
+ pneumatik[2]=1; //sliderA
+ pneumatik[3]=1; //sliderB
+ pn=0;
+ pn2=0;
+ modeauto = 0;
+ theta = 0.0;
+ theta_prev = 0.0;
+ theta_s = 0;
+ theta_error_prev = 0;
+ sum_theta_error = 0;
+ theta_error = 0;
+ }
+
+ // Rotasi
+ if ((ps2.read(PS_PAD::PAD_R1)==1) && (ps2.read(PS_PAD::PAD_L1)==0)){ // Pivot Kanan
+ //theta = 0.0;
+ //theta_prev = 0.0;
+ theta_s = theta*RAD_TO_DEG;
+ theta_error_prev = 0;
+ sum_theta_error = 0;
+ theta_error = 0;
+ if (Vr_max==0)
+ Vw = 0.2;
+ else
+ Vw = 0.2;
+ }
+ else if ((ps2.read(PS_PAD::PAD_L1)==1) && (ps2.read(PS_PAD::PAD_R1)==0)){ // Pivot Kiri
+ //theta = 0.0;
+ //theta_prev = 0.0;
+ theta_s = theta*RAD_TO_DEG;
+ theta_error_prev = 0;
+ sum_theta_error = 0;
+ theta_error = 0;
+ if (Vr_max==0)
+ Vw = -0.2;
+ else
+ Vw = -0.2;
+ }
+
+ if (ps2.read(PS_PAD::PAD_R2)==1){
+ // Mode Sprint
+ Vmax=1.0;
+ } else {
+ Vmax=0.8;
+ }
+
+ 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)){
+ count=0;
+ brake_state=1;
+ }else{
+ if (count<200)
+ count++;
+ else
+ count=500;
+ brake_state=0;
+ }
+ // Linier
+ if ((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==0)){
+ a = (-90/RAD_TO_DEG); // Maju
+ Vr_max = Vmax;
+ }
+ else if ((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_TOP)==0)){
+ a = (90/RAD_TO_DEG); // Mundur
+ Vr_max = Vmax;
+ }
+ else if ((ps2.read(PS_PAD::PAD_RIGHT)==1) && (ps2.read(PS_PAD::PAD_TOP)==1)){
+ a = (-45/RAD_TO_DEG); // Serong Atas Kanan
+ Vr_max = Vmax;
+ }
+ else if ((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_RIGHT)==1)){
+ a = (45/RAD_TO_DEG); // Serong Bawah Kanan
+ Vr_max = Vmax;
+ }
+ else if ((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_LEFT)==1)){
+ a = (-135/RAD_TO_DEG); // Serong Atas Kiri
+ Vr_max = Vmax;
+ }
+ else if ((ps2.read(PS_PAD::PAD_LEFT)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==1)){
+ a = (135/RAD_TO_DEG); // Serong Bawah Kiri
+ Vr_max = Vmax;
+ }
+ else if ((ps2.read(PS_PAD::PAD_RIGHT)==1) && (ps2.read(PS_PAD::PAD_LEFT)==0)){
+ a = (0/RAD_TO_DEG); // Kanan
+ Vr_max = Vmax;
+ }
+ else if ((ps2.read(PS_PAD::PAD_RIGHT)==0) && (ps2.read(PS_PAD::PAD_LEFT)==1)){
+ a = (180/RAD_TO_DEG); // Kiri
+ Vr_max = Vmax;
+ }
+ else {
+ Vr_max = 0;
+ }
+
+
+}
+void case_pneumatic(){
+ // Control Pneumatic
+
+ 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)) {
+ countX=0;
+ countKotak=0;
+ countSegitiga=0;
+ countCircle=0;
+ } else if ((ps2.read(PS_PAD::PAD_X)==1)) {
+ //SILANG : sekuensial tiap tangan gripper
+ countX++;
+ }
+
+ if (countX>5 && countX<=300) {
+ pn2=0;
+ countX=325;
+ if (pn<5 && pn>=0)
+ pn++;
+ else
+ pn=0;
+
+ if(pn==0 || pn==22){
+ pneumatik[0]=1;//gripA
+ pneumatik[1]=1;//gripB
+ pneumatik[2]=1;//sliderA
+ pneumatik[3]=1;//sliderB
+ } else if (pn==1){
+ pneumatik[2] = 0;
+ } else if (pn==2){
+ pneumatik[0] = 0;
+ wait_ms(70);
+ pneumatik[2] = 1;
+ } else if (pn==3){
+ pneumatik[0]=1;
+ pneumatik[2]=1;
+ } else if(pn==4){
+ pneumatik[3]=0;
+ } else if (pn==5){
+ pneumatik[1] = 0;
+ wait_ms(20);
+ pneumatik[3] = 1;
+ }
+
+ }
+
+ if (ps2.read(PS_PAD::PAD_SQUARE)==1){
+ //KOTAK : maju 2 glider , 2 kali kotak 1 kali lingkaran
+ countKotak++;
+ }
+
+ if (countKotak>5 && countKotak<300){
+ pn=0;
+ countKotak=325;
+ if (pn2<5 && pn2>=0)
+ pn2++;
+ else
+ pn2=0;
+
+ if(pn2==0){
+ pneumatik[0]=1;
+ pneumatik[1]=1;
+ pneumatik[2]=1;
+ pneumatik[3]=1;
+ } else if(pn2==1){
+ pneumatik[2]=0;
+ pneumatik[3]=0;
+ } else if(pn2==2){
+ pneumatik[0]=0;
+ pneumatik[1]=0;
+ wait_ms(50);
+ pneumatik[0]=0;
+ pneumatik[1]=0;
+ pneumatik[2]=1;
+ pneumatik[3]=1;
+ } else if (pn2==3){
+ pneumatik[0]=0;
+ pneumatik[1]=0;
+ }
+
+ }
+
+ if (ps2.read(PS_PAD::PAD_TRIANGLE)==1 && countSegitiga<400)
+ countSegitiga++;
+ if (countSegitiga>=7 && countSegitiga<300){
+ //SEGITIGA : buka tutup kedua gripper
+ countSegitiga=500;
+ pneumatik[0]=!pneumatik[0];
+ pneumatik[1]=!pneumatik[1];
+ pneumatik[2]=1;
+ pneumatik[3]=1;
+ pn=0;
+ pn2=0;
+ }
+
+
+ if (ps2.read(PS_PAD::PAD_CIRCLE)==1 && countCircle<400)
+ countCircle++;
+ if (countCircle>=5 && countCircle<200){
+ //LINGKARAN : glider AB mundur, gripper AB buka, (kombinasikan dg kotak utk passing golde sc)
+ countCircle=500;
+ pneumatik[0]=0; //gripA
+ pneumatik[1]=0; //gripB
+ pneumatik[2]=1; //sliderA
+ pneumatik[3]=1; //sliderB
+ pn=0;
+ pn2=0;
+ }
+
+}