EL4121 Embedded System / Mbed OS odometry_omni_3roda_v5

kodingan full omni 3roda embedded

Dependencies:   Motor PS_PAD TextLCD mbed-os

Fork of odometry_omni_3roda_v3 by EL4121 Embedded System

main.cpp@7:f139bb3b2401, 2017-12-16 (annotated)

Committer:
rizqicahyo
Date:
Sat Dec 16 21:08:40 2017 +0000
Revision:
7:f139bb3b2401
Parent:
6:f10c0d9f228d 
perbaikan dan/atau penambahan tampilan LCD, input joystik, FSM, mode manual dan otomatis, 3 jenis mapping.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
rizqicahyo 0:837acb06c892 1 #include "mbed.h"
rizqicahyo 0:837acb06c892 2 #include "TextLCD.h"
rizqicahyo 3:a03ce2084ceb 3 #include "PS_PAD.h"
be_bryan 4:cd5de3b14797 4 #include "Motor.h"
be_bryan 4:cd5de3b14797 5 #include "encoderKRAI.h"
rizqicahyo 0:837acb06c892 6
rizqicahyo 5:0b555929c5b2 7 #define PI 3.14159265359
rizqicahyo 5:0b555929c5b2 8 #define RAD_TO_DEG 57.2957795131
rizqicahyo 5:0b555929c5b2 9
rizqicahyo 5:0b555929c5b2 10 #define PULSE_TO_MM 0.1177 //rev/pulse * K_lingkaran_roda
rizqicahyo 5:0b555929c5b2 11 #define L 144.0 // lengan roda dari pusat robot (mm)
rizqicahyo 5:0b555929c5b2 12 #define Ts 2.0 // Time Sampling sistem 2ms
rizqicahyo 5:0b555929c5b2 13
rizqicahyo 5:0b555929c5b2 14 #define MAX_SPEED 912.175 //Vresultan max (mm/s)
rizqicahyo 5:0b555929c5b2 15 #define MAX_W_SPEED 1314.72 //Vw max (mm/s)
rizqicahyo 5:0b555929c5b2 16 #define SPEED 1 //V robot
rizqicahyo 5:0b555929c5b2 17
rizqicahyo 5:0b555929c5b2 18 #define BOUNDARY_TOLERANCE 70.0
rizqicahyo 5:0b555929c5b2 19
rizqicahyo 5:0b555929c5b2 20 // konstanta PID untuk kendali Posisi (x y)
rizqicahyo 5:0b555929c5b2 21 #define Kp_s 10.0
rizqicahyo 5:0b555929c5b2 22 #define Ki_s 0.0
rizqicahyo 5:0b555929c5b2 23 #define Kd_s 1.6
rizqicahyo 5:0b555929c5b2 24
rizqicahyo 5:0b555929c5b2 25 // konstanta PID untuk kendali arah (theta)
rizqicahyo 5:0b555929c5b2 26 #define Kp_w 0.2
rizqicahyo 5:0b555929c5b2 27 #define Ki_w 0.0
rizqicahyo 5:0b555929c5b2 28 #define Kd_w 0.01
rizqicahyo 5:0b555929c5b2 29
rizqicahyo 7:f139bb3b2401 30 //STATE dari Sistem
rizqicahyo 7:f139bb3b2401 31 #define state_Idle 1
rizqicahyo 7:f139bb3b2401 32 #define state_ManualControl 2
rizqicahyo 7:f139bb3b2401 33 #define state_AutoControl 3
rizqicahyo 7:f139bb3b2401 34
rizqicahyo 7:f139bb3b2401 35 #define BUTTON_offAwal 6
rizqicahyo 7:f139bb3b2401 36 #define BUTTON_onAwal 7
rizqicahyo 7:f139bb3b2401 37 #define BUTTON_onEnd 8
rizqicahyo 7:f139bb3b2401 38
be_bryan 4:cd5de3b14797 39 Thread thread1(osPriorityNormal, OS_STACK_SIZE, NULL);
rizqicahyo 5:0b555929c5b2 40 Thread thread2(osPriorityNormal, OS_STACK_SIZE, NULL);
be_bryan 4:cd5de3b14797 41 Thread thread3(osPriorityNormal, OS_STACK_SIZE, NULL);
rizqicahyo 5:0b555929c5b2 42 Thread thread4(osPriorityNormal, OS_STACK_SIZE, NULL);
rizqicahyo 5:0b555929c5b2 43 Thread thread5(osPriorityNormal, OS_STACK_SIZE, NULL);
rizqicahyo 3:a03ce2084ceb 44
be_bryan 4:cd5de3b14797 45 TextLCD lcd(PA_9, PC_3, PC_2, PA_8, PB_0, PC_15, TextLCD::LCD20x4); //rs,e,d4-d7
be_bryan 4:cd5de3b14797 46 encoderKRAI enc1 (PC_14, PC_13, 11, encoderKRAI::X4_ENCODING);
be_bryan 4:cd5de3b14797 47 encoderKRAI enc2 (PC_0, PC_1, 11, encoderKRAI::X4_ENCODING);
be_bryan 4:cd5de3b14797 48 encoderKRAI enc3 (PB_10, PB_3, 11, encoderKRAI::X4_ENCODING);
rizqicahyo 3:a03ce2084ceb 49 PS_PAD ps2(PB_15,PB_14,PB_13, PC_4); //(mosi, miso, sck, ss)
rizqicahyo 3:a03ce2084ceb 50
rizqicahyo 5:0b555929c5b2 51 Motor motor3(PB_7, PA_14, PA_15); //motor4
rizqicahyo 5:0b555929c5b2 52 Motor motor2(PA_11, PA_6, PA_5); //motor2
rizqicahyo 5:0b555929c5b2 53 Motor motor1(PB_6, PA_7, PB_12); //motor 3
rizqicahyo 5:0b555929c5b2 54 //Motor motor1(PA_10, PB_5, PB_4); //motor_griper
rizqicahyo 5:0b555929c5b2 55
rizqicahyo 5:0b555929c5b2 56 Serial pc(USBTX,USBRX);
rizqicahyo 5:0b555929c5b2 57
rizqicahyo 7:f139bb3b2401 58 /*------------Variabel Global----------------*/
rizqicahyo 5:0b555929c5b2 59 float x = 0;
rizqicahyo 5:0b555929c5b2 60 float y = 0;
rizqicahyo 5:0b555929c5b2 61 float theta = 0;
rizqicahyo 5:0b555929c5b2 62
rizqicahyo 7:f139bb3b2401 63 float Vr = 0;
rizqicahyo 7:f139bb3b2401 64 float Vw = 0;
rizqicahyo 5:0b555929c5b2 65 float a = 0;
rizqicahyo 5:0b555929c5b2 66
rizqicahyo 5:0b555929c5b2 67 float Vx = 0;
rizqicahyo 5:0b555929c5b2 68 float Vy = 0;
rizqicahyo 5:0b555929c5b2 69 float W = 0;
rizqicahyo 5:0b555929c5b2 70
rizqicahyo 7:f139bb3b2401 71 int state = state_Idle;
rizqicahyo 7:f139bb3b2401 72 int stateSelect = BUTTON_offAwal;
rizqicahyo 7:f139bb3b2401 73 int stateStart = BUTTON_offAwal; //state awal
rizqicahyo 7:f139bb3b2401 74
rizqicahyo 7:f139bb3b2401 75 typedef struct map {
rizqicahyo 7:f139bb3b2401 76 int n;
rizqicahyo 7:f139bb3b2401 77 float x_pos[16];
rizqicahyo 7:f139bb3b2401 78 float y_pos[16];
rizqicahyo 7:f139bb3b2401 79 float theta_pos[16];
rizqicahyo 7:f139bb3b2401 80 } mapping;
rizqicahyo 7:f139bb3b2401 81
rizqicahyo 7:f139bb3b2401 82 const mapping map_square = {16,
rizqicahyo 7:f139bb3b2401 83 { 0,150,300,450,600,600,600,600,600,450,300,150, 0, 0, 0, 0},
rizqicahyo 7:f139bb3b2401 84 { 0, 0, 0, 0, 0,150,300,450,600,600,600,600,600,450,300,150},
rizqicahyo 7:f139bb3b2401 85 { 0, 0, 0, 0, 90, 90, 90, 90,180,180,180,180,270,270,270,270}};
rizqicahyo 7:f139bb3b2401 86 const mapping map_triangle= {13,
rizqicahyo 7:f139bb3b2401 87 { 0,150,300,450,600,525,450,375,300,225,150, 75, 0},
rizqicahyo 7:f139bb3b2401 88 { 0, 0, 0, 0, 0,130,260,390,519,390,260,130, 0},
rizqicahyo 7:f139bb3b2401 89 { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}};
rizqicahyo 7:f139bb3b2401 90 const mapping map_circle = {16,
rizqicahyo 7:f139bb3b2401 91 {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},
rizqicahyo 7:f139bb3b2401 92 {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},
rizqicahyo 7:f139bb3b2401 93 {0,22.5,45,67.5, 90,112.5,135,157.5,180,202.5,225,247.5,270,292.5, 315, 337.5}};
rizqicahyo 7:f139bb3b2401 94 const mapping map_NULL = {0,
rizqicahyo 7:f139bb3b2401 95 { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
rizqicahyo 7:f139bb3b2401 96 { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
rizqicahyo 7:f139bb3b2401 97 { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}};
rizqicahyo 7:f139bb3b2401 98 mapping map_data;
rizqicahyo 7:f139bb3b2401 99
rizqicahyo 7:f139bb3b2401 100 /*------------- DEKLARASI FUNGSI ----------------*/
rizqicahyo 7:f139bb3b2401 101 void dataJoystick();
rizqicahyo 7:f139bb3b2401 102 void lcd_out();
rizqicahyo 7:f139bb3b2401 103 void self_localization();
rizqicahyo 7:f139bb3b2401 104 void motor_out();
rizqicahyo 7:f139bb3b2401 105 void PTP_movement();
rizqicahyo 7:f139bb3b2401 106
rizqicahyo 7:f139bb3b2401 107 void FSM(int input1, int input2, int *state);
rizqicahyo 7:f139bb3b2401 108 int fsmButton(int input,int *stateButton);
rizqicahyo 7:f139bb3b2401 109 int calculate_PID(float x_s, float y_s, float theta_s, bool isLast);
rizqicahyo 7:f139bb3b2401 110 float moving_direction( float x_s, float y_s, float x, float y,float theta);
rizqicahyo 7:f139bb3b2401 111 /*-----------------------------------------------*/
rizqicahyo 0:837acb06c892 112
rizqicahyo 0:837acb06c892 113 int main()
rizqicahyo 0:837acb06c892 114 {
rizqicahyo 0:837acb06c892 115 pc.baud(115200);
rizqicahyo 3:a03ce2084ceb 116 ps2.init();
rizqicahyo 7:f139bb3b2401 117
be_bryan 4:cd5de3b14797 118 thread1.start(dataJoystick);
rizqicahyo 7:f139bb3b2401 119 thread2.start(lcd_out);
rizqicahyo 5:0b555929c5b2 120 thread3.start(self_localization);
rizqicahyo 5:0b555929c5b2 121 thread4.start(motor_out);
rizqicahyo 5:0b555929c5b2 122 thread5.start(PTP_movement);
rizqicahyo 7:f139bb3b2401 123
rizqicahyo 0:837acb06c892 124 while (1)
rizqicahyo 0:837acb06c892 125 {
rizqicahyo 5:0b555929c5b2 126 //do nothing
be_bryan 4:cd5de3b14797 127 }
be_bryan 4:cd5de3b14797 128 }
be_bryan 4:cd5de3b14797 129
be_bryan 4:cd5de3b14797 130 void dataJoystick(){
be_bryan 4:cd5de3b14797 131 while(true){
rizqicahyo 7:f139bb3b2401 132 ps2.poll();
rizqicahyo 7:f139bb3b2401 133
rizqicahyo 7:f139bb3b2401 134 FSM(fsmButton(ps2.read(PS_PAD::PAD_SELECT),&stateSelect),fsmButton(ps2.read(PS_PAD::PAD_START),&stateStart), &state);
rizqicahyo 7:f139bb3b2401 135
rizqicahyo 7:f139bb3b2401 136 if(state == state_ManualControl){
rizqicahyo 7:f139bb3b2401 137 Vr = 0.5;
rizqicahyo 7:f139bb3b2401 138 if(ps2.read(PS_PAD::PAD_LEFT)) a = 180/RAD_TO_DEG;
rizqicahyo 7:f139bb3b2401 139 else if(ps2.read(PS_PAD::PAD_BOTTOM)) a = -90/RAD_TO_DEG;
rizqicahyo 7:f139bb3b2401 140 else if(ps2.read(PS_PAD::PAD_RIGHT)) a = 0/RAD_TO_DEG;
rizqicahyo 7:f139bb3b2401 141 else if(ps2.read(PS_PAD::PAD_TOP)) a = 90/RAD_TO_DEG;
rizqicahyo 7:f139bb3b2401 142 else Vr = 0;
rizqicahyo 7:f139bb3b2401 143
rizqicahyo 7:f139bb3b2401 144 if(ps2.read(PS_PAD::PAD_L1)) Vw = 0.2;
rizqicahyo 7:f139bb3b2401 145 else if(ps2.read(PS_PAD::PAD_R1)) Vw = -0.2;
rizqicahyo 7:f139bb3b2401 146 else Vw = 0.0;
rizqicahyo 7:f139bb3b2401 147 }
rizqicahyo 7:f139bb3b2401 148 else if(state == state_AutoControl){
rizqicahyo 7:f139bb3b2401 149 if(ps2.read(PS_PAD::PAD_X)){
rizqicahyo 7:f139bb3b2401 150 map_data = map_NULL;
rizqicahyo 7:f139bb3b2401 151 Vr = 0;
rizqicahyo 7:f139bb3b2401 152 Vw = 0;
rizqicahyo 7:f139bb3b2401 153 }
rizqicahyo 7:f139bb3b2401 154 else if(ps2.read(PS_PAD::PAD_SQUARE)){
rizqicahyo 7:f139bb3b2401 155 map_data = map_square;
rizqicahyo 7:f139bb3b2401 156 }
rizqicahyo 7:f139bb3b2401 157 else if(ps2.read(PS_PAD::PAD_TRIANGLE)){
rizqicahyo 7:f139bb3b2401 158 map_data = map_triangle;
rizqicahyo 7:f139bb3b2401 159 }
rizqicahyo 7:f139bb3b2401 160 else if(ps2.read(PS_PAD::PAD_CIRCLE)){
rizqicahyo 7:f139bb3b2401 161 map_data = map_circle;
rizqicahyo 7:f139bb3b2401 162 }
rizqicahyo 7:f139bb3b2401 163
rizqicahyo 7:f139bb3b2401 164 }
rizqicahyo 7:f139bb3b2401 165 else if(state == state_Idle){
rizqicahyo 7:f139bb3b2401 166
rizqicahyo 7:f139bb3b2401 167 }
rizqicahyo 7:f139bb3b2401 168
rizqicahyo 7:f139bb3b2401 169 Thread::wait(1);
rizqicahyo 0:837acb06c892 170 }
rizqicahyo 0:837acb06c892 171 }
be_bryan 4:cd5de3b14797 172
rizqicahyo 7:f139bb3b2401 173 void FSM(int input1, int input2, int *state){
rizqicahyo 7:f139bb3b2401 174 switch(*state){
rizqicahyo 7:f139bb3b2401 175 case state_Idle :
rizqicahyo 7:f139bb3b2401 176 if (input1 == 1) *state = state_ManualControl;
rizqicahyo 7:f139bb3b2401 177 else if (input2 == 1) *state = state_AutoControl;
rizqicahyo 7:f139bb3b2401 178 break;
rizqicahyo 7:f139bb3b2401 179 case state_ManualControl :
rizqicahyo 7:f139bb3b2401 180 if (input1 == 1) *state = state_Idle;
rizqicahyo 7:f139bb3b2401 181 break;
rizqicahyo 7:f139bb3b2401 182 case state_AutoControl :
rizqicahyo 7:f139bb3b2401 183 if (input2 == 1) *state = state_Idle;
rizqicahyo 7:f139bb3b2401 184 break;
rizqicahyo 7:f139bb3b2401 185 }
rizqicahyo 7:f139bb3b2401 186 }
rizqicahyo 7:f139bb3b2401 187
rizqicahyo 7:f139bb3b2401 188 int fsmButton(int input,int *stateButton)
rizqicahyo 7:f139bb3b2401 189 {
rizqicahyo 7:f139bb3b2401 190 switch (*stateButton)
rizqicahyo 7:f139bb3b2401 191 {
rizqicahyo 7:f139bb3b2401 192 case BUTTON_offAwal :
rizqicahyo 7:f139bb3b2401 193 if (input == 1)
rizqicahyo 7:f139bb3b2401 194 *stateButton = BUTTON_onAwal;
rizqicahyo 7:f139bb3b2401 195 else
rizqicahyo 7:f139bb3b2401 196 *stateButton = BUTTON_offAwal;
rizqicahyo 7:f139bb3b2401 197 return 0;
rizqicahyo 7:f139bb3b2401 198 //break;
rizqicahyo 7:f139bb3b2401 199 case BUTTON_onAwal :
rizqicahyo 7:f139bb3b2401 200 if(input == 0)
rizqicahyo 7:f139bb3b2401 201 *stateButton = BUTTON_onEnd;
rizqicahyo 7:f139bb3b2401 202 else
rizqicahyo 7:f139bb3b2401 203 *stateButton = BUTTON_onAwal;
rizqicahyo 7:f139bb3b2401 204 return 0;
rizqicahyo 7:f139bb3b2401 205 //break;
rizqicahyo 7:f139bb3b2401 206 case BUTTON_onEnd :
rizqicahyo 7:f139bb3b2401 207 *stateButton = BUTTON_offAwal;
rizqicahyo 7:f139bb3b2401 208 return 1;
rizqicahyo 7:f139bb3b2401 209 //break;
rizqicahyo 7:f139bb3b2401 210 }
rizqicahyo 7:f139bb3b2401 211 }
rizqicahyo 7:f139bb3b2401 212
rizqicahyo 7:f139bb3b2401 213 void lcd_out(){
rizqicahyo 7:f139bb3b2401 214 lcd.locate(0,0);
rizqicahyo 7:f139bb3b2401 215 lcd.printf("Sistem Pergerakan");
rizqicahyo 7:f139bb3b2401 216 lcd.locate(0,1);
rizqicahyo 7:f139bb3b2401 217 lcd.printf("Objek dengan 3 Roda");
rizqicahyo 7:f139bb3b2401 218 lcd.locate(0,2);
rizqicahyo 7:f139bb3b2401 219 lcd.printf("Berbasis Odometry");
rizqicahyo 7:f139bb3b2401 220 Thread::wait(1500);
rizqicahyo 7:f139bb3b2401 221
rizqicahyo 7:f139bb3b2401 222 lcd.cls();
rizqicahyo 7:f139bb3b2401 223 lcd.locate(0,0);
rizqicahyo 7:f139bb3b2401 224 lcd.printf("Bryan Christy P");
rizqicahyo 7:f139bb3b2401 225 lcd.locate(0,1);
rizqicahyo 7:f139bb3b2401 226 lcd.printf(" 13214073");
rizqicahyo 7:f139bb3b2401 227 lcd.locate(0,2);
rizqicahyo 7:f139bb3b2401 228 lcd.printf("Rizqi Cahyo Y");
rizqicahyo 7:f139bb3b2401 229 lcd.locate(0,3);
rizqicahyo 7:f139bb3b2401 230 lcd.printf(" 13214090");
rizqicahyo 7:f139bb3b2401 231 Thread::wait(1500);
be_bryan 4:cd5de3b14797 232 while (true){
rizqicahyo 7:f139bb3b2401 233 lcd.cls();
rizqicahyo 7:f139bb3b2401 234 if (state == state_ManualControl){
rizqicahyo 7:f139bb3b2401 235 lcd.locate(0,0);
rizqicahyo 7:f139bb3b2401 236 lcd.printf("Mode : Manual");
rizqicahyo 7:f139bb3b2401 237 lcd.locate(0,1);
rizqicahyo 7:f139bb3b2401 238 lcd.printf("Vx = %.2f", Vx);
rizqicahyo 7:f139bb3b2401 239 lcd.locate(0,2);
rizqicahyo 7:f139bb3b2401 240 lcd.printf("Vy = %.2f", Vy);
rizqicahyo 7:f139bb3b2401 241 lcd.locate(0,3);
rizqicahyo 7:f139bb3b2401 242 lcd.printf("W = %.2f", W*RAD_TO_DEG);
rizqicahyo 7:f139bb3b2401 243 }
rizqicahyo 7:f139bb3b2401 244 else if (state == state_AutoControl){
rizqicahyo 7:f139bb3b2401 245 lcd.locate(0,0);
rizqicahyo 7:f139bb3b2401 246 lcd.printf("Mode : Autonomous");
rizqicahyo 7:f139bb3b2401 247 lcd.locate(0,1);
rizqicahyo 7:f139bb3b2401 248 lcd.printf("x = %.2f",x);
rizqicahyo 7:f139bb3b2401 249 lcd.locate(0,2);
rizqicahyo 7:f139bb3b2401 250 lcd.printf("y = %.2f",y);
rizqicahyo 7:f139bb3b2401 251 lcd.locate(0,3);
rizqicahyo 7:f139bb3b2401 252 lcd.printf("theta = %.2f", theta*RAD_TO_DEG);
rizqicahyo 7:f139bb3b2401 253 }
rizqicahyo 7:f139bb3b2401 254 else if(state == state_Idle){
rizqicahyo 7:f139bb3b2401 255 lcd.locate(0,0);
rizqicahyo 7:f139bb3b2401 256 lcd.printf("PAD_START");
rizqicahyo 7:f139bb3b2401 257 lcd.locate(0,1);
rizqicahyo 7:f139bb3b2401 258 lcd.printf(" => Mode Auto");
rizqicahyo 7:f139bb3b2401 259 lcd.locate(0,2);
rizqicahyo 7:f139bb3b2401 260 lcd.printf("PAD_SELECT");
rizqicahyo 7:f139bb3b2401 261 lcd.locate(0,3);
rizqicahyo 7:f139bb3b2401 262 lcd.printf(" => Mode Manual");
rizqicahyo 7:f139bb3b2401 263 }
rizqicahyo 7:f139bb3b2401 264 Thread::wait(50);
rizqicahyo 5:0b555929c5b2 265 }
rizqicahyo 5:0b555929c5b2 266 }
rizqicahyo 5:0b555929c5b2 267
rizqicahyo 5:0b555929c5b2 268 void self_localization(){
rizqicahyo 7:f139bb3b2401 269 static float x_prev = 0;
rizqicahyo 7:f139bb3b2401 270 static float y_prev = 0;
rizqicahyo 7:f139bb3b2401 271 static float theta_prev = 0;
rizqicahyo 7:f139bb3b2401 272
rizqicahyo 5:0b555929c5b2 273 while(true){
rizqicahyo 5:0b555929c5b2 274 float d1 = enc1.getPulses()*PULSE_TO_MM;
rizqicahyo 5:0b555929c5b2 275 float d2 = enc2.getPulses()*PULSE_TO_MM;
rizqicahyo 5:0b555929c5b2 276 float d3 = enc3.getPulses()*PULSE_TO_MM;
rizqicahyo 7:f139bb3b2401 277
rizqicahyo 5:0b555929c5b2 278 x = x_prev + (2*d1 - d2 - d3)/3*cos(theta_prev) - (-d2+d3)*0.5773*sin(theta_prev);
rizqicahyo 5:0b555929c5b2 279 y = y_prev + (2*d1 - d2 - d3)/3*sin(theta_prev) + (-d2+d3)*0.5773*cos(theta_prev);
rizqicahyo 5:0b555929c5b2 280 theta = theta_prev + (d1 + d2 + d3)/(3*L); // // 0.132629 => 180 / (3. L. pi)
rizqicahyo 5:0b555929c5b2 281
rizqicahyo 5:0b555929c5b2 282 Vx = (x - x_prev)/0.002;
rizqicahyo 5:0b555929c5b2 283 Vy = (y - y_prev)/0.002;
rizqicahyo 5:0b555929c5b2 284 W = (theta - theta_prev)/0.002;
rizqicahyo 5:0b555929c5b2 285
rizqicahyo 7:f139bb3b2401 286 x_prev = x;
rizqicahyo 7:f139bb3b2401 287 y_prev = y;
rizqicahyo 7:f139bb3b2401 288 theta_prev = theta;
rizqicahyo 7:f139bb3b2401 289
rizqicahyo 5:0b555929c5b2 290 enc1.reset();
rizqicahyo 5:0b555929c5b2 291 enc2.reset();
rizqicahyo 5:0b555929c5b2 292 enc3.reset();
rizqicahyo 5:0b555929c5b2 293
rizqicahyo 7:f139bb3b2401 294 if((state == state_AutoControl) && (map_data.n == 0)){
rizqicahyo 7:f139bb3b2401 295 x_prev = 0;
rizqicahyo 7:f139bb3b2401 296 y_prev = 0;
rizqicahyo 7:f139bb3b2401 297 theta_prev = 0;
rizqicahyo 7:f139bb3b2401 298 }
rizqicahyo 5:0b555929c5b2 299 Thread::wait(Ts); //frekuensi sampling = 500 Hz
rizqicahyo 5:0b555929c5b2 300 }
rizqicahyo 5:0b555929c5b2 301 }
rizqicahyo 5:0b555929c5b2 302
rizqicahyo 5:0b555929c5b2 303 void PTP_movement(){
rizqicahyo 7:f139bb3b2401 304 int i = 0;
rizqicahyo 7:f139bb3b2401 305 while(true){
rizqicahyo 7:f139bb3b2401 306 if(state == state_AutoControl){
rizqicahyo 7:f139bb3b2401 307 while(i < map_data.n){
rizqicahyo 7:f139bb3b2401 308 i += calculate_PID(map_data.x_pos[i],map_data.y_pos[i],map_data.theta_pos[i],(i==(map_data.n-1)));
rizqicahyo 7:f139bb3b2401 309 //if (i == map.n) i = 0;
rizqicahyo 7:f139bb3b2401 310 Thread::wait(Ts);
rizqicahyo 7:f139bb3b2401 311 }
rizqicahyo 6:f10c0d9f228d 312 i = 0;
rizqicahyo 6:f10c0d9f228d 313 }
rizqicahyo 7:f139bb3b2401 314 else i = 0;
rizqicahyo 5:0b555929c5b2 315 Thread::wait(Ts);
be_bryan 4:cd5de3b14797 316 }
be_bryan 4:cd5de3b14797 317 }
be_bryan 4:cd5de3b14797 318
rizqicahyo 7:f139bb3b2401 319 int calculate_PID(float x_s, float y_s, float theta_s, bool isLast){
rizqicahyo 7:f139bb3b2401 320 //error_prev & sum_error
rizqicahyo 5:0b555929c5b2 321 static float S_error_prev = 0;
rizqicahyo 5:0b555929c5b2 322 static float theta_error_prev = 0;
rizqicahyo 5:0b555929c5b2 323
rizqicahyo 5:0b555929c5b2 324 static float sum_S_error = 0;
rizqicahyo 5:0b555929c5b2 325 static float sum_theta_error = 0;
rizqicahyo 5:0b555929c5b2 326
rizqicahyo 7:f139bb3b2401 327 //menghitung error jarak x,y terhaadap xs,ys
rizqicahyo 7:f139bb3b2401 328 float S_error = sqrt((x_s-x)*(x_s-x) + (y_s-y)*(y_s-y));
rizqicahyo 5:0b555929c5b2 329 //menghitung error arah
rizqicahyo 7:f139bb3b2401 330 float theta_error = theta_s - theta*RAD_TO_DEG;
rizqicahyo 5:0b555929c5b2 331
rizqicahyo 5:0b555929c5b2 332 sum_S_error += S_error;
rizqicahyo 5:0b555929c5b2 333 sum_theta_error += theta_error;
rizqicahyo 5:0b555929c5b2 334
rizqicahyo 7:f139bb3b2401 335 float Vs = Kp_s*S_error + Ki_s*Ts*sum_S_error + Kd_s*(S_error - S_error_prev)/Ts;
rizqicahyo 5:0b555929c5b2 336 float w = Kp_w*theta_error + Ki_w*Ts*sum_theta_error + Kd_w*(theta_error - theta_error_prev)/Ts;
rizqicahyo 5:0b555929c5b2 337
rizqicahyo 7:f139bb3b2401 338 Vr = Vs/MAX_SPEED*0.5;
rizqicahyo 7:f139bb3b2401 339 Vw = w*L/MAX_W_SPEED*0.5;
rizqicahyo 7:f139bb3b2401 340 a = moving_direction(x_s,y_s,x,y,theta);
rizqicahyo 5:0b555929c5b2 341
rizqicahyo 5:0b555929c5b2 342 S_error_prev = S_error;
rizqicahyo 5:0b555929c5b2 343 theta_error_prev = theta_error;
rizqicahyo 5:0b555929c5b2 344
rizqicahyo 5:0b555929c5b2 345 if(isLast == true){
rizqicahyo 7:f139bb3b2401 346 if ((abs(x_s - x) < 20) && (abs(y_s - y) < 20)){
rizqicahyo 7:f139bb3b2401 347 Vw = 0;
rizqicahyo 7:f139bb3b2401 348 Vr = 0;
rizqicahyo 7:f139bb3b2401 349 return 1;
rizqicahyo 5:0b555929c5b2 350 }
rizqicahyo 5:0b555929c5b2 351 else return 0;
rizqicahyo 5:0b555929c5b2 352 }
rizqicahyo 5:0b555929c5b2 353 else{
rizqicahyo 7:f139bb3b2401 354 if ((abs(x_s - x) < BOUNDARY_TOLERANCE) && (abs(y_s - y) < BOUNDARY_TOLERANCE)) return 1;
rizqicahyo 5:0b555929c5b2 355 else return 0;
rizqicahyo 5:0b555929c5b2 356 }
rizqicahyo 5:0b555929c5b2 357 }
rizqicahyo 5:0b555929c5b2 358
rizqicahyo 5:0b555929c5b2 359
rizqicahyo 7:f139bb3b2401 360 float moving_direction( float x_s, float y_s, float x, float y,float theta){
rizqicahyo 7:f139bb3b2401 361 float temp = atan((y_s - y)/(x_s - x)) - theta;
rizqicahyo 5:0b555929c5b2 362
rizqicahyo 7:f139bb3b2401 363 if (x_s < x) return temp + PI;
rizqicahyo 5:0b555929c5b2 364 else return temp;
rizqicahyo 5:0b555929c5b2 365 }
rizqicahyo 5:0b555929c5b2 366
rizqicahyo 5:0b555929c5b2 367 void motor_out() {
rizqicahyo 7:f139bb3b2401 368 Thread::wait(1500);
rizqicahyo 7:f139bb3b2401 369
rizqicahyo 7:f139bb3b2401 370 while(1){
rizqicahyo 7:f139bb3b2401 371 motor1.speed(SPEED*(Vr*cos(a) + Vw));
rizqicahyo 7:f139bb3b2401 372 motor2.speed(SPEED*(Vr*(-0.5*cos(a) - 0.866*sin(a)) + Vw));
rizqicahyo 7:f139bb3b2401 373 motor3.speed(SPEED*(Vr*(-0.5*cos(a) + 0.866*sin(a)) + Vw));
rizqicahyo 7:f139bb3b2401 374
rizqicahyo 7:f139bb3b2401 375 if((Vr == 0) && (Vw == 0)){
rizqicahyo 7:f139bb3b2401 376 motor1.brake(BRAKE_HIGH);
rizqicahyo 7:f139bb3b2401 377 motor2.brake(BRAKE_HIGH);
rizqicahyo 7:f139bb3b2401 378 motor3.brake(BRAKE_HIGH);
rizqicahyo 5:0b555929c5b2 379 }
rizqicahyo 7:f139bb3b2401 380 }
rizqicahyo 5:0b555929c5b2 381 }
rizqicahyo 5:0b555929c5b2 382