<!--
倒立振子の制御シミュレーション
参考：
https://developer.mozilla.org/ja/docs/Web/API/Canvas_API
https://qiita.com/kyrieleison/items/a3ebf7c55295c3e7d8f0
https://sbfl.net/blog/2017/08/21/javascript-canvas-animation/#i
https://myenigma.hatenablog.com/entry/2017/06/26/113719
-->
<!DOCTYPE html>
<html lang="jp" dir="ltr">
<head>
<meta charset="utf-8">
<title>Inversed Pendulum</title>
<script src="https://cdn.jsdelivr.net/gh/nicolaspanel/[email protected]/dist/numjs.min.js"></script>
</head>
<body>
<script type="text/javascript">
const canvas_width = 640;
const canvas_height = 400;
const canvas = document.createElement('canvas');
canvas.width = canvas_width;
canvas.height = canvas_height;
var context = canvas.getContext('2d');
document.body.appendChild(canvas);
function setup() {
context.fillRect(0, canvas_height-10, canvas_width, 10);
drawGrid();
}
function drawTruck(){
context.beginPath();
context.fillStyle = '#808080';
context.arc(-20, canvas_height-30, circle.r, 0, 2 * Math.PI);
context.arc( 20, canvas_height-30, circle.r, 0, 2 * Math.PI);
context.fill();
context.fillStyle = 'rgb(255, 0, 0)';
context.fillRect(rectangle.x, rectangle.y, rectangle.width, rectangle.height);
}
function drawArm(state){
//state.get(0,2) = 30*Math.PI/180;
dx_theta = 120*Math.sin(state.get(0,2));
dy_theta = 120*Math.cos(state.get(0,2));
context.translate(dx_theta, -70+canvas_height-dy_theta);
context.rotate( state.get(0,2));
context.beginPath();
context.fillStyle = 'rgb(0, 255, 0)';
context.arc(circle.x, circle.y, circle.r, 0, 2 * Math.PI);
context.fill();
context.fillStyle = 'rgb(0, 0, 255)';
context.fillRect(arm.x, arm.y, arm.width, arm.height);
context.rotate( -state.get(0,2));
}
function drawGrid(){
for (var i = 1; i < 10; i++){
var x = canvas_width / 10 * i;
if(i==5){ //中心線だけ太くする
context.lineWidth = 1.0;
} else {
context.lineWidth = 0.1;
}
context.beginPath();
context.moveTo(x,0);
context.lineTo(x,canvas_height);
context.closePath();
context.stroke();
}
}
var circle = {x:0, y:0, r:20};
var rectangle = {x:-50, y:canvas_height-80, width:100, height:60};
var arm = {x:-10, y:20, width:20, height:100};
const l_bar = 2.0
const M = 1.0
const m = 0.3
const g = 9.8
var A = nj.array([
[0.0, 1.0, 0.0, 0.0],
[0.0, 0.0, -m * g / (M+2*m), 0.0],
[0.0, 0.0, 0.0, 1.0],
[0.0, 0.0, g * (M + m) / (l_bar * (M+2*m)), 0.0]
]);
var B = nj.array([
[0.0],
[1.0 / (M+2*m)],
[0.0],
[1.0 / (l_bar * (M+2*m))]
]);
var Bu = nj.array([[0.0], [0.0], [0.0], [0.0]]);
var Ax = nj.array([[0.0], [0.0], [0.0], [0.0]]);
var state = nj.array([[0.0], [0.0], [0.2], [0.0]]);
var x_dot = nj.array([[0.0], [0.0], [0.0], [0.0]]);
var integrated = nj.array([[0.0], [0.0], [0.0], [0.0]]);
function loop(timestamp) {
// 制御入力とシステム更新
integrated = integrated.add(state);
const u =
+1.0 * state.get(0,0)
+0.2 * state.get(0,1)
-100.0 * state.get(0,2)
-10.0 * state.get(0,3);
console.log(u);
Bu = B.multiply(u);
Ax = A.dot(state);
x_dot = Ax.add(Bu);
state = state.add(x_dot.divide(60));
// canvasの初期化
context.clearRect(0, 0, canvas_width, canvas_height);
setup();
context.font = "italic bold 20px sans-serif";
txtinfo = (timestamp/1000).toFixed( 2 ) + '[s]';
context.fillText(txtinfo, 10, 50);
txtinfo = state.get(0,0).toFixed( 2 ) + '[m]';
context.fillText(txtinfo, 10, 70);
txtinfo = state.get(0,1).toFixed( 2 ) + '[m/s]';
context.fillText(txtinfo, 10, 90);
txtinfo = (state.get(0,2) * 180 / Math.PI).toFixed( 2 ) + '[deg]';
context.fillText(txtinfo, 10, 110);
txtinfo = (state.get(0,3) * 180 / Math.PI).toFixed( 2 ) + '[deg/s]';
context.fillText(txtinfo, 10, 130);
context.translate(320+state.get(0,0)*canvas_width/10, 0);
drawTruck();
drawArm(state);
context.translate(-320 -state.get(0,0)*canvas_width/10 -dx_theta, +70-(canvas_height-dy_theta));
window.requestAnimationFrame((ts) => loop(ts));
}
setup();
window.requestAnimationFrame((ts) => loop(ts));
</script>
</body>
</html>