///
/// VW_Type2
/// 6ch HC-05 BT serial receiver
///
#include
int ND0 = 0; //serial data 0 L_UD
int ND1 = 0; //serial data 1 L_LR
int ND2 = 0; //serial data 2 R_LR
int ND3 = 0; //serial data 3 R_UD
int ND4 = 0; //serial data 4 LSW
int ND5 = 0; //serial data 5 RSW
int PWML = 0; //左速度
float SPD = 1; //走行スピード係数
int CENTER = 95; //ステアリングトリム
int ROT; //ステアリングサーボ
float k = -0.20; //ステアリング角度範囲計数
Servo servo09; //サーボの名称を設定
void setup() {
pinMode(3, OUTPUT); //L正転(490Hz)
pinMode(5, OUTPUT); //L反転(977Hz)
pinMode(12, OUTPUT); //ライトオン
servo09.attach( 9); //サーボ割り当て
servo09.write(CENTER); //センター初期位置
Serial.begin(115200); // HC05 serial speed
Serial.write(65); //'A'
}
void loop() {
//バッファ領域に7個以上のデータがあるとき
if (Serial.available() > 6 ) {
if (Serial.read() == 63 ) { //'?'
ND0 = Serial.read(); //L_UD //L_UD
ND1 = Serial.read(); //R_UD //L_LR
ND2 = Serial.read(); //L_LR //R_LR
ND3 = Serial.read(); //R_LR //R_UD
ND4 = Serial.read(); //LSW
ND5 = Serial.read(); //RSW
// ND0をPWML, ND1をPWMRに割り当て
// NDxのセンターを90度としてPWM出力
// 112〜144を不感帯とする。
// 5:L正転、3:L反転
if (ND0 > 144 ) {
PWML = (ND0 - 128) * SPD ; //128以上はマイナス128してSPD倍に
analogWrite(3, PWML) ; //L正転
analogWrite(5, 0 ) ;
}
else if (ND0 < 112 ) {
PWML = (127 - ND0) * SPD ; //128未満は反転してSPD倍に
analogWrite(3, 0 ) ;
analogWrite(5, PWML) ; //L反転
}
else {
PWML = 0;
analogWrite(3, 0) ; //L stop
analogWrite(5, 0) ; //L stop
}
// ステアリングサーボ
//if (ND2 > 128 ) {
// ROT = CENTER + (ND2 - 128) * k ;
//}
//else if (ND2 < 128 ) {
// ROT = CENTER + (ND2 - 128) * k ;
//}
ROT = CENTER + (127 - ND2) * k ;
servo09.write(ROT);
// 2.50V(128 7Fh)A(3)ボタン判定
if (ND5 > 108 && ND5 < 148 ) {
SPD = 1 ; // normal speed
//} else {
}
// 3.33V(170 AAh)B(4)ボタン判定
if (ND5 > 150 && ND5 < 190 ) {
SPD = 2 ; // double speed
//} else {
}
// 0.83V(043 2Ah)Yボタン判定
if (ND5 > 23 && ND5 < 63 ) {
digitalWrite(12, LOW); //消灯
//} else {
}
// 1.67V(085 54h)Xボタン判定
if (ND5 > 65 && ND5 < 105 ) {
digitalWrite(12, HIGH); //点灯
//} else {
}
Serial.write(65);//合図用のデータ'A'を送る
}
}
}
