Skip to content

Instantly share code, notes, and snippets.

@ide-an

ide-an/main.js

Created October 14, 2014 07:45
Show Gist options
  • Save ide-an/29f05e03943a597f88d2 to your computer and use it in GitHub Desktop.
Save ide-an/29f05e03943a597f88d2 to your computer and use it in GitHub Desktop.
Download ZIP
PID control example(マウスカーソルの追跡)
Raw
main.js
$(function(){
var canvas = $("#cv");
var ctx = canvas.get(0).getContext("2d");
var mouse_x = 0;
var mouse_y = 0;
var is_stopped = false;
canvas.mousemove(function(e){
var offset = canvas.offset();
mouse_x = e.pageX - offset.left;
mouse_y = e.pageY - offset.top;
});
function clampAngle(angle){
if (angle <= - Math.PI) {
return clampAngle(angle + 2*Math.PI);
} else if (Math.PI < angle) {
return clampAngle(angle - 2*Math.PI);
}
return angle;
}
var dt = 10;//msec
function setPoint(){
var dx = mouse_x - dot.x;
var dy = mouse_y - dot.y;
return Math.atan2(dy, dx);
}
var dot = {
x: 100,
y: 100,
speed: 5,
angle: 0,
work: function(y){
this.angle += y;
//this.angle = clampAngle(this.angle);
this.x += this.speed * Math.cos(this.angle);
this.y += this.speed * Math.sin(this.angle);
return this.angle;
},
draw: function(ctx){
ctx.save();
ctx.strokeStyle = "#000000";
ctx.beginPath();
ctx.translate(this.x, this.y);
ctx.rotate(clampAngle(this.angle));
ctx.moveTo(0, 10);
ctx.lineTo(20, 0);
ctx.lineTo(0, -10);
ctx.closePath();
ctx.stroke();
ctx.restore();
}
};
var controler = {
work: function(e){
var d = e - this.prev;
this.integral += e;
this.prev = e;
return this.kp * e + this.ki * this.integral + this.kd * d;
},
prev: 0,
integral: 0,
kp: 0.07,
kd: 0.0,
ki: 0
};
var u = 0;
function controlLoop(){
var r = setPoint();
var e = clampAngle(r - u);
var y = controler.work(e);
u = dot.work(y);
console.log(r,e,y,u)
}
function draw(){
ctx.clearRect(0, 0, 1000, 800);
dot.draw(ctx);
}
(function loop(){
if (!is_stopped) {
controlLoop();
}
draw();
setTimeout(loop, dt);
})();
["kp","ki","kd"].forEach(function(v,i){
$("#"+v).on("change", function(e){
var val = $(this).val();
controler[v] = val;
$("#exact-"+v).text(val);
}).val(controler[v]);
$("#exact-"+v).text(controler[v]);
});
$("#toggle-play").click(function(){
if (is_stopped) {
$(this).val("stop");
} else {
$(this).val("play");
}
is_stopped = !is_stopped;
});
});
Raw
test.html
<!doctype html >
<html>
<head>
<meta charset="utf-8"/>
<title>pid control</title>
<script type="text/javascript" src="http://code.jquery.com/jquery-2.1.1.min.js"></script>
<script type="text/javascript" src="main.js"></script>
<style>
#pid-params {
position: absolute;
top: 0;
right: 0;
}
</style>
</head>
<body>
<canvas id="cv" width="1000" height="800"></canvas>
<div id="pid-params">
<label for="kp">K_p</label>: <span id="exact-kp"></span><br/><input type="range" id="kp" min="0" max="2" step="0.01"/><br/>
<label for="ki">K_i</label>: <span id="exact-ki"></span><br/><input type="range" id="ki" min="0" max="2" step="0.01"/><br/>
<label for="kd">K_d</label>: <span id="exact-kd"></span><br/><input type="range" id="kd" min="0" max="2" step="0.01"/><br/>
<input type="button" value="stop" id="toggle-play"/>
</div>
</body>
</html>
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment