KRTMI 2019
taker KRAI 2018
Dependencies: mbed encoderKRTMI Motornew millis
Diff: main.cpp
- Revision:
- 0:dddc43348c25
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Sun Feb 24 09:47:02 2019 +0000
@@ -0,0 +1,504 @@
+
+////////////////////////////////////////////////////////////////////////////////
+// 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 "encoderKRTMI.h"
+#include "JoystickPS3.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);
+
+// Pneumatik
+DigitalOut pneumatik[5]{PIN_PNEUMATIK_1, PIN_PNEUMATIK_2, PIN_PNEUMATIK_5, PIN_PNEUMATIK_6, PIN_PNEUMATIK_9};
+
+// Encoder
+encoderKRTMI encoder_A(PIN_A_CHANNEL_A, PIN_A_CHANNEL_B, 540, encoderKRTMI::X4_ENCODING);
+encoderKRTMI encoder_B(PIN_B_CHANNEL_A, PIN_B_CHANNEL_B, 540, encoderKRTMI::X4_ENCODING);
+encoderKRTMI encoder_C(PIN_C_CHANNEL_A, PIN_C_CHANNEL_B, 540, encoderKRTMI::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 countCircle = 0;
+int countSegitiga=0;
+
+////////////////////////////////////////////////////////////////////////////////
+// Main Program //
+////////////////////////////////////////////////////////////////////////////////
+int main(){
+ stick.setup();
+ stick.idle();
+
+ while(1){
+ if(stick.readable() ) {
+ // Panggil fungsi pembacaan joystik
+ stick.baca_data();
+ // Panggil fungsi pengolahan data joystik
+ stick.olah_data();
+ if ( (!stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(!stick.kiri)&&(!stick.L1)&&(!stick.R1) && (!stick.R2) && (!stick.L2) ){
+
+ pc.printf("tidak ada input\n");
+ } else {
+ pc.printf("ada input\n");
+ }
+
+
+ }
+
+ }/*
+ while(1){
+ //doing nothing
+
+ pulse_A = encoder_C.getPulses();
+ pc.printf("%.2f \n", pulse_A);
+ if (pulse_A>10000)
+ motor3.speed(0.0);
+ else
+ motor3.speed(-0.9);
+ Thread::wait(2);
+ }*/
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// 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);
+
+ //Update value
+ x_prev = x;
+ y_prev = y;
+ theta_prev = theta;
+
+ encoder_A.reset();
+ encoder_B.reset();
+ encoder_C.reset();
+}
+
+void hitungPID(double theta_s){
+ //menghitung error theta
+ 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 Kecepatan Vw (kec rotasi)
+ if (Vw > Vw_max){
+ Vw = Vw_max;
+ }
+ else if ( Vw < -1*Vw_max){
+ Vw = -1*Vw_max;
+ }
+}
+
+void gerakMotor(){
+ // Berhenti 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 > 100) && Vr < Vr_max){
+ if (Vr < 0.39){
+ Vr = 0.39;
+ }
+ else if ( (Vr >= 0.39)&& (Vr<=0.45)){
+ Vr+= 0.05;
+ } else if ((Vr>0.45) && (Vr<1.00) ){
+ Vr+=0.12;
+ } else {
+ Vr = 1.2;
+ }
+ last_mt_aksel = millis();
+ }
+ } else {
+ Vr=0.42;
+ }
+
+ // 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("%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(){
+//brake biasa
+ motor1.speed(0);
+ motor2.speed(0);
+ motor2.speed(0);
+}
+
+void motor_ForceStop(){
+//FORCEBRAKE
+ motor1.brake(BRAKE_HIGH);
+ motor2.brake(BRAKE_HIGH);
+ motor3.brake(BRAKE_HIGH);
+}
+
+void case_gerak(){
+ if(stick.SELECT_click){
+ //reset semua variable
+ pneumatik[0]=1; //gripA
+ pneumatik[1]=1; //gripB
+ pneumatik[2]=1; //sliderA
+ pneumatik[3]=1; //sliderB
+ pn=0;
+ pn2=0;
+ pn3=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 (!stick.L1 && stick.R1){ // 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.25;
+ else
+ Vw = 0.17;
+ }
+ else if (!stick.R1 && stick.L1){ // 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.25;
+ else
+ Vw = -0.17;
+ }
+
+
+ if (stick.R2){
+ // Mode Sprint
+ Vmax=1.25;
+ } else
+ Vmax=0.83;
+
+
+ if ( (!stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(!stick.kiri)&&(!stick.L1)&&(!stick.R1) && (!stick.R2) && (!stick.L2) ){
+ count=0;
+ brake_state=1;
+ theta = 0.0;
+ theta_prev = 0.0;
+ theta_s = 0.0;
+ theta_error_prev = 0;
+ sum_theta_error = 0;
+ theta_error = 0;
+ }else{
+ if (count<200)
+ count++;
+ else
+ count=500;
+ brake_state=0;
+ }
+ // Linier
+
+ if ((!stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(!stick.kiri)&&(stick.L2)){
+ //L2 : serong kiri bawah + pivot kiri (mundur saat kasih SC
+ a = (-22/RAD_TO_DEG); // mundur saat setelah pengambilan
+ Vr_max = 0.84;
+ Vw=-0.19;
+ }
+ else if ((stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(!stick.kiri)&&(stick.L2)){
+ a = (5/RAD_TO_DEG); // L2+atas : mundur saat setelah pengambilan
+ Vr_max = 0.87;
+ Vw=-0.05;
+ }
+ else if ((stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(!stick.kiri)){
+ a = (90/RAD_TO_DEG); // Maju
+ Vr_max = Vmax;
+ }
+ else if ((!stick.atas)&&(stick.bawah)&&(!stick.kanan)&&(!stick.kiri)){
+ a = (-96/RAD_TO_DEG); // Mundur
+ Vr_max = Vmax;
+ }
+ else if ((stick.atas)&&(!stick.bawah)&&(!stick.kiri)&&(stick.kanan)){
+ a = (135/RAD_TO_DEG); // Serong Atas Kanan
+ Vr_max = Vmax;
+ }
+ else if ((!stick.atas)&&(stick.bawah)&&(!stick.kiri)&&(stick.kanan)){
+ a = (-135/RAD_TO_DEG); // Serong Bawah Kanan
+ Vr_max = Vmax;
+ }
+ else if ((stick.atas)&&(!stick.bawah)&&(stick.kiri)&&(!stick.kanan)){
+ a = (45/RAD_TO_DEG); // Serong Atas Kiri
+ Vr_max = Vmax;
+ }
+ else if ((!stick.atas)&&(stick.bawah)&&(stick.kiri)&&(!stick.kanan)){
+ a = (-45/RAD_TO_DEG); // Serong Bawah Kiri
+ Vr_max = Vmax;
+ }
+ else if ((!stick.atas)&&(!stick.bawah)&&(stick.kanan)&&(!stick.kiri)){
+ a = (180/RAD_TO_DEG); // Kanan
+ Vr_max = 0.8;
+ }
+ else if ((!stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(stick.kiri)){
+ a = (0/RAD_TO_DEG); // Kiri
+ Vr_max = 0.8;
+ }
+ else {
+ Vr_max = 0;
+ }
+ //}
+}
+void case_pneumatic(){
+ // Control Pneumatic
+ /**
+ * pneumatik[0] = Gripper A
+ * pneumatik[1] = Gripper B
+ * pneumatik[3] = Slider A
+ * pneumatik[4] = Slider B
+ **/
+ if((!stick.silang) && (!stick.kotak) && (!stick.lingkaran) && (!stick.segitiga)) {
+ // reset state pneumatik
+ countX=0;
+ countCircle=0;
+ countSegitiga=0;
+ countKotak=0;
+ }
+ if ((stick.silang && countX<50)) {
+ //SILANG : sekuensial tiap tangan gripper
+ countX++;
+ }
+
+ if (countX>0 && countX<100) {
+ //state button X, lengan Kiri - lengakn Kanan
+ countX=150;
+ if (pn<4 && 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(50);
+ pneumatik[2] = 1;
+ } else if (pn==3){
+ pneumatik[3]=0;
+ } else if (pn==4){
+ pneumatik[1] = 0;
+ wait_ms(50);
+ pneumatik[3] = 1;
+ }
+ pn2=0;
+ pn3=0;
+ }
+
+ if ((stick.kotak) && countKotak<50){
+ //KOTAK : maju 2 glider
+ countKotak++;
+ }
+
+ if (countKotak>5 && countKotak<100){
+ //state button KOTAK
+ countKotak=250;
+ pn=0;
+ pn3=0;
+ if (pn2<2 && 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[2]=1;
+ pneumatik[3]=1;
+ }
+
+ }
+
+ if ((stick.segitiga) && countSegitiga<50)
+ //SEGITIGA : buka tutup kedua gripper
+ countSegitiga++;
+ if (countSegitiga>4 && countSegitiga<100) {
+ //state button SEGITIGA
+ countSegitiga=250;
+ if ( (pneumatik[0]==0 && pneumatik[1]==1) || (pneumatik[0]==1 && pneumatik[1]==0) ){
+ pneumatik[0]=1;
+ pneumatik[1]=1;
+ pneumatik[2]=1;
+ pneumatik[3]=1;
+ } else {
+ pneumatik[0]=!pneumatik[0];
+ pneumatik[1]=!pneumatik[1];
+ pneumatik[2]=1;
+ pneumatik[3]=1;
+ }
+ pn=0;
+ pn2=0;
+ pn3=0;
+ }
+
+ if ((stick.lingkaran) && countCircle<50){
+ //KOTAK : maju 2 glider , 2 kali kotak 1 kali lingkaran
+ countCircle++;
+ }
+
+ if (countCircle>4 && countCircle<100){
+ //LINGKARAN : lengan kanan - lengan kiri
+ countCircle=250;
+ pn=0;
+ pn2=0;
+ if (pn3<4 && pn3>=0)
+ pn3++;
+ else
+ pn3=0;
+ if(pn3==0 || pn3==22){
+ pneumatik[0]=1;//gripA
+ pneumatik[1]=1;//gripB
+ pneumatik[2]=1;//sliderA
+ pneumatik[3]=1;//sliderB
+ } else if (pn3==1){
+ pneumatik[3] = 0;
+ } else if (pn3==2){
+ pneumatik[1] = 0;
+ wait_ms(50);
+ pneumatik[3] = 1;
+ } else if (pn3==3){
+ pneumatik[2]=0;
+ } else if (pn3==4){
+ pneumatik[0] = 0;
+ wait_ms(50);
+ pneumatik[2] = 1;
+ }
+ pn2=0;
+ }
+
+}