EL4121 Embedded System / Mbed OS odometry_omni_3roda_v1

PID omniwheel 3 roda

Dependencies:   Motor PS_PAD TextLCD mbed-os

Fork of cobaLCDJoyMotor_Thread by EL4121 Embedded System

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?

UserRevisionLine numberNew 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