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@2:863436c840bf, 2018-03-01 (annotated)

Committer:
Fathoni17
Date:
Thu Mar 01 06:39:48 2018 +0000
Revision:
2:863436c840bf
Parent:
1:735173a3b218
Child:
3:b1403fcdaeb1
Koreksi teta mantep

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 "pinList.h"
Fathoni17 2:863436c840bf 6 #include "millis.h"
Fathoni17 0:22acd37ed695 7
Fathoni17 0:22acd37ed695 8 #define PI 3.141592653593
Fathoni17 0:22acd37ed695 9 #define RAD_TO_DEG 57.2957795131
Fathoni17 2:863436c840bf 10 #define MAX_W_SPEED 15000 //max angular speed of robot
Fathoni17 2:863436c840bf 11
Fathoni17 2:863436c840bf 12 #define TOLERANCET 1.2 //theta tolerance
Fathoni17 2:863436c840bf 13 #define PULSE_TO_JARAK 0.581776 //kll roda / pulses
Fathoni17 2:863436c840bf 14 #define L 298.0 //roda to center of robot
Fathoni17 2:863436c840bf 15 #define TS 2.0 //time sampling
Fathoni17 2:863436c840bf 16 #define LIMITPWM 0.4 //limit pwm motor
Fathoni17 2:863436c840bf 17
Fathoni17 2:863436c840bf 18 //Konstanta PID Sudut
Fathoni17 2:863436c840bf 19 #define KP_W 5.0
Fathoni17 2:863436c840bf 20 #define KI_W 0.02
Fathoni17 2:863436c840bf 21 #define KD_W 10.0
Fathoni17 0:22acd37ed695 22
Fathoni17 0:22acd37ed695 23 #define MOTOR_LIMIT_MAX 1
Fathoni17 0:22acd37ed695 24 #define MOTOR_LIMIT_MIN -1
Fathoni17 0:22acd37ed695 25
Fathoni17 0:22acd37ed695 26 #define DEBUG 1
Fathoni17 0:22acd37ed695 27
Fathoni17 0:22acd37ed695 28 // Serial
Fathoni17 0:22acd37ed695 29 Serial pc(USBTX,USBRX);
Fathoni17 0:22acd37ed695 30 joysticknucleo stick(PIN_TX, PIN_RX);
Fathoni17 0:22acd37ed695 31
Fathoni17 0:22acd37ed695 32 // Pneumatik
Fathoni17 0:22acd37ed695 33 DigitalOut pneumatik(PIN_PNEUMATIK);
Fathoni17 0:22acd37ed695 34
Fathoni17 0:22acd37ed695 35 // Encoder
Fathoni17 0:22acd37ed695 36 encoderKRAI encoder_A(PIN_A_CHANNEL_A, PIN_A_CHANNEL_B, 540, encoderKRAI::X4_ENCODING);
Fathoni17 0:22acd37ed695 37 encoderKRAI encoder_B(PIN_B_CHANNEL_A, PIN_B_CHANNEL_B, 540, encoderKRAI::X4_ENCODING);
Fathoni17 0:22acd37ed695 38 encoderKRAI encoder_C(PIN_C_CHANNEL_A, PIN_C_CHANNEL_B, 540, encoderKRAI::X4_ENCODING);
Fathoni17 0:22acd37ed695 39
Fathoni17 0:22acd37ed695 40 // Motor
Fathoni17 0:22acd37ed695 41 Motor motor1(PIN_PWM_A, PIN_FWD_A, PIN_REV_A);
Fathoni17 0:22acd37ed695 42 Motor motor2(PIN_PWM_B, PIN_FWD_B, PIN_REV_B);
Fathoni17 0:22acd37ed695 43 Motor motor3(PIN_PWM_C, PIN_FWD_C, PIN_REV_C);
Fathoni17 0:22acd37ed695 44
Fathoni17 0:22acd37ed695 45 // Fungsi dan Prosedur
Fathoni17 0:22acd37ed695 46 void gerakMotor();
Fathoni17 2:863436c840bf 47 void hitungPID(float theta_s);
Fathoni17 0:22acd37ed695 48 void printPulse();
Fathoni17 0:22acd37ed695 49 void case_gerak();
Fathoni17 0:22acd37ed695 50
Fathoni17 0:22acd37ed695 51 // Variable-variable
Fathoni17 0:22acd37ed695 52 int joystick;
Fathoni17 2:863436c840bf 53 float pulse_A=0;
Fathoni17 2:863436c840bf 54 float pulse_B=0;
Fathoni17 2:863436c840bf 55 float pulse_C=0;
Fathoni17 0:22acd37ed695 56 float Vr = 0;
Fathoni17 0:22acd37ed695 57 float Vw = 0;
Fathoni17 0:22acd37ed695 58 float a = 0;
Fathoni17 2:863436c840bf 59 float w = 0;
Fathoni17 2:863436c840bf 60 float theta_s = 0;
Fathoni17 2:863436c840bf 61 float theta = 0;
Fathoni17 2:863436c840bf 62 float theta_prev = 0;
Fathoni17 2:863436c840bf 63 float theta_error_prev = 0;
Fathoni17 2:863436c840bf 64 float sum_theta_error = 0;
Fathoni17 2:863436c840bf 65 float theta_error;
Fathoni17 2:863436c840bf 66 unsigned long last_mt_print, last_mt_pid, last_mt_rotasi;
Fathoni17 2:863436c840bf 67 bool print_pulse = 0;
Fathoni17 0:22acd37ed695 68
Fathoni17 0:22acd37ed695 69 int main(){
Fathoni17 0:22acd37ed695 70 encoder_A.reset();
Fathoni17 0:22acd37ed695 71 encoder_B.reset();
Fathoni17 0:22acd37ed695 72 encoder_C.reset();
Fathoni17 2:863436c840bf 73 pc.baud(115200);
Fathoni17 0:22acd37ed695 74 stick.setup();
Fathoni17 0:22acd37ed695 75 stick.idle();
Fathoni17 0:22acd37ed695 76 pneumatik = 0;
Fathoni17 2:863436c840bf 77 startMillis();
Fathoni17 0:22acd37ed695 78
Fathoni17 0:22acd37ed695 79 while(1){
Fathoni17 0:22acd37ed695 80 // do nothing
Fathoni17 2:863436c840bf 81 if(stick.readable() ) {
Fathoni17 0:22acd37ed695 82 // Panggil fungsi pembacaan joystik
Fathoni17 0:22acd37ed695 83 stick.baca_data();
Fathoni17 0:22acd37ed695 84 // Panggil fungsi pengolahan data joystik
Fathoni17 0:22acd37ed695 85 stick.olah_data();
Fathoni17 2:863436c840bf 86 // Ambil data joystick
Fathoni17 2:863436c840bf 87 case_gerak();
Fathoni17 2:863436c840bf 88
Fathoni17 2:863436c840bf 89 gerakMotor();
Fathoni17 0:22acd37ed695 90
Fathoni17 2:863436c840bf 91 if ( (millis() - last_mt_pid > TS) && !(fabs(theta_s - (theta*RAD_TO_DEG))<TOLERANCET) ){
Fathoni17 2:863436c840bf 92 hitungPID(theta_s);
Fathoni17 2:863436c840bf 93 last_mt_pid = millis();
Fathoni17 1:735173a3b218 94 }
Fathoni17 2:863436c840bf 95 if (fabs(theta_s - (theta*RAD_TO_DEG))<TOLERANCET){
Fathoni17 2:863436c840bf 96 pulse_A = encoder_A.getPulses()*PULSE_TO_JARAK;
Fathoni17 2:863436c840bf 97 pulse_B = encoder_B.getPulses()*PULSE_TO_JARAK;
Fathoni17 2:863436c840bf 98 pulse_C = encoder_C.getPulses()*PULSE_TO_JARAK;
Fathoni17 2:863436c840bf 99
Fathoni17 2:863436c840bf 100 //Compute value
Fathoni17 2:863436c840bf 101 theta = theta_prev + (pulse_A + pulse_C + pulse_B)/(3.0*L);
Fathoni17 2:863436c840bf 102
Fathoni17 2:863436c840bf 103 //Update value
Fathoni17 2:863436c840bf 104 theta_prev = theta;
Fathoni17 2:863436c840bf 105
Fathoni17 2:863436c840bf 106 encoder_A.reset();
Fathoni17 2:863436c840bf 107 encoder_B.reset();
Fathoni17 2:863436c840bf 108 encoder_C.reset();
Fathoni17 2:863436c840bf 109 Vw = 0;
Fathoni17 2:863436c840bf 110 }
Fathoni17 2:863436c840bf 111 if (millis() - last_mt_print > TS+5){
Fathoni17 2:863436c840bf 112 if (print_pulse && DEBUG)
Fathoni17 2:863436c840bf 113 printPulse();
Fathoni17 2:863436c840bf 114 last_mt_print = millis();
Fathoni17 2:863436c840bf 115 }
Fathoni17 2:863436c840bf 116 }
Fathoni17 1:735173a3b218 117 }
Fathoni17 0:22acd37ed695 118 }
Fathoni17 0:22acd37ed695 119
Fathoni17 2:863436c840bf 120 void hitungPID(float theta_s){
Fathoni17 2:863436c840bf 121 pulse_A = encoder_A.getPulses()*PULSE_TO_JARAK;
Fathoni17 2:863436c840bf 122 pulse_B = encoder_B.getPulses()*PULSE_TO_JARAK;
Fathoni17 2:863436c840bf 123 pulse_C = encoder_C.getPulses()*PULSE_TO_JARAK;
Fathoni17 2:863436c840bf 124
Fathoni17 2:863436c840bf 125 //Compute value
Fathoni17 2:863436c840bf 126 theta = theta_prev + (pulse_A + pulse_C + pulse_B)/(3.0*L);
Fathoni17 2:863436c840bf 127
Fathoni17 2:863436c840bf 128 //Update value
Fathoni17 2:863436c840bf 129 theta_prev = theta;
Fathoni17 2:863436c840bf 130
Fathoni17 2:863436c840bf 131 encoder_A.reset();
Fathoni17 2:863436c840bf 132 encoder_B.reset();
Fathoni17 2:863436c840bf 133 encoder_C.reset();
Fathoni17 2:863436c840bf 134
Fathoni17 2:863436c840bf 135 //theta_s = theta_s/RAD_TO_DEG;
Fathoni17 2:863436c840bf 136 //menghitung error jarak x,y terhaadap xs,ys
Fathoni17 2:863436c840bf 137 theta_error = theta_s - (theta*RAD_TO_DEG);
Fathoni17 2:863436c840bf 138 sum_theta_error += theta_error;
Fathoni17 2:863436c840bf 139
Fathoni17 2:863436c840bf 140 //kalkulasi PID Theta
Fathoni17 2:863436c840bf 141 w = KP_W*theta_error + KI_W*TS*sum_theta_error + KD_W*(theta_error - theta_error_prev)/TS;
Fathoni17 2:863436c840bf 142 Vw = (w*L/MAX_W_SPEED)*LIMITPWM;
Fathoni17 2:863436c840bf 143
Fathoni17 2:863436c840bf 144 //update
Fathoni17 2:863436c840bf 145 theta_error_prev = theta_error;
Fathoni17 2:863436c840bf 146 //saturasi vw
Fathoni17 2:863436c840bf 147 if (Vw > 0.3){
Fathoni17 2:863436c840bf 148 Vw = 0.3;
Fathoni17 0:22acd37ed695 149 }
Fathoni17 2:863436c840bf 150 else if ( Vw < -0.3){
Fathoni17 2:863436c840bf 151 Vw = -0.3;
Fathoni17 2:863436c840bf 152 }
Fathoni17 0:22acd37ed695 153 }
Fathoni17 0:22acd37ed695 154
Fathoni17 0:22acd37ed695 155 void gerakMotor(){
Fathoni17 2:863436c840bf 156 if ((Vw == 0) && (Vr == 0)){
Fathoni17 2:863436c840bf 157 motor1.brake(BRAKE_HIGH);
Fathoni17 2:863436c840bf 158 motor2.brake(BRAKE_HIGH);
Fathoni17 2:863436c840bf 159 motor3.brake(BRAKE_HIGH);
Fathoni17 2:863436c840bf 160 print_pulse = 0;
Fathoni17 2:863436c840bf 161 } else {
Fathoni17 2:863436c840bf 162 motor1.speed((-1*Vr*cos(a) + Vw));
Fathoni17 2:863436c840bf 163 motor2.speed((Vr*(0.5*cos(a) + 0.866*sin(a)) + Vw));
Fathoni17 2:863436c840bf 164 motor3.speed((Vr*(0.5*cos(a) - 0.866*sin(a)) + Vw));
Fathoni17 2:863436c840bf 165 print_pulse = 1;
Fathoni17 2:863436c840bf 166 }
Fathoni17 0:22acd37ed695 167 }
Fathoni17 0:22acd37ed695 168
Fathoni17 0:22acd37ed695 169 void printPulse(){
Fathoni17 2:863436c840bf 170 pc.printf("%.2f\t%.2f\t%.2f\t%.2f\t%.2f\n", pulse_A, pulse_B, pulse_C, theta*RAD_TO_DEG, theta_s);
Fathoni17 0:22acd37ed695 171 }
Fathoni17 0:22acd37ed695 172
Fathoni17 0:22acd37ed695 173 void case_gerak(){
Fathoni17 0:22acd37ed695 174 // Rotasi
Fathoni17 2:863436c840bf 175 if (!stick.L1_click && stick.R1_click) // Pivot Kanan
Fathoni17 2:863436c840bf 176 theta_s += 10;
Fathoni17 2:863436c840bf 177 else if (!stick.R1_click && stick.L1_click) // Pivot Kiri
Fathoni17 2:863436c840bf 178 theta_s -= 10;
Fathoni17 0:22acd37ed695 179
Fathoni17 0:22acd37ed695 180 // Linier
Fathoni17 0:22acd37ed695 181 Vr = 0.5;
Fathoni17 0:22acd37ed695 182 if ((stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(!stick.kiri))
Fathoni17 0:22acd37ed695 183 a = -90/RAD_TO_DEG; // Maju
Fathoni17 0:22acd37ed695 184 else if ((!stick.atas)&&(stick.bawah)&&(!stick.kanan)&&(!stick.kiri))
Fathoni17 0:22acd37ed695 185 a = 90/RAD_TO_DEG; // Mundur
Fathoni17 0:22acd37ed695 186 else if ((stick.atas)&&(!stick.bawah)&&(!stick.kiri)&&(stick.kanan))
Fathoni17 0:22acd37ed695 187 a = -135/RAD_TO_DEG; // Serong Atas Kanan
Fathoni17 0:22acd37ed695 188 else if ((!stick.atas)&&(stick.bawah)&&(!stick.kiri)&&(stick.kanan))
Fathoni17 0:22acd37ed695 189 a = 135/RAD_TO_DEG; // Serong Bawah Kanan
Fathoni17 0:22acd37ed695 190 else if ((stick.atas)&&(!stick.bawah)&&(stick.kiri)&&(!stick.kanan))
Fathoni17 0:22acd37ed695 191 a = -45/RAD_TO_DEG; // Serong Atas Kiri
Fathoni17 0:22acd37ed695 192 else if ((!stick.atas)&&(stick.bawah)&&(stick.kiri)&&(!stick.kanan))
Fathoni17 0:22acd37ed695 193 a = 45/RAD_TO_DEG; // Serong Bawah Kiri
Fathoni17 0:22acd37ed695 194 else if ((!stick.atas)&&(!stick.bawah)&&(stick.kanan)&&(!stick.kiri))
Fathoni17 0:22acd37ed695 195 a = 180/RAD_TO_DEG; // Kanan
Fathoni17 0:22acd37ed695 196 else if ((!stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(stick.kiri))
Fathoni17 0:22acd37ed695 197 a = 0/RAD_TO_DEG; // Kiri
Fathoni17 0:22acd37ed695 198 else {
Fathoni17 0:22acd37ed695 199 Vr = 0;
Fathoni17 2:863436c840bf 200 a = 0;
Fathoni17 0:22acd37ed695 201 }
Fathoni17 0:22acd37ed695 202
Fathoni17 0:22acd37ed695 203 if ((stick.silang_click)&&(!stick.kotak)&&(!stick.segitiga)&&(!stick.lingkaran))
Fathoni17 0:22acd37ed695 204 pneumatik = !pneumatik; // Silang = Toggle pneumatik
Fathoni17 0:22acd37ed695 205 }