![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
PID + multiple point
Dependencies: Motor PS_PAD TextLCD mbed-os
Fork of cobaLCDJoyMotor_Thread by
Revision 5:0b555929c5b2, committed 2017-12-16
- Comitter:
- rizqicahyo
- Date:
- Sat Dec 16 10:25:27 2017 +0000
- Parent:
- 4:cd5de3b14797
- Commit message:
- multiple point movement, PID
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r cd5de3b14797 -r 0b555929c5b2 main.cpp --- a/main.cpp Tue Dec 12 07:48:20 2017 +0000 +++ b/main.cpp Sat Dec 16 10:25:27 2017 +0000 @@ -8,9 +8,34 @@ #include <string> using namespace std; -Thread thread2(osPriorityNormal, OS_STACK_SIZE, NULL); +#define PI 3.14159265359 +#define RAD_TO_DEG 57.2957795131 + +#define PULSE_TO_MM 0.1177 //rev/pulse * K_lingkaran_roda +#define L 144.0 // lengan roda dari pusat robot (mm) +#define Ts 2.0 // Time Sampling sistem 2ms + +#define MAX_SPEED 912.175 //Vresultan max (mm/s) +#define MAX_W_SPEED 1314.72 //Vw max (mm/s) +#define SPEED 1 //V robot + +#define BOUNDARY_TOLERANCE 70.0 + +// konstanta PID untuk kendali Posisi (x y) +#define Kp_s 10.0 +#define Ki_s 0.0 +#define Kd_s 1.6 + +// konstanta PID untuk kendali arah (theta) +#define Kp_w 0.2 +#define Ki_w 0.0 +#define Kd_w 0.01 + Thread thread1(osPriorityNormal, OS_STACK_SIZE, NULL); +Thread thread2(osPriorityNormal, OS_STACK_SIZE, NULL); Thread thread3(osPriorityNormal, OS_STACK_SIZE, NULL); +Thread thread4(osPriorityNormal, OS_STACK_SIZE, NULL); +Thread thread5(osPriorityNormal, OS_STACK_SIZE, NULL); TextLCD lcd(PA_9, PC_3, PC_2, PA_8, PB_0, PC_15, TextLCD::LCD20x4); //rs,e,d4-d7 encoderKRAI enc1 (PC_14, PC_13, 11, encoderKRAI::X4_ENCODING); @@ -18,17 +43,41 @@ encoderKRAI enc3 (PB_10, PB_3, 11, encoderKRAI::X4_ENCODING); PS_PAD ps2(PB_15,PB_14,PB_13, PC_4); //(mosi, miso, sck, ss) - Motor motor3(PB_7, PA_14, PA_15); //motor4 - Motor motor2(PA_11, PA_6, PA_5); //motor2 - //Motor motor1(PA_10, PB_5, PB_4); //motor_griper - Motor motor1(PB_6, PA_7, PB_12); //motor 3 -string a; - +Motor motor3(PB_7, PA_14, PA_15); //motor4 +Motor motor2(PA_11, PA_6, PA_5); //motor2 +Motor motor1(PB_6, PA_7, PB_12); //motor 3 +//Motor motor1(PA_10, PB_5, PB_4); //motor_griper + +Serial pc(USBTX,USBRX); + void dataJoystick(); void lcdPrint(); -void motorP(); +void self_localization(); +void motor_out(); +void PTP_movement(); + +int calculate_PID(float *x_set, float *y_set, float *theta_set, bool isLast); +float moving_direction( float xs, float ys, float x, float y,float theta); + +/*------------buruk----------------*/ +float x = 0; +float y = 0; +float theta = 0; -Serial pc(USBTX,USBRX); +float x_prev = 0; +float y_prev = 0; +float theta_prev = 0; + +float vr = 0; +float vw = 0; +float a = 0; + +float Vx = 0; +float Vy = 0; +float W = 0; + +string str; +/*---------------------------------------*/ int main() { @@ -36,69 +85,144 @@ ps2.init(); thread1.start(dataJoystick); thread2.start(lcdPrint); - thread3.start(motorP); + thread3.start(self_localization); + thread4.start(motor_out); + thread5.start(PTP_movement); + while (1) { - // baca input -/* - ps2.poll(); - if(ps2.read(PS_PAD::PAD_X)==1) a = "silang"; - else if(ps2.read(PS_PAD::PAD_CIRCLE)==1) a = "lingkaran"; - else if(ps2.read(PS_PAD::PAD_TRIANGLE)==1) a = "segitiga"; - else if(ps2.read(PS_PAD::PAD_SQUARE)==1) a = "kotak"; - else a = "NULL"; - */ -/* - //tampilkan LCD - lcd.locate(0,0); - lcd.printf("input joystik :"); - lcd.locate(0,1); - lcd.printf("%s",a); - - wait_ms(10); - lcd.cls(); -*/ + //do nothing } } void dataJoystick(){ while(true){ ps2.poll(); - if(ps2.read(PS_PAD::PAD_X)==1) a = "silang"; - else if(ps2.read(PS_PAD::PAD_CIRCLE)==1) a = "lingkaran"; - else if(ps2.read(PS_PAD::PAD_TRIANGLE)==1) a = "segitiga"; - else if(ps2.read(PS_PAD::PAD_SQUARE)==1) a = "kotak"; - else a = "NULL"; + if(ps2.read(PS_PAD::PAD_X)==1) str = "silang"; + else if(ps2.read(PS_PAD::PAD_CIRCLE)==1) str = "lingkaran"; + else if(ps2.read(PS_PAD::PAD_TRIANGLE)==1) str = "segitiga"; + else if(ps2.read(PS_PAD::PAD_SQUARE)==1) str = "kotak"; + else str = "NULL"; } } void lcdPrint(){ while (true){ - lcd.cls(); - int pulse1 = enc1.getPulses(); - int pulse2 = enc2.getPulses(); - int pulse3 = enc3.getPulses(); - lcd.locate(0,0); - lcd.printf("input : %s", a); - lcd.locate(0,1); - lcd.printf("enc1 = %d", pulse1); - lcd.locate(0,2); - lcd.printf("enc2 = %d", pulse2); - lcd.locate(0,3); - lcd.printf("enc3 = %d", pulse3); - Thread::wait(20); + lcd.cls(); + lcd.locate(0,0); + lcd.printf("input : %s", str); + lcd.locate(0,1); + //lcd.printf("Vr = %.2f", sqrt(Vx*Vx + Vy*Vy)); + lcd.printf("x = %.2f", x); + lcd.locate(0,2); + lcd.printf("y = %.2f", y); + lcd.locate(0,3); + lcd.printf("theta = %.2f", theta*RAD_TO_DEG); + //lcd.printf("a = %.2f", a*RAD_TO_DEG); + + Thread::wait(100); + } +} + + +void self_localization(){ + while(true){ + float d1 = enc1.getPulses()*PULSE_TO_MM; + float d2 = enc2.getPulses()*PULSE_TO_MM; + float d3 = enc3.getPulses()*PULSE_TO_MM; + + x_prev = x; + y_prev = y; + theta_prev = theta; + + x = x_prev + (2*d1 - d2 - d3)/3*cos(theta_prev) - (-d2+d3)*0.5773*sin(theta_prev); + y = y_prev + (2*d1 - d2 - d3)/3*sin(theta_prev) + (-d2+d3)*0.5773*cos(theta_prev); + theta = theta_prev + (d1 + d2 + d3)/(3*L); // // 0.132629 => 180 / (3. L. pi) + + Vx = (x - x_prev)/0.002; + Vy = (y - y_prev)/0.002; + W = (theta - theta_prev)/0.002; + + enc1.reset(); + enc2.reset(); + enc3.reset(); + + Thread::wait(Ts); //frekuensi sampling = 500 Hz + } +} + +void PTP_movement(){ + //mapping lokasi + float map_x[16] = {150,300,450,600,600,600,600,600,450,300,150, 0, 0, 0, 0, 0}; + float map_y[16] = { 0, 0, 0, 0,150,300,450,600,600,600,600,600,450,300,150, 0}; + float map_theta[16] = { 0, 0, 0, 90, 90, 90, 90,180,180,180,180,270,270,270,270,360}; + + int i=0; + + while(i < 16){ + i += calculate_PID(&map_x[i],&map_y[i],&map_theta[i],false); + + if (i == 16) i = 0; + Thread::wait(Ts); } } -void motorP() { - while(true){ - motor1.speed(0.5); - motor2.speed(0.5); - motor3.speed(0.5); - Thread::wait(2000); - motor1.speed(-0.5); - motor2.speed(-0.5); - motor3.speed(-0.5); - Thread::wait(2000); - } -} \ No newline at end of file +int calculate_PID(float *x_set, float *y_set, float *theta_set, bool isLast){ + // variabel tambahan + static float S_error_prev = 0; + static float theta_error_prev = 0; + + static float sum_S_error = 0; + static float sum_theta_error = 0; + + //menghitung error posisi + float S_error = sqrt((*x_set-x)*(*x_set-x) + (*y_set-y)*(*y_set-y)); + //menghitung error arah + float theta_error = *theta_set - theta*RAD_TO_DEG; + + sum_S_error += S_error; + sum_theta_error += theta_error; + + float vs = Kp_s*S_error + Ki_s*Ts*sum_S_error + Kd_s*(S_error - S_error_prev)/Ts; + float w = Kp_w*theta_error + Ki_w*Ts*sum_theta_error + Kd_w*(theta_error - theta_error_prev)/Ts; + + vr = vs/MAX_SPEED*0.5; + vw = w*L/MAX_W_SPEED*0.5; + a = moving_direction(*x_set,*y_set,x,y,theta); + + S_error_prev = S_error; + theta_error_prev = theta_error; + + if(isLast == true){ + if ((abs(*x_set - x) < 20) && (abs(*y_set - y) < 20)){ + vw = 0; + vr = 0; + return 1; + } + else return 0; + } + else{ + if ((abs(*x_set - x) < BOUNDARY_TOLERANCE) && (abs(*y_set - y) < BOUNDARY_TOLERANCE)) return 1; + else return 0; + } +} + + +float moving_direction( float xs, float ys, float x, float y,float theta){ + float temp = atan((ys - y)/(xs - x)) - theta; + + if (xs < x) return temp + PI; + else return temp; +} + + +void motor_out() { + Thread::wait(1500); + + while(1){ + motor1.speed(SPEED*(vr*cos(a) + vw)); + motor2.speed(SPEED*(vr*(-0.5*cos(a) - 0.866*sin(a)) + vw)); + motor3.speed(SPEED*(vr*(-0.5*cos(a) + 0.866*sin(a)) + vw)); + } +} + \ No newline at end of file