ロボットの動作確認中

一応動いているようだけど手首の腕軸周りの回転が上手くいっていない。
あと、各サーボの動作確認をするためのモードが欲しいかな。いまいち動きの悪いサーボがあるので、プログラムが悪いのか、ハードが悪いのか切り分けられるようにしたい。
首回りのサーボを変える必要があるかも。首を持ちあげられない。電流が弱いのかパワーが足りないのか。