Marchio Kevin / Mbed 2 deprecated Robo_Taker_Nasional_2018

Buat agip

Dependencies:   Motor_1 encoderKRAI mbed millis

Fork of Robo_Taker_Nasional_2018 by KRAI 2018

main.cpp@0:22acd37ed695, 2018-02-24 (annotated)

Committer:
Fathoni17
Date:
Sat Feb 24 07:57:51 2018 +0000
Revision:
0:22acd37ed695
Child:
1:735173a3b218
Joystick masalah

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Fathoni17 0:22acd37ed695 1 #include "mbed.h"
Fathoni17 0:22acd37ed695 2 #include "Motor.h"
Fathoni17 0:22acd37ed695 3 #include "encoderKRAI.h"
Fathoni17 0:22acd37ed695 4 #include "JoystickPS3.h"
Fathoni17 0:22acd37ed695 5 #include "rtos.h"
Fathoni17 0:22acd37ed695 6 #include "pinList.h"
Fathoni17 0:22acd37ed695 7
Fathoni17 0:22acd37ed695 8 #define PI 3.141592653593
Fathoni17 0:22acd37ed695 9 #define RAD_TO_DEG 57.2957795131
Fathoni17 0:22acd37ed695 10 #define PULSE_TO_MM 0.1177 //rev/pulse * K_lingkaran_roda
Fathoni17 0:22acd37ed695 11 #define L 298.0
Fathoni17 0:22acd37ed695 12
Fathoni17 0:22acd37ed695 13 #define MOTOR_LIMIT_MAX 1
Fathoni17 0:22acd37ed695 14 #define MOTOR_LIMIT_MIN -1
Fathoni17 0:22acd37ed695 15
Fathoni17 0:22acd37ed695 16 #define DEBUG 1
Fathoni17 0:22acd37ed695 17
Fathoni17 0:22acd37ed695 18 // Serial
Fathoni17 0:22acd37ed695 19 Serial pc(USBTX,USBRX);
Fathoni17 0:22acd37ed695 20 joysticknucleo stick(PIN_TX, PIN_RX);
Fathoni17 0:22acd37ed695 21
Fathoni17 0:22acd37ed695 22 // Pneumatik
Fathoni17 0:22acd37ed695 23 DigitalOut pneumatik(PIN_PNEUMATIK);
Fathoni17 0:22acd37ed695 24
Fathoni17 0:22acd37ed695 25 // Encoder
Fathoni17 0:22acd37ed695 26 encoderKRAI encoder_A(PIN_A_CHANNEL_A, PIN_A_CHANNEL_B, 540, encoderKRAI::X4_ENCODING);
Fathoni17 0:22acd37ed695 27 encoderKRAI encoder_B(PIN_B_CHANNEL_A, PIN_B_CHANNEL_B, 540, encoderKRAI::X4_ENCODING);
Fathoni17 0:22acd37ed695 28 encoderKRAI encoder_C(PIN_C_CHANNEL_A, PIN_C_CHANNEL_B, 540, encoderKRAI::X4_ENCODING);
Fathoni17 0:22acd37ed695 29
Fathoni17 0:22acd37ed695 30 // Motor
Fathoni17 0:22acd37ed695 31 Motor motor1(PIN_PWM_A, PIN_FWD_A, PIN_REV_A);
Fathoni17 0:22acd37ed695 32 Motor motor2(PIN_PWM_B, PIN_FWD_B, PIN_REV_B);
Fathoni17 0:22acd37ed695 33 Motor motor3(PIN_PWM_C, PIN_FWD_C, PIN_REV_C);
Fathoni17 0:22acd37ed695 34
Fathoni17 0:22acd37ed695 35 // List Thread
Fathoni17 0:22acd37ed695 36 Thread thread1;
Fathoni17 0:22acd37ed695 37 Thread thread2;
Fathoni17 0:22acd37ed695 38 Thread thread3;
Fathoni17 0:22acd37ed695 39 //Thread thread4;
Fathoni17 0:22acd37ed695 40
Fathoni17 0:22acd37ed695 41 // Fungsi dan Prosedur
Fathoni17 0:22acd37ed695 42 void getJoystick();
Fathoni17 0:22acd37ed695 43 void gerakMotor();
Fathoni17 0:22acd37ed695 44 void hitungPID();
Fathoni17 0:22acd37ed695 45 void printPulse();
Fathoni17 0:22acd37ed695 46 void case_gerak();
Fathoni17 0:22acd37ed695 47
Fathoni17 0:22acd37ed695 48 // Variable-variable
Fathoni17 0:22acd37ed695 49 int joystick;
Fathoni17 0:22acd37ed695 50 int pulse_A=0;
Fathoni17 0:22acd37ed695 51 int pulse_B=0;
Fathoni17 0:22acd37ed695 52 int pulse_C=0;
Fathoni17 0:22acd37ed695 53 float Vr = 0;
Fathoni17 0:22acd37ed695 54 float Vw = 0;
Fathoni17 0:22acd37ed695 55 float a = 0;
Fathoni17 0:22acd37ed695 56 double pid_t=0;
Fathoni17 0:22acd37ed695 57 short sp_teta;
Fathoni17 0:22acd37ed695 58 int sum=0;
Fathoni17 0:22acd37ed695 59
Fathoni17 0:22acd37ed695 60 int main(){
Fathoni17 0:22acd37ed695 61 encoder_A.reset();
Fathoni17 0:22acd37ed695 62 encoder_B.reset();
Fathoni17 0:22acd37ed695 63 encoder_C.reset();
Fathoni17 0:22acd37ed695 64 pc.baud(9600);
Fathoni17 0:22acd37ed695 65 stick.setup();
Fathoni17 0:22acd37ed695 66 stick.idle();
Fathoni17 0:22acd37ed695 67 pneumatik = 0;
Fathoni17 0:22acd37ed695 68
Fathoni17 0:22acd37ed695 69 // Thread
Fathoni17 0:22acd37ed695 70 // thread1.start(getJoystick);
Fathoni17 0:22acd37ed695 71 thread2.start(gerakMotor);
Fathoni17 0:22acd37ed695 72 //thread3.start(printPulse);
Fathoni17 0:22acd37ed695 73
Fathoni17 0:22acd37ed695 74 while(1){
Fathoni17 0:22acd37ed695 75 // do nothing
Fathoni17 0:22acd37ed695 76 if(stick.readable() ) {
Fathoni17 0:22acd37ed695 77 // Panggil fungsi pembacaan joystik
Fathoni17 0:22acd37ed695 78 stick.baca_data();
Fathoni17 0:22acd37ed695 79 // Panggil fungsi pengolahan data joystik
Fathoni17 0:22acd37ed695 80 stick.olah_data();
Fathoni17 0:22acd37ed695 81
Fathoni17 0:22acd37ed695 82 case_gerak();
Fathoni17 0:22acd37ed695 83
Fathoni17 0:22acd37ed695 84 pc.printf("Rotasi: %.2f\t", Vw);
Fathoni17 0:22acd37ed695 85 pc.printf("Tarnslasi: %.2f, %.1f\n", Vr, a);
Fathoni17 0:22acd37ed695 86
Fathoni17 0:22acd37ed695 87 } else {
Fathoni17 0:22acd37ed695 88 Vr=0;
Fathoni17 0:22acd37ed695 89 Vw=0;
Fathoni17 0:22acd37ed695 90 pc.printf("Masalah Joystick\n");
Fathoni17 0:22acd37ed695 91 }
Fathoni17 0:22acd37ed695 92 }
Fathoni17 0:22acd37ed695 93 }
Fathoni17 0:22acd37ed695 94
Fathoni17 0:22acd37ed695 95 void getJoystick(){
Fathoni17 0:22acd37ed695 96 while(1){
Fathoni17 0:22acd37ed695 97
Fathoni17 0:22acd37ed695 98 Thread::wait(1);
Fathoni17 0:22acd37ed695 99 }
Fathoni17 0:22acd37ed695 100 }
Fathoni17 0:22acd37ed695 101
Fathoni17 0:22acd37ed695 102 void hitungPID(){
Fathoni17 0:22acd37ed695 103
Fathoni17 0:22acd37ed695 104 }
Fathoni17 0:22acd37ed695 105
Fathoni17 0:22acd37ed695 106 void gerakMotor(){
Fathoni17 0:22acd37ed695 107 while(1){
Fathoni17 0:22acd37ed695 108 if(Vw != 0){
Fathoni17 0:22acd37ed695 109 motor1.speed((Vr*cos(a) + Vw));
Fathoni17 0:22acd37ed695 110 motor2.speed((Vr*(-0.5*cos(a) - 0.866*sin(a)) + Vw));
Fathoni17 0:22acd37ed695 111 motor3.speed((Vr*(-0.5*cos(a) + 0.866*sin(a)) + Vw));
Fathoni17 0:22acd37ed695 112 } else if (Vw == 0) { // mungkin bisa (a+pid_t)
Fathoni17 0:22acd37ed695 113 motor1.speed((Vr*cos(a) + Vw));
Fathoni17 0:22acd37ed695 114 motor2.speed((Vr*(-0.5*cos(a) - 0.866*sin(a)) + Vw));
Fathoni17 0:22acd37ed695 115 motor3.speed((Vr*(-0.5*cos(a) + 0.866*sin(a)) + Vw));
Fathoni17 0:22acd37ed695 116 } else {
Fathoni17 0:22acd37ed695 117 motor1.brake(BRAKE_HIGH);
Fathoni17 0:22acd37ed695 118 motor2.brake(BRAKE_HIGH);
Fathoni17 0:22acd37ed695 119 motor3.brake(BRAKE_HIGH);
Fathoni17 0:22acd37ed695 120 }
Fathoni17 0:22acd37ed695 121 Thread::wait(1);
Fathoni17 0:22acd37ed695 122 }
Fathoni17 0:22acd37ed695 123 }
Fathoni17 0:22acd37ed695 124
Fathoni17 0:22acd37ed695 125 void printPulse(){
Fathoni17 0:22acd37ed695 126 while(1){
Fathoni17 0:22acd37ed695 127 pulse_A = encoder_A.getPulses();
Fathoni17 0:22acd37ed695 128 pulse_B = encoder_B.getPulses();
Fathoni17 0:22acd37ed695 129 pulse_C = encoder_C.getPulses();
Fathoni17 0:22acd37ed695 130 pc.printf("%d\t%d\t%d\n", pulse_A, pulse_B, pulse_C);
Fathoni17 0:22acd37ed695 131 encoder_A.reset();
Fathoni17 0:22acd37ed695 132 encoder_B.reset();
Fathoni17 0:22acd37ed695 133 encoder_C.reset();//*/
Fathoni17 0:22acd37ed695 134 Thread::wait(20);
Fathoni17 0:22acd37ed695 135 }
Fathoni17 0:22acd37ed695 136 }
Fathoni17 0:22acd37ed695 137
Fathoni17 0:22acd37ed695 138 void case_gerak(){
Fathoni17 0:22acd37ed695 139 // Rotasi
Fathoni17 0:22acd37ed695 140 if (!stick.L1 && stick.R1) // Pivot Kanan
Fathoni17 0:22acd37ed695 141 Vw = 0.3;
Fathoni17 0:22acd37ed695 142 else if (!stick.R1 && stick.L1) // Pivot Kiri
Fathoni17 0:22acd37ed695 143 Vw = -0.3;
Fathoni17 0:22acd37ed695 144 else
Fathoni17 0:22acd37ed695 145 Vw = 0;
Fathoni17 0:22acd37ed695 146
Fathoni17 0:22acd37ed695 147 // Linier
Fathoni17 0:22acd37ed695 148 Vr = 0.5;
Fathoni17 0:22acd37ed695 149 if ((stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(!stick.kiri))
Fathoni17 0:22acd37ed695 150 a = -90/RAD_TO_DEG; // Maju
Fathoni17 0:22acd37ed695 151 else if ((!stick.atas)&&(stick.bawah)&&(!stick.kanan)&&(!stick.kiri))
Fathoni17 0:22acd37ed695 152 a = 90/RAD_TO_DEG; // Mundur
Fathoni17 0:22acd37ed695 153 else if ((stick.atas)&&(!stick.bawah)&&(!stick.kiri)&&(stick.kanan))
Fathoni17 0:22acd37ed695 154 a = -135/RAD_TO_DEG; // Serong Atas Kanan
Fathoni17 0:22acd37ed695 155 else if ((!stick.atas)&&(stick.bawah)&&(!stick.kiri)&&(stick.kanan))
Fathoni17 0:22acd37ed695 156 a = 135/RAD_TO_DEG; // Serong Bawah Kanan
Fathoni17 0:22acd37ed695 157 else if ((stick.atas)&&(!stick.bawah)&&(stick.kiri)&&(!stick.kanan))
Fathoni17 0:22acd37ed695 158 a = -45/RAD_TO_DEG; // Serong Atas Kiri
Fathoni17 0:22acd37ed695 159 else if ((!stick.atas)&&(stick.bawah)&&(stick.kiri)&&(!stick.kanan))
Fathoni17 0:22acd37ed695 160 a = 45/RAD_TO_DEG; // Serong Bawah Kiri
Fathoni17 0:22acd37ed695 161 else if ((!stick.atas)&&(!stick.bawah)&&(stick.kanan)&&(!stick.kiri))
Fathoni17 0:22acd37ed695 162 a = 180/RAD_TO_DEG; // Kanan
Fathoni17 0:22acd37ed695 163 else if ((!stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(stick.kiri))
Fathoni17 0:22acd37ed695 164 a = 0/RAD_TO_DEG; // Kiri
Fathoni17 0:22acd37ed695 165 else {
Fathoni17 0:22acd37ed695 166 Vr = 0;
Fathoni17 0:22acd37ed695 167 }
Fathoni17 0:22acd37ed695 168
Fathoni17 0:22acd37ed695 169 if ((stick.silang_click)&&(!stick.kotak)&&(!stick.segitiga)&&(!stick.lingkaran))
Fathoni17 0:22acd37ed695 170 pneumatik = !pneumatik; // Silang = Toggle pneumatik
Fathoni17 0:22acd37ed695 171
Fathoni17 0:22acd37ed695 172 }