Alright so I have rewritten my code:
#include <Servo.h>
Servo servo;
int servoCase = 1;
int sensorCase = 1;
int steeringCase = 1;
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 = 2;
}
void loop()
{
//servo FSM
switch (servoCase)
{
case 1:
break;
case 2:
servoPos = heading - scan;
if (servoPos < 0)
{
servoPos = 0;
}
servo.write(servoPos);
servoCase = 3;
tms = millis();
break;
case 3:
if (millis() - tms >= 200)
{
sensorCase = 2;
servoCase = 1;
break;
}
break;
case 4:
servoPos = heading + scan;
if (servoPos > 180)
{
servoPos = 180;
}
servo.write(servoPos);
servoCase = 3;
break;
case 5:
steeringCase = 4;
servoCase = 1;
}
//Sensor FSM
switch (sensorCase)
{
case 1:
break;
case 2:
pinMode(sensorPin, OUTPUT);
digitalWrite(sensorPin, LOW);
delayMicroseconds(2);
digitalWrite(sensorPin, HIGH);
delayMicroseconds(5);
digitalWrite(sensorPin, LOW);
pinMode(sensorPin, INPUT);
sensorRead = pulseIn(sensorPin, HIGH);
tms = millis();
if ((sensorRead < shortest || sensorRead > farthest) && n <=5)
{
n += 1;
sensorCase = 2;
break;
}
if (servoPos < heading)
{
steeringCase = 2;
}
else
{
steeringCase = 3;
}
sensorCase = 5;
n = 1;
break;
case 5:
if (tms >= 100)
{
sensorCase = 1;
if (servoPos < heading)
{
servoCase = 4;
}
else
{
servoCase = 5;
}
break;
}
break;
}
//steering FSM
switch (steeringCase)
{
case 1:
break;
case 2:
if (sensorRead > shortest && sensorRead < farthest)
{
detectR = true;
}
else
{
detectR = false;
}
steeringCase = 1;
break;
case 3:
if (sensorRead > shortest && sensorRead < farthest)
{
detectL = true;
}
else
{
detectL = false;
}
steeringCase = 1;
break;
case 4:
steerVal = abs(heading - 90);
if (detectR && detectL)
{
steeringCase = 5;
}
if (!detectR && detectL)
{
steeringCase = 6;
}
if (detectR && !detectL)
{
steeringCase = 7;
}
if (!detectR && !detectL)
{
steeringCase = 8;
}
break;
case 5:
rMot = 255;
lMot = 255;
servoCase = 2;
steeringCase = 1;
break;
case 6:
rMot = base;
lMot = 0;
tms = millis();
if(millis() - tms >= steerVal)
{
lMot = base;
heading += scan;
if (heading > 180)
{
heading = 180;
}
servoCase = 2;
steeringCase = 1;
}
break;
case 7:
rMot = 0;
lMot = 255;
tms = millis();
if (millis() - tms > steerVal)
{
rMot = base;
heading -= scan;
if(heading < 0)
{
heading = 0;
}
servoCase = 2;
steeringCase = 1;
}
break;
case 8:
rMot = 0;
lMot = 0;
heading = 90;
servoCase = 2;
steeringCase = 1;
break;
}
analogWrite(rMotPin, rMot);
analogWrite(lMotPin, lMot);
Serial.print(detectR);
Serial.print(" ");
Serial.print(detectL);
Serial.print(" ");
Serial.println(millis());
}
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);
}
Initially I was still experiencing the same lag issues I was having. When I plug the serial monitor in I seem to get HUGE bursts of information. The robot will stop doing things for a second, the suddenly things will go grazy for about half a second and like 80 loops worth of Serial.print will show up in the serial monitor, which is really odd because the servo the sensor is mounted on will only go a short distance in one direction; there's just no way my code is being executed. Does any one see any specific problems with my code or anything? Am I doing this FSM thing right?
One thing that seemed strange to me was the case 4 of the steering FSM switch structure. Perhaps a switch structure isn't the way this should be happening? Am I asking this arduino to do too much?