/********************************************************************
ECEN 240/301 Lab Code
Light-Following Robot
The approach of this code is to use an architectured that employs
three different processes:
Perception
Planning
Action
By separating these processes, this allows one to focus on the
individual elements needed to do these tasks that are general
to most robotics.
Version History
1.1.3 11 January 2023 Creation by Dr. Mazzeo and TAs from 2022 version
********************************************************************/
/* These initial includes allow you to use necessary libraries for
your sensors and servos. */
#include "Arduino.h"
#include "math.h" // need this for log()
#include
#include
#include
//Note the filter is the Resistor and Capacitor
// #define FILTER_IN_PIN 10
// #define FILTER_OUT_PIN A0
//
// Compiler defines: the compiler replaces each name with its assignment
// (These make your code so much more readable.)
//
/***********************************************************/
// Hardware pin definitions
// Replace the pin numbers with those you connect to your robot
// Button pins. These will be replaced with the photodiode variables in lab 5
// #define BUTTON_1 A2 // Far left Button - Servo Up
// #define BUTTON_2 A3 // Left middle button - Left Motor
#define BUTTON_3 A4 // Middle Button - Collision
// #define BUTTON_4 A5 // Right middle button - Right Motor
// #define BUTTON_5 A6 // Far right button - Servo Down
// LED pins (note that digital pins do not need "D" in front of them)
#define LED_1 6 // Far Left LED - Servo Up
#define LED_3 4 // Middle LED - Collision
#define LED_5 2 // Far Right LED - Servo Down
// Motor enable pins - Lab 3
#define H_BRIDGE_ENA 5 // Left Motor
#define H_BRIDGE_ENB 3 // Right Motor
// These will replace LEDs 2 and 4
#define BATTERY_PIN A1
// Photodiode pins - Lab 5
// These will replace buttons 1, 2, 4, 5
#define PHOTO_PIN_1 A2 //Servo Up Photodiode
#define PHOTO_PIN_2 A3 // Motor Left Photodiode
#define PHOTO_PIN_3 A5 // Motor Right Photodiode
#define PHOTO_PIN_4 A6 // Servo Down Photodiode
// Capacitive sensor pins - Lab 4
#define CAP_SENSOR_SEND 11
#define CAP_SENSOR_RECEIVE 7
// Ultrasonic sensor pin - Lab 6
// This will replace button 3 and LED 3 will no longer be needed
#define TRIGGER_PIN 12
#define ECHO_PIN 9
// Servo pin - Lab 6
// This will replace LEDs 1 and 5
#define SERVO_PIN 10
/***********************************************************/
// Configuration parameter definitions
// Replace the parameters with those that are appropriate for your robot
// Voltage at which a button is considered to be pressed
#define BUTTON_THRESHOLD 2.5
// Voltage at which a photodiode voltage is considered to be present - Lab 5
#define PHOTODIODE_LIGHT_THRESHOLD 3.15
// Number of samples that the capacitor sensor will use in a measurement - Lab 4
#define CAP_SENSOR_SAMPLES 40
#define CAP_SENSOR_TAU_THRESHOLD 25
// Parameters for servo control as well as instantiation - Lab 6
#define SERVO_START_ANGLE 135
#define SERVO_UP_LIMIT 180
#define SERVO_DOWN_LIMIT 80
static Servo myServo;
// Parameters for ultrasonic sensor and instantiation - Lab 6
#define MAX_DISTANCE 200
static NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);
// Parameter to define when the ultrasonic sensor detects a collision - Lab 6
/***********************************************************/
// Defintions that allow one to set states
// Sensor state definitions
#define DETECTION_NO 0
#define DETECTION_YES 1
// Motor speed definitions - Lab 4
#define SPEED_100 255
#define SPEED_66 168.3
#define SPEED_33 84.15
#define SPEED_OFF 0
// Collision definitions
#define COLLISION_ON 0
#define COLLISION_OFF 1
// Driving direction definitions
#define DRIVE_STOP 0
#define DRIVE_LEFT 1
#define DRIVE_RIGHT 2
#define DRIVE_STRAIGHT 3
// Servo movement definitions
#define SERVO_MOVE_STOP 0
#define SERVO_MOVE_UP 1
#define SERVO_MOVE_DOWN 2
/***********************************************************/
// Global variables that define PERCEPTION and initialization
// Collision (using Definitions)
int SensedCollision = DETECTION_NO;
// Photodiode inputs (using Definitions) - The button represent the photodiodes for lab 2
int SensedLightRight = DETECTION_NO;
int SensedLightLeft = DETECTION_NO;
int SensedLightUp = DETECTION_NO;
int SensedLightDown = DETECTION_NO;
// Capacitive sensor input (using Definitions) - Lab 4
int SensedCapacitiveTouch = DETECTION_NO;
/***********************************************************/
// Global variables that define ACTION and initialization
// Collision Actions (using Definitions)
int ActionCollision = COLLISION_OFF;
// Main motors Action (using Definitions)
int ActionRobotDrive = DRIVE_STOP;
// Add speed action in Lab 4
int ActionRobotSpeed = SPEED_100;
// Servo Action (using Definitions)
int ActionServoMove = SERVO_MOVE_STOP;
//battery monitor variables
const int greenLEDPin = 90; // was 9
const int redLEDPin = 100; // was 10
const int blueLEDPin = 80; // was 8
int redValue = 0;
int greenValue = 0;
int blueValue = 0;
float currentV = 0;
/********************************************************************
SETUP function - this gets executed at power up, or after a reset
********************************************************************/
void setup() {
//Set up serial connection at 9600 Baud
Serial.begin(9600);
// Serial.println(getPinVoltage(A4));
//Set up output pins
pinMode(LED_1, OUTPUT);
pinMode(H_BRIDGE_ENA, OUTPUT);
pinMode(LED_3, OUTPUT);
pinMode(H_BRIDGE_ENB, OUTPUT);
pinMode(LED_5, OUTPUT);
pinMode(greenLEDPin,OUTPUT);
pinMode(redLEDPin,OUTPUT);
pinMode(blueLEDPin,OUTPUT);
//Set up input pins
// pinMode(BUTTON_1, INPUT);
// pinMode(BUTTON_2, INPUT);
pinMode(BUTTON_3, INPUT);
// pinMode(BUTTON_4, INPUT);
// pinMode(BUTTON_5, INPUT);
pinMode(BATTERY_PIN, INPUT);
// measuring capacitance lab 4
//pinMode(FILTER_IN_PIN, OUTPUT); // setup pins
//digitalWrite(FILTER_IN_PIN, HIGH); // initially set input as high
//pinMode(FILTER_OUT_PIN,INPUT);
pinMode(CAP_SENSOR_RECEIVE, INPUT);
pinMode(CAP_SENSOR_SEND, OUTPUT);
// Lab 5
pinMode(PHOTO_PIN_1, INPUT);
pinMode(PHOTO_PIN_2, INPUT);
pinMode(PHOTO_PIN_3, INPUT);
pinMode(PHOTO_PIN_4, INPUT);
//Set up servo - Lab 6
myServo.attach(SERVO_PIN);
myServo.write(SERVO_START_ANGLE);
pinMode(TRIGGER_PIN, OUTPUT); // pulse sent out through TRIGGER_PIN
pinMode(ECHO_PIN, INPUT); // return signal read through ECHO_PIN
}
/********************************************************************
Main LOOP function - this gets executed in an infinite loop until
power off or reset. - Notice: PERCEPTION, PLANNING, ACTION
********************************************************************/
void loop() {
// This DebugStateOutput flag can be used to easily turn on the
// serial debugging to know what the robot is perceiving and what
// actions the robot wants to take.
int DebugStateOutput = false; // Change false to true to debug
// float tau1 = fsmComputeTau();
// Serial.println(getPinVoltage(BUTTON_3));
static CapacitiveSensor sensor
= CapacitiveSensor(CAP_SENSOR_SEND, CAP_SENSOR_RECEIVE);
long tau
= sensor.capacitiveSensor(CAP_SENSOR_SAMPLES);
// Serial.print("tau: ");
// Serial.println(tau);
RobotPerception(); // PERCEPTION
if (DebugStateOutput) {
Serial.print("Perception:");
Serial.print(SensedLightUp);
Serial.print(SensedLightLeft);
Serial.print(SensedCollision);
Serial.print(SensedLightRight);
Serial.print(SensedLightDown);
Serial.print(SensedCapacitiveTouch); // Lab 4
}
RobotPlanning(); // PLANNING
if (DebugStateOutput) {
Serial.print(" Action:");
Serial.print(ActionCollision);
Serial.print(ActionRobotDrive);
Serial.print(ActionRobotSpeed); // Lab 4
Serial.println(ActionServoMove);
}
RobotAction(); // ACTION
fsmBatteryMonitor();
analogWrite(redLEDPin, redValue);
analogWrite(greenLEDPin, greenValue);
analogWrite(blueLEDPin, blueValue);
}
/**********************************************************************************************************
Robot PERCEPTION - all of the sensing
********************************************************************/
void RobotPerception() {
// This function polls all of the sensors and then assigns sensor outputs
// that can be used by the robot in subsequent stages
// Photodiode Sensing
//Serial.println(getPinVoltage(BUTTON_2)); //uncomment for debugging
isLightTest(PHOTO_PIN_1);
if (isLight(PHOTO_PIN_2)){
SensedLightLeft = DETECTION_YES;
} else {
SensedLightLeft = DETECTION_NO;
}
// Remember, you can find the buttons and which one goes to what towards the top of the file
if (isLight(PHOTO_PIN_3)) {
SensedLightRight = DETECTION_YES;
} else {
SensedLightRight = DETECTION_NO;
}
// code to determine if light is up or down
if (isLight(PHOTO_PIN_1)) {
SensedLightUp = DETECTION_YES;
} else {
SensedLightUp = DETECTION_NO;
}
if (isLight(PHOTO_PIN_4)) {
SensedLightDown = DETECTION_YES;
} else {
SensedLightDown = DETECTION_NO;
}
// Capacitive Sensor
if (isCapacitiveSensorTouched()) {
SensedCapacitiveTouch = DETECTION_YES;
} else {
SensedCapacitiveTouch = DETECTION_NO;
}
// Collision Sensor
if (isCollision()) { // Add code in isCollision() function for lab 2 milestone 1
SensedCollision = DETECTION_YES;
} else {
SensedCollision = DETECTION_NO;
}
}
void getTau() {
static CapacitiveSensor sensor
= CapacitiveSensor(CAP_SENSOR_SEND, CAP_SENSOR_RECEIVE);
long tau
= sensor.capacitiveSensor(CAP_SENSOR_SAMPLES);
// Serial.println(tau);
return(tau);
}
bool isLight(int pin) {
float light = getPinVoltage(pin);
//Serial.println(light); // Use this line to test
return (light > PHOTODIODE_LIGHT_THRESHOLD);
}
bool isLightTest(int pin) {
float light = getPinVoltage(pin);
Serial.println(light); // Use this line to test
return (light > PHOTODIODE_LIGHT_THRESHOLD);
}
////////////////////////////////////////////////////////////////////
// Function to read pin voltage
////////////////////////////////////////////////////////////////////
float getPinVoltage(int pin) {
//This function can be used for many different tasks in the labs
//Study this line of code to understand what is going on!!
//What does analogRead(pin) do?
//Why is (float) needed?
//Why divide by 1024?
//Why multiply by 5?
return 5 * (float)analogRead(pin) / 1024;
}
////////////////////////////////////////////////////////////////////
// Function to determine if a button is pushed or not
////////////////////////////////////////////////////////////////////
bool isButtonPushed(int button_pin) {
//This function can be used to determine if a said button is pushed.
//Remember that when the voltage is 0, it's only close to zero.
//Hint: Call the getPinVoltage function and if that value is greater
// than the BUTTON_THRESHOLD variable toward the top of the file, return true.
if (getPinVoltage(button_pin) > BUTTON_THRESHOLD){
return true;
} else {
return false;
}
}
////////////////////////////////////////////////////////////////////
// Function that detects if there is an obstacle in front of robot
////////////////////////////////////////////////////////////////////
bool isCollision() {
//This is where you add code that tests if the collision button
// was pushed (BUTTON_3)
//In lab 6 you will add a sonar sensor to detect collision and
// the code for the sonar sensor will go in this function.
// Until then we will use a button to model the sensor.
// if (isButtonPushed(BUTTON_3)) {
// return true;
// } else {
// return false;
// }
int sonar_distance = sonar.ping_cm(); // If the distance is too big, it returns 0.
// Serial.println(sonar_distance);
if(sonar_distance != 0){
return (sonar_distance < 12);
} else {
return false;
}
}
////////////////////////////////////////////////////////////////////
// Function that detects if the capacitive sensor is being touched
////////////////////////////////////////////////////////////////////
bool isCapacitiveSensorTouched() {
static CapacitiveSensor sensor
= CapacitiveSensor(CAP_SENSOR_SEND, CAP_SENSOR_RECEIVE);
long tau
= sensor.capacitiveSensor(CAP_SENSOR_SAMPLES);
if (tau > 750) {
return true;
} else {
return false;
}
}
/**********************************************************************************************************
Robot PLANNING - using the sensing to make decisions
**********************************************************************************************************/
void RobotPlanning(void) {
// The planning FSMs that are used by the robot to assign actions
// based on the sensing from the Perception stage.
fsmCollisionDetection(); // Milestone 1
fsmMoveServoUpAndDown(); // Milestone 3
// fsmBatteryMonitor();
fsmCapacitiveSensorSpeedControl();
// Add Speed Control State Machine in lab 4
}
////////////////////////////////////////////////////////////////////
// State machine for detecting collisions, and stopping the robot
// if necessary.
////////////////////////////////////////////////////////////////////
void fsmCollisionDetection() {
static int collisionDetectionState = 0;
// Serial.println(collisionDetectionState); //uncomment for debugging
switch (collisionDetectionState) {
case 0: //collision detected
//There is an obstacle, stop the robot
ActionCollision = COLLISION_ON; // Sets the action to turn on the collision LED
/* Add code in milestone 2 to stop the robot's wheels - Hint: ActionRobotDrive = ________ */
ActionRobotDrive = DRIVE_RIGHT;
//State transition logic
if ( SensedCollision == DETECTION_NO) {
collisionDetectionState = 1; //if no collision, go to no collision state
}
break;
case 1: //no collision
//There is no obstacle, drive the robot
ActionCollision = COLLISION_OFF; // Sets action to turn off the collision LED
fsmSteerRobot(); // Milestone 2
//State transition logic
if ( SensedCollision == DETECTION_YES) {
collisionDetectionState = 0; //if collision, go to collision state
}
break;
default: // error handling
{
collisionDetectionState = 0;
}
break;
}
}
////////////////////////////////////////////////////////////////////
// State machine for detecting if light is to the right or left,
// and steering the robot accordingly.
////////////////////////////////////////////////////////////////////
void fsmSteerRobot() {
static int steerRobotState = 0;
// Serial.println(steerRobotState); //uncomment for debugging
switch (steerRobotState) {
case 0: //light is not detected
ActionRobotDrive = DRIVE_STOP;
//State transition logic
if ( SensedLightLeft == DETECTION_YES ) {
steerRobotState = 1; //if light on left of robot, go to left state
} else if ( SensedLightRight == DETECTION_YES ) {
steerRobotState = 2; //if light on right of robot, go to right state
}
break;
case 1: //light is to the left of robot
//The light is on the left, turn left
ActionRobotDrive = DRIVE_LEFT; //Add appropriate variable to set the action to turn left
//State transition logic
if ( SensedLightRight == DETECTION_YES ) {
steerRobotState = 3; //if light is on right, then go straight
} else if ( SensedLightLeft == DETECTION_NO) {
steerRobotState = 0; //if light is not on left, go back to stop state
}
break;
case 2: //light is to the right of robot
//The light is on the right, turn right
ActionRobotDrive = DRIVE_RIGHT;
//State transition logic
if (SensedLightLeft == DETECTION_YES) {
steerRobotState = 3;
} else if (SensedLightRight == DETECTION_NO) {
steerRobotState = 0;
}
break;
// light is on both right and left
case 3:
ActionRobotDrive = DRIVE_STRAIGHT;
if (SensedLightLeft == DETECTION_NO) {
steerRobotState = 2;
} else if (SensedLightRight == DETECTION_NO) {
steerRobotState = 1;
}
break;
default: // error handling
{
steerRobotState = 0;
}
}
}
////////////////////////////////////////////////////////////////////
// State machine for detecting if light is above or below center,
// and moving the servo accordingly.
////////////////////////////////////////////////////////////////////
void fsmMoveServoUpAndDown() {
static int moveServoState = 0;
// Serial.println(moveServoState); //uncomment for debugging
switch(moveServoState) {
case 0:
ActionServoMove = SERVO_MOVE_STOP;
if (SensedLightUp == DETECTION_YES & SensedLightDown == DETECTION_YES) {
moveServoState = 0;
} else if (SensedLightDown == DETECTION_YES) {
moveServoState = 2;
} else if (SensedLightUp == DETECTION_YES) {
moveServoState = 1;
}
break;
case 1:
ActionServoMove = SERVO_MOVE_UP;
if (SensedLightUp == DETECTION_NO) {
moveServoState = 0;
} else if (SensedLightDown == DETECTION_YES) {
moveServoState = 0;
}
break;
case 2:
ActionServoMove = SERVO_MOVE_DOWN;
if (SensedLightDown == DETECTION_NO) {
moveServoState = 0;
} else if (SensedLightUp == DETECTION_YES) {
moveServoState = 0;
}
break;
}
// Milestone 3
//Create a state machine modeled after the ones in milestones 1 and 2
// to plan the servo action based off of the perception of the robot
//Remember no light or light in front = servo doesn't move
//Light above = servo moves up
//Light below = servo moves down
}
////////////////////////////////////////////////////////////////////
// State machine for detecting when the capacitive sensor is
// touched, and changing the robot's speed.
////////////////////////////////////////////////////////////////////
void fsmCapacitiveSensorSpeedControl() {
static int SpeedControlState = 0;
//Serial.println(SpeedControlState);
switch(SpeedControlState) {
case 0:
if (SensedCapacitiveTouch == DETECTION_YES) {
SpeedControlState = 1;
}
break;
case 1:
if (SensedCapacitiveTouch == DETECTION_NO) {
SpeedControlState = 2;
}
break;
case 2:
fsmChangeSpeed();
SpeedControlState = 0;
break;
}
}
////////////////////////////////////////////////////////////////////
// State machine for cycling through the robot's speeds.
////////////////////////////////////////////////////////////////////
void fsmChangeSpeed() {
static int SpeedLevel = 0;
// Serial.println(SpeedLevel);
switch(SpeedLevel) {
case 0:
ActionRobotSpeed = SPEED_OFF;
SpeedLevel = 1;
break;
case 1:
ActionRobotSpeed = SPEED_33;
SpeedLevel = 2;
break;
case 2:
ActionRobotSpeed = SPEED_66;
SpeedLevel = 3;
break;
case 3:
ActionRobotSpeed = SPEED_100;
SpeedLevel = 0;
break;
}
}
void fsmBatteryMonitor() {
static int BatteryState = 0;
currentV = getPinVoltage(BATTERY_PIN);
// Serial.println(currentV);
switch(BatteryState) {
// full charged
case 0:
// find voltage and compare
redValue = 0;
greenValue = 255;
blueValue = 0;
if (currentV < 4.2) {
BatteryState = 1;
}
break;
// medium charge
case 1:
redValue = 0;
greenValue = 0;
blueValue = 255;
if (currentV < 3.75) {
BatteryState = 2;
} else if (currentV > 4.2) {
BatteryState = 0;
}
break;
// low charge
case 2:
redValue = 255;
greenValue = 0;
blueValue = 0;
if (currentV < 3.20) {
BatteryState = 3;
} else if (currentV > 3.75) {
BatteryState = 1;
}
break;
// need to replace
case 3:
redValue = 0;
greenValue = 0;
blueValue = 0;
if (currentV > 3.20) {
BatteryState = 2;
}
break;
}
}
/**********************************************************************************************************
Robot ACTION - implementing the decisions from planning to specific actions
********************************************************************/
void RobotAction() {
// Here the results of planning are implented so the robot does something
// This turns the collision LED on and off
switch(ActionCollision) {
case COLLISION_OFF:
doTurnLedOff(LED_3, 0); //Collision LED off - DON'T FORGET TO ADD CODE TO doTurnLedOff()
// AND doTurnLedOn() OR ELSE YOUR LEDS WON'T WORK!!!
break;
case COLLISION_ON:
doTurnLedOn(LED_3, 255);
/* Add code to turn the collision LED on. This would be LED_3 */
break;
}
// This drives the main motors on the robot
switch(ActionRobotDrive) {
case DRIVE_STOP:
doTurnLedOff(H_BRIDGE_ENA, 0);
doTurnLedOff(H_BRIDGE_ENB, 0);
break;
case DRIVE_LEFT:
doTurnLedOff(H_BRIDGE_ENB, 0);
doTurnLedOn(H_BRIDGE_ENA, ActionRobotSpeed);
break;
case DRIVE_RIGHT:
doTurnLedOff(H_BRIDGE_ENA, 0);
doTurnLedOn(H_BRIDGE_ENB, ActionRobotSpeed);
break;
case DRIVE_STRAIGHT:
doTurnLedOn(H_BRIDGE_ENA, ActionRobotSpeed);
doTurnLedOn(H_BRIDGE_ENB, ActionRobotSpeed);
break;
}
// This calls a function to move the servo
MoveServo();
}
////////////////////////////////////////////////////////////////////
// Function that causes the servo to move up or down.
////////////////////////////////////////////////////////////////////
void MoveServo() {
// Note that there needs to be some logic in the action of moving
// the servo so that it does not exceed its range
/* Add CurrentServoAngle in lab 6 */
static int CurrentServoAngle = SERVO_START_ANGLE;
switch(ActionServoMove) {
case SERVO_MOVE_STOP:
doTurnLedOff(LED_1, 0);
doTurnLedOff(LED_5, 0);
break;
case SERVO_MOVE_UP:
doTurnLedOff(LED_5, 0);
doTurnLedOn(LED_1, 255);
if (CurrentServoAngle < SERVO_UP_LIMIT) {
CurrentServoAngle += 1;
}
break;
case SERVO_MOVE_DOWN:
doTurnLedOff(LED_1, 0);
doTurnLedOn(LED_5, 255);
if (CurrentServoAngle > SERVO_DOWN_LIMIT) {
CurrentServoAngle -= 1;
}
break;
}
myServo.write(CurrentServoAngle);
}
/**********************************************************************************************************
AUXILIARY functions that may be useful in performing diagnostics
********************************************************************/
// Function to turn LED on
void doTurnLedOn(int led_pin, int voltage)
{
analogWrite(led_pin, voltage);
}
// Function to turn LED off
void doTurnLedOff(int led_pin, int voltage)
{
analogWrite(led_pin, voltage);
}
// Lab 4 Capacitance functions
// function to compute tau
float computeTau(float measuredTime, float beta)
{
float tau1 = measuredTime / log((1 - beta) / beta);
return(tau1);
}
// functions that returns voltage on pin
float pinVoltage(int pin)
{
float v_pin = 5.0 * ( (float) analogRead(pin) / 1024.0 );
return(v_pin);
}
float fsmComputeTau() // state machine to compute tau
{
static int state = 0;
static float tau1=0;
static unsigned long startTime = millis(); // for time in state
unsigned long currentTime;
float capVoltage = pinVoltage(CAP_SENSOR_RECEIVE); // voltage across capacitor
// Serial.println(capVoltage); // plot voltage vs. time
float Vcc = 5; // pin voltage when HIGH
float beta = 0.1; // beta: switching threshold
switch (state)
{
case 0: // charging capacitor (pin is HIGH)
if (capVoltage >= (1-beta)*Vcc){
state = 1; // next state
currentTime = millis();
float T = ((float)(currentTime-startTime))/1000.0;
startTime = currentTime;
tau1 = computeTau(T, beta);
digitalWrite(CAP_SENSOR_SEND, LOW);
}
break;
case 1: // discharging capacitor (pin is Low)
if (capVoltage <= beta*Vcc){
state = 0; // next state
currentTime = millis();
float T = ((float)(currentTime-startTime))/1000.0;
startTime = currentTime;
tau1 = computeTau(T, beta);
digitalWrite(CAP_SENSOR_SEND, HIGH);
}
break;
default:
state=0;
break;
}
// Serial.print("tau: ");
// Serial.println(tau);
return(tau1);
}