Here is the code... Keep in mind, I'm making all this up as I go so some of this might not do what I think it does. Also, I comment the shit out of my code, asking myself questions, saying what I think things do, there might be some spelling issues.
Some things have not been implemented yet. I stopped combining when I had the question about the multiple states. There are a few empty functions and functions that are never called
one question about my code. At some point in one function is use delayMicroseconds(), I have no clue why I'm using this and if it blocks the code just like delay() does
#include <IRremote.hpp> // to make the ir remote work
#include <Servo.h> // to make the servo work
#include <elapsedMillis.h> // to make the timers work
#define IR_RECEIVE_PIN 3 // pin IR receiver it attached to
#define trigPin A1 // ultrasonic sensor trig pin
#define echoPin A0 // ultrasonic sensor echo pin
#define motorPin1 2 // digital motor pin
#define motorPin2 4 // digital motor pin
#define motorPin3 7 // digital motor pin
#define motorPin4 8 // digital motor pin
#define motorPin5 5 // analog motor pin
#define motorPin6 6 // analog motor pin
#define laserPin A5 // pin for laser emmitter
#define servoPin A2 // pin for the servo motor
Servo myServo; // creates the variable to control the servo
// The list of possible states and modes of our system
enum {EARS, FORWARD, BACKWARD, LEFT, RIGHT, STOP, LASER} currentState; // the drive states - should I move LASER to a mode?
enum {BOOT, WAIT, LOITER, DRIVE} currentMode; // the modes states
enum {START_EARS, STOP_AT_MIDDLE, STEP_TO_MIN, STEP_TO_MIDDLE} earMode; // special stuff just for the ears - Should I rename this?
// globals
int speedK9 = 200; // sets the speed of the motors
const unsigned StepPeriod = 0; // steps needed for turning the ears - does this need to be here?
const int StepIncrement = 2; // needed for turning the ears - does this need to be here?
const uint8_t MAX_SWEEP=180; // the maximum turn of the ear servo
const uint8_t MIN_SWEEP=0; // the minimum turn of the ear servo
const uint8_t MID_SWEEP=90; // middle of the ear servo
const uint32_t STEP_RATE= 10; // speed of the ears - is this still used?
const int turnDelay = 1000; // the timer for turning - works better here so it can be used in both the left turn and the right turn
elapsedMillis taskTimer = 3000; // for firing the laser - does this need to be here?
elapsedMillis turnTimer = 3000; // timer for the turns - works better here so it can be used in both the left turn and the right turn
void setup() {
// put your setup code here, to run once:
Serial.begin(115200); // initializes the serial port to send data to the monitor
IrReceiver.begin(IR_RECEIVE_PIN, ENABLE_LED_FEEDBACK); // starts the IR receiver
pinMode(trigPin, OUTPUT); // Starts the Ultrasonic Sensor
pinMode(echoPin, INPUT); // Starts the Ultrasonic Sensor
myServo.attach(servoPin); // Starts the servo
//myServo.write(90); // puts the servo in the middle (90 degree) position - due to earMode, is this even needed?
currentState = STOP; // sets the start currentState at STOP - keeps him from going anywhere
currentMode = BOOT; // sets the start currentMode as BOOT - not that it does anything there yet
earMode = STOP_AT_MIDDLE; // sets the start earMode as STOP_AT_MIDDLE - this should put the ears at 90?
}
void checkDistance() {
// Sets the minimun forward distance needed to not hit something
const int d_min = 20; // 20mm? - should I move it to the top to make it easier to change?
float duration, distance;
digitalWrite(trigPin, LOW);
delayMicroseconds(2); // when did I start using delayMicroseconds? - is this blocking the code?
digitalWrite(trigPin, HIGH);
delayMicroseconds(10); // when did I start using delayMicroseconds? - is this blocking the code?
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH);
distance = (duration*.0343)/2;
// sanity check
// Serial.print("Distance: ");
// Serial.println(distance);
// Checks to make sure the data from the sensor is valid - greater than 0 and less than 500
// and stops the motors if the distance is less than d_min
if (distance <= d_min && distance != 0 && distance <= 500){
if (currentState == FORWARD) { // only changes state to stop when it is moving forward
currentState = STOP;
}
}
}
void FireLaser () {
// elapsedMillis taskTimer; // moved to the top
unsigned int taskADelay; // should this be moved to the top?
unsigned int taskBDelay; // should this be moved to the top?
taskADelay = 3000; // should this be moved to the top?
taskBDelay = 7000; // should this be moved to the top?
taskTimer = 0; // resets timer to 0
if (taskTimer > taskADelay) {
// taskTimer = 0; // should the the timer be reset to 0 here?
digitalWrite(laserPin, LOW);
}
if (taskTimer > taskBDelay) {
// taskTimer = 0; // should the the timer be reset to 0 here?
digitalWrite(laserPin, HIGH);
}
}
void turnLeft() { // partial turn to the left
turnTimer = 0; // resets the timer to 0
if (turnTimer > turnDelay) { // if the timere is longer than the delay it changes currentMode to
currentState = STOP; // do i need both of these here?
Stop_Robot(); // do i need both of these here?
}
else {
currentState = LEFT; // do i need both of these here?
Rotate_Left(); // do i need both of these here?
}
}
void Move_Backward() { // Function to get the wheels going backward
digitalWrite(motorPin1, HIGH); // writes data to pin
digitalWrite(motorPin2, LOW); // writes data to pin
digitalWrite(motorPin3, HIGH); // writes data to pin
digitalWrite(motorPin4, LOW); // writes data to pin
analogWrite(motorPin5, speedK9); // writes data to anaglog pin
analogWrite(motorPin6, speedK9); // writes data to ana log pin
}
// This is what caused the ears to move. when the earMode is set
// to START_EARS it starts a single radar sweep of the ears.
// As it runs it automaticaly changes the state takining it from
// max to min then back to center where it stops
void moveEars() {
static uint32_t stepTimer=0; // state machine timer, removes the need for delay()
static uint8_t position = MID_SWEEP; // if moveEars is called without a change earMode it keeps the servo in the middle position
if(millis() - stepTimer > STEP_RATE) {// starts the timer making a step every few milliseconds
stepTimer=millis(); //resets the timer
switch (earMode) { // defining the state machine states
case START_EARS: // initial position
if(position++ >= MAX_SWEEP) // takes the current position and adds 1 to it until it gets to the MAX_SWEEP
earMode=STEP_TO_MIN; // once it hits max it changes the machine state to STEP_TO_MIN
break;
case STEP_TO_MIN: // the current position should be the max position
if(position-- <= MIN_SWEEP) // takes the current position and subtracts 1 from it until it gets to the MIN_SWEEP
earMode=STEP_TO_MIDDLE; // once it hits min it changes the machine state to STEP_TO_MIDDLE
break;
case STEP_TO_MIDDLE: // the current postion needs to be less than the mid to work
if(position++ >= MID_SWEEP) // takes the current position and adds 1 to it until it gets to the MID_SWEEP
earMode=STOP_AT_MIDDLE; // once it hits the middle it changes the machine state to STOP_AT_MIDDLE
break;
case STOP_AT_MIDDLE: // the current postion should be MID_SWEEP with no further movement
position=MID_SWEEP; // this makes sure that the servo is in the MID_SWEEP postition
// digitalWrite(LEDPin, LOW); // this turns of the LED - currently not used
break;
} //switch end of the machine states
myServo.write(position); // moves servo
} // timer
}
void Move_Forward() { // Function to get the wheels going forward
// checkDistance(); // checks for front distance to make sure the head does not bump anything - is this the right spot for this?
digitalWrite(motorPin1, LOW); // writes data to pin
digitalWrite(motorPin2, HIGH); // writes data to pin
digitalWrite(motorPin3, LOW); // writes data to pin
digitalWrite(motorPin4, HIGH); // writes data to pin
analogWrite(motorPin5, speedK9); // writes data to analog pin
analogWrite(motorPin6, speedK9); // writes data to analog pin
}
void Rotate_Left() { // Function to get the wheels moving in opposite directons to turn left
digitalWrite(motorPin1, LOW); // writes data to pin
digitalWrite(motorPin2, HIGH); // writes data to pin
digitalWrite(motorPin3, HIGH); // writes data to pin
digitalWrite(motorPin4, LOW); // writes data to pin
analogWrite(motorPin5, speedK9); // writes data to analog pin
analogWrite(motorPin6, speedK9); // writes data to analog pin
}
void Rotate_Right() { // Function to get the wheels moving in opposite directons to turn right
digitalWrite(motorPin1, HIGH); // writes data to pin
digitalWrite(motorPin2, LOW); // writes data to pin
digitalWrite(motorPin3, LOW); // writes data to pin
digitalWrite(motorPin4, HIGH); // writes data to pin
analogWrite(motorPin5, speedK9); // writes data to analog pin
analogWrite(motorPin6, speedK9); // writes data to analog pin
}
void readRemote() { // deals with the signals from the ir remote
if (IrReceiver.decode()) { // sanity check - what is this code saying?
Serial.println(IrReceiver.decodedIRData.decodedRawData, HEX); // Print "old" raw data
IrReceiver.printIRResultShort(&Serial); // Print complete received data in one line
IrReceiver.printIRSendUsage(&Serial); // Print the statement required to send this data
Serial.println(IrReceiver.decodedIRData.command); // prints the decoded command
IrReceiver.resume(); // Enable receiving of the next value
if (IrReceiver.decodedIRData.command == 24) { // 24 is the command code for the up button
currentState = FORWARD;
IrReceiver.resume(); // Enable receiving of the next value
} else if (IrReceiver.decodedIRData.command == 28) { // 28 is the command code for the OK button
currentState = STOP;
IrReceiver.resume(); // Enable receiving of the next value
} else if (IrReceiver.decodedIRData.command == 8) { // 8 is the command code for the left button
currentState = LEFT;
IrReceiver.resume(); // Enable receiving of the next value
} else if (IrReceiver.decodedIRData.command == 90) { // 90 is the command code for the right button
currentState = RIGHT;
IrReceiver.resume(); // Enable receiving of the next value
} else if (IrReceiver.decodedIRData.command == 82) { // 82 is the command code for the down button
currentState = BACKWARD;
IrReceiver.resume(); // Enable receiving of the next value
}
// sanity check for the moveEars function
else if (IrReceiver.decodedIRData.command == 25) { // 25 is the command code for the "0" button
currentState = EARS;
IrReceiver.resume(); // Enable receiving of the next value
}
// checking to see if the turn code actually works
else if (IrReceiver.decodedIRData.command == 13) { // 25 is the command code for the "0" button
turnLeft();
IrReceiver.resume(); // Enable receiving of the next value
}
}
}
void Stop_Robot(){ // Function to stop the wheels from turning
digitalWrite(motorPin1, HIGH); // writes data to pin
digitalWrite(motorPin2, HIGH); // writes data to pin
digitalWrite(motorPin3, HIGH); // writes data to pin
digitalWrite(motorPin4, HIGH); // writes data to pin
analogWrite(motorPin5, speedK9); // writes data to pin
analogWrite(motorPin6, speedK9); // writes data to pin
}
void Other_Stop() { // Function to stop other things that need to be stopped
// not doing anything right now
}
void checkStates() { // Function that checks to see what state the robot is in and take an appropriate actions
switch (currentState) {
case STOP:
Stop_Robot();
currentState = STOP; // do i need this here?
IrReceiver.resume();
break;
case FORWARD:
currentState = FORWARD; // do i need this here
Move_Forward();
IrReceiver.resume();
break;
case BACKWARD:
Move_Backward();
currentState = BACKWARD; // do i need this here
IrReceiver.resume();
break;
case LEFT:
Rotate_Left();
currentState = LEFT; // do i need this here
IrReceiver.resume();
break;
case RIGHT:
Rotate_Right();
currentState = RIGHT; // do i need this here
IrReceiver.resume();
break;
case LASER: // makes the laser figher - do i even need this state?
currentState = LASER; // do i need this here
IrReceiver.resume();
break;
}
}
void sanityCheck() { // prints all sorts of shit to the serial monitor for debugging
if (currentState == FORWARD) {
Serial.println("State Forward");
} else if (currentState == STOP) {
Serial.println("State STOP");
} else if (currentState == LEFT) {
Serial.println("State LEFT");
} else if (currentState == RIGHT) {
Serial.println("State RIGHT");
} else if (currentState == WAIT){
Serial.println("State WAIT");
} else if (currentState == BACKWARD){
Serial.println("State BACKWARD");
} else if (currentState == EARS){
Serial.println("State EARS");
} else if (currentState == BOOT){
Serial.println("State BOOT");
}
}
void runStateMachine(){ // calls all the functions needed to run the robot. Hopefully with no delay...
readRemote(); // looks for data coming into the IR sensor from the remote
checkStates(); // looks to see if the currentStates has changed
// checkMode(); // checks to see if the currentMode has changed - need to actually code this - chould this come before checkStates? Does it matter?
// checkEars(); // checks to see if the earMode has changed - need to actually code this - chould this come before checkStates? Does it matter?
if (currentState == FORWARD){
checkDistance(); // Does this work better here or in the Move_Forward function?
}
// sanity check - check to make sure what state the robot is in
// sanityCheck();
}
void loop() { // put your main code here, to run repeatedly:
runStateMachine(); // this runs the whole enchilada
}
