Hi, I have a a sort of car I made, and I wanted to have a feature that allowed the car to drive itself around until it became too close to a wall, in which it would backup. The problem (I think) is with the code function named "Machine_operated." Any help would be great! Thank you in advance!
//SERVO Variables and such
#include <Servo.h> // include file for servo control
int pin_trig = 11; // trigger pin connected to ultrasonic sensor
int pin_echo = 9; // echo pin connected to ultrasonic sensor
Servo servo1; // create servo object to control servo #1
int position = 25 ; // state variable
boolean forward = false ; // state variable
//ULTRASONIC Variables and such
int trigState = LOW;
int printState = LOW;
float true_distance;
//PushButton LED
int buttonPin = 25;
int ledPin = 27;
int buttonState = HIGH;
int ledState = LOW;
int lastButtonState = HIGH;
int PressedVariable = 0;
//TIME VARIABLES TO AVOID USE OF DELAY
const long interval2 = 1;
const long interval3 = 500;
const long interval = 2000;
const long interval4 = 1000;
unsigned long previousMillis = 0;
unsigned long previousMillis2 = 0;
unsigned long previousMillis4 = 0;
unsigned long previousMillis5 = 0;
unsigned long previousMillis6 = 0;
unsigned long previousMillis7 = 0;
unsigned long previousMillis8 = 0;
unsigned long previousMillis9 = 0;
//LCD STUFF
// include the library code:
#include <LiquidCrystal.h>
// initialize the library with the numbers of the interface pins
LiquidCrystal lcd(52, 51, 50, 49, 48, 47);
//H-Bridge Variables and such
int motorSpeedA = 0;
int motorSpeedB = 0;
int Disable_forward = 0;
#define enA 2 //
#define fwd_left 3 // in1
#define rvrs_left 4 //in2
#define enB 7 //
#define fwd_right 5 // in3
#define rvrs_right 6 //in4
//FUNCTION PROTOTYPES
void lcd_function();
void servo_function();
void initialize_LCD();
int Motor_Controls(int Disable_forward);
void Ultrasonic(unsigned long currentMillis);
void LEDBUTTON (unsigned long currentMillis);
void Machine_operated(unsigned long currentMillis);
void setup() {
//Servo Tower initialization stuff
servo1.attach(10); // connect pin 7 to servo #1
pinMode(pin_trig, OUTPUT);
pinMode(pin_echo, INPUT);
Serial.begin(9600);
//H-Brdige initialization Stuff
pinMode(enA, OUTPUT);
pinMode(enB, OUTPUT);
pinMode(fwd_left, OUTPUT);
pinMode(rvrs_left, OUTPUT);
pinMode(fwd_right, OUTPUT);
pinMode(rvrs_right, OUTPUT);
//Cute LCD prompter
initialize_LCD();
//lEDBUTTON STUFF
pinMode (buttonPin, INPUT);
pinMode (ledPin, OUTPUT);
digitalWrite (ledPin, ledState); //set led to "off"
}
void loop() {
unsigned long currentMillis2 = millis();
unsigned long currentMillis = millis();
//LEDBUTTON
LEDBUTTON (currentMillis);
if (PressedVariable == 0){ //If joy stick mpde activated
//H-Bridge Controls
Motor_Controls (Disable_forward);
//Servo Section
servo_function(currentMillis2);
//Ultrasonic Sensor Section
Ultrasonic(currentMillis);
//LCD function
lcd_function(currentMillis);
}
else if(PressedVariable == 1){ //Obstacle Avoidance Mode
//MACHINE OPERATED MODE
Machine_operated(currentMillis);
//Ultrasonic Sensor Section
Ultrasonic(currentMillis);
//LCD function
lcd_function(currentMillis);
}
}
void LEDBUTTON (unsigned long currentMillis){
int currentButtonState = digitalRead (buttonPin);
if (currentButtonState != lastButtonState){ //record the time of change
previousMillis5 = currentMillis;
}
if (millis() - previousMillis5 > 10) { //skip bounce aka skip unwanted clicks
if (buttonState != currentButtonState){
buttonState = currentButtonState;
if (buttonState == LOW){
ledState =! ledState; //reverse ledState
digitalWrite (ledPin, ledState); //change led state
}
else{
Serial.print ("ButtonReleased");
}
//PRINT MODE TO SCREEN
if (ledState == LOW){
PressedVariable = 0;
lcd.clear();
lcd.print("Joystick Mode");
delay(2000);
}
if (ledState == HIGH){
PressedVariable = 1;
lcd.clear();
lcd.print("Machine Op Mode");
delay(2000);
}
}
}
lastButtonState = currentButtonState;
Serial.print (PressedVariable);
}
void lcd_function(unsigned long currentMillis){
//LCD STUFF
// set the cursor to column 0, line 1
// (note: line 1 is the second row, since counting begins with 0):
if (Disable_forward != 1){
lcd.setCursor(0, 0);
lcd.print("Distance: ");
lcd.setCursor(0, 1);
if (currentMillis - previousMillis4 >= interval4){
previousMillis4 += interval4;
lcd.print(true_distance,1);
lcd.setCursor(5,1);
lcd.print(" ");
}
}
if (Disable_forward == 1){
if (currentMillis - previousMillis4 >= interval4){
previousMillis4 += interval4;
lcd.setCursor(0, 0);
lcd.print("Forward disabled");
lcd.setCursor(0,1);
lcd.print("Back Up");
}
}
}
void servo_function(unsigned long currentMillis2){
//servo
if (currentMillis2 - previousMillis >= interval){
previousMillis += interval;
if (forward){
servo1.write (position = position - 15);
if (position ==25)
forward = false;
}else{
servo1.write (position = position +15);
if (position == 70)
forward = true;
//Serial.println(currentMillis2);
}
}
}
void initialize_LCD(){
//LCD (Fun Initialization) STUFF
// set up the LCD's number of columns and rows:
lcd.begin(16, 2);
// Print a message to the LCD.
lcd.print("Welcome");
delay (2000);
lcd.setCursor(0, 1);
lcd.print("My name is Walle");
delay(3000);
lcd.clear();
delay(1000);
for (int i= 1; i <=3; i++){
int counter = 4;
lcd.setCursor(0, 0);
lcd.print("Starting in...");
lcd.setCursor(0, 1);
lcd.print (counter - i);
delay(1000);
}
}
void Ultrasonic(unsigned long currentMillis){
if (currentMillis-previousMillis2 >= interval2) {
previousMillis2 = currentMillis;
if (trigState == LOW){ //if no pulse then pulse
(trigState = HIGH);
}
else { //if pulsed, then stop pulse
(trigState = LOW);
}
}
// printing if statement
if (currentMillis-previousMillis2 >= interval3) {
previousMillis2 = currentMillis;
if (printState == LOW){
(printState = HIGH);
}
else {
(printState = LOW);
}
}
digitalWrite(pin_trig,trigState);
float duration;
float distance;
float c = 343; //speed of sounds (m/s)
duration = pulseIn(pin_echo,HIGH,300000);//pulseIn records time between high and low. 300,000us = 300milli sec delay
duration *= 1.0e-6; //Convert to seconds
distance = c*100* duration/2 ; // distance in cm
if (distance != 0.0 && distance <400){ //Range on sensor is 400cm
true_distance = distance;
}
//if (printState = HIGH){
//Serial.print ("\ndistance (cm) = ");
//Serial.print(true_distance);
//}
if (true_distance <= 25.0){
Disable_forward = 1;
}
else {
Disable_forward = 0;
}
}
void Machine_operated(unsigned long currentMillis){
if (true_distance >25){ //forward
digitalWrite(fwd_left, HIGH);
digitalWrite(rvrs_left, LOW);
digitalWrite(fwd_right, HIGH);
digitalWrite(rvrs_right, LOW);
}
if (currentMillis-previousMillis6 >= interval3) {
previousMillis6 = currentMillis;
if (true_distance < 24){ //allow to roll from 25 to 24 with momentum
digitalWrite(fwd_left, LOW);
digitalWrite(rvrs_left, LOW);
digitalWrite(fwd_right, LOW);
digitalWrite(rvrs_right, LOW);
}
}
if (currentMillis-previousMillis7 >= interval3) {
previousMillis7 = currentMillis;
//Reverse NOW
digitalWrite(fwd_left, LOW);
digitalWrite(rvrs_left, HIGH);
digitalWrite(fwd_right, LOW);
digitalWrite(rvrs_right, HIGH);
}
if (currentMillis-previousMillis8 >= interval3) {
previousMillis8 = currentMillis;
//Roll to stop
digitalWrite(fwd_left, LOW);
digitalWrite(rvrs_left, LOW);
digitalWrite(fwd_right, LOW);
digitalWrite(rvrs_right, LOW);
}
if (currentMillis-previousMillis9 >= interval3) {
previousMillis9 = currentMillis;
//Turn Right
digitalWrite(fwd_left, LOW);
digitalWrite(rvrs_left, LOW);
digitalWrite(fwd_right, HIGH);
digitalWrite(rvrs_right, LOW);
}
}