EL4121 Embedded System
/
odometry_omni_3roda_v5
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: Motor PS_PAD TextLCD mbed-os
Fork of
odometry_omni_3roda_v3
by 
EL4121 Embedded System
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 #include "mbed.h" 00002 #include "TextLCD.h" 00003 #include "PS_PAD.h" 00004 #include "Motor.h" 00005 #include "encoderKRAI.h" 00006 00007 #define PI 3.14159265359 00008 #define RAD_TO_DEG 57.2957795131 00009 00010 #define PULSE_TO_MM 0.1177 //rev/pulse * K_lingkaran_roda 00011 #define L 144.0 // lengan roda dari pusat robot (mm) 00012 #define Ts 2.0 // Time Sampling sistem 2ms 00013 00014 #define MAX_SPEED 912.175 //Vresultan max (mm/s) 00015 #define MAX_W_SPEED 1314.72 //Vw max (mm/s) 00016 #define SPEED 1 //V robot 00017 00018 #define BOUNDARY_TOLERANCE 70.0 00019 00020 // konstanta PID untuk kendali Posisi (x y) 00021 #define Kp_s 10.0 00022 #define Ki_s 0.0 00023 #define Kd_s 1.6 00024 00025 // konstanta PID untuk kendali arah (theta) 00026 #define Kp_w 0.2 00027 #define Ki_w 0.0 00028 #define Kd_w 0.01 00029 00030 //STATE dari Sistem 00031 #define state_Idle 1 00032 #define state_ManualControl 2 00033 #define state_AutoControl 3 00034 00035 #define BUTTON_offAwal 6 00036 #define BUTTON_onAwal 7 00037 #define BUTTON_onEnd 8 00038 00039 Thread thread1(osPriorityNormal, OS_STACK_SIZE, NULL); 00040 Thread thread2(osPriorityNormal, OS_STACK_SIZE, NULL); 00041 Thread thread3(osPriorityNormal, OS_STACK_SIZE, NULL); 00042 Thread thread4(osPriorityNormal, OS_STACK_SIZE, NULL); 00043 Thread thread5(osPriorityNormal, OS_STACK_SIZE, NULL); 00044 00045 TextLCD lcd(PA_9, PC_3, PC_2, PA_8, PB_0, PC_15, TextLCD::LCD20x4); //rs,e,d4-d7 00046 encoderKRAI enc1 (PC_14, PC_13, 11, encoderKRAI::X4_ENCODING); 00047 encoderKRAI enc2 (PC_0, PC_1, 11, encoderKRAI::X4_ENCODING); 00048 encoderKRAI enc3 (PB_10, PB_3, 11, encoderKRAI::X4_ENCODING); 00049 PS_PAD ps2(PB_15,PB_14,PB_13, PC_4); //(mosi, miso, sck, ss) 00050 00051 Motor motor3(PB_7, PA_14, PA_15); //motor4 00052 Motor motor2(PA_11, PA_6, PA_5); //motor2 00053 Motor motor1(PB_6, PA_7, PB_12); //motor 3 00054 //Motor motor1(PA_10, PB_5, PB_4); //motor_griper 00055 00056 Serial pc(USBTX,USBRX); 00057 00058 /*------------Variabel Global----------------*/ 00059 float x = 0; 00060 float y = 0; 00061 float theta = 0; 00062 00063 float Vr = 0; 00064 float Vw = 0; 00065 float a = 0; 00066 00067 float Vx = 0; 00068 float Vy = 0; 00069 float W = 0; 00070 00071 int state = state_Idle; 00072 int stateSelect = BUTTON_offAwal; 00073 int stateStart = BUTTON_offAwal; //state awal 00074 00075 typedef struct map { 00076 int n; 00077 float x_pos[16]; 00078 float y_pos[16]; 00079 float theta_pos[16]; 00080 } mapping; 00081 00082 const mapping map_square = {16, 00083 { 0,150,300,450,600,600,600,600,600,450,300,150, 0, 0, 0, 0}, 00084 { 0, 0, 0, 0, 0,150,300,450,600,600,600,600,600,450,300,150}, 00085 { 0, 0, 0, 0, 90, 90, 90, 90,180,180,180,180,270,270,270,270}}; 00086 const mapping map_triangle= {13, 00087 { 0,150,300,450,600,525,450,375,300,225,150, 75, 0}, 00088 { 0, 0, 0, 0, 0,130,260,390,519,390,260,130, 0}, 00089 { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}}; 00090 const mapping map_circle = {16, 00091 {0,114.88,212.17,277.17,300,277,211.83,114.44,0,-115.1,-212.34,-277.26,-300,-277,-212,-114.66}, 00092 {0,22.68,87.57,184.78,299.52,414.77,512,577.13,600,577.22,512.25,415,300.23,185.44,88.07,22.96}, 00093 {0,22.5,45,67.5, 90,112.5,135,157.5,180,202.5,225,247.5,270,292.5, 315, 337.5}}; 00094 const mapping map_NULL = {0, 00095 { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, 00096 { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, 00097 { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}}; 00098 mapping map_data; 00099 00100 /*------------- DEKLARASI FUNGSI ----------------*/ 00101 void dataJoystick(); 00102 void lcd_out(); 00103 void self_localization(); 00104 void motor_out(); 00105 void PTP_movement(); 00106 00107 void FSM(int input1, int input2, int *state); 00108 int fsmButton(int input,int *stateButton); 00109 int calculate_PID(float x_s, float y_s, float theta_s, bool isLast); 00110 float moving_direction( float x_s, float y_s, float x, float y,float theta); 00111 /*-----------------------------------------------*/ 00112 00113 int main() 00114 { 00115 pc.baud(115200); 00116 ps2.init(); 00117 00118 thread1.start(dataJoystick); 00119 thread2.start(lcd_out); 00120 thread3.start(self_localization); 00121 thread4.start(motor_out); 00122 thread5.start(PTP_movement); 00123 00124 while (1) 00125 { 00126 //do nothing 00127 } 00128 } 00129 00130 void dataJoystick(){ 00131 while(true){ 00132 ps2.poll(); 00133 00134 FSM(fsmButton(ps2.read(PS_PAD::PAD_SELECT),&stateSelect),fsmButton(ps2.read(PS_PAD::PAD_START),&stateStart), &state); 00135 00136 if(state == state_ManualControl){ 00137 Vr = 0.5; 00138 if(ps2.read(PS_PAD::PAD_LEFT)) a = 180/RAD_TO_DEG; 00139 else if(ps2.read(PS_PAD::PAD_BOTTOM)) a = -90/RAD_TO_DEG; 00140 else if(ps2.read(PS_PAD::PAD_RIGHT)) a = 0/RAD_TO_DEG; 00141 else if(ps2.read(PS_PAD::PAD_TOP)) a = 90/RAD_TO_DEG; 00142 else Vr = 0; 00143 00144 if(ps2.read(PS_PAD::PAD_L1)) Vw = 0.2; 00145 else if(ps2.read(PS_PAD::PAD_R1)) Vw = -0.2; 00146 else Vw = 0.0; 00147 } 00148 else if(state == state_AutoControl){ 00149 if(ps2.read(PS_PAD::PAD_X)){ 00150 map_data = map_NULL; 00151 Vr = 0; 00152 Vw = 0; 00153 } 00154 else if(ps2.read(PS_PAD::PAD_SQUARE)){ 00155 map_data = map_square; 00156 } 00157 else if(ps2.read(PS_PAD::PAD_TRIANGLE)){ 00158 map_data = map_triangle; 00159 } 00160 else if(ps2.read(PS_PAD::PAD_CIRCLE)){ 00161 map_data = map_circle; 00162 } 00163 00164 } 00165 else if(state == state_Idle){ 00166 00167 } 00168 00169 Thread::wait(1); 00170 } 00171 } 00172 00173 void FSM(int input1, int input2, int *state){ 00174 switch(*state){ 00175 case state_Idle : 00176 if (input1 == 1) *state = state_ManualControl; 00177 else if (input2 == 1) *state = state_AutoControl; 00178 break; 00179 case state_ManualControl : 00180 if (input1 == 1) *state = state_Idle; 00181 break; 00182 case state_AutoControl : 00183 if (input2 == 1) *state = state_Idle; 00184 break; 00185 } 00186 } 00187 00188 int fsmButton(int input,int *stateButton) 00189 { 00190 switch (*stateButton) 00191 { 00192 case BUTTON_offAwal : 00193 if (input == 1) 00194 *stateButton = BUTTON_onAwal; 00195 else 00196 *stateButton = BUTTON_offAwal; 00197 return 0; 00198 //break; 00199 case BUTTON_onAwal : 00200 if(input == 0) 00201 *stateButton = BUTTON_onEnd; 00202 else 00203 *stateButton = BUTTON_onAwal; 00204 return 0; 00205 //break; 00206 case BUTTON_onEnd : 00207 *stateButton = BUTTON_offAwal; 00208 return 1; 00209 //break; 00210 } 00211 } 00212 00213 void lcd_out(){ 00214 lcd.locate(0,0); 00215 lcd.printf("Sistem Pergerakan"); 00216 lcd.locate(0,1); 00217 lcd.printf("Objek dengan 3 Roda"); 00218 lcd.locate(0,2); 00219 lcd.printf("Berbasis Odometry"); 00220 Thread::wait(1500); 00221 00222 lcd.cls(); 00223 lcd.locate(0,0); 00224 lcd.printf("Bryan Christy P"); 00225 lcd.locate(0,1); 00226 lcd.printf(" 13214073"); 00227 lcd.locate(0,2); 00228 lcd.printf("Rizqi Cahyo Y"); 00229 lcd.locate(0,3); 00230 lcd.printf(" 13214090"); 00231 Thread::wait(1500); 00232 while (true){ 00233 lcd.cls(); 00234 if (state == state_ManualControl){ 00235 lcd.locate(0,0); 00236 lcd.printf("Mode : Manual"); 00237 lcd.locate(0,1); 00238 lcd.printf("Vx = %.2f", Vx); 00239 lcd.locate(0,2); 00240 lcd.printf("Vy = %.2f", Vy); 00241 lcd.locate(0,3); 00242 lcd.printf("W = %.2f", W*RAD_TO_DEG); 00243 } 00244 else if (state == state_AutoControl){ 00245 lcd.locate(0,0); 00246 lcd.printf("Mode : Autonomous"); 00247 lcd.locate(0,1); 00248 lcd.printf("x = %.2f",x); 00249 lcd.locate(0,2); 00250 lcd.printf("y = %.2f",y); 00251 lcd.locate(0,3); 00252 lcd.printf("theta = %.2f", theta*RAD_TO_DEG); 00253 } 00254 else if(state == state_Idle){ 00255 lcd.locate(0,0); 00256 lcd.printf("PAD_START"); 00257 lcd.locate(0,1); 00258 lcd.printf(" => Mode Auto"); 00259 lcd.locate(0,2); 00260 lcd.printf("PAD_SELECT"); 00261 lcd.locate(0,3); 00262 lcd.printf(" => Mode Manual"); 00263 } 00264 Thread::wait(50); 00265 } 00266 } 00267 00268 void self_localization(){ 00269 static float x_prev = 0; 00270 static float y_prev = 0; 00271 static float theta_prev = 0; 00272 00273 while(true){ 00274 float d1 = enc1.getPulses()*PULSE_TO_MM; 00275 float d2 = enc2.getPulses()*PULSE_TO_MM; 00276 float d3 = enc3.getPulses()*PULSE_TO_MM; 00277 00278 x = x_prev + (2*d1 - d2 - d3)/3*cos(theta_prev) - (-d2+d3)*0.5773*sin(theta_prev); 00279 y = y_prev + (2*d1 - d2 - d3)/3*sin(theta_prev) + (-d2+d3)*0.5773*cos(theta_prev); 00280 theta = theta_prev + (d1 + d2 + d3)/(3*L); // // 0.132629 => 180 / (3. L. pi) 00281 00282 Vx = (x - x_prev)/0.002; 00283 Vy = (y - y_prev)/0.002; 00284 W = (theta - theta_prev)/0.002; 00285 00286 x_prev = x; 00287 y_prev = y; 00288 theta_prev = theta; 00289 00290 enc1.reset(); 00291 enc2.reset(); 00292 enc3.reset(); 00293 00294 if((state == state_AutoControl) && (map_data.n == 0)){ 00295 x_prev = 0; 00296 y_prev = 0; 00297 theta_prev = 0; 00298 } 00299 Thread::wait(Ts); //frekuensi sampling = 500 Hz 00300 } 00301 } 00302 00303 void PTP_movement(){ 00304 int i = 0; 00305 while(true){ 00306 if(state == state_AutoControl){ 00307 while(i < map_data.n){ 00308 i += calculate_PID(map_data.x_pos[i],map_data.y_pos[i],map_data.theta_pos[i],(i==(map_data.n-1))); 00309 //if (i == map.n) i = 0; 00310 Thread::wait(Ts); 00311 } 00312 i = 0; 00313 } 00314 else i = 0; 00315 Thread::wait(Ts); 00316 } 00317 } 00318 00319 int calculate_PID(float x_s, float y_s, float theta_s, bool isLast){ 00320 //error_prev & sum_error 00321 static float S_error_prev = 0; 00322 static float theta_error_prev = 0; 00323 00324 static float sum_S_error = 0; 00325 static float sum_theta_error = 0; 00326 00327 //menghitung error jarak x,y terhaadap xs,ys 00328 float S_error = sqrt((x_s-x)*(x_s-x) + (y_s-y)*(y_s-y)); 00329 //menghitung error arah 00330 float theta_error = theta_s - theta*RAD_TO_DEG; 00331 00332 sum_S_error += S_error; 00333 sum_theta_error += theta_error; 00334 00335 float Vs = Kp_s*S_error + Ki_s*Ts*sum_S_error + Kd_s*(S_error - S_error_prev)/Ts; 00336 float w = Kp_w*theta_error + Ki_w*Ts*sum_theta_error + Kd_w*(theta_error - theta_error_prev)/Ts; 00337 00338 Vr = Vs/MAX_SPEED*0.5; 00339 Vw = w*L/MAX_W_SPEED*0.5; 00340 a = moving_direction(x_s,y_s,x,y,theta); 00341 00342 S_error_prev = S_error; 00343 theta_error_prev = theta_error; 00344 00345 if(isLast == true){ 00346 if ((abs(x_s - x) < 20) && (abs(y_s - y) < 20)){ 00347 Vw = 0; 00348 Vr = 0; 00349 return 1; 00350 } 00351 else return 0; 00352 } 00353 else{ 00354 if ((abs(x_s - x) < BOUNDARY_TOLERANCE) && (abs(y_s - y) < BOUNDARY_TOLERANCE)) return 1; 00355 else return 0; 00356 } 00357 } 00358 00359 00360 float moving_direction( float x_s, float y_s, float x, float y,float theta){ 00361 float temp = atan((y_s - y)/(x_s - x)) - theta; 00362 00363 if (x_s < x) return temp + PI; 00364 else return temp; 00365 } 00366 00367 void motor_out() { 00368 Thread::wait(1500); 00369 00370 while(1){ 00371 motor1.speed(SPEED*(Vr*cos(a) + Vw)); 00372 motor2.speed(SPEED*(Vr*(-0.5*cos(a) - 0.866*sin(a)) + Vw)); 00373 motor3.speed(SPEED*(Vr*(-0.5*cos(a) + 0.866*sin(a)) + Vw)); 00374 00375 if((Vr == 0) && (Vw == 0)){ 00376 motor1.brake(BRAKE_HIGH); 00377 motor2.brake(BRAKE_HIGH); 00378 motor3.brake(BRAKE_HIGH); 00379 } 00380 } 00381 } 00382
Generated on Mon Jul 18 2022 14:20:14 by
1.7.2