EL4121 Embedded System / Mbed OS odometry_omni_3roda_v3

PID + multiple point

Dependencies:   Motor PS_PAD TextLCD mbed-os

Fork of cobaLCDJoyMotor_Thread by EL4121 Embedded System

main.cpp@5:0b555929c5b2, 2017-12-16 (annotated)

Committer:
rizqicahyo
Date:
Sat Dec 16 10:25:27 2017 +0000
Revision:
5:0b555929c5b2
Parent:
4:cd5de3b14797 
multiple point movement, PID

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