![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Buat agip
Dependencies: Motor_1 encoderKRAI mbed millis
Fork of Robo_Taker_Nasional_2018 by
main.cpp
- Committer:
- Fathoni17
- Date:
- 2018-02-26
- Revision:
- 1:735173a3b218
- Parent:
- 0:22acd37ed695
- Child:
- 2:863436c840bf
File content as of revision 1:735173a3b218:
#include "mbed.h" #include "Motor.h" #include "encoderKRAI.h" #include "JoystickPS3.h" #include "rtos.h" #include "pinList.h" #define PI 3.141592653593 #define RAD_TO_DEG 57.2957795131 #define PULSE_TO_MM 0.1177 //rev/pulse * K_lingkaran_roda #define L 298.0 #define MOTOR_LIMIT_MAX 1 #define MOTOR_LIMIT_MIN -1 #define DEBUG 1 // Serial Serial pc(USBTX,USBRX); joysticknucleo stick(PIN_TX, PIN_RX); // Pneumatik DigitalOut pneumatik(PIN_PNEUMATIK); // 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); // List Thread Thread thread1; Thread thread2; Thread thread3; //Thread thread4; // Fungsi dan Prosedur void getJoystick(); void gerakMotor(); void hitungPID(); void printPulse(); void case_gerak(); // Variable-variable int joystick; int pulse_A=0; int pulse_B=0; int pulse_C=0; float Vr = 0; float Vw = 0; float a = 0; double pid_t=0; short sp_teta; int sum=0; int print_pulse = 0; int main(){ encoder_A.reset(); encoder_B.reset(); encoder_C.reset(); pc.baud(9600); stick.setup(); stick.idle(); pneumatik = 0; // Thread thread1.start(getJoystick); thread2.start(gerakMotor); thread3.start(printPulse); while(1){ // do nothing if(stick.readable() ) { // Panggil fungsi pembacaan joystik stick.baca_data(); // Panggil fungsi pengolahan data joystik stick.olah_data(); // Rotasi if (!stick.L1 && stick.R1) // Pivot Kanan Vw = 0.3; else if (!stick.R1 && stick.L1) // Pivot Kiri Vw = -0.3; else Vw = 0; // Linier Vr = 0.5; if ((stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(!stick.kiri)) a = -90/RAD_TO_DEG; // Maju else if ((!stick.atas)&&(stick.bawah)&&(!stick.kanan)&&(!stick.kiri)) a = 90/RAD_TO_DEG; // Mundur else if ((stick.atas)&&(!stick.bawah)&&(!stick.kiri)&&(stick.kanan)) a = -135/RAD_TO_DEG; // Serong Atas Kanan else if ((!stick.atas)&&(stick.bawah)&&(!stick.kiri)&&(stick.kanan)) a = 135/RAD_TO_DEG; // Serong Bawah Kanan else if ((stick.atas)&&(!stick.bawah)&&(stick.kiri)&&(!stick.kanan)) a = -45/RAD_TO_DEG; // Serong Atas Kiri else if ((!stick.atas)&&(stick.bawah)&&(stick.kiri)&&(!stick.kanan)) a = 45/RAD_TO_DEG; // Serong Bawah Kiri else if ((!stick.atas)&&(!stick.bawah)&&(stick.kanan)&&(!stick.kiri)) a = 180/RAD_TO_DEG; // Kanan else if ((!stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(stick.kiri)) a = 0/RAD_TO_DEG; // Kiri else { Vr = 0; a = 0; } if ((stick.silang_click)&&(!stick.kotak)&&(!stick.segitiga)&&(!stick.lingkaran)) pneumatik = !pneumatik; // Silang = Toggle pneumatik //pc.printf("Rotasi: %.2f\t", Vw); //pc.printf("Tarnslasi: %.2f, %.1f\n", Vr, a); } } } void getJoystick(){ while(1){ Thread::wait(1); } } void hitungPID(){ } void gerakMotor(){ while(1){ if ((Vw == 0) && (Vr == 0)){ motor1.brake(BRAKE_HIGH); motor2.brake(BRAKE_HIGH); motor3.brake(BRAKE_HIGH); print_pulse = 0; } else { motor1.speed((-1*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; } Thread::wait(1); } } void printPulse(){ while(1){ pulse_A = encoder_A.getPulses(); pulse_B = encoder_B.getPulses(); pulse_C = encoder_C.getPulses(); if (print_pulse) pc.printf("%d\t%d\t%d\n", pulse_A, pulse_B, pulse_C); encoder_A.reset(); encoder_B.reset(); encoder_C.reset();//*/ Thread::wait(20); } } void case_gerak(){ // Rotasi if (!stick.L1 && stick.R1) // Pivot Kanan Vw = 0.3; else if (!stick.R1 && stick.L1) // Pivot Kiri Vw = -0.3; else Vw = 0; // Linier Vr = 0.5; if ((stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(!stick.kiri)) a = -90/RAD_TO_DEG; // Maju else if ((!stick.atas)&&(stick.bawah)&&(!stick.kanan)&&(!stick.kiri)) a = 90/RAD_TO_DEG; // Mundur else if ((stick.atas)&&(!stick.bawah)&&(!stick.kiri)&&(stick.kanan)) a = -135/RAD_TO_DEG; // Serong Atas Kanan else if ((!stick.atas)&&(stick.bawah)&&(!stick.kiri)&&(stick.kanan)) a = 135/RAD_TO_DEG; // Serong Bawah Kanan else if ((stick.atas)&&(!stick.bawah)&&(stick.kiri)&&(!stick.kanan)) a = -45/RAD_TO_DEG; // Serong Atas Kiri else if ((!stick.atas)&&(stick.bawah)&&(stick.kiri)&&(!stick.kanan)) a = 45/RAD_TO_DEG; // Serong Bawah Kiri else if ((!stick.atas)&&(!stick.bawah)&&(stick.kanan)&&(!stick.kiri)) a = 180/RAD_TO_DEG; // Kanan else if ((!stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(stick.kiri)) a = 0/RAD_TO_DEG; // Kiri else { Vr = 0; } if ((stick.silang_click)&&(!stick.kotak)&&(!stick.segitiga)&&(!stick.lingkaran)) pneumatik = !pneumatik; // Silang = Toggle pneumatik }