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@1:735173a3b218, 2018-02-26 (annotated)

Committer:
Fathoni17
Date:
Mon Feb 26 07:29:05 2018 +0000
Revision:
1:735173a3b218
Parent:
0:22acd37ed695
Child:
2:863436c840bf
open loop sudah oke; todo: cari konstanta PID, implementasi PID teta

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 1:735173a3b218 59 int print_pulse = 0;
Fathoni17 0:22acd37ed695 60
Fathoni17 0:22acd37ed695 61 int main(){
Fathoni17 0:22acd37ed695 62 encoder_A.reset();
Fathoni17 0:22acd37ed695 63 encoder_B.reset();
Fathoni17 0:22acd37ed695 64 encoder_C.reset();
Fathoni17 0:22acd37ed695 65 pc.baud(9600);
Fathoni17 0:22acd37ed695 66 stick.setup();
Fathoni17 0:22acd37ed695 67 stick.idle();
Fathoni17 0:22acd37ed695 68 pneumatik = 0;
Fathoni17 0:22acd37ed695 69
Fathoni17 0:22acd37ed695 70 // Thread
Fathoni17 1:735173a3b218 71 thread1.start(getJoystick);
Fathoni17 0:22acd37ed695 72 thread2.start(gerakMotor);
Fathoni17 1:735173a3b218 73 thread3.start(printPulse);
Fathoni17 0:22acd37ed695 74
Fathoni17 0:22acd37ed695 75 while(1){
Fathoni17 0:22acd37ed695 76 // do nothing
Fathoni17 1:735173a3b218 77 if(stick.readable() ) {
Fathoni17 0:22acd37ed695 78 // Panggil fungsi pembacaan joystik
Fathoni17 0:22acd37ed695 79 stick.baca_data();
Fathoni17 0:22acd37ed695 80 // Panggil fungsi pengolahan data joystik
Fathoni17 0:22acd37ed695 81 stick.olah_data();
Fathoni17 0:22acd37ed695 82
Fathoni17 1:735173a3b218 83 // Rotasi
Fathoni17 1:735173a3b218 84 if (!stick.L1 && stick.R1) // Pivot Kanan
Fathoni17 1:735173a3b218 85 Vw = 0.3;
Fathoni17 1:735173a3b218 86 else if (!stick.R1 && stick.L1) // Pivot Kiri
Fathoni17 1:735173a3b218 87 Vw = -0.3;
Fathoni17 1:735173a3b218 88 else
Fathoni17 1:735173a3b218 89 Vw = 0;
Fathoni17 0:22acd37ed695 90
Fathoni17 1:735173a3b218 91 // Linier
Fathoni17 1:735173a3b218 92 Vr = 0.5;
Fathoni17 1:735173a3b218 93 if ((stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(!stick.kiri))
Fathoni17 1:735173a3b218 94 a = -90/RAD_TO_DEG; // Maju
Fathoni17 1:735173a3b218 95 else if ((!stick.atas)&&(stick.bawah)&&(!stick.kanan)&&(!stick.kiri))
Fathoni17 1:735173a3b218 96 a = 90/RAD_TO_DEG; // Mundur
Fathoni17 1:735173a3b218 97 else if ((stick.atas)&&(!stick.bawah)&&(!stick.kiri)&&(stick.kanan))
Fathoni17 1:735173a3b218 98 a = -135/RAD_TO_DEG; // Serong Atas Kanan
Fathoni17 1:735173a3b218 99 else if ((!stick.atas)&&(stick.bawah)&&(!stick.kiri)&&(stick.kanan))
Fathoni17 1:735173a3b218 100 a = 135/RAD_TO_DEG; // Serong Bawah Kanan
Fathoni17 1:735173a3b218 101 else if ((stick.atas)&&(!stick.bawah)&&(stick.kiri)&&(!stick.kanan))
Fathoni17 1:735173a3b218 102 a = -45/RAD_TO_DEG; // Serong Atas Kiri
Fathoni17 1:735173a3b218 103 else if ((!stick.atas)&&(stick.bawah)&&(stick.kiri)&&(!stick.kanan))
Fathoni17 1:735173a3b218 104 a = 45/RAD_TO_DEG; // Serong Bawah Kiri
Fathoni17 1:735173a3b218 105 else if ((!stick.atas)&&(!stick.bawah)&&(stick.kanan)&&(!stick.kiri))
Fathoni17 1:735173a3b218 106 a = 180/RAD_TO_DEG; // Kanan
Fathoni17 1:735173a3b218 107 else if ((!stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(stick.kiri))
Fathoni17 1:735173a3b218 108 a = 0/RAD_TO_DEG; // Kiri
Fathoni17 1:735173a3b218 109 else {
Fathoni17 1:735173a3b218 110 Vr = 0;
Fathoni17 1:735173a3b218 111 a = 0;
Fathoni17 1:735173a3b218 112 }
Fathoni17 1:735173a3b218 113
Fathoni17 1:735173a3b218 114 if ((stick.silang_click)&&(!stick.kotak)&&(!stick.segitiga)&&(!stick.lingkaran))
Fathoni17 1:735173a3b218 115 pneumatik = !pneumatik; // Silang = Toggle pneumatik
Fathoni17 1:735173a3b218 116
Fathoni17 1:735173a3b218 117 //pc.printf("Rotasi: %.2f\t", Vw);
Fathoni17 1:735173a3b218 118 //pc.printf("Tarnslasi: %.2f, %.1f\n", Vr, a);
Fathoni17 0:22acd37ed695 119
Fathoni17 1:735173a3b218 120 }
Fathoni17 1:735173a3b218 121 }
Fathoni17 0:22acd37ed695 122 }
Fathoni17 0:22acd37ed695 123
Fathoni17 0:22acd37ed695 124 void getJoystick(){
Fathoni17 0:22acd37ed695 125 while(1){
Fathoni17 0:22acd37ed695 126 Thread::wait(1);
Fathoni17 0:22acd37ed695 127 }
Fathoni17 0:22acd37ed695 128 }
Fathoni17 0:22acd37ed695 129
Fathoni17 0:22acd37ed695 130 void hitungPID(){
Fathoni17 0:22acd37ed695 131
Fathoni17 0:22acd37ed695 132 }
Fathoni17 0:22acd37ed695 133
Fathoni17 0:22acd37ed695 134 void gerakMotor(){
Fathoni17 0:22acd37ed695 135 while(1){
Fathoni17 1:735173a3b218 136 if ((Vw == 0) && (Vr == 0)){
Fathoni17 0:22acd37ed695 137 motor1.brake(BRAKE_HIGH);
Fathoni17 0:22acd37ed695 138 motor2.brake(BRAKE_HIGH);
Fathoni17 0:22acd37ed695 139 motor3.brake(BRAKE_HIGH);
Fathoni17 1:735173a3b218 140 print_pulse = 0;
Fathoni17 1:735173a3b218 141 } else {
Fathoni17 1:735173a3b218 142 motor1.speed((-1*Vr*cos(a) + Vw));
Fathoni17 1:735173a3b218 143 motor2.speed((Vr*(0.5*cos(a) + 0.866*sin(a)) + Vw));
Fathoni17 1:735173a3b218 144 motor3.speed((Vr*(0.5*cos(a) - 0.866*sin(a)) + Vw));
Fathoni17 1:735173a3b218 145 print_pulse = 1;
Fathoni17 0:22acd37ed695 146 }
Fathoni17 0:22acd37ed695 147 Thread::wait(1);
Fathoni17 0:22acd37ed695 148 }
Fathoni17 0:22acd37ed695 149 }
Fathoni17 0:22acd37ed695 150
Fathoni17 0:22acd37ed695 151 void printPulse(){
Fathoni17 0:22acd37ed695 152 while(1){
Fathoni17 0:22acd37ed695 153 pulse_A = encoder_A.getPulses();
Fathoni17 0:22acd37ed695 154 pulse_B = encoder_B.getPulses();
Fathoni17 0:22acd37ed695 155 pulse_C = encoder_C.getPulses();
Fathoni17 1:735173a3b218 156 if (print_pulse)
Fathoni17 1:735173a3b218 157 pc.printf("%d\t%d\t%d\n", pulse_A, pulse_B, pulse_C);
Fathoni17 0:22acd37ed695 158 encoder_A.reset();
Fathoni17 0:22acd37ed695 159 encoder_B.reset();
Fathoni17 0:22acd37ed695 160 encoder_C.reset();//*/
Fathoni17 0:22acd37ed695 161 Thread::wait(20);
Fathoni17 0:22acd37ed695 162 }
Fathoni17 0:22acd37ed695 163 }
Fathoni17 0:22acd37ed695 164
Fathoni17 0:22acd37ed695 165 void case_gerak(){
Fathoni17 0:22acd37ed695 166 // Rotasi
Fathoni17 0:22acd37ed695 167 if (!stick.L1 && stick.R1) // Pivot Kanan
Fathoni17 0:22acd37ed695 168 Vw = 0.3;
Fathoni17 0:22acd37ed695 169 else if (!stick.R1 && stick.L1) // Pivot Kiri
Fathoni17 0:22acd37ed695 170 Vw = -0.3;
Fathoni17 0:22acd37ed695 171 else
Fathoni17 0:22acd37ed695 172 Vw = 0;
Fathoni17 0:22acd37ed695 173
Fathoni17 0:22acd37ed695 174 // Linier
Fathoni17 0:22acd37ed695 175 Vr = 0.5;
Fathoni17 0:22acd37ed695 176 if ((stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(!stick.kiri))
Fathoni17 0:22acd37ed695 177 a = -90/RAD_TO_DEG; // Maju
Fathoni17 0:22acd37ed695 178 else if ((!stick.atas)&&(stick.bawah)&&(!stick.kanan)&&(!stick.kiri))
Fathoni17 0:22acd37ed695 179 a = 90/RAD_TO_DEG; // Mundur
Fathoni17 0:22acd37ed695 180 else if ((stick.atas)&&(!stick.bawah)&&(!stick.kiri)&&(stick.kanan))
Fathoni17 0:22acd37ed695 181 a = -135/RAD_TO_DEG; // Serong Atas Kanan
Fathoni17 0:22acd37ed695 182 else if ((!stick.atas)&&(stick.bawah)&&(!stick.kiri)&&(stick.kanan))
Fathoni17 0:22acd37ed695 183 a = 135/RAD_TO_DEG; // Serong Bawah Kanan
Fathoni17 0:22acd37ed695 184 else if ((stick.atas)&&(!stick.bawah)&&(stick.kiri)&&(!stick.kanan))
Fathoni17 0:22acd37ed695 185 a = -45/RAD_TO_DEG; // Serong Atas Kiri
Fathoni17 0:22acd37ed695 186 else if ((!stick.atas)&&(stick.bawah)&&(stick.kiri)&&(!stick.kanan))
Fathoni17 0:22acd37ed695 187 a = 45/RAD_TO_DEG; // Serong Bawah Kiri
Fathoni17 0:22acd37ed695 188 else if ((!stick.atas)&&(!stick.bawah)&&(stick.kanan)&&(!stick.kiri))
Fathoni17 0:22acd37ed695 189 a = 180/RAD_TO_DEG; // Kanan
Fathoni17 0:22acd37ed695 190 else if ((!stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(stick.kiri))
Fathoni17 0:22acd37ed695 191 a = 0/RAD_TO_DEG; // Kiri
Fathoni17 0:22acd37ed695 192 else {
Fathoni17 0:22acd37ed695 193 Vr = 0;
Fathoni17 0:22acd37ed695 194 }
Fathoni17 0:22acd37ed695 195
Fathoni17 0:22acd37ed695 196 if ((stick.silang_click)&&(!stick.kotak)&&(!stick.segitiga)&&(!stick.lingkaran))
Fathoni17 0:22acd37ed695 197 pneumatik = !pneumatik; // Silang = Toggle pneumatik
Fathoni17 0:22acd37ed695 198
Fathoni17 0:22acd37ed695 199 }