Tank Robot の動き方をいろいろ調整して、ほぼ満足のゆく状況になってきた。Sketch については、前回のものに対して少し加筆修正も行ったが･･･、Sketch内の各ステートメントで、変数の意味は何で、また何の処理をしているのか、後で見直した際にも分かるように、コメントを追記した。
《 Sketch 》
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; // Distance to the obstacle in the direction of the sensor
float Dlh; // Distance to left obstacle
float Drh; // Distance to right obstacle
float Dfr; // Distance to front obstacle
int Ls = 35 ; // short range threshold changed from 30
int Ll = 55 ; // long range threshold = 55
int angle;
int pinSRB = 9; // servo motor output pin (PWM)
int random_No4;
float multiplier = 1.35; // add variable and changed number from 1.3
int delaytime = 350; // chaged from 400
// 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);
// Data setting for 7-segment display
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); // display "go to foward"
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 "go to backward"
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); // display "go to foward"
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 "Stop"
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 "back right"
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);
analogWrite(pinRF,FullSpeed);
}
void BackLeft() // turn left
{
display.setSegments(Letter_L,1,0); // display "back left"
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); // Generate random numbers
// --- Forward obstacle detection and moving ---
// angle = 90;
myServo.write(90); // use fixed number(90deg)
delay(10); // changed delay time from 50
label1: // add a statement
ask_pin_F(); // Fdistance
if(Fdistance <= Ls)
{
Stopp(); // stop for a while
goto label2;
}
if((Fdistance > Ls ) & (Fdistance <= Ll ))
{
MidSpeed(); // at a slightly reduced speed
goto label1; // changed statement from labelF
}
if(Fdistance > Ll )
{
Advance(); // at full speed
goto label1; // changed statement from labelF
}
label2:
// angle = 90; // Check the distance ahead again
myServo.write(90); // use fixed number(angle --> 90)
delay(10); // changed delay time from 50
ask_pin_F();
if(Fdistance < Ls )
{
Back(); // go back a little
delay(random_No1); // backward progress time
Stopp();
}
// Measure left and right space
angle = 3; // check for obstacles in the right direction
myServo.write(angle);
delay(1000); // time to reserve for servo motor
ask_pin_F();
Drh=Fdistance;
angle = 177; // check for obstacles in the left direction
myServo.write(angle);
delay(1000); // time to reserve for servo motor
ask_pin_F();
Dlh=Fdistance;
angle = 90; // check for obstacles ahead
myServo.write(angle);
delay(1000); // time to reserve for servo motor
ask_pin_F(); // check forward distance
Dfr=Fdistance;
label3:
if(Dlh > Drh)
{
display.setSegments(Letter_GT,1,0); // display ">"
delay(350); //changed delay time from 300
// Calculate the time to change the moving direction of the tank
// "ie. delay(random_No4)" from the distance to the obstacles
// in the front and rear direction and the side direction
float rn1 = Dlh / Dfr ;
if( rn1 >= multiplier ) rn1 = multiplier ; // changed sketch
random_No4 = delaytime * rn1 ;
BackRight();
delay(random_No4); // backward progress time
Stopp();
delay(400); // display time for display unit
goto labelF;
}
else if( Dlh <= Drh )
{
display.setSegments(Letter_LT,1,0); // display "<"
delay(350); //changed delay time from 300
float rn2 = Drh / Dfr ;
if( rn2 >= multiplier ) rn2 = multiplier ; // changed sketch
random_No4 = delaytime * rn2 ;
BackLeft();
delay(random_No4); // backward progress time
Stopp();
delay(400); // display time for display unit
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 2micro_S
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); // distance to front obstacle
delay(200); // changed display time from 300
}
Recent Comments