センサーの値からサーボを動かす

サーボの角度っぽいものがやっと取得出来たので、
Arduinoでセンサーの値からサーボを動かすところまでやってみます。

いつもお世話になっている建築発明工作ゼミさんのArduino サーボ制御を見ながら進めます。

私がやりたいのはこんなのです。
わかりづらい図ですが…
20131112_0000

今日やりたいのは色をつけた部分。
20131112_0000

まず、「センサーの値が一定以上になったら
サーボを指定の角度まで回転させる」
というArduinoのスケッチを書いてみます。

[cpp]
#include <Servo.h>
Servo servo;

void setup(){
Serial.begin(9600) ;

servo.attach(9, 955, 2000);
}

void loop(){
int sensor1 = analogRead(0); //0~1023

Serial.print("sensor1 = ");
Serial.println(sensor1); //sensor1の値を表示

if(sensor1 < 400){
servo.writeMicroseconds(955); //0°
}else {
servo.writeMicroseconds(1500); //180°くらい
}

delay(100);

}

[/cpp]

結果:だいたいやりたいことはできました!
ただ、サーボモータが回るのが思ったより早いので、私の思っているようなゆっくりとした回転にするにはまだ色々試す必要がありそうですが。
それは後でにします。

次に、複数のセンサーの値から動かします。
[cpp]
#include <Servo.h>
Servo servo;

void setup(){
Serial.begin(9600) ;

servo.attach(9, 955, 2000);
}

void loop(){
int sensor0 = analogRead(0); //0~1023
int sensor1 = analogRead(1); //0~1023

Serial.print(sensor0); //sensor0の値を表示
Serial.print(" , ");
Serial.println(sensor1); //sensor1の値を表示

if(sensor0 > 400 , sensor1 < 400){
servo.writeMicroseconds(1500); //180°くらい

}else if(sensor0 < 400, sensor1 > 400){
servo.writeMicroseconds(1200); //90°くらい?

}else {
servo.writeMicroseconds(955); //0°
}

delay(100);

}
[/cpp]

結果:ガクガクした。
多分スケッチはだいたい合ってるんですが、
センサーの値がおかしい…
Analog pin0, 1で読み取った値がなぜか一緒に動いてしまっています。
明日先生に聞いてみます。

さて、上のスケッチだとセンサの値によっては、
サーボが指定位置まで回転する前に戻ってしまったりして、思った挙動はしてくれません。でもどうやればいいのかわからない…
なので、友人のいのさん(プログラミングに詳しい)に考えてもらいました!!
[cpp]
#include <Servo.h>
Servo servo;
int swi,cou;

void setup(){
Serial.begin(9600) ;

servo.attach(9, 955, 2000);

swi = 0;
cou = 0;
}

void loop(){

int sensor0 = analogRead(0); //0~1023
int sensor1 = analogRead(5); //0~1023

Serial.print(sensor0); //sensor0の値を表示
Serial.print(" , ");
Serial.print(sensor1); //sensor1の値を表示
Serial.print(" , ");
Serial.println(swi);

int a = 400;

//小さい動きに反応しないようにする
if(sensor0 < 100){ //100以下は反応しない
sensor0 = 0;
}

//センサーが一定以上になったら、一度だけ回転
if(swi == 0){ //回転中でない
if (sensor0 > a) { //a以上で反応する
swi = 1;
}
if (sensor1 > a) { //a以上で反応する
swi = 2;
}

}

if(swi == 1){ //回転中
servo.writeMicroseconds(1500); //180°くらい
delay(100);//回転終了後のディレイ

//サーボ回転終了を判別したい…。
cou ++ ;
if (cou > 100){
swi = 3; //回転終了
}
}

if(swi ==2){ //回転中
servo.writeMicroseconds(955); //0°くらい
delay(100);//回転終了後のディレイ

//サーボ回転終了を判別したい…。
cou ++ ;
if (cou > 100){
swi = 3; //回転終了
}
}

if(swi == 3){ //回転終了中
if (sensor0 < a && sensor1 < a) { //a以下で回転終了
delay(100);
swi = 0;
}
}

}
[/cpp]

センサーの値が正しく取れれば、これでオッケー…のはず!

参考
if文 Arduino 日本語リファレンス