forked from: pid

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

// forked from hashito's pid
// 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;
    }
}