PID omniwheel 3 roda
Dependencies: Motor PS_PAD TextLCD mbed-os
Fork of cobaLCDJoyMotor_Thread by
main.cpp
- Committer:
- rizqicahyo
- Date:
- 2017-12-16
- Revision:
- 5:6fe4f78a6b47
- Parent:
- 4:cd5de3b14797
File content as of revision 5:6fe4f78a6b47:
#include "mbed.h" #include "TextLCD.h" #include "PS_PAD.h" #include "Motor.h" #include "encoderKRAI.h" #include <string> using namespace std; #define PI 3.14159265359 #define RAD_TO_DEG 57.2957795131 #define PULSE_TO_MM 0.1177 //rev/pulse * K_lingkaran_roda #define L 144 // lengan roda dari pusat robot (mm) #define Ts 2 // Time Sampling sistem 2ms #define MAX_SPEED 0.3 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); encoderKRAI enc2 (PC_0, PC_1, 11, encoderKRAI::X4_ENCODING); 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(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 self_localization(); void motorP(); void calculate_PID(); float moving_direction( float xs, float ys, float x, float y,float theta); /*------------buruk----------------*/ float x = 0; float y = 0; float theta = 0; float x_prev = 0; float y_prev = 0; float theta_prev = 0; float vr = 0; float w = 0; float a = 0; //float Vx = 0; //float Vy = 0; //float W = 0; string str; /*---------------------------------------*/ int main() { pc.baud(115200); ps2.init(); thread1.start(dataJoystick); thread2.start(lcdPrint); thread3.start(self_localization); thread4.start(motorP); thread5.start(calculate_PID); 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(); */ } } void dataJoystick(){ while(true){ ps2.poll(); 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(); lcd.locate(0,0); lcd.printf("input : %s", str); lcd.locate(0,1); 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 calculate_PID(){ // konstanta PID untuk kendali Posisi (x y) float Kp_s = 0.05; float Ki_s = 0; float Kd_s = 0.5; // konstanta PID untuk kendali arah (theta) float Kp_w = 0.12; float Ki_w = 0.0; float Kd_w = 0.4; // setpoint sistem float x_set = 400; float y_set = 400; float theta_set = 90; float S_set = sqrt(x_set*x_set + y_set*y_set); // variabel tambahan float S_error_prev = 0; float theta_error_prev = 0; float sum_S_error = 0; float sum_theta_error = 0; while(true){ //menghitung error posisi //float S = sqrt(x*x + y*y); //float S_error = S_set - S; 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; vr = Kp_s*S_error + Ki_s*Ts*sum_S_error + Kd_s*(S_error - S_error_prev)/Ts; w = Kp_w*theta_error + Ki_w*Ts*sum_theta_error + Kd_w*(theta_error - theta_error_prev)/Ts; a = moving_direction(x_set,y_set,x,y,theta); S_error_prev = S_error; theta_error_prev = theta_error; Thread::wait(Ts); } } 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 motorP() { Thread::wait(1500); while(1){ motor1.speed(MAX_SPEED*(vr*cos(a) + w)); motor2.speed(MAX_SPEED*(vr*(-0.5*cos(a) - 0.866*sin(a)) + w)); motor3.speed(MAX_SPEED*(vr*(-0.5*cos(a) + 0.866*sin(a)) + w)); } motor1.speed(0); motor2.speed(0); motor3.speed(0); }