ラジコンカー(5)･･仮想ジョイスティック

前回

forward　前進
back 後退
stop 停止
right 右転
left 左転
という５つのコマンドを用意しましたが、まあ、もっと滑らかにいろいろな動きをさせたい、となると、ジョイスティック使いたいですね。

方法としては、もう一組のArduinoとBluetoothモジュール、ジョイステックモジュール

でリアルのジョイステックコントローラーを組むのが一つ。

もう一つ、Androidスマホから仮想ジョイスティックのアプリを利用する手があります。自分でプログラミングしなくても便利なツールが公開されています。私はこれを使いました。

Arduino Bluetooth Controlled Joystick - Apps on Google Play

試してみます。Arduino NANOとBluetoothモジュールの載ったブレッドボードを一旦ラジコンカーから外して

このときの、LCDをつないだ構成に戻します。スケッチもこのときのそのままで。

あれ、なんか違いますね。

アプリの説明を読んでみたら（先に読めよ(^_^;)）終端文字には\nではなくて、#を使っているようです。そこスケッチを修正。

　あと、上の写真は普通の「ジョイスティックモード」で、8方向の方向キー機能だけを有効にした場合になります。

このように、Advancedの方を選ぶと、
1. Angle [ 0 - 359 ]　3桁
2. Strength [ 0 - 100 ]　3桁
3.末尾一桁は×○△□のコマンドキー
・・7桁の数字文字列で情報が得られるようです。

やってみた。

では、これをラジコンカーの２つのモーター制御にどう結び付けるか。Angleをθとおいて、
前進（θ=90°）なら、左右前進（FF）
後退（θ=270°）なら左右後退(BB)
右転（θ=0°）なら、左進右退（FB）
左転（θ=180°）なら、左退右進（BF）
はすぐわかります。あとはこれをなめらかにつなげるようにざっくり、
θ＝0°　～90°　では、左 1.0　右（-1.0～1.0 ：-cos(2θ)）
θ＝90°　～180°　では、右 1.0 左(1.0～-1.0：-cos(2θ))
θ＝180°　～270°　では、左-1.0　右（1.0～-1.0：cos(2θ)）
θ＝270°　～360°　では、右-1.0  左(-1.0～1.0：cos(2θ))

これでどうでしょうね。右車輪、左車輪の出力をGoogleスプレッドシートでグラフに描いてみましょう。

（90度、270度で）左右対称になっているし、滑らかだし、なんか、行けそうな気がする。

　と思って、やってみたらまあ、動くには動いたんですが、遊びが全然なくて、少しの変動ですぐ大きく方向が変わってしまう。すごく方向のコントロールが難しい。

　じゃあ、どう変えようかと考えて思いついたのが昔、大学の先生から聞いた「ルート法」です。試験問題が難しくて合格点とれない人が続出した時があって、本来60点で合格のところ、36点くらいでも合格にしたい。それでルート取って10倍するというやり方を使ったそうです。
　これだと、０点は0点、100点は100点で変わりませんが、36点の人は60点になるという寸法。
　同じようなことをやってみる。どれくらい違ってくるかグラフ描いてみました。
これみて、えいやで「ルート法二回で行こう」と決めました。

あと、PWM用のanalog出力は0-255ですが、遊びを設けるため、0-258にマップしたうえで、constrain関数使って、上限255で切ることにしました。

スケッチ

#define IN1 4
#define IN2 5
#define ENA 3
#define IN3 7
#define IN4 8
#define ENB 9

#include <SoftwareSerial.h>
SoftwareSerial mySerial(11, 10); //RX, TX

String receiveStr, angle, strength;
long l_angle, l_strength;
double r_pow, l_pow; //出力 -1.0から1.0で

void setup() {
 mySerial.begin(9600);
 
 pinMode(IN1, OUTPUT);
 pinMode(IN2, OUTPUT);
 pinMode(IN3, OUTPUT);
 pinMode(IN4, OUTPUT);
}

void loop() {
 if(mySerial.available() > 0){  
   receiveStr=mySerial.readStringUntil('#');
   //receiveStrとして確保
   angle = receiveStr.substring(0,3);
   l_angle = angle.toInt();
   strength =receiveStr.substring(3,6);
   l_strength = strength.toInt();
   double cos2theta;
   cos2theta = cos (2.0 * l_angle * M_PI / 180.0);
   if(l_angle<90){
     r_pow = -cos2theta;
     l_pow = 1.0;
   }else if(l_angle<180){
     r_pow = 1.0;
     l_pow = -cos2theta;      
   }else if(l_angle<270){
     r_pow = cos2theta;
     l_pow = -1.0;    
   }else{  //270～359
     r_pow = -1.0;
     l_pow = cos2theta;   
   }
   //出力とかけあわせ
   r_pow *= l_strength;
   l_pow *= l_strength;

   //Hブリッジの向き
   if (r_pow >0){
     //right_forward();
     digitalWrite(IN1,LOW);
     digitalWrite(IN2,HIGH); 
   }else{
     //right_back();
     digitalWrite(IN1,HIGH);
     digitalWrite(IN2,LOW); 
   }
   if (l_pow >0){
     //left_forward();
     digitalWrite(IN3,HIGH);
     digitalWrite(IN4,LOW); 
   }else{
     //left_back();
     digitalWrite(IN3,LOW);
     digitalWrite(IN4,HIGH);
   }
   //ルート法二回＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝
   //出力右
   analogWrite(ENA, 
   constrain(map(int(sqrt(sqrt(abs(r_pow))*10.0)*10.0),0,100,0,258),0,255));
   //出力左
   analogWrite(ENB,
   constrain(map(int(sqrt(sqrt(abs(l_pow))*10.0)*10.0),0,100,0,258),0,255));  
 }
}

仮装ジョイスティックで、ラジコンを走らせてみました。

遊べます。