Arduino Tank Robot Sketch修正

Tank Robotを動かして、その挙動を見ていると、いろいろ気になるところが出てくる。間違えてプログラムを書いていたところもある。
こうすれば、もっと無駄な動きが少なくなるだろうと思って複雑にしてゆくと、逆に思わぬところでフリーズしてしまう。
それでプログラムは、なるべくシンプルにして･･･　超音波距離測定も、ライブラリーを使っていたが、計測距離が大きく表示されるので、それらの課題も入れて、少し直してみた。
動かしている際の問題点として、下記のようなものがある。
　①センサーの高さが高いので、高さの低い障害物との距離を計測出来ない。
　②平面を持った障害物に対して、棒や細い柱のようなものは上手く補足できない。
　③クローラーの高さに近い高さの窪みに嵌り込んで、動けなくなる。
　④段差がある場所や、ちょっと太めの電線ケーブル等を跨いでしまうと、動かなくなる場合がある。
　⑤摩擦係数の低い床では、クローラーが滑ってスケートでもしているような動きになる。
　⑥超音波センサーの高さを床面から50mmぐらいの高さで設置できれば、上記の課題の幾つかは解消できるかもしれない。

《Sketch 2019/8/6 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
double Fdistance = 0;
double Dlh;
double Drh;
double Dfr;
double Ls = 30; // short range threshold
double Ll = 55; // long range threshold
int angle;
int pinSRB= 9; // servo motor output pin (PWM)

// 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 = 200; // 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:
// --- Forward obstacle detection and moving ---
  angle = 90;
  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
  ask_pin_F();
  if(Fdistance < Ls )
  {
    Back(); // go back a little
    if(Fdistance < 30 ) delay( 30);
    if(Fdistance < 20 ) delay( 60);
    if(Fdistance < 10 ) delay(120);
  Stopp();
  }
// Measure left and right space
  angle = 3;
  ask_pin_F();
  Drh=Fdistance;
  delay(700);
  angle = 177;
  ask_pin_F();
  Dlh=Fdistance;
  delay(700);
  angle =90; //added
  ask_pin_F(); // check forward
  Dfr=Fdistance;
  delay(700);

if((Dfr > Dlh) & (Dfr > Drh)) goto labelF; // added
if(Dlh < Drh)
{
  display.setSegments(Letter_LT,1,0);
  delay(1000);
  BackLeft();
  delay(600);
  Stopp();
  delay(400);
  goto labelF;
}
//
else
{
  display.setSegments(Letter_GT,1,0);
  delay(1000);
  BackRight();
  delay(600);
  Stopp();
  delay(400);
  goto labelF;
}
}

void ask_pin_F() // Measure the distance at the specified angle
{
  myServo.write(angle);
  delay(10);
  digitalWrite(PIN_SONAR_TRIGGER,LOW);
  delayMicroseconds(1);
  digitalWrite(PIN_SONAR_TRIGGER,HIGH);
  delayMicroseconds(11);
  digitalWrite(PIN_SONAR_TRIGGER,LOW);
  unsigned long t = pulseIn(PIN_SONAR_ECHO,HIGH);
  unsigned long distance = t*0.017;
  Fdistance = distance;
  if((  89 < angle) & (angle <  91)) display.setSegments(Letter_F,1,0);
  if((    0 < angle) & (angle <  10)) display.setSegments(Letter_r,1,0);
  if(( 170 < angle) & (angle < 180)) display.setSegments(Letter_L,1,0);
  display.showNumberDec(distance / 100,true, 1, 1);
  display.showNumberDec(int( distance - int(distance/100)*100 )/10,false, 1, 2);
  display.showNumberDec(int( distance - int(distance/10)*10 ),false, 1, 3);
  delay(30);
}