乱数発生関数で Tank Robot の動きに変化を付ける

何時も決められた同じような動きしかしないので、乱数発生関数を利用して、Sketchの数か所を書き直し、Tank Robotの動きに変化を付けてみた。
加えて、Tank Robotが斜め前方に進めるように、左右方向と前方、それぞれの干渉物までの距離を利用して、後方回転で方向変換する時間を調整するようにした。
乱数発生機能を使った事によって、時々意外な動きが加えられたので、見ていて、その動きに飽きなくなった。
電源のパワーが小さいので、これまでと同様に、超音波センサーで検出できない低い障害物などに捕まって、停まってしまう状況はあまり変わらない。
注記1）Sketchの中で、上記以外の新たな改善や変更も行った。
  モーターを低速で動かそうとして「HalfSpeed」という項目を作ったが、値が低いとモーターのパワーも落ちてしまうので、どれくらいの値にするのが良いか悩ましいところだ。ここでは、230としてみた。動画をアップロードしたいところだが･･･
注記2）Tank Robot が計測している示される方向と障害物迄の距離が合っていない。調べると、Sketch上で変数の型を無茶苦茶にして使っていた。いろいろな所から持ってきたり変更したりする間に、確認せず使っていた。これを見直さないとめちゃくちゃな処理になる。事実そうなっている。加えて、サーボモータを任意の角度で正しく止めるには、delayの時間を1000msは必要になる。しかし直進状態で前方との距離を計測する際には、サーボモーターは前方を向いているので、delay時間は必要無いだろう。入れることによって設定した距離で止まらず、障害物に激突してしまう。
ask_pin_F()の中に入れていた「myServo.write(angle)」を外に出して、直進状態と停まって左右を確認する場合のdelay時間を分けられるようにした。
その他、いろいろ変更したので･･･ 08/07 version は削除し、08/08 versionに置き換えた。未だ、問題点などあるかもしれないが･･･

《Sketch 2019/08/08 version》

const int PIN_SONAR_ECHO = 5; // define pin for sensor echo
const int PIN_SONAR_TRIGGER = 6; // define pin for sensor trig
#include <Servo.h>
#include <TM1637Display.h>
#define CLK 7 //can be any digital pin
#define DIO 8 //can be any digital pin
int Fdistance = 0;
float Dlh;
float Drh;
float Dfr; 
int Ls=30 ; // short range threshold 
int Ll=55 ; // long range threshold 
int angle;
int pinSRB= 9; // servo motor output pin (PWM)
int random_No4;
// next pins(10~13) are depended on L298P motorshield
int pinLB = 12; // direction control function
int pinLF = 10; // motors' control chip with speed control function from 10
int pinRB = 13; // direction control function
int pinRF = 11; // motors' control chip with speed control function
int FullSpeed = 255; // max 255
int HalfSpeed = 240; // when the number is less than 180 will be stopp
Servo myServo; // set myservo

TM1637Display display(CLK, DIO);
const uint8_t Letter_F[] = { SEG_A | SEG_E | SEG_F | SEG_G }; // F
const uint8_t Letter_b[] = { SEG_C | SEG_D | SEG_E | SEG_F | SEG_G }; // b
const uint8_t Letter_H[] = { SEG_B | SEG_C | SEG_E | SEG_F | SEG_G }; // H
const uint8_t Letter_r[] = { SEG_E | SEG_G }; // r
const uint8_t Letter_L[] = { SEG_D | SEG_E | SEG_F }; // L
const uint8_t Letter_A[] = { SEG_A | SEG_B | SEG_C | SEG_E | SEG_F | SEG_G }; // A
const uint8_t Letter_c[] = { SEG_D | SEG_E | SEG_G }; // c
const uint8_t Letter_t[] = { SEG_D | SEG_E | SEG_F | SEG_G }; // t
const uint8_t Letter_LT[] = { SEG_A | SEG_F | SEG_G }; // <
const uint8_t Letter_GT[] = { SEG_A | SEG_B | SEG_G }; // >

void setup()
{
  Serial.begin(9600);
  pinMode(PIN_SONAR_TRIGGER,OUTPUT);
  pinMode(PIN_SONAR_ECHO,INPUT);
  display.setBrightness(0x0f); // 0x01 << dark　bright >> 0x0f
  display.clear();
// Define motor output pin
  pinMode(pinLB,OUTPUT); // pin 12 direction control function
  pinMode(pinLF,OUTPUT); // pin 10 (PWM)
  pinMode(pinRB,OUTPUT); // pin 13 direction control function
  pinMode(pinRF,OUTPUT); // pin 11 (PWM)
  myServo.attach(pinSRB); // Define servo motor output pin(PWM)
}

void Advance() // move to forward
{
  display.setSegments(Letter_F,1,0);
  digitalWrite(pinLB,LOW); // left wheel moves forward
  digitalWrite(pinRB,LOW); // right wheel moves forward
  analogWrite(pinLF,FullSpeed);
  analogWrite(pinRF,FullSpeed);
}

void Back() // move backward
{
  display.clear();
  display.setSegments(Letter_b,1,1);
  display.setSegments(Letter_A,1,2);
  display.setSegments(Letter_c,1,3);
  delay(50);
  digitalWrite(pinLB,HIGH); // motor moves to left rear
  digitalWrite(pinRB,HIGH); // motor moves to right rear
  analogWrite(pinLF,FullSpeed);
  analogWrite(pinRF,FullSpeed);
}

void MidSpeed() // move to forward
{
  display.setSegments(Letter_F,1,0);
  digitalWrite(pinLB,LOW); // left wheel moves forward
  digitalWrite(pinRB,LOW); // right wheel moves forward
  analogWrite(pinLF,HalfSpeed);
  analogWrite(pinRF,HalfSpeed);
}

void Stopp() // all wheel stop
{
  display.setSegments(Letter_H,1,0);
  display.setSegments(Letter_A,1,1);
  display.setSegments(Letter_L,1,2);
  display.setSegments(Letter_t,1,3);
  digitalWrite(pinLB,HIGH); // left wheel stop
  digitalWrite(pinRB,HIGH); // right wheel stop
  analogWrite(pinLF,0);
  analogWrite(pinRF,0);
}

void BackRight() // turn right
{
  display.setSegments(Letter_r,1,0);
  display.setSegments(Letter_b,1,1);
  display.setSegments(Letter_A,1,2);
  display.setSegments(Letter_c,1,3);
  digitalWrite(pinLB,HIGH);
  digitalWrite(pinRB, LOW); // wheel on the right moves backward
  analogWrite(pinLF,FullSpeed); // from FullSpeed
  analogWrite(pinRF,FullSpeed); // from FullSpeed
}

void BackLeft() // turn left
{
  display.setSegments(Letter_L,1,0);
  display.setSegments(Letter_b,1,1);
  display.setSegments(Letter_A,1,2);
  display.setSegments(Letter_c,1,3);
  digitalWrite(pinLB, LOW); // wheel on the left moves backward
  digitalWrite(pinRB,HIGH);
  analogWrite(pinLF,FullSpeed);
  analogWrite(pinRF,FullSpeed);
}

void loop()
{
labelF:
  long random_No1 = random( 20, 120);
// --- Forward obstacle detection and moving ---
// angle = 90;
  myServo.write(90);
  delay(50);
  ask_pin_F(); // Fdistance

  if(Fdistance <= Ls)
  {
    Stopp(); // stop for a while
    goto label2;
  }
  if((Fdistance > Ls  ) & (Fdistance <= Ll ))
  {
    MidSpeed();
    goto labelF;
  }
  if(Fdistance > Ll )
  {
    Advance();
    goto labelF;
  }

label2:
// angle = 90; // Check the distance ahead again
  myServo.write(90);
  delay(50);
  ask_pin_F();
  if(Fdistance < Ls )
  {
    Back(); // go back a little
    delay(random_No1);
    Stopp();
  }
// Measure left and right space
  angle = 3;
  myServo.write(angle);
  delay(1000);
  ask_pin_F();
  Drh=Fdistance;

  angle = 177;
  myServo.write(angle);
  delay(1000);
  ask_pin_F();
  Dlh=Fdistance;

  angle =90; //added
  myServo.write(angle);
  delay(1000);
  ask_pin_F(); // check forward
  Dfr=Fdistance;

label3:
  if(Dlh > Drh)
  {
    display.setSegments(Letter_GT,1,0);
    delay(300);

      float rn1 = Dlh / Dfr ;

      if( rn1 >= 1.3 ) rn1 = 1.3 ;
      random_No4 = 400 * rn1 ;
      BackRight();
      delay(random_No4);
      Stopp();
      delay(400);
      goto labelF;

  }
  else if( Dlh <= Drh )
  {
    display.setSegments(Letter_LT,1,0);
    delay(300);

      float rn2 = Drh / Dfr ;

      if( rn2 >= 1.3 ) rn2 = 1.3 ;
      random_No4 = 400 * rn2 ;
      BackLeft();
      delay(random_No4);
      Stopp();
      delay(400);
      goto labelF;

  }
}

void ask_pin_F() // Measure the distance at the specified angle
{
  digitalWrite(PIN_SONAR_TRIGGER,LOW);// Set the trigger pin to low for 2uS
  delayMicroseconds(2);
  digitalWrite(PIN_SONAR_TRIGGER,HIGH);// Send a 10uS high to trigger ranging
  delayMicroseconds(10);
  digitalWrite(PIN_SONAR_TRIGGER,LOW);// Send pin low again
  float distance = pulseIn(PIN_SONAR_ECHO,HIGH);// Read in times pulse
  Fdistance = distance/58;// Calculate distance from time of pulse
  delay(60);
  if(( 89 < angle) & (angle < 91)) display.setSegments(Letter_F,1,0);
  if(( 0 < angle) & (angle < 35)) display.setSegments(Letter_r,1,0);
  if(( 145 < angle) & (angle < 180)) display.setSegments(Letter_L,1,0);
  display.showNumberDec(Fdistance,true, 3, 1);
  delay(300);
}