PID omniwheel 3 roda
Dependencies: Motor PS_PAD TextLCD mbed-os
Fork of cobaLCDJoyMotor_Thread by
main.cpp@5:6fe4f78a6b47, 2017-12-16 (annotated)
- Committer:
- rizqicahyo
- Date:
- Sat Dec 16 13:39:24 2017 +0000
- Revision:
- 5:6fe4f78a6b47
- Parent:
- 4:cd5de3b14797
belum ada mapping lokasi;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
rizqicahyo | 1:2bf3dac65b08 | 1 | |
rizqicahyo | 0:837acb06c892 | 2 | #include "mbed.h" |
rizqicahyo | 0:837acb06c892 | 3 | #include "TextLCD.h" |
rizqicahyo | 3:a03ce2084ceb | 4 | #include "PS_PAD.h" |
be_bryan | 4:cd5de3b14797 | 5 | #include "Motor.h" |
be_bryan | 4:cd5de3b14797 | 6 | #include "encoderKRAI.h" |
rizqicahyo | 0:837acb06c892 | 7 | |
rizqicahyo | 3:a03ce2084ceb | 8 | #include <string> |
rizqicahyo | 3:a03ce2084ceb | 9 | using namespace std; |
rizqicahyo | 3:a03ce2084ceb | 10 | |
rizqicahyo | 5:6fe4f78a6b47 | 11 | #define PI 3.14159265359 |
rizqicahyo | 5:6fe4f78a6b47 | 12 | #define RAD_TO_DEG 57.2957795131 |
rizqicahyo | 5:6fe4f78a6b47 | 13 | |
rizqicahyo | 5:6fe4f78a6b47 | 14 | #define PULSE_TO_MM 0.1177 //rev/pulse * K_lingkaran_roda |
rizqicahyo | 5:6fe4f78a6b47 | 15 | #define L 144 // lengan roda dari pusat robot (mm) |
rizqicahyo | 5:6fe4f78a6b47 | 16 | #define Ts 2 // Time Sampling sistem 2ms |
rizqicahyo | 5:6fe4f78a6b47 | 17 | #define MAX_SPEED 0.3 |
rizqicahyo | 5:6fe4f78a6b47 | 18 | |
be_bryan | 4:cd5de3b14797 | 19 | Thread thread1(osPriorityNormal, OS_STACK_SIZE, NULL); |
rizqicahyo | 5:6fe4f78a6b47 | 20 | Thread thread2(osPriorityNormal, OS_STACK_SIZE, NULL); |
be_bryan | 4:cd5de3b14797 | 21 | Thread thread3(osPriorityNormal, OS_STACK_SIZE, NULL); |
rizqicahyo | 5:6fe4f78a6b47 | 22 | Thread thread4(osPriorityNormal, OS_STACK_SIZE, NULL); |
rizqicahyo | 5:6fe4f78a6b47 | 23 | Thread thread5(osPriorityNormal, OS_STACK_SIZE, NULL); |
rizqicahyo | 3:a03ce2084ceb | 24 | |
be_bryan | 4:cd5de3b14797 | 25 | TextLCD lcd(PA_9, PC_3, PC_2, PA_8, PB_0, PC_15, TextLCD::LCD20x4); //rs,e,d4-d7 |
be_bryan | 4:cd5de3b14797 | 26 | encoderKRAI enc1 (PC_14, PC_13, 11, encoderKRAI::X4_ENCODING); |
be_bryan | 4:cd5de3b14797 | 27 | encoderKRAI enc2 (PC_0, PC_1, 11, encoderKRAI::X4_ENCODING); |
be_bryan | 4:cd5de3b14797 | 28 | encoderKRAI enc3 (PB_10, PB_3, 11, encoderKRAI::X4_ENCODING); |
rizqicahyo | 3:a03ce2084ceb | 29 | PS_PAD ps2(PB_15,PB_14,PB_13, PC_4); //(mosi, miso, sck, ss) |
rizqicahyo | 3:a03ce2084ceb | 30 | |
rizqicahyo | 5:6fe4f78a6b47 | 31 | Motor motor3(PB_7, PA_14, PA_15); //motor4 |
rizqicahyo | 5:6fe4f78a6b47 | 32 | Motor motor2(PA_11, PA_6, PA_5); //motor2 |
rizqicahyo | 5:6fe4f78a6b47 | 33 | Motor motor1(PB_6, PA_7, PB_12); //motor 3 |
rizqicahyo | 5:6fe4f78a6b47 | 34 | //Motor motor1(PA_10, PB_5, PB_4); //motor_griper |
rizqicahyo | 5:6fe4f78a6b47 | 35 | |
rizqicahyo | 5:6fe4f78a6b47 | 36 | Serial pc(USBTX,USBRX); |
rizqicahyo | 5:6fe4f78a6b47 | 37 | |
be_bryan | 4:cd5de3b14797 | 38 | void dataJoystick(); |
be_bryan | 4:cd5de3b14797 | 39 | void lcdPrint(); |
rizqicahyo | 5:6fe4f78a6b47 | 40 | void self_localization(); |
be_bryan | 4:cd5de3b14797 | 41 | void motorP(); |
rizqicahyo | 5:6fe4f78a6b47 | 42 | void calculate_PID(); |
rizqicahyo | 5:6fe4f78a6b47 | 43 | |
rizqicahyo | 5:6fe4f78a6b47 | 44 | float moving_direction( float xs, float ys, float x, float y,float theta); |
rizqicahyo | 5:6fe4f78a6b47 | 45 | |
rizqicahyo | 5:6fe4f78a6b47 | 46 | |
rizqicahyo | 0:837acb06c892 | 47 | |
rizqicahyo | 5:6fe4f78a6b47 | 48 | /*------------buruk----------------*/ |
rizqicahyo | 5:6fe4f78a6b47 | 49 | float x = 0; |
rizqicahyo | 5:6fe4f78a6b47 | 50 | float y = 0; |
rizqicahyo | 5:6fe4f78a6b47 | 51 | float theta = 0; |
rizqicahyo | 5:6fe4f78a6b47 | 52 | |
rizqicahyo | 5:6fe4f78a6b47 | 53 | float x_prev = 0; |
rizqicahyo | 5:6fe4f78a6b47 | 54 | float y_prev = 0; |
rizqicahyo | 5:6fe4f78a6b47 | 55 | float theta_prev = 0; |
rizqicahyo | 5:6fe4f78a6b47 | 56 | |
rizqicahyo | 5:6fe4f78a6b47 | 57 | float vr = 0; |
rizqicahyo | 5:6fe4f78a6b47 | 58 | float w = 0; |
rizqicahyo | 5:6fe4f78a6b47 | 59 | float a = 0; |
rizqicahyo | 5:6fe4f78a6b47 | 60 | |
rizqicahyo | 5:6fe4f78a6b47 | 61 | //float Vx = 0; |
rizqicahyo | 5:6fe4f78a6b47 | 62 | //float Vy = 0; |
rizqicahyo | 5:6fe4f78a6b47 | 63 | //float W = 0; |
rizqicahyo | 5:6fe4f78a6b47 | 64 | |
rizqicahyo | 5:6fe4f78a6b47 | 65 | string str; |
rizqicahyo | 5:6fe4f78a6b47 | 66 | /*---------------------------------------*/ |
rizqicahyo | 0:837acb06c892 | 67 | |
rizqicahyo | 0:837acb06c892 | 68 | int main() |
rizqicahyo | 0:837acb06c892 | 69 | { |
rizqicahyo | 0:837acb06c892 | 70 | pc.baud(115200); |
rizqicahyo | 3:a03ce2084ceb | 71 | ps2.init(); |
be_bryan | 4:cd5de3b14797 | 72 | thread1.start(dataJoystick); |
be_bryan | 4:cd5de3b14797 | 73 | thread2.start(lcdPrint); |
rizqicahyo | 5:6fe4f78a6b47 | 74 | thread3.start(self_localization); |
rizqicahyo | 5:6fe4f78a6b47 | 75 | thread4.start(motorP); |
rizqicahyo | 5:6fe4f78a6b47 | 76 | thread5.start(calculate_PID); |
rizqicahyo | 5:6fe4f78a6b47 | 77 | |
rizqicahyo | 0:837acb06c892 | 78 | while (1) |
rizqicahyo | 0:837acb06c892 | 79 | { |
rizqicahyo | 3:a03ce2084ceb | 80 | // baca input |
be_bryan | 4:cd5de3b14797 | 81 | /* |
rizqicahyo | 3:a03ce2084ceb | 82 | ps2.poll(); |
rizqicahyo | 3:a03ce2084ceb | 83 | if(ps2.read(PS_PAD::PAD_X)==1) a = "silang"; |
rizqicahyo | 3:a03ce2084ceb | 84 | else if(ps2.read(PS_PAD::PAD_CIRCLE)==1) a = "lingkaran"; |
rizqicahyo | 3:a03ce2084ceb | 85 | else if(ps2.read(PS_PAD::PAD_TRIANGLE)==1) a = "segitiga"; |
rizqicahyo | 3:a03ce2084ceb | 86 | else if(ps2.read(PS_PAD::PAD_SQUARE)==1) a = "kotak"; |
rizqicahyo | 3:a03ce2084ceb | 87 | else a = "NULL"; |
be_bryan | 4:cd5de3b14797 | 88 | */ |
be_bryan | 4:cd5de3b14797 | 89 | /* |
rizqicahyo | 3:a03ce2084ceb | 90 | //tampilkan LCD |
rizqicahyo | 3:a03ce2084ceb | 91 | lcd.locate(0,0); |
rizqicahyo | 3:a03ce2084ceb | 92 | lcd.printf("input joystik :"); |
rizqicahyo | 3:a03ce2084ceb | 93 | lcd.locate(0,1); |
rizqicahyo | 3:a03ce2084ceb | 94 | lcd.printf("%s",a); |
rizqicahyo | 0:837acb06c892 | 95 | |
rizqicahyo | 0:837acb06c892 | 96 | wait_ms(10); |
rizqicahyo | 0:837acb06c892 | 97 | lcd.cls(); |
be_bryan | 4:cd5de3b14797 | 98 | */ |
be_bryan | 4:cd5de3b14797 | 99 | } |
be_bryan | 4:cd5de3b14797 | 100 | } |
be_bryan | 4:cd5de3b14797 | 101 | |
be_bryan | 4:cd5de3b14797 | 102 | void dataJoystick(){ |
be_bryan | 4:cd5de3b14797 | 103 | while(true){ |
be_bryan | 4:cd5de3b14797 | 104 | ps2.poll(); |
rizqicahyo | 5:6fe4f78a6b47 | 105 | if(ps2.read(PS_PAD::PAD_X)==1) str = "silang"; |
rizqicahyo | 5:6fe4f78a6b47 | 106 | else if(ps2.read(PS_PAD::PAD_CIRCLE)==1) str = "lingkaran"; |
rizqicahyo | 5:6fe4f78a6b47 | 107 | else if(ps2.read(PS_PAD::PAD_TRIANGLE)==1) str = "segitiga"; |
rizqicahyo | 5:6fe4f78a6b47 | 108 | else if(ps2.read(PS_PAD::PAD_SQUARE)==1) str = "kotak"; |
rizqicahyo | 5:6fe4f78a6b47 | 109 | else str = "NULL"; |
rizqicahyo | 0:837acb06c892 | 110 | } |
rizqicahyo | 0:837acb06c892 | 111 | } |
be_bryan | 4:cd5de3b14797 | 112 | |
be_bryan | 4:cd5de3b14797 | 113 | void lcdPrint(){ |
be_bryan | 4:cd5de3b14797 | 114 | while (true){ |
rizqicahyo | 5:6fe4f78a6b47 | 115 | lcd.cls(); |
rizqicahyo | 5:6fe4f78a6b47 | 116 | lcd.locate(0,0); |
rizqicahyo | 5:6fe4f78a6b47 | 117 | lcd.printf("input : %s", str); |
rizqicahyo | 5:6fe4f78a6b47 | 118 | lcd.locate(0,1); |
rizqicahyo | 5:6fe4f78a6b47 | 119 | lcd.printf("x = %.2f", x); |
rizqicahyo | 5:6fe4f78a6b47 | 120 | lcd.locate(0,2); |
rizqicahyo | 5:6fe4f78a6b47 | 121 | lcd.printf("y = %.2f", y); |
rizqicahyo | 5:6fe4f78a6b47 | 122 | lcd.locate(0,3); |
rizqicahyo | 5:6fe4f78a6b47 | 123 | //lcd.printf("theta = %.2f", theta*RAD_TO_DEG); |
rizqicahyo | 5:6fe4f78a6b47 | 124 | lcd.printf("a = %.2f", a*RAD_TO_DEG); |
rizqicahyo | 5:6fe4f78a6b47 | 125 | Thread::wait(100); |
rizqicahyo | 5:6fe4f78a6b47 | 126 | } |
rizqicahyo | 5:6fe4f78a6b47 | 127 | } |
rizqicahyo | 5:6fe4f78a6b47 | 128 | |
rizqicahyo | 5:6fe4f78a6b47 | 129 | |
rizqicahyo | 5:6fe4f78a6b47 | 130 | void self_localization(){ |
rizqicahyo | 5:6fe4f78a6b47 | 131 | while(true){ |
rizqicahyo | 5:6fe4f78a6b47 | 132 | float d1 = enc1.getPulses()*PULSE_TO_MM; |
rizqicahyo | 5:6fe4f78a6b47 | 133 | float d2 = enc2.getPulses()*PULSE_TO_MM; |
rizqicahyo | 5:6fe4f78a6b47 | 134 | float d3 = enc3.getPulses()*PULSE_TO_MM; |
rizqicahyo | 5:6fe4f78a6b47 | 135 | |
rizqicahyo | 5:6fe4f78a6b47 | 136 | x_prev = x; |
rizqicahyo | 5:6fe4f78a6b47 | 137 | y_prev = y; |
rizqicahyo | 5:6fe4f78a6b47 | 138 | theta_prev = theta; |
rizqicahyo | 5:6fe4f78a6b47 | 139 | |
rizqicahyo | 5:6fe4f78a6b47 | 140 | x = x_prev + (2*d1 - d2 - d3)/3*cos(theta_prev) - (-d2+d3)*0.5773*sin(theta_prev); |
rizqicahyo | 5:6fe4f78a6b47 | 141 | y = y_prev + (2*d1 - d2 - d3)/3*sin(theta_prev) + (-d2+d3)*0.5773*cos(theta_prev); |
rizqicahyo | 5:6fe4f78a6b47 | 142 | theta = theta_prev + (d1 + d2 + d3)/(3*L); // // 0.132629 => 180 / (3. L. pi) |
rizqicahyo | 5:6fe4f78a6b47 | 143 | |
rizqicahyo | 5:6fe4f78a6b47 | 144 | //Vx = (x - x_prev)/0.002; |
rizqicahyo | 5:6fe4f78a6b47 | 145 | //Vy = (y - y_prev)/0.002; |
rizqicahyo | 5:6fe4f78a6b47 | 146 | //W = (theta - theta_prev)/0.002; |
rizqicahyo | 5:6fe4f78a6b47 | 147 | |
rizqicahyo | 5:6fe4f78a6b47 | 148 | enc1.reset(); |
rizqicahyo | 5:6fe4f78a6b47 | 149 | enc2.reset(); |
rizqicahyo | 5:6fe4f78a6b47 | 150 | enc3.reset(); |
rizqicahyo | 5:6fe4f78a6b47 | 151 | |
rizqicahyo | 5:6fe4f78a6b47 | 152 | Thread::wait(Ts); //frekuensi sampling = 500 Hz |
be_bryan | 4:cd5de3b14797 | 153 | } |
be_bryan | 4:cd5de3b14797 | 154 | } |
be_bryan | 4:cd5de3b14797 | 155 | |
rizqicahyo | 5:6fe4f78a6b47 | 156 | |
rizqicahyo | 5:6fe4f78a6b47 | 157 | void calculate_PID(){ |
rizqicahyo | 5:6fe4f78a6b47 | 158 | // konstanta PID untuk kendali Posisi (x y) |
rizqicahyo | 5:6fe4f78a6b47 | 159 | float Kp_s = 0.05; |
rizqicahyo | 5:6fe4f78a6b47 | 160 | float Ki_s = 0; |
rizqicahyo | 5:6fe4f78a6b47 | 161 | float Kd_s = 0.5; |
rizqicahyo | 5:6fe4f78a6b47 | 162 | |
rizqicahyo | 5:6fe4f78a6b47 | 163 | // konstanta PID untuk kendali arah (theta) |
rizqicahyo | 5:6fe4f78a6b47 | 164 | float Kp_w = 0.12; |
rizqicahyo | 5:6fe4f78a6b47 | 165 | float Ki_w = 0.0; |
rizqicahyo | 5:6fe4f78a6b47 | 166 | float Kd_w = 0.4; |
rizqicahyo | 5:6fe4f78a6b47 | 167 | |
rizqicahyo | 5:6fe4f78a6b47 | 168 | // setpoint sistem |
rizqicahyo | 5:6fe4f78a6b47 | 169 | float x_set = 400; |
rizqicahyo | 5:6fe4f78a6b47 | 170 | float y_set = 400; |
rizqicahyo | 5:6fe4f78a6b47 | 171 | float theta_set = 90; |
rizqicahyo | 5:6fe4f78a6b47 | 172 | float S_set = sqrt(x_set*x_set + y_set*y_set); |
rizqicahyo | 5:6fe4f78a6b47 | 173 | |
rizqicahyo | 5:6fe4f78a6b47 | 174 | // variabel tambahan |
rizqicahyo | 5:6fe4f78a6b47 | 175 | float S_error_prev = 0; |
rizqicahyo | 5:6fe4f78a6b47 | 176 | float theta_error_prev = 0; |
rizqicahyo | 5:6fe4f78a6b47 | 177 | |
rizqicahyo | 5:6fe4f78a6b47 | 178 | float sum_S_error = 0; |
rizqicahyo | 5:6fe4f78a6b47 | 179 | float sum_theta_error = 0; |
rizqicahyo | 5:6fe4f78a6b47 | 180 | |
rizqicahyo | 5:6fe4f78a6b47 | 181 | while(true){ |
rizqicahyo | 5:6fe4f78a6b47 | 182 | //menghitung error posisi |
rizqicahyo | 5:6fe4f78a6b47 | 183 | //float S = sqrt(x*x + y*y); |
rizqicahyo | 5:6fe4f78a6b47 | 184 | //float S_error = S_set - S; |
rizqicahyo | 5:6fe4f78a6b47 | 185 | float S_error = sqrt((x_set-x)*(x_set-x) + (y_set-y)*(y_set-y)); |
rizqicahyo | 5:6fe4f78a6b47 | 186 | //menghitung error arah |
rizqicahyo | 5:6fe4f78a6b47 | 187 | float theta_error = theta_set - theta*RAD_TO_DEG; |
rizqicahyo | 5:6fe4f78a6b47 | 188 | |
rizqicahyo | 5:6fe4f78a6b47 | 189 | sum_S_error += S_error; |
rizqicahyo | 5:6fe4f78a6b47 | 190 | sum_theta_error += theta_error; |
rizqicahyo | 5:6fe4f78a6b47 | 191 | |
rizqicahyo | 5:6fe4f78a6b47 | 192 | vr = Kp_s*S_error + Ki_s*Ts*sum_S_error + Kd_s*(S_error - S_error_prev)/Ts; |
rizqicahyo | 5:6fe4f78a6b47 | 193 | w = Kp_w*theta_error + Ki_w*Ts*sum_theta_error + Kd_w*(theta_error - theta_error_prev)/Ts; |
rizqicahyo | 5:6fe4f78a6b47 | 194 | |
rizqicahyo | 5:6fe4f78a6b47 | 195 | a = moving_direction(x_set,y_set,x,y,theta); |
rizqicahyo | 5:6fe4f78a6b47 | 196 | |
rizqicahyo | 5:6fe4f78a6b47 | 197 | S_error_prev = S_error; |
rizqicahyo | 5:6fe4f78a6b47 | 198 | theta_error_prev = theta_error; |
rizqicahyo | 5:6fe4f78a6b47 | 199 | |
rizqicahyo | 5:6fe4f78a6b47 | 200 | Thread::wait(Ts); |
rizqicahyo | 5:6fe4f78a6b47 | 201 | } |
rizqicahyo | 5:6fe4f78a6b47 | 202 | } |
rizqicahyo | 5:6fe4f78a6b47 | 203 | |
rizqicahyo | 5:6fe4f78a6b47 | 204 | |
rizqicahyo | 5:6fe4f78a6b47 | 205 | float moving_direction( float xs, float ys, float x, float y,float theta){ |
rizqicahyo | 5:6fe4f78a6b47 | 206 | float temp = atan((ys - y)/(xs - x)) - theta; |
rizqicahyo | 5:6fe4f78a6b47 | 207 | |
rizqicahyo | 5:6fe4f78a6b47 | 208 | if (xs < x) return temp + PI; |
rizqicahyo | 5:6fe4f78a6b47 | 209 | else return temp; |
rizqicahyo | 5:6fe4f78a6b47 | 210 | } |
rizqicahyo | 5:6fe4f78a6b47 | 211 | |
rizqicahyo | 5:6fe4f78a6b47 | 212 | |
rizqicahyo | 5:6fe4f78a6b47 | 213 | |
rizqicahyo | 5:6fe4f78a6b47 | 214 | void motorP() { |
rizqicahyo | 5:6fe4f78a6b47 | 215 | Thread::wait(1500); |
rizqicahyo | 5:6fe4f78a6b47 | 216 | |
rizqicahyo | 5:6fe4f78a6b47 | 217 | while(1){ |
rizqicahyo | 5:6fe4f78a6b47 | 218 | motor1.speed(MAX_SPEED*(vr*cos(a) + w)); |
rizqicahyo | 5:6fe4f78a6b47 | 219 | motor2.speed(MAX_SPEED*(vr*(-0.5*cos(a) - 0.866*sin(a)) + w)); |
rizqicahyo | 5:6fe4f78a6b47 | 220 | motor3.speed(MAX_SPEED*(vr*(-0.5*cos(a) + 0.866*sin(a)) + w)); |
rizqicahyo | 5:6fe4f78a6b47 | 221 | } |
rizqicahyo | 5:6fe4f78a6b47 | 222 | |
rizqicahyo | 5:6fe4f78a6b47 | 223 | motor1.speed(0); |
rizqicahyo | 5:6fe4f78a6b47 | 224 | motor2.speed(0); |
rizqicahyo | 5:6fe4f78a6b47 | 225 | motor3.speed(0); |
rizqicahyo | 5:6fe4f78a6b47 | 226 | } |
rizqicahyo | 5:6fe4f78a6b47 | 227 |