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@3:b1403fcdaeb1, 2018-03-01 (annotated)

Committer:
MarchioKevin
Date:
Thu Mar 01 11:02:03 2018 +0000
Revision:
3:b1403fcdaeb1
Parent:
2:863436c840bf
Child:
4:3c890389e256
Child:
5:4a70c53d7f86
Start: Ganti mode open loop/mode close loop; Close loop aman kecuali pivot (masih osilasi); Makjleb

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
MarchioKevin 3:b1403fcdaeb1 12 #define TOLERANCET 0.8 //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
MarchioKevin 3:b1403fcdaeb1 19 #define KP_W 1.0
MarchioKevin 3:b1403fcdaeb1 20 #define KI_W 0.0065
MarchioKevin 3:b1403fcdaeb1 21 #define KD_W 125
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();
MarchioKevin 3:b1403fcdaeb1 50 float compute_Alpha(float x_s, float y_s, float x, float y,float theta);
Fathoni17 0:22acd37ed695 51
Fathoni17 0:22acd37ed695 52 // Variable-variable
Fathoni17 0:22acd37ed695 53 int joystick;
Fathoni17 2:863436c840bf 54 float pulse_A=0;
Fathoni17 2:863436c840bf 55 float pulse_B=0;
Fathoni17 2:863436c840bf 56 float pulse_C=0;
Fathoni17 0:22acd37ed695 57 float Vr = 0;
Fathoni17 0:22acd37ed695 58 float Vw = 0;
Fathoni17 0:22acd37ed695 59 float a = 0;
Fathoni17 2:863436c840bf 60 float w = 0;
MarchioKevin 3:b1403fcdaeb1 61 float x =0;
MarchioKevin 3:b1403fcdaeb1 62 float x_s = 0;
MarchioKevin 3:b1403fcdaeb1 63 float y =0;
MarchioKevin 3:b1403fcdaeb1 64 float y_s = 0;
MarchioKevin 3:b1403fcdaeb1 65 float x_prev=0;
MarchioKevin 3:b1403fcdaeb1 66 float y_prev=0;
Fathoni17 2:863436c840bf 67 float theta_s = 0;
Fathoni17 2:863436c840bf 68 float theta = 0;
Fathoni17 2:863436c840bf 69 float theta_prev = 0;
Fathoni17 2:863436c840bf 70 float theta_error_prev = 0;
Fathoni17 2:863436c840bf 71 float sum_theta_error = 0;
Fathoni17 2:863436c840bf 72 float theta_error;
Fathoni17 2:863436c840bf 73 unsigned long last_mt_print, last_mt_pid, last_mt_rotasi;
Fathoni17 2:863436c840bf 74 bool print_pulse = 0;
MarchioKevin 3:b1403fcdaeb1 75 bool modeauto = 0;
Fathoni17 0:22acd37ed695 76
Fathoni17 0:22acd37ed695 77 int main(){
Fathoni17 0:22acd37ed695 78 encoder_A.reset();
Fathoni17 0:22acd37ed695 79 encoder_B.reset();
Fathoni17 0:22acd37ed695 80 encoder_C.reset();
Fathoni17 2:863436c840bf 81 pc.baud(115200);
Fathoni17 0:22acd37ed695 82 stick.setup();
Fathoni17 0:22acd37ed695 83 stick.idle();
Fathoni17 0:22acd37ed695 84 pneumatik = 0;
Fathoni17 2:863436c840bf 85 startMillis();
Fathoni17 0:22acd37ed695 86
Fathoni17 0:22acd37ed695 87 while(1){
Fathoni17 0:22acd37ed695 88 // do nothing
Fathoni17 2:863436c840bf 89 if(stick.readable() ) {
Fathoni17 0:22acd37ed695 90 // Panggil fungsi pembacaan joystik
Fathoni17 0:22acd37ed695 91 stick.baca_data();
Fathoni17 0:22acd37ed695 92 // Panggil fungsi pengolahan data joystik
Fathoni17 0:22acd37ed695 93 stick.olah_data();
Fathoni17 2:863436c840bf 94 // Ambil data joystick
Fathoni17 2:863436c840bf 95 case_gerak();
Fathoni17 2:863436c840bf 96
Fathoni17 2:863436c840bf 97 gerakMotor();
Fathoni17 0:22acd37ed695 98
MarchioKevin 3:b1403fcdaeb1 99 if ( (millis() - last_mt_pid > TS) && !(fabs(theta_s - (theta*RAD_TO_DEG))<TOLERANCET) && modeauto ){
Fathoni17 2:863436c840bf 100 hitungPID(theta_s);
Fathoni17 2:863436c840bf 101 last_mt_pid = millis();
Fathoni17 1:735173a3b218 102 }
MarchioKevin 3:b1403fcdaeb1 103 if (fabs(theta_s - (theta*RAD_TO_DEG))<TOLERANCET || !modeauto){
Fathoni17 2:863436c840bf 104 pulse_A = encoder_A.getPulses()*PULSE_TO_JARAK;
Fathoni17 2:863436c840bf 105 pulse_B = encoder_B.getPulses()*PULSE_TO_JARAK;
Fathoni17 2:863436c840bf 106 pulse_C = encoder_C.getPulses()*PULSE_TO_JARAK;
Fathoni17 2:863436c840bf 107
Fathoni17 2:863436c840bf 108 //Compute value
Fathoni17 2:863436c840bf 109 theta = theta_prev + (pulse_A + pulse_C + pulse_B)/(3.0*L);
MarchioKevin 3:b1403fcdaeb1 110 x = x_prev + (2*pulse_A - pulse_C - pulse_B)/3*cos(theta_prev) - (-pulse_C+pulse_B)*0.5773*sin(theta_prev);
MarchioKevin 3:b1403fcdaeb1 111 y = y_prev + (2*pulse_A - pulse_C - pulse_B)/3*sin(theta_prev) + (-pulse_C+pulse_B)*0.5773*cos(theta_prev);
Fathoni17 2:863436c840bf 112
Fathoni17 2:863436c840bf 113 //Update value
Fathoni17 2:863436c840bf 114 theta_prev = theta;
MarchioKevin 3:b1403fcdaeb1 115 x_prev = x;
MarchioKevin 3:b1403fcdaeb1 116 y_prev = y;
Fathoni17 2:863436c840bf 117
Fathoni17 2:863436c840bf 118 encoder_A.reset();
Fathoni17 2:863436c840bf 119 encoder_B.reset();
Fathoni17 2:863436c840bf 120 encoder_C.reset();
MarchioKevin 3:b1403fcdaeb1 121
MarchioKevin 3:b1403fcdaeb1 122 if(modeauto) Vw = 0;
Fathoni17 2:863436c840bf 123 }
Fathoni17 2:863436c840bf 124 if (millis() - last_mt_print > TS+5){
Fathoni17 2:863436c840bf 125 if (print_pulse && DEBUG)
Fathoni17 2:863436c840bf 126 printPulse();
Fathoni17 2:863436c840bf 127 last_mt_print = millis();
Fathoni17 2:863436c840bf 128 }
Fathoni17 2:863436c840bf 129 }
Fathoni17 1:735173a3b218 130 }
Fathoni17 0:22acd37ed695 131 }
Fathoni17 0:22acd37ed695 132
Fathoni17 2:863436c840bf 133 void hitungPID(float theta_s){
Fathoni17 2:863436c840bf 134 pulse_A = encoder_A.getPulses()*PULSE_TO_JARAK;
Fathoni17 2:863436c840bf 135 pulse_B = encoder_B.getPulses()*PULSE_TO_JARAK;
Fathoni17 2:863436c840bf 136 pulse_C = encoder_C.getPulses()*PULSE_TO_JARAK;
Fathoni17 2:863436c840bf 137
Fathoni17 2:863436c840bf 138 //Compute value
MarchioKevin 3:b1403fcdaeb1 139 x = x_prev + (2*pulse_A - pulse_C - pulse_B)/3*cos(theta_prev) - (-pulse_C+pulse_B)*0.5773*sin(theta_prev);
MarchioKevin 3:b1403fcdaeb1 140 y = y_prev + (2*pulse_A - pulse_C - pulse_B)/3*sin(theta_prev) + (-pulse_C+pulse_B)*0.5773*cos(theta_prev);
Fathoni17 2:863436c840bf 141 theta = theta_prev + (pulse_A + pulse_C + pulse_B)/(3.0*L);
Fathoni17 2:863436c840bf 142
Fathoni17 2:863436c840bf 143 //Update value
MarchioKevin 3:b1403fcdaeb1 144 x_prev = x;
MarchioKevin 3:b1403fcdaeb1 145 y_prev = y;
Fathoni17 2:863436c840bf 146 theta_prev = theta;
Fathoni17 2:863436c840bf 147
Fathoni17 2:863436c840bf 148 encoder_A.reset();
Fathoni17 2:863436c840bf 149 encoder_B.reset();
Fathoni17 2:863436c840bf 150 encoder_C.reset();
Fathoni17 2:863436c840bf 151
Fathoni17 2:863436c840bf 152 //theta_s = theta_s/RAD_TO_DEG;
Fathoni17 2:863436c840bf 153 //menghitung error jarak x,y terhaadap xs,ys
Fathoni17 2:863436c840bf 154 theta_error = theta_s - (theta*RAD_TO_DEG);
Fathoni17 2:863436c840bf 155 sum_theta_error += theta_error;
Fathoni17 2:863436c840bf 156
Fathoni17 2:863436c840bf 157 //kalkulasi PID Theta
Fathoni17 2:863436c840bf 158 w = KP_W*theta_error + KI_W*TS*sum_theta_error + KD_W*(theta_error - theta_error_prev)/TS;
MarchioKevin 3:b1403fcdaeb1 159 Vw += (w*L/MAX_W_SPEED)*LIMITPWM;
Fathoni17 2:863436c840bf 160
Fathoni17 2:863436c840bf 161 //update
Fathoni17 2:863436c840bf 162 theta_error_prev = theta_error;
Fathoni17 2:863436c840bf 163 //saturasi vw
MarchioKevin 3:b1403fcdaeb1 164 if (Vw > 0.2){
MarchioKevin 3:b1403fcdaeb1 165 Vw = 0.2;
Fathoni17 0:22acd37ed695 166 }
MarchioKevin 3:b1403fcdaeb1 167 else if ( Vw < -0.2){
MarchioKevin 3:b1403fcdaeb1 168 Vw = -0.2;
Fathoni17 2:863436c840bf 169 }
Fathoni17 0:22acd37ed695 170 }
Fathoni17 0:22acd37ed695 171
Fathoni17 0:22acd37ed695 172 void gerakMotor(){
Fathoni17 2:863436c840bf 173 if ((Vw == 0) && (Vr == 0)){
Fathoni17 2:863436c840bf 174 motor1.brake(BRAKE_HIGH);
Fathoni17 2:863436c840bf 175 motor2.brake(BRAKE_HIGH);
Fathoni17 2:863436c840bf 176 motor3.brake(BRAKE_HIGH);
Fathoni17 2:863436c840bf 177 print_pulse = 0;
Fathoni17 2:863436c840bf 178 } else {
MarchioKevin 3:b1403fcdaeb1 179 //a = compute_Alpha(x_s, y_s, x, y, theta);
Fathoni17 2:863436c840bf 180 motor1.speed((-1*Vr*cos(a) + Vw));
Fathoni17 2:863436c840bf 181 motor2.speed((Vr*(0.5*cos(a) + 0.866*sin(a)) + Vw));
Fathoni17 2:863436c840bf 182 motor3.speed((Vr*(0.5*cos(a) - 0.866*sin(a)) + Vw));
Fathoni17 2:863436c840bf 183 print_pulse = 1;
Fathoni17 2:863436c840bf 184 }
Fathoni17 0:22acd37ed695 185 }
Fathoni17 0:22acd37ed695 186
Fathoni17 0:22acd37ed695 187 void printPulse(){
MarchioKevin 3:b1403fcdaeb1 188 pc.printf("%.2f\t%.2f\n", theta*RAD_TO_DEG, theta_s);
MarchioKevin 3:b1403fcdaeb1 189 }
MarchioKevin 3:b1403fcdaeb1 190
MarchioKevin 3:b1403fcdaeb1 191 float compute_Alpha(float x_s, float y_s, float x, float y,float theta){
MarchioKevin 3:b1403fcdaeb1 192 //fungsi untuk menghitung alpha sebagai arah gerak robot
MarchioKevin 3:b1403fcdaeb1 193 float temp = atan((y_s - y)/(x_s - x)) - theta;
MarchioKevin 3:b1403fcdaeb1 194
MarchioKevin 3:b1403fcdaeb1 195 if (x_s < x) return temp + PI;
MarchioKevin 3:b1403fcdaeb1 196 else return temp;
Fathoni17 0:22acd37ed695 197 }
Fathoni17 0:22acd37ed695 198
Fathoni17 0:22acd37ed695 199 void case_gerak(){
Fathoni17 0:22acd37ed695 200 // Rotasi
MarchioKevin 3:b1403fcdaeb1 201 if(modeauto){
MarchioKevin 3:b1403fcdaeb1 202 if (!stick.L1 && stick.R1) // Pivot Kanan
MarchioKevin 3:b1403fcdaeb1 203 theta_s += 0.15;
MarchioKevin 3:b1403fcdaeb1 204 else if (!stick.R1 && stick.L1) // Pivot Kiri
MarchioKevin 3:b1403fcdaeb1 205 theta_s -= 0.15;
MarchioKevin 3:b1403fcdaeb1 206
MarchioKevin 3:b1403fcdaeb1 207 }
MarchioKevin 3:b1403fcdaeb1 208 else if(!modeauto){
MarchioKevin 3:b1403fcdaeb1 209 if (!stick.L1 && stick.R1) // Pivot Kanan
MarchioKevin 3:b1403fcdaeb1 210 Vw = 0.3;
MarchioKevin 3:b1403fcdaeb1 211 else if (!stick.R1 && stick.L1) // Pivot Kiri
MarchioKevin 3:b1403fcdaeb1 212 Vw = -0.3;
MarchioKevin 3:b1403fcdaeb1 213 else
MarchioKevin 3:b1403fcdaeb1 214 Vw = 0.0;
MarchioKevin 3:b1403fcdaeb1 215 }
MarchioKevin 3:b1403fcdaeb1 216
MarchioKevin 3:b1403fcdaeb1 217 if(stick.START_click){
MarchioKevin 3:b1403fcdaeb1 218 modeauto = !modeauto;
MarchioKevin 3:b1403fcdaeb1 219 theta = 0.0;
MarchioKevin 3:b1403fcdaeb1 220 theta_prev = 0.0;
MarchioKevin 3:b1403fcdaeb1 221 theta_s = 0;
MarchioKevin 3:b1403fcdaeb1 222 theta_error_prev = 0;
MarchioKevin 3:b1403fcdaeb1 223 sum_theta_error = 0;
MarchioKevin 3:b1403fcdaeb1 224 theta_error;
MarchioKevin 3:b1403fcdaeb1 225 }
Fathoni17 0:22acd37ed695 226
Fathoni17 0:22acd37ed695 227 // Linier
Fathoni17 0:22acd37ed695 228 Vr = 0.5;
MarchioKevin 3:b1403fcdaeb1 229 if ((stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(!stick.kiri)){
Fathoni17 0:22acd37ed695 230 a = -90/RAD_TO_DEG; // Maju
MarchioKevin 3:b1403fcdaeb1 231 // x_s = 0;// Maju
MarchioKevin 3:b1403fcdaeb1 232 // y_s = 10000;
MarchioKevin 3:b1403fcdaeb1 233 }
MarchioKevin 3:b1403fcdaeb1 234 else if ((!stick.atas)&&(stick.bawah)&&(!stick.kanan)&&(!stick.kiri)){
Fathoni17 0:22acd37ed695 235 a = 90/RAD_TO_DEG; // Mundur
MarchioKevin 3:b1403fcdaeb1 236 // x_s = 0; // Mundur
MarchioKevin 3:b1403fcdaeb1 237 // y_s = -10000;
MarchioKevin 3:b1403fcdaeb1 238 }
MarchioKevin 3:b1403fcdaeb1 239 else if ((stick.atas)&&(!stick.bawah)&&(!stick.kiri)&&(stick.kanan)){
Fathoni17 0:22acd37ed695 240 a = -135/RAD_TO_DEG; // Serong Atas Kanan
MarchioKevin 3:b1403fcdaeb1 241 // x_s = 50000; // Maju+Kanan
MarchioKevin 3:b1403fcdaeb1 242 // y_s = 50000;
MarchioKevin 3:b1403fcdaeb1 243 }
MarchioKevin 3:b1403fcdaeb1 244 else if ((!stick.atas)&&(stick.bawah)&&(!stick.kiri)&&(stick.kanan)){
Fathoni17 0:22acd37ed695 245 a = 135/RAD_TO_DEG; // Serong Bawah Kanan
MarchioKevin 3:b1403fcdaeb1 246 // x_s = 50000; // Mundur+Kanan
MarchioKevin 3:b1403fcdaeb1 247 // y_s = -50000;
MarchioKevin 3:b1403fcdaeb1 248 }
MarchioKevin 3:b1403fcdaeb1 249 else if ((stick.atas)&&(!stick.bawah)&&(stick.kiri)&&(!stick.kanan)){
Fathoni17 0:22acd37ed695 250 a = -45/RAD_TO_DEG; // Serong Atas Kiri
MarchioKevin 3:b1403fcdaeb1 251 // x_s = 50000; // Maju+Kiri
MarchioKevin 3:b1403fcdaeb1 252 // y_s = -50000;
MarchioKevin 3:b1403fcdaeb1 253 }
MarchioKevin 3:b1403fcdaeb1 254 else if ((!stick.atas)&&(stick.bawah)&&(stick.kiri)&&(!stick.kanan)){
Fathoni17 0:22acd37ed695 255 a = 45/RAD_TO_DEG; // Serong Bawah Kiri
MarchioKevin 3:b1403fcdaeb1 256 // x_s = -50000; // Mundur+Kiri
MarchioKevin 3:b1403fcdaeb1 257 // y_s = -50000;
MarchioKevin 3:b1403fcdaeb1 258 }
MarchioKevin 3:b1403fcdaeb1 259 else if ((!stick.atas)&&(!stick.bawah)&&(stick.kanan)&&(!stick.kiri)){
Fathoni17 0:22acd37ed695 260 a = 180/RAD_TO_DEG; // Kanan
MarchioKevin 3:b1403fcdaeb1 261 // x_s = 50000; // Kanan
MarchioKevin 3:b1403fcdaeb1 262 // y_s = 0;
MarchioKevin 3:b1403fcdaeb1 263 }
MarchioKevin 3:b1403fcdaeb1 264 else if ((!stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(stick.kiri)){
Fathoni17 0:22acd37ed695 265 a = 0/RAD_TO_DEG; // Kiri
MarchioKevin 3:b1403fcdaeb1 266 // x_s = -50000; // Kiri
MarchioKevin 3:b1403fcdaeb1 267 // y_s = 0;
MarchioKevin 3:b1403fcdaeb1 268 }
Fathoni17 0:22acd37ed695 269 else {
Fathoni17 0:22acd37ed695 270 Vr = 0;
Fathoni17 2:863436c840bf 271 a = 0;
Fathoni17 0:22acd37ed695 272 }
Fathoni17 0:22acd37ed695 273
Fathoni17 0:22acd37ed695 274 if ((stick.silang_click)&&(!stick.kotak)&&(!stick.segitiga)&&(!stick.lingkaran))
Fathoni17 0:22acd37ed695 275 pneumatik = !pneumatik; // Silang = Toggle pneumatik
Fathoni17 0:22acd37ed695 276 }