Hi
This is my first time using coding and have no knowledge prior to writing the below code.
I’m trying to get my robot to tank until something is within a certain range using the ultrasonic sensor HC-SR04 on an UNO board.
I have made the program ignore all the areas that I’m having issues with and the rest of the program works is intended. I have only got this far through supplied samples and google but cannot work out the U/S sensor.
I need to be able to the have the robot tank to the left or right until an object is within a set range. I have included the ranges (in mm) in brackets in the program.
It would be great if the distance ranges could be adjusted in a similar way to the motor speeds as per drive forward.
// Global
// Button pin
const int buttonPin = A0;
// Right motor pins (Out 1 Out 2)
const int dir1PinRight = 0;
const int dir2PinRight = 1;
const int speedPinRight = 5; // PWM pin
// Left motor pins (Out 1 Out 2)
const int dir1PinLeft = 2;
const int dir2PinLeft = 4;
const int speedPinLeft = 6; // PWM pin
// Hall effect sensors
const int hallPinRight = 3;
// Variables for hall effect sensors on wheels using interrupts
volatile int rightCount;
const int bumpStop = 13;
const int ball4release = 10;
const int ball3release = 9;
const int ball2release = 8;
const int ball1release = 7;
// Ultrasonic sensor
//const int triggerPin = 11; // send pulse on this pin
//const int echoPin = 12; // detect return pulse on this pin
//long distToObject = 0; // distance between sensor and object
//-----------------------------------
void setup() {
// Define button pin
pinMode(buttonPin, INPUT_PULLUP);
// Define hall effect sensor pins
pinMode(hallPinRight, INPUT);
// Attach interrupts to the ISR vectors
attachInterrupt(digitalPinToInterrupt(hallPinRight), hallRight_ISR, RISING);
// Right motor
pinMode(dir1PinRight, OUTPUT);
pinMode(dir2PinRight, OUTPUT);
pinMode(speedPinRight, OUTPUT);
//Left motor
pinMode(dir1PinLeft, OUTPUT);
pinMode(dir2PinLeft, OUTPUT);
pinMode(speedPinLeft, OUTPUT);
// Define hall effect sensor pins
pinMode(hallPinRight, INPUT);
// define Bump Stop
pinMode(bumpStop, INPUT_PULLUP);
// define ball release
pinMode(ball4release, OUTPUT);
pinMode(ball3release, OUTPUT);
pinMode(ball2release, OUTPUT);
pinMode(ball1release, OUTPUT);
// Define ultrasonic sensor pins
//pinMode(triggerPin, OUTPUT);
//pinMode(echoPin, INPUT);
// Serial monitor
//Serial.begin (9600);
// end setup()
}
//---------------------------------
void loop() {
// put your main code here, to run repeatedly:
while (digitalRead(buttonPin) == HIGH) {
// Wait for button press
};
delay(200);
while (digitalRead(buttonPin) == LOW) {
// Wait for button release
};
delay(200);
driveForward(190, 200, 9);
delay(500);
//tankRight(500-625);
delay(500);
driveToBumpSwitch(190, 200, bumpStop);
ball4();
driveBackward(200, 185, 16);
delay(1000);
//tankLeft(1000-1060);
delay(500);
driveToBumpSwitch(190, 200, bumpStop);
ball3();
driveBackward(200, 185, 4);
delay(500);
//tankRight(600-680);
driveToBumpSwitch(190, 200, bumpStop);
delay(500);
ball2();
driveBackward(200, 185, 9);
//tankLeft(520-600);
driveToBumpSwitch(180, 200, bumpStop);
ball1();
driveBackward(200, 185, 45);
tankLeftCount(0);
delay(500);
driveBackward(200, 185, 12);
} // end void loop()
//--------------------------------
void driveForward(int driveSpeedLeft, int driveSpeedRight, int count) {
rightCount = 0;
// Set speed
analogWrite(speedPinLeft, driveSpeedLeft);
analogWrite(speedPinRight, driveSpeedRight);
digitalWrite(dir1PinRight, HIGH);
digitalWrite(dir2PinRight, LOW);
digitalWrite(dir1PinLeft, LOW);
digitalWrite(dir2PinLeft, HIGH);
while (rightCount <= count) {
// Wait for hall effect to register required counts
}
// Stop both motors
digitalWrite(dir1PinLeft, LOW);
digitalWrite(dir2PinLeft, LOW);
digitalWrite(dir1PinRight, LOW);
digitalWrite(dir2PinRight, LOW);
} // end driveforward()
//--------------------------------
void driveBackward(int driveSpeedLeft, int driveSpeedRight, int count) {
rightCount = 0;
// Set speed
analogWrite(speedPinLeft, driveSpeedLeft);
analogWrite(speedPinRight, driveSpeedRight);
digitalWrite(dir1PinRight, LOW);
digitalWrite(dir2PinRight, HIGH);
digitalWrite(dir1PinLeft, HIGH);
digitalWrite(dir2PinLeft, LOW);
while (rightCount <= count) {
// Wait for hall effect to register required counts
}
// Stop both motors
digitalWrite(dir1PinLeft, LOW);
digitalWrite(dir2PinLeft, LOW);
digitalWrite(dir1PinRight, LOW);
digitalWrite(dir2PinRight, LOW);
} // end driveBackward()
//------------------------------
void tankRight(int count) {
rightCount = 0;
digitalWrite(dir1PinRight, HIGH);
digitalWrite(dir2PinRight, LOW);
digitalWrite(dir1PinLeft, HIGH);
digitalWrite(dir2PinLeft, LOW);
while (rightCount <= count) {
// Wait for hall effect to register required counts
}
// Stop both motors
digitalWrite(dir1PinLeft, LOW);
digitalWrite(dir2PinLeft, LOW);
digitalWrite(dir1PinRight, LOW);
digitalWrite(dir2PinRight, LOW);
} // end tankRight()
//-----------------------------
void tankLeft(int count) {
rightCount = 0;
digitalWrite(dir1PinRight, LOW);
digitalWrite(dir2PinRight, HIGH);
digitalWrite(dir1PinLeft, LOW);
digitalWrite(dir2PinLeft, HIGH);
while (rightCount <= count) {
// Wait for hall effect to register required counts
}
// Stop both motors
digitalWrite(dir1PinLeft, LOW);
digitalWrite(dir2PinLeft, LOW);
digitalWrite(dir1PinRight, LOW);
digitalWrite(dir2PinRight, LOW);
} // end tankLeft()
//---------------------------
void driveToBumpSwitch(int driveSpeedLeft, int driveSpeedRight, int bumpStop) {
// Note: Assumes bumpSwitchPin is initially HIGH
// Both motors forward
digitalWrite(dir1PinLeft, LOW);
digitalWrite(dir2PinLeft, HIGH);
digitalWrite(dir1PinRight, HIGH);
digitalWrite(dir2PinRight, LOW);
while (digitalRead(bumpStop) == HIGH) {
delay(50);
};
delay(200);
// Stop both motors
digitalWrite(dir1PinLeft, LOW);
digitalWrite(dir2PinLeft, LOW);
digitalWrite(dir1PinRight, LOW);
digitalWrite(dir2PinRight, LOW);
} // end driveToBumpStop()
//---------------------------
void tankLeftCount(int count) {
rightCount = 0;
digitalWrite(dir1PinRight, LOW);
digitalWrite(dir2PinRight, HIGH);
digitalWrite(dir1PinLeft, LOW);
digitalWrite(dir2PinLeft, LOW);
while (rightCount <= count) {
// Wait for hall effect to register required counts
}
// Stop both motors
digitalWrite(dir1PinLeft, LOW);
digitalWrite(dir2PinLeft, LOW);
digitalWrite(dir1PinRight, LOW);
digitalWrite(dir2PinRight, LOW);
} // end tankLeftCount()
//-----------------------
void hallRight_ISR() {
// Right wheel sensor has triggered
rightCount++;
}
//----------------------
void ball4() {
digitalWrite(ball4release, HIGH);
delay(5000);
digitalWrite(ball4release, LOW);
}
//----------------------
void ball3() {
digitalWrite(ball3release, HIGH);
delay(5000);
digitalWrite(ball3release, LOW);
}
//----------------------
void ball2() {
digitalWrite(ball2release, HIGH);
delay(3000);
digitalWrite(ball2release, LOW);
}
//----------------------
void ball1() {
digitalWrite(ball1release, HIGH);
delay(2000);
digitalWrite(ball1release, LOW);
}
//------------------------
Many Thanks
;D