enc_data += (int32_t)(TIM_GetCounter(TIM4) - (TIMER_PERIOD >> 1));
x.mat[1][0] = enc_data / GEAR_REDUCTION_RATIO * 360.0 / ENCODER_PULSE;
x.mat[1][0] *= PI / 180.0;
TIM_SetCounter(TIM3, TIMER_PERIOD >> 1);
TIM_SetCounter(TIM4, TIMER_PERIOD >> 1);
close all;
clear all;
%for test
InitialValue = 0;
M = 1e20; % mass of actuator (kg)
m = 1; % mass of bag (kg)
lg = 1; % length between actuator and bag (m)
d = 20e-3; % diameter of tire (m)
r = d/2; % radius of tire (m)
Dt = 0; % damping coefficient of tire (kg.m.m/s)
Dp = 0; % damping coefficient of pendulum (kg.m.m/s)
Jt = 0; % inertia of tire (kg.m.m)
Jp = 0; % inertia of pendulum (kg.m.m)
Jm = 0; % inertia of motor (kg.m.m)
g = 9.81; % acceleration of gravity (m/s/s)
gr = 1; % gear reduction ratio (-)
eta = 1; % efficiency (%)
Km = 1; % torque constant (N.m/A)
%{
M = 1; % mass of actuator (kg)
m = 1; % mass of bag (kg)
lg = 1; % length between actuator and bag (m)
d = 20e-3; % diameter of tire (m)
r = d/2; % radius of tire (m)
Dt = 0.001; % damping coefficient of tire (kg.m.m/s)
Dp = 0.1; % damping coefficient of pendulum (kg.m.m/s)
Jt = 0; % inertia of tire (kg.m.m)
Jp = 0; % inertia of pendulum (kg.m.m)
Jm = 0; % inertia of motor (kg.m.m)
g = 9.8; % acceleration of gravity (m/s/s)
gr = 1; % gear reduction ratio (-)
eta = 1; % efficiency (%)
Km = 1; % torque constant (N.m/A)
%}
psi = pi/6; % inclination of handrail (rad)
% phi = 0;
% test
width = 0.2;
% 手すり区間
L1 = 0.250; % 手すり平面部の長さ (m)
L2 = 1.000; % 手すり傾斜部の長さ (m)
L3 = 0.250; % 手すり平面部の長さ (m)
% sectiontemp = zeros(7,1);
% sectiontemp(1) = L1 - r*tan(psi/2); % 平面に両輪が乗った状態（傾斜に差し掛かるまで）
% sectiontemp(2) = L1 - r*tan(psi/2) + width; % 傾斜に片輪が乗った状態
% sectiontemp(3) = L1 + L2 - 2*r*tan(psi/2); % 傾斜に両輪が乗った状態
% sectiontemp(4) = L1 + L2 - 2*r*tan(psi/2) + r*psi; % 角を前輪が乗り越えている状態
% sectiontemp(5) = L1 + L2 - 2*r*tan(psi/2) + r*psi + width -0.01; % 平面に片輪が乗った状態
% sectiontemp(5) = L1 + L2 - 2*r*tan(psi/2) + r*psi + sqrt(width^2-(r*tan(psi/2)*sin(psi))^2) - (1+cos(psi))*r*tan(psi/2);
section = zeros(7,1);
section(1) = L1 - r*tan(psi/2); % 平面に両輪が乗った状態（傾斜に差し掛かるまで）
section(2) = L1 - r*tan(psi/2) + width; % 傾斜に片輪が乗った状態
section(3) = section(1) + L2 - r*tan(psi/2); % 傾斜に両輪が乗った状態
section(4) = section(3) + r*psi; % 角を前輪が乗り越えている状態
section(5) = section(4) + sqrt(width^2-(r*tan(psi/2)*sin(psi))^2) - (1+cos(psi))*r*tan(psi/2); % 平面に片輪が乗った状態
% section(6) = section(5) + r*psi;
section(6) = section(4) + width; % こっちな気がする
Mmat = [(M+m)*r^2+2*Jt+Jm*gr^2 m*r*lg;m*r*lg m*lg^2+Jp];
Dmat = [Dt 0;0 Dp];
Kmat = [0 0;0 lg*m*g];
% Gmat = [m*g*sin(phi);lg*m*g*sin(phi)];
% Umat = [1;0];
%
% A = [0 0 1 0;
% 0 0 0 1;
% -Mmat\Kmat -Mmat\Dmat];
% B = [0;0;Mmat\Umat];
% C = [1 0 0 0;0 1 0 0];
% D = [0;0];
%
% Atemp = [[A;-C] zeros(6,2)];
% Btemp = [B;0;0];
%
% Q = [1 0 0 0 0 0;
% 0 1 0 0 0 0;
% 0 0 1 0 0 0;
% 0 0 0 1 0 0;
% 0 0 0 0 100 0;
% 0 0 0 0 0 1000];
%
% R = eye(1);
%
% gain = lqr(Atemp, Btemp, Q, R);
function y = fcn(x, psi, r, width, section)
if(x < section(1)) % 平面に両輪が乗った状態（傾斜に差し掛かるまで）
y = 0;
elseif(x < section(2)) % 傾斜に片輪が乗った状態
xtemp = x - section(1);
y = asin((xtemp*sin(psi))/(width));
elseif(x < section(3)) % 傾斜に両輪が乗った状態
y = psi;
elseif(x < section(4)) % 角を前輪が乗り越えている状態
xtemp = x - section(3);
y = psi - asin((1-cos(xtemp/r))*r/width);
elseif(x < section(5)) % 平面に片輪が乗った状態
xtemp = x - section(4);
y = psi - asin((r*tan(psi/2)+xtemp)*sin(psi)/width);
elseif(x < section(6)) % 角を後輪が乗り越えている状態
xtemp = x - section(5);
y = asin((1-cos(psi-xtemp/r))*r/width);
else % 平面に両輪が乗った状態
y = 0;
end
xPos = 1/s . cos(Psi) . r dTheta
yPos = 1/s . sin(Psi) . r dTheta
