pid

/**
 * Copyright hashito ( http://wonderfl.net/user/hashito )
 * MIT License ( http://www.opensource.org/licenses/mit-license.php )
 * Downloaded from: http://wonderfl.net/c/egHW
 */

// MainStage
package {
	import flash.display.Sprite;
	[SWF(frameRate=60,width=456,height=456)]
	public class MainStage extends Sprite {
		public function MainStage(){
			addChild(new Main());
		}
	}
}

import flash.display.*;
import flash.filters.BlurFilter;
import flash.geom.Point;
import flash.geom.Matrix;
import flash.geom.ColorTransform;
import flash.events.*;
import flash.utils.Timer;
import flash.text.TextField;
import flash.text.TextFormat;
import net.hires.debug.Stats;

class debug{public static var out:TextField=new TextField();}
//
/*
	t.hashito
*/
class MF{
	public static function AFFORD (x:Number,aim:Number,a:Number):Boolean{return (x<=(a+aim))&&(x>=(a-aim));}
	public static function RAN (n:Number=1):Number{return (Math.random()*n);}
	public static function ISPM (n:Number):Number{if(n<0){return -1;}else if(n>0){return 1;}return 0;}
	public static function MAX (n:Number,max:Number):Number{return (n>max)?max:n;}
	public static function MIN (n:Number,min:Number):Number{return (n<min)?min:n;}
	public static function LIMIT (n:Number,max:Number,min:Number):Number{if(n>max){return max;}else if(n<min){return min;}else{return n};}
	public static function RAD_ANG(n:Number):Number{return n*180/Math.PI}
	public static function ANG_RAD(n:Number):Number{return n*Math.PI/180 }
}
/*
	pid制御作ったつもりだけど・・・・間違ってたら教えて！
*/
class Main extends Sprite{
	public const H:Number = 456,W:Number = 456;
	public var c:Sprite=new Sprite();
	public var xpid:Pid=new Pid(1.3,0.003,0.001),ypid:Pid=new Pid(1.3,0.003,0.001);
	public var oldt:Date;
	// public static var debugout:TextField=new TextField();
	//Constructor
	public function Main() {addEventListener(Event.ADDED_TO_STAGE, init);}
	//init
	public function init(e:*):void{
		
		c.graphics.beginFill(MF.RAN(0xffffff));
		c.graphics.drawCircle(0,0,10);
		c.x=W/2;c.y=H/2;
		addChild(c);
		oldt=new Date
		// debug
		// addChild(debugout);
		// debugout.autoSize = flash.text.TextFieldAutoSize.LEFT;
		
		addEventListener(Event.ENTER_FRAME ,update);
		//stage.addEventListener(MouseEvent.MOUSE_DOWN ,mousedown);
	}
	public function update(e:Event=null):void{
		var now:Date =new Date(),t:Number;
		t = (now.getTime()- oldt.getTime())*0.001;
		oldt=now;
		
		c.x+=xpid.update(t , c.x, mouseX);
		c.y+=ypid.update(t , c.y, mouseY);
	}
}
class Pid extends Sprite{
	public var kp:Number, ki:Number, kd:Number;
	public var i:Number=0;
	public var old:Number=0;
	public function Pid(kp:Number=1,ki:Number=0.1,kd:Number=0){
		this.kp=kp;this.ki=ki;this.kd=kd;
	}
	public function update(t:Number,v:Number,tg:Number):Number{
		var ret:Number=0,d:Number;
		d=(tg-v);
		d=(d==0)?0:d*t;
		// p
		ret  = d * kp;
		// i
		i   += d * ki;
		ret += i;
		// d
		ret +=(d - old)* kd;
		
		
		//Main.debugout.text="t="+t+" p="+( d*kp )+" i="+i +" d="+((d - old)* kd )+" ret="+(ret);
		
		
		return ret;
	}
}