とりあえず目標の動きができるようになりました。後はログ出力です。