Ok I finally have something working. There may still be some bugs (in fact I'm sure I have an incongruity with left and right cases between FSM's, but that's easy enough to fix), but at least I have a program that is working and a robot that I can test so I can fine tune a few things (the steerVal is probably too short, but I'll need to test to see if I need 2x, 5x or something like a square of what I have) but I wanna say think you guys for helping me out. I've learned a lot about getting programs to run in real time and debugging and stuff.
if you're curious with where I was wanting to go or what I ended up with:
#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_steer 5
#define steering_loop 6
#define steering_stop 7
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 = 9000;
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)
{
// Stand-by state for servo FSM
case servo_wait:
break;
// Turns servo left and initiates sensor FSM
case servo_leftscan:
servoPos = heading - scan;
if (servoPos < 0)
{
servoPos = 0;
}
servo.write(servoPos);
servoCase = servo_motionwait;
tms = millis();
break;
// Creates delay for program that gives the servo .5 seconds to
// reach the desired position with out interrupting the program.
// The delay is to make sure the servo is facing the direction it
// is supposed to before getting reading from the sensor mounted
// on it.
case servo_motionwait:
if (millis() - tms >= 500)
{
sensorCase = sensor_sense;
servoCase = servo_wait;
break;
}
break;
// Turns servo right and initates sensor FSM
case servo_rightscan:
servoPos = heading + scan;
if (servoPos > 180)
{
servoPos = 180;
}
servo.write(servoPos);
servoCase = servo_motionwait;
break;
// Initiates steering FSM and puts the servo FSM on standby
case servo_reset:
steeringCase = steering_compare;
servoCase = servo_wait;
}
//Sensor FSM
switch (sensorCase)
{
// Stand- by state for sensor FSM
case sensor_wait:
break;
// Sensor process: Sends out 5 us digital high signal to initiate
// then reads in the digital high signal sent from the sensor. The
// sensor is an ultra sonic range finder which sends out an
// ultrasonic signal then "listens" for the "echo" and outputs a
// digital high pulse as long as the time it took for the
// ultrasonic signal to travel from the sensor to the target and
// back in us. Experimentation showed the process does not work
// with 100% reliability, so this case allows the sensor 5 "tries"
// to detect the runner within the speficied range. If the runner
// is not detected in the speficied range, the program will go
// into a short delay case as specified by the specifications
// of the sensor. If the runner is detected in the specified range
// the program will throw the reading to the a steering FSM case
// corresponding to the left or right servo position.
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, 12000);
Serial.println(sensorRead);
tms = millis();
if ((sensorRead < shortest || sensorRead > farthest) && n <=5)
{
n += 1;
sensorCase = sensor_delay;
break;
}
else
{
if (servoPos < heading)
{
steeringCase = steering_detectR;
}
else
{
steeringCase = steering_detectL;
}
n = 1;
sensorCase = sensor_wait;
break;
}
// Delays the program before reinitiating the sensor with out
// interrupting the program.
case sensor_delay:
if (millis()-tms >= 10)
{
sensorCase = sensor_sense;
}
break;
}
//steering FSM
switch (steeringCase)
{
// Stand-by case for steering FSM
case steering_wait:
break;
// Processes sensor reading from right detectiong into binary.
// Throws steering FSM back into stand by and initiates servo FSM
// to turn right
case steering_detectR:
if (sensorRead > shortest && sensorRead < farthest)
{
detectR = true;
}
else
{
detectR = false;
}
steeringCase = steering_wait;
servoCase = servo_rightscan;
break;
// Processes sensor reading from left detection into binary.
// Throws steering FSM into stand-by and servo FSM into reset
case steering_detectL:
if (sensorRead > shortest && sensorRead < farthest)
{
detectL = true;
}
else
{
detectL = false;
}
steeringCase = steering_wait;
servoCase = servo_reset;
break;
// Compares both left and right detections to determine what the
// robots heading should be to follow the runner and puts the
// steering FSM into a state that will actually change the
// direction of the robot. If the runner was not detected at all
// be the steering FSM is thrown into the "stop" state
case steering_compare:
tms = millis();
if (detectR && detectL)
{
steeringCase = steering_steer;
}
if (!detectR && detectL)
{
heading -= scan;
if (heading < 0)
{
heading = 0;
}
steeringCase = steering_steer;
}
if (detectR && !detectL)
{
heading += scan;
if (heading > 180)
{
heading = 180;
}
steeringCase = steering_steer;
}
if (!detectR && !detectL)
{
steeringCase = steering_stop;
}
break;
// Steering is implimented by turning of one of the motors for a
// short time. Which motor is determined by the heading of the
// robot and the length of the turn is determined by the
// ammount the robots heading is off of going striaght forward.
// The delay is done with another steering FSM case, similiar
// to the other delays in the other FSM's.
case steering_steer:
steerVal = abs(heading-90);
if(heading > 90)
{
rMot = base;
lMot = 0;
}
else
{
rMot = 0;
lMot = base;
}
steeringCase = steering_loop;
break;
// Delay for executing a turn done so that it does not interupt
// the program. When the delay is complete, the steering FSM is
// thrown back into stand by and the servo FSM is reinitated with
// the left servo turn.
case steering_loop:
if (millis() - tms > steerVal)
{
rMot = base;
lMot = base;
servoCase = servo_leftscan;
steeringCase = steering_wait;
}
break;
// Because the runner was not detected, the robot stops itself,
// resets its heading straight forward, throws the steering FSM
// back into stand by, and reinitatiates the servo FSM with the
// left servo turn.
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);
}