So now the robot seems to successfully do nothing at all. The good news (I suppose) is that the serial monitor seems to be outputting a steady stream of data instead of large bursts with long pauses, so I guess that means the loop is running more consistently? I have also case names for clarity
#include <Servo.h>
#define servo_wait 1
#define servo_leftscan 2
#define servo_motionwait 3
#define servo_rightscan 4
#define servo_reset 5
#define sensor_wait 1
#define sensor_sense 2
#define sensor_delay 3
#define sensor_reset 4
#define steering_wait 1
#define steering_detectR 2
#define steering_detectL 3
#define steering_compare 4
#define steering_forward 5
#define steering_left 6
#define steering_right 7
#define steering_stop 8
Servo servo;
int servoCase = servo_wait;
int sensorCase = sensor_wait;
int steeringCase = steering_wait;
int heading = 90;
const int scan = 15;
const int shortest = 3000;
const int farthest = 11000;
boolean detectR;
boolean detectL;
unsigned long sensorRead;
int steerVal;
int rMot;
int lMot;
int servoPos = 90;
int base = 255;
unsigned long tus;
unsigned long tms;
int n = 1;
int rMotPin = 2;
int lMotPin = 3;
int servoPin = 14;
int sensorPin = 15;
int hBdg1 = 16;
int hBdg2 = 17;
int hBdg3 = 18;
int hBdg4 = 19;
void setup()
{
Serial.begin(9600);
servo.attach(servoPin);
servo.write(servoPos);
pinMode(rMotPin, OUTPUT);
pinMode(lMotPin, OUTPUT);
pinMode(hBdg1, OUTPUT);
pinMode(hBdg2, OUTPUT);
pinMode(hBdg3, OUTPUT);
pinMode(hBdg4, OUTPUT);
forward();
servoCase = servo_leftscan;
}
void loop()
{
//servo FSM
switch (servoCase)
{
case servo_wait:
break;
case servo_leftscan:
servoPos = heading - scan;
if (servoPos < 0)
{
servoPos = 0;
}
servo.write(servoPos);
servoCase = servo_motionwait;
tms = millis();
break;
case servo_motionwait:
if (millis() - tms >= 400)
{
sensorCase = sensor_sense;
servoCase = servo_wait;
break;
}
break;
case servo_rightscan:
servoPos = heading + scan;
if (servoPos > 180)
{
servoPos = 180;
}
servo.write(servoPos);
servoCase = servo_motionwait;
break;
case servo_reset:
steeringCase = steering_compare;
servoCase = servo_wait;
}
//Sensor FSM
switch (sensorCase)
{
case sensor_wait:
break;
case sensor_sense:
pinMode(sensorPin, OUTPUT);
digitalWrite(sensorPin, LOW);
delayMicroseconds(2);
digitalWrite(sensorPin, HIGH);
delayMicroseconds(5);
digitalWrite(sensorPin, LOW);
pinMode(sensorPin, INPUT);
sensorRead = pulseIn(sensorPin, HIGH, 5);
tms = millis();
if ((sensorRead < shortest || sensorRead > farthest) && n <=5)
{
n += 1;
sensorCase = sensor_delay;
break;
}
if (servoPos < heading)
{
steeringCase = steering_detectR;
}
else
{
steeringCase = steering_detectL;
}
n = 1;
sensorCase = sensor_wait;
break;
case sensor_delay:
if (tms >= 100)
{
sensorCase = sensor_sense;
}
break;
case sensor_reset:
if (servoPos < heading)
{
servoCase = servo_rightscan;
}
else
{
servoCase = servo_reset;
}
break;
}
//steering FSM
switch (steeringCase)
{
case steering_wait:
break;
case steering_detectR:
if (sensorRead > shortest && sensorRead < farthest)
{
detectR = true;
}
else
{
detectR = false;
}
steeringCase = steering_wait;
break;
case steering_detectL:
if (sensorRead > shortest && sensorRead < farthest)
{
detectL = true;
}
else
{
detectL = false;
}
steeringCase = steering_wait;
break;
case steering_compare:
steerVal = abs(heading - 90);
if (detectR && detectL)
{
steeringCase = steering_forward;
}
if (!detectR && detectL)
{
steeringCase = steering_left;
}
if (detectR && !detectL)
{
steeringCase = steering_right;
}
if (!detectR && !detectL)
{
steeringCase = steering_stop;
}
break;
case steering_forward:
rMot = 255;
lMot = 255;
servoCase = servo_leftscan;
steeringCase = steering_wait;
break;
case steering_left:
rMot = base;
lMot = 0;
tms = millis();
if(millis() - tms >= steerVal)
{
lMot = base;
heading += scan;
if (heading > 180)
{
heading = 180;
}
servoCase = servo_leftscan;
steeringCase = steering_wait;
}
break;
case steering_right:
rMot = 0;
lMot = 255;
tms = millis();
if (millis() - tms > steerVal)
{
rMot = base;
heading -= scan;
if(heading < 0)
{
heading = 0;
}
servoCase = servo_leftscan;
steeringCase = steering_wait;
}
break;
case steering_stop:
rMot = 0;
lMot = 0;
heading = 90;
servoCase = servo_leftscan;
steeringCase = steering_wait;
break;
}
analogWrite(rMotPin, rMot);
analogWrite(lMotPin, lMot);
Serial.print(detectR);
Serial.print(" ");
Serial.print(detectL);
Serial.print(" ");
Serial.println(heading);
}
void forward()
{
digitalWrite(hBdg1, HIGH);
digitalWrite(hBdg2, LOW);
digitalWrite(hBdg3, HIGH);
digitalWrite(hBdg4, LOW);
}
void reverse()
{
digitalWrite(hBdg1, LOW);
digitalWrite(hBdg2, HIGH);
digitalWrite(hBdg3, LOW);
digitalWrite(hBdg4, HIGH);
}