This is a second bot. My first bot was a servo motor based tank with dual HC-SR04. I decided to go with a slightly larger base and brushed motors instead of servos. The track base is from a lego technics excavator and the motor chassis used to be a lego spybotic. I added 2 HC-SR04 ultrasonic sensors and the motors are running from a L293D. I also purchased a BT2S bluetooth module so I could one day remotely control it from my android phone.
I like the Arduino and making the robots, I just have a heck of a time programming...I have been modifying sketches that I find and try to make them work. For this project, I have had the motors working in a test program, but have not been successful at making this one autonomous with the dual ultrasonics and bluetooth control seems out of my ability at this time.
Below is my currenty frankenstein sketch:
#include <Ultrasonic.h>
#define motor1pole1 2
#define motor1pole2 3
#define motor2pole1 7
#define motor2pole2 8
#define enablePin1 9
#define enablePin2 10
#define M1_MAX_SPEED 100
#define M2_MAX_SPEED 100
#define motordelay 30
#define debugmotorsec 3000
const int trigPin = 4;
const int pingEcho = 5;
const int trigPin2 = 11;
const int pingEcho2 = 12;
long duration, distance; //duration used to calculate distance
void setup()
{
pinMode(trigPin, OUTPUT);
pinMode(pingEcho, INPUT);
pinMode(trigPin2, OUTPUT);
pinMode(pingEcho2, INPUT);
pinMode(13, OUTPUT);
pinMode(enablePin1, OUTPUT);
pinMode(enablePin2, OUTPUT);
pinMode( motor1pole1 , OUTPUT);
pinMode( motor1pole2, OUTPUT);
pinMode( motor2pole1, OUTPUT);
pinMode( motor2pole2 , OUTPUT);
motorspeed(0, 0);
}
void loop()
{
long duration, cm, cm2;
cm=10; //default distances in case ultrasonic sensor stops working. This
cm2=10;
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
duration = pulseIn(pingEcho, HIGH);
cm = microsecondsToCentimeters(duration);
digitalWrite(trigPin2, LOW);
delayMicroseconds(2);
digitalWrite(trigPin2, HIGH);
delayMicroseconds(5);
digitalWrite(trigPin2, LOW);
duration = pulseIn(pingEcho2, HIGH);
cm2 = microsecondsToCentimeters(duration);
if((cm < 11)||(cm2 < 11)){
motorstop(1);
motorstop(2);
digitalWrite(13, HIGH); // set the LED on
delay(500);
digitalWrite(13, LOW); // set the LED off
delay(500);
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(5);
digitalWrite(trigPin, LOW);
duration = pulseIn(pingEcho, HIGH);
cm2 = microsecondsToCentimeters(duration);
//Go backwards
motorback(1);
motorback(2);
delay(400);
if(cm<cm2){
motorforward(1);
motorback(2);
}else{
motorback(1);
motorforward(2);
}
delay(900);
}else{
motorforward(1);
motorforward(2);
}
delay(400);
}
long microsecondsToCentimeters(long microseconds)
{
return microseconds / 29 / 2;
}
void motorstop(int motornum){
delay(motordelay);
if (motornum == 1) {
digitalWrite(motor1pole1, LOW);
digitalWrite(motor1pole2, LOW);
}
else if (motornum == 2) {
digitalWrite(motor2pole1, LOW);
digitalWrite(motor2pole2, LOW);
}
delay(motordelay);
}
void motorforward(int motornum){
if (motornum == 1) {
digitalWrite(motor1pole1, HIGH);
digitalWrite(motor1pole2, LOW);
}
else if (motornum == 2) {
digitalWrite(motor2pole1, LOW);
digitalWrite(motor2pole2, HIGH);
}
delay(motordelay);
}
void motorback(int motornum){
if (motornum == 1) {
digitalWrite(motor1pole1, LOW);
digitalWrite(motor1pole2, HIGH);
}
else if (motornum == 2) {
digitalWrite(motor2pole1, HIGH);
digitalWrite(motor2pole2, LOW);
}
delay(motordelay);
}
void motorspeed(int motor1speed, int motor2speed) {
if (motor1speed > M1_MAX_SPEED ) motor1speed = M1_MAX_SPEED; // limit top speed
if (motor2speed > M2_MAX_SPEED ) motor2speed = M2_MAX_SPEED; // limit top speed
if (motor1speed < 0) motor1speed = 0; // keep motor above 0
if (motor2speed < 0) motor2speed = 0; // keep motor speed above 0
analogWrite(enablePin1, motor1speed);
analogWrite(enablePin2, motor2speed);
}
Avoid hitting objects and eventually remote controlled with bluetooth
- Actuators / output devices: LEGO spybotics geared chassis + lego technics tracks
- Control method: Bluetooth and Autonomous
- CPU: Arduino Micro
- Programming language: Arduino C++
- Sensors / input devices: HC-SR04, L293D H-Bridge, BT2S
- Target environment: indoor
This is a companion discussion topic for the original entry at https://community.robotshop.com/robots/show/arduino-micro-lego-1