1. Equipment and wires
1.1 Equipment
MiniQ Chassis of desk robot
- Diameter of chassis：122mm
- Diameter of wheel：42mm
- Height of chassis：15mm
- Arduino board及 Romeo controller fixed hole
- Motor parameters：
N20 motor voltage：3-9V
no-load speed：13000rpm
50：1 reduction gearbox
260rpm@6V
40mA@6V
360mA locked rotor@6V
10 Ounce inch torque @6V
Romeo three-in-one Arduino controller
- use Atmel Atmega328 single chip.
- Arduino UNO bootloader
- Port layout of fully compatible Arduino UNO board
- Integrated APC220 wireless data transmission and DF-Bluetooth V3 (SKU: TEL0026) Bluetooth module interface
- Five I2C Bus Interfaces Supported
- Supporting two-way motor drive, peak current 2A, 4 control ports using jumper switching
- External input voltage range：6V~20V
- More details can be seen in the website attached to Note page.
MiniQ Upper mounting plate of trolley
Benewake TFmini slandered board
TFmini Detailed parameters are on the product datasheet please down load through: en.benewake.com
9g The control actuator
1.2 Connect Wires
2. AGV Obstacle Avoidance theory
After the AGV start, it begin move forward. When the LiDAR detect obstacle within threshold, it stop moving, and scan from left to right to find a way to go. The control actuator carrying TFmni scan from 90°to 180°, then from 180° to 0°.
When the scanning direction is no obstacles, AGV will turn to that direction, the control actuator back to 90°.If there is no way is available, AGV goes back, the control actuator return.
The logical flow chart is shown below：
3. Notes
The current obstacle avoidance principle model is only used to throw bricks and attract jade, and explore the feasibility of using TFmini to avoid obstacles. It can not be applied to large-scale commercial scenarios. If necessary, the code of professional software developers should be taken as the criterion.
When the external power supply is too heavy, it will affect the friction force of the wheels of the AGV. Maybe the speed of the two wheels is not the same, resulting in the AGV can not follow the trajectory.
AGV wheels may cause idling on smooth ground, leading to the AGV can not go straight.
If the TFmini external power supply is supplied separately, the external power supply and the control board should be processed together.
If you are carrying a more complex program, you need to consider the chip's ability. The current development board has found that there will be a carton phenomenon when running the program.
4.1 Code
[HASHTAG]#include[/HASHTAG] <Servo.h>
Servo myservo;
int pos=90; // Define control actuator angle
bool flag=true;//Define control actuator direction
float dist_f;//Define foward Distance
float dist_s;//Define sideway Distance
int E1=5; //Define M1 Enable
int E2=6; //Define M2 Enable
int M1=4; //Define M1 Control
int M2=7; //Define M2 Control
int temp_distance =0;
/
* Two wheels stop
*/
void brake(void){
digitalWrite(E1,LOW); //Offer E1 low electricity level
digitalWrite(E2,LOW); //Offer E2 low electricity level
}
/**
* Two wheels go forward
*/
void advance(char a, char b){
analogWrite(E1,a);
digitalWrite(M1,LOW);
analogWrite(E2,b);
digitalWrite(M2,LOW);
}
/**
* Two wheels go back
*/
void back(char a, char b){
analogWrite(E1,a);
digitalWrite(M1,HIGH);
analogWrite(E2,b);
digitalWrite(M2,HIGH);
}
/**
* Turn left
*/
void turn_L(char a, char b){
analogWrite(E1,a);
digitalWrite(M1,LOW);
analogWrite(E2,b);
digitalWrite(M2,HIGH);
}
/**
* Turn left
*/
void turn_R(char a, char b){
analogWrite(E1,a);
digitalWrite(M1,HIGH);
analogWrite(E2,b);
digitalWrite(M2,LOW);
}
/**
* Check the detected distance
*/
void getTFminiData(int* distance, int* strength) { static char i = 0;
char j = 0;
int checksum = 0;
static int rx[9];
if(Serial.available()) {
rx = Serial.read();
if(rx[0] != 0x59) {
i = 0;
} else if(i == 1 && rx[1] != 0x59) {
i = 0;
} else if(i == 8) {
for(j = 0; j < 8; j++) {
checksum += rx[j];
}
/*
if(rx[8] == (checksum % 256)) {
*distance = rx[2] + rx[3] * 256;
*strength = rx[4] + rx[5] * 256;
}*/
*distance = rx[2] + rx[3] * 256;
*strength = rx[4] + rx[5] * 256;
i = 0;
} else {
i++;
}
}
}
void setup() {
// put your setup code here, to run once:
Serial.begin(115200);
// The control actuator’s jack at 4
myservo.attach(4);
brake();
/*
* Face the LiDAR straight.
*/
myservo.write(pos);
/*
* Set the wheel engine outlets.
*/
pinMode(4,OUTPUT);
pinMode(5,OUTPUT);
pinMode(6,OUTPUT);
pinMode(7,OUTPUT);
delay(10);
}
void loop() {
/*
* Check the distance.
*/
int distance = 0; int strength = 0; getTFminiData(&distance, &strength); while(!distance) {
getTFminiData(&distance, &strength);
Serial.print("Distance: ");
Serial.print(distance);
Serial.print("cm ");
Serial.print("strength: ");
Serial.println(strength);
}
/*
* Set the threshold 30CM
*/
if(distance <= 30 && distance > 0){
temp_distance = distan
delay(10);
/*
* check the detected distance.
* If the number is under the threshold, then the AGV stops, and scanning frim life to right, until there is a way to go, then the wheel starts and scanner returns.
If the number is over the threshold then the AGV starts.
*/
if(temp_distance <= 30 && temp_distance >= 0){ brake();
/*
* Decide the control actuator changes its direction to life or right.
*/
if(flag){
if(pos<170){
pos=pos+45;
}else{
flag = false;
}
/*
* If the distance is over the threshold, then control actuator returns, then the AGV changes direction.
*/
if(distance > 32){
pos = 90;
myservo.write(pos);
delay(1200);
// Decide which direction to change.
if(pos >= 90){
turn_L(35,35);
}else{
turn_R(35,35);
delay(250);
temp_distance = distance;
}
/*
* If the distance is over the threshold, then keep scanning.
*/
else{
myservo.write(pos);
delay(1200);
}
}else{
if(pos>10){
pos=pos-45;
}else{
flag=true;
}
/*
* If the distance is over the threshold, then control actuator returns and AGV changes dirction
*/
if(distance > 32){
pos = 90;
myservo.write(pos);
delay(1200);
//Decide which direction to change
if(pos >= 90){
turn_L(35,35);
}else{
turn_R(35,35);
}
delay(250);
temp_distance = distance;
}
/*
* If the distance is under the threshold, then keeping scanning.
*/
else{
myservo.write(pos);
delay(1200)
}
/*
* If the brake();
}
}
1.1 Equipment
MiniQ Chassis of desk robot
- Diameter of chassis：122mm
- Diameter of wheel：42mm
- Height of chassis：15mm
- Arduino board及 Romeo controller fixed hole
- Motor parameters：
N20 motor voltage：3-9V
no-load speed：13000rpm
50：1 reduction gearbox
260rpm@6V
40mA@6V
360mA locked rotor@6V
10 Ounce inch torque @6V
Romeo three-in-one Arduino controller
- use Atmel Atmega328 single chip.
- Arduino UNO bootloader
- Port layout of fully compatible Arduino UNO board
- Integrated APC220 wireless data transmission and DF-Bluetooth V3 (SKU: TEL0026) Bluetooth module interface
- Five I2C Bus Interfaces Supported
- Supporting two-way motor drive, peak current 2A, 4 control ports using jumper switching
- External input voltage range：6V~20V
- More details can be seen in the website attached to Note page.
MiniQ Upper mounting plate of trolley
Benewake TFmini slandered board
TFmini Detailed parameters are on the product datasheet please down load through: en.benewake.com
9g The control actuator
1.2 Connect Wires
2. AGV Obstacle Avoidance theory
After the AGV start, it begin move forward. When the LiDAR detect obstacle within threshold, it stop moving, and scan from left to right to find a way to go. The control actuator carrying TFmni scan from 90°to 180°, then from 180° to 0°.
When the scanning direction is no obstacles, AGV will turn to that direction, the control actuator back to 90°.If there is no way is available, AGV goes back, the control actuator return.
The logical flow chart is shown below：
3. Notes
The current obstacle avoidance principle model is only used to throw bricks and attract jade, and explore the feasibility of using TFmini to avoid obstacles. It can not be applied to large-scale commercial scenarios. If necessary, the code of professional software developers should be taken as the criterion.
When the external power supply is too heavy, it will affect the friction force of the wheels of the AGV. Maybe the speed of the two wheels is not the same, resulting in the AGV can not follow the trajectory.
AGV wheels may cause idling on smooth ground, leading to the AGV can not go straight.
If the TFmini external power supply is supplied separately, the external power supply and the control board should be processed together.
If you are carrying a more complex program, you need to consider the chip's ability. The current development board has found that there will be a carton phenomenon when running the program.
4.1 Code
[HASHTAG]#include[/HASHTAG] <Servo.h>
Servo myservo;
int pos=90; // Define control actuator angle
bool flag=true;//Define control actuator direction
float dist_f;//Define foward Distance
float dist_s;//Define sideway Distance
int E1=5; //Define M1 Enable
int E2=6; //Define M2 Enable
int M1=4; //Define M1 Control
int M2=7; //Define M2 Control
int temp_distance =0;
/
* Two wheels stop
*/
void brake(void){
digitalWrite(E1,LOW); //Offer E1 low electricity level
digitalWrite(E2,LOW); //Offer E2 low electricity level
}
/**
* Two wheels go forward
*/
void advance(char a, char b){
analogWrite(E1,a);
digitalWrite(M1,LOW);
analogWrite(E2,b);
digitalWrite(M2,LOW);
}
/**
* Two wheels go back
*/
void back(char a, char b){
analogWrite(E1,a);
digitalWrite(M1,HIGH);
analogWrite(E2,b);
digitalWrite(M2,HIGH);
}
/**
* Turn left
*/
void turn_L(char a, char b){
analogWrite(E1,a);
digitalWrite(M1,LOW);
analogWrite(E2,b);
digitalWrite(M2,HIGH);
}
/**
* Turn left
*/
void turn_R(char a, char b){
analogWrite(E1,a);
digitalWrite(M1,HIGH);
analogWrite(E2,b);
digitalWrite(M2,LOW);
}
/**
* Check the detected distance
*/
void getTFminiData(int* distance, int* strength) { static char i = 0;
char j = 0;
int checksum = 0;
static int rx[9];
if(Serial.available()) {
rx = Serial.read();
if(rx[0] != 0x59) {
i = 0;
} else if(i == 1 && rx[1] != 0x59) {
i = 0;
} else if(i == 8) {
for(j = 0; j < 8; j++) {
checksum += rx[j];
}
/*
if(rx[8] == (checksum % 256)) {
*distance = rx[2] + rx[3] * 256;
*strength = rx[4] + rx[5] * 256;
}*/
*distance = rx[2] + rx[3] * 256;
*strength = rx[4] + rx[5] * 256;
i = 0;
} else {
i++;
}
}
}
void setup() {
// put your setup code here, to run once:
Serial.begin(115200);
// The control actuator’s jack at 4
myservo.attach(4);
brake();
/*
* Face the LiDAR straight.
*/
myservo.write(pos);
/*
* Set the wheel engine outlets.
*/
pinMode(4,OUTPUT);
pinMode(5,OUTPUT);
pinMode(6,OUTPUT);
pinMode(7,OUTPUT);
delay(10);
}
void loop() {
/*
* Check the distance.
*/
int distance = 0; int strength = 0; getTFminiData(&distance, &strength); while(!distance) {
getTFminiData(&distance, &strength);
Serial.print("Distance: ");
Serial.print(distance);
Serial.print("cm ");
Serial.print("strength: ");
Serial.println(strength);
}
/*
* Set the threshold 30CM
*/
if(distance <= 30 && distance > 0){
temp_distance = distan
delay(10);
/*
* check the detected distance.
* If the number is under the threshold, then the AGV stops, and scanning frim life to right, until there is a way to go, then the wheel starts and scanner returns.
If the number is over the threshold then the AGV starts.
*/
if(temp_distance <= 30 && temp_distance >= 0){ brake();
/*
* Decide the control actuator changes its direction to life or right.
*/
if(flag){
if(pos<170){
pos=pos+45;
}else{
flag = false;
}
/*
* If the distance is over the threshold, then control actuator returns, then the AGV changes direction.
*/
if(distance > 32){
pos = 90;
myservo.write(pos);
delay(1200);
// Decide which direction to change.
if(pos >= 90){
turn_L(35,35);
}else{
turn_R(35,35);
delay(250);
temp_distance = distance;
}
/*
* If the distance is over the threshold, then keep scanning.
*/
else{
myservo.write(pos);
delay(1200);
}
}else{
if(pos>10){
pos=pos-45;
}else{
flag=true;
}
/*
* If the distance is over the threshold, then control actuator returns and AGV changes dirction
*/
if(distance > 32){
pos = 90;
myservo.write(pos);
delay(1200);
//Decide which direction to change
if(pos >= 90){
turn_L(35,35);
}else{
turn_R(35,35);
}
delay(250);
temp_distance = distance;
}
/*
* If the distance is under the threshold, then keeping scanning.
*/
else{
myservo.write(pos);
delay(1200)
}
/*
* If the brake();
}
}