By: Matt Shellhammer (Electronics & Control Engineer for ModWheels) & Mark Huffman (Project Manager for Goliath)
Approved by: Lucas Gutierrez (Project Manager for ModWheels)
Introduction
This software is simply an outline and requires modification for each project. The software translation is to ease the implementation of your projects specific software such as turning, detection, line following, etc. Some subroutines are completed (e.g. the virtual instructions: turnInMaze, stepInMaze, and roomInMaze) and others are left empty for each project to develop their specific robot implementations.
Software
400D_Software_Translation.ino (MAIN SETUP & LOOP)
////////////////////////////////////////////////////////////////
// Name : 400D Software Translation //
// Author : Matt Shellhammer & Mark Huffman //
// Date : 29 November, 2017 //
// Version : 1.0 //
////////////////////////////////////////////////////////////////
#define __PROG_TYPES_COMPAT__ // __PROG_TYPES_COMPAT__
#include <avr/pgmspace.h>
#include <Robot3DoTBoard.h>
#include <EEPROM.h>
#include <Wire.h>
#include <Servo.h>
#include "maze.h"
Robot3DoTBoard Robot3DoT; // define a 3DoT Robot instance
Motor mA; // define a motor A instance
Motor mB; // define a motor B instance
// Define motor driver pins
#define AIN1 5
#define BIN1 19
#define AIN2 10
#define BIN2 20
#define PWMA 9
#define PWMB 6
#define STBY A0
// Define encoder pins for specific 400D project
#define encoderL A3
#define encoderR A4
void setup() {
Serial.begin(9600);
Robot3DoT.begin();
// Define motor driver pins
mA.begin(AIN1,AIN2,PWMA);
mB.begin(BIN1,BIN2,PWMB);
// Make Encoder pins inputs
pinMode(encoderL,INPUT);
pinMode(encoderR,INPUT);
delay(5000);
}
void loop() {
static uint8_t type;
static myRobot_t robot_inst; // create an instance of myRobot_t called robot_inst
static sensors_t sensors_inst; // create an instance of sensors_t called sensors_inst
static motors_t motors_inst; // create an instance of motors_t called motors_inst
static PID_t PID_inst; // create an instance of PID_t called PID_inst
sensors_inst = readSensors(sensors_inst); // Read sensor values
PID_inst = PIDcalc(sensors_inst, PID_inst); // Calculate PID value
motors_inst = errorCorrection(PID_inst, motors_inst); // Correct for error with motors
forward(motors_inst); // send new speed to motors to move forward
// Check if at an intersection (NOTE: MUST BE MODIFIED FOR EACH PROJECT, THIS IS JUST HOW I DID IT)
float sensorAvg = (sensors_inst.IR0 + sensors_inst.IR1 - sensors_inst.IR2)/3.0;
if (sensorAvg > 0.125){
robot_inst = enterRoom(robot_inst);
type = roomType(robot_inst);
robot_inst.dir = whichWay(type, robot_inst);
}
}
maze.h (Structure, array, and variable definitions)
// PID constants (NOTE: MUST BE MODIFIED FOR EACH PROJECT, THIS IS JUST HOW I DID IT)
const uint8_t Kp = 100;
const float Ki = 0.1;
const uint8_t Kd = 50;
const uint8_t base_speed = 60;
struct coord_t{
uint8_t row = 0x13; // Robot is initially outside of the maze
uint8_t col = 0x00; // Robot is initially outside of the maze
};
struct myRobot_t{
uint8_t dir = 0x03; // Robot is initially facing north
uint8_t turn = 0x00; // First action is no turn
coord_t maze;
uint8_t room = 0x00; // Initial room is empty
uint8_t bees = 0x00; // No bees present
};
struct sensors_t{
float IR0; // Left IR sensor (normalized)
float IR1; // Middle IR sensor (normalized)
float IR2; // Right IR sensor (normalized)
};
struct motors_t{
int16_t leftSpeed = 60; // Initial motor speed: 60
int16_t rightSpeed = 60; // Initial motor speed: 60
};
// Define PID structure
struct PID_t{
float present = 0;
float set_point = 0;
float proportional = 0;
float integral = 0;
float derivative = 0;
int16_t PIDvalue = 0;
float error = 0;
float previous_error = 0;
};
const uint8_t hit_table[] PROGMEM =
{0x08, // South (dir == 0b00)
0x02, // East (dir == 0b01)
0x04, // West (dir == 0b10)
0x01}; // North (dir == 0b11)
//Compass S E W N
//dir 00 01 10 11
const uint8_t turn_table[] PROGMEM =
{0b00, 0b01, 0b10, 0b11, // 00 no turn
0b10, 0b00, 0b11, 0b01, // 01 turn right
0b01, 0b11, 0b00, 0b10, // 10 turn left
0b11, 0b10, 0b01, 0b00 // 11 turn around
};
// row col dir
const int8_t map_table[] PROGMEM =
{1 , 0, // 00
0 , 1, // 01
0 , -1, // 10
-1 , 0 // 11
};
const int maze_length = 399;
const uint8_t theMaze[] PROGMEM =
// 00 01 02 03 04 05 06 07 08 09 0A 0B 0C 0D 0E 0F 10 11 12 13 14
{0x05,0x09,0x09,0x09,0x09,0x09,0x01,0x03,0x05,0x09,0x09,0x09,0x09,0x09,0x09,0x09,0x29,0x09,0x09,0x09,0x02, // 00
0x0C,0x09,0x09,0x03,0x05,0x09,0x0A,0x06,0x06,0x05,0x09,0x09,0x09,0x09,0x09,0x09,0x09,0x03,0x05,0x03,0x06, // 01
0x05,0x09,0x0B,0x06,0x06,0x05,0x09,0x0A,0x06,0x0C,0x09,0x09,0x09,0x09,0x09,0x01,0x0B,0x0C,0x0A,0x06,0x06, // 02
0x06,0x0D,0x09,0x0A,0x06,0x06,0x05,0x03,0x0C,0x09,0x09,0x03,0x05,0x09,0x09,0x0A,0x05,0x09,0x09,0x08,0x02, // 03
0x06,0x05,0x09,0x09,0x0A,0x06,0x06,0x0C,0x09,0x09,0x09,0x0A,0x0C,0x09,0x09,0x03,0x06,0x05,0x09,0x09,0x0A, // 04
0x06,0x0C,0x03,0x05,0x09,0x02,0x06,0x05,0x09,0x09,0x09,0x09,0x09,0x09,0x03,0x06,0x06,0x0C,0x03,0x05,0x03, // 05
0x06,0x05,0x0A,0x0C,0x03,0x06,0x06,0x06,0x05,0x01,0x03,0x07,0x05,0x03,0x06,0x06,0x06,0x05,0x0A,0x06,0x06, // 06
0x06,0x0C,0x09,0x03,0x0E,0x0C,0x08,0x02,0x06,0x06,0x06,0x06,0x06,0x06,0x0C,0x02,0x06,0x0C,0x09,0x02,0x06, // 07
0x06,0x05,0x0B,0x0C,0x09,0x09,0x09,0x08,0x02,0x06,0x06,0x06,0x06,0x0C,0x09,0x0A,0x04,0x09,0x0B,0x06,0x06, // 08
0x0C,0x08,0x09,0x09,0x09,0x09,0x01,0x01,0x02,0x06,0x0C,0x08,0x08,0x09,0x01,0x09,0x08,0x09,0x03,0x06,0x06, // 09
0x05,0x01,0x09,0x09,0x0B,0x07,0x06,0x04,0x02,0x0C,0x09,0x09,0x09,0x03,0x04,0x09,0x03,0x07,0x06,0x06,0x06, // 0A
0x06,0x0C,0x09,0x09,0x09,0x02,0x06,0x04,0x02,0x0D,0x09,0x09,0x09,0x0A,0x0C,0x03,0x06,0x06,0x06,0x06,0x06, // 0B
0x06,0x05,0x09,0x09,0x09,0x0A,0x06,0x0C,0x0A,0x05,0x09,0x09,0x09,0x09,0x03,0x06,0x06,0x06,0x06,0x06,0x06, // 0C
0x06,0x0C,0x09,0x09,0x09,0x03,0x04,0x09,0x09,0x08,0x0B,0x05,0x03,0x05,0x0A,0x06,0x06,0x06,0x06,0x06,0x06, // 0D
0x04,0x09,0x09,0x09,0x09,0x08,0x02,0x05,0x01,0x09,0x03,0x06,0x06,0x06,0x05,0x0A,0x0E,0x06,0x06,0x06,0x06, // 0E
0x06,0x05,0x09,0x09,0x09,0x09,0x0A,0x0E,0x06,0x07,0x06,0x06,0x06,0x06,0x06,0x05,0x09,0x0A,0x06,0x06,0x06, // 0F
0x06,0x0C,0x09,0x09,0x09,0x09,0x09,0x09,0x0A,0x06,0x06,0x06,0x06,0x0E,0x0E,0x06,0x05,0x09,0x0A,0x06,0x06, // 10
0x04,0x09,0x09,0x09,0x09,0x09,0x09,0x09,0x09,0x0A,0x0C,0x0A,0x06,0x05,0x09,0x0A,0x06,0x0D,0x09,0x0A,0x06, // 11
0x04,0x09,0x09,0x09,0x09,0x09,0x09,0x09,0x09,0x09,0x09,0x09,0x08,0x08,0x09,0x09,0x08,0x09,0x09,0x09,0x0A, // 12
};
subroutines.ino
myRobot_t enterRoom(myRobot_t robot){
robot = turnInMaze(robot);
robot = stepInMaze(robot);
robot = roomInMaze(robot);
return robot;
}
// Returns updated direction based on current direction and turn value
// values returned in robot structure
myRobot_t turnInMaze(myRobot_t robot){
// index = 4*turn_val + dir_val
uint8_t index = (robot.turn << 2) + robot.dir;
robot.dir = pgm_read_byte_near(turn_table + index);
return robot;
}
// Returns updated row and column values after taking a step in current direction
// values returned in robot structure
myRobot_t stepInMaze(myRobot_t robot){
// index = 2*robot.dir
uint8_t index = (robot.dir << 1);
robot.maze.row += pgm_read_byte_near(map_table + index); // Add either -1, 0, or 1 to current row value
robot.maze.col += pgm_read_byte_near(map_table + index + 1); // Add either -1, 0, or 1 to current column value
return robot;
}
// Returns updated room and bees values using current row and column values
// values returned in robot structure
myRobot_t roomInMaze(myRobot_t robot){
// index = 21*robot.maze.row + robot.maze.col
uint16_t index = (21*robot.maze.row) + robot.maze.col;
uint8_t maze_val = pgm_read_byte_near(theMaze + index);
robot.room = maze_val & 0x0F; // clear upper nibble and store as the room value
uint8_t temp_bees = (maze_val & 0xF0) >> 4; // clear lower nibble and store as the temp bees value
robot.bees += temp_bees; // add temp_bees to curret bees value
return robot;
}
// Room Type subroutine
uint8_t roomType(myRobot_t robot){
bool leftWall = leftHit(robot); // Test if hiting left wall
bool hit = hitWall(robot); // Test if facing wall
bool rightWall = rightHit(robot); // Test if hiting right wall
uint8_t room = (uint8_t(leftWall) << 2)|(uint8_t(hit) << 1)|uint8_t(rightWall); // Convert to room type
return room;
}
// Returns true if there is a wall and false if there is no wall
bool hitWall(myRobot_t robot){
// index = dir_val
robot = roomInMaze(robot); // Determine room value
uint8_t wallVal = pgm_read_byte_near(hit_table + robot.dir); // Determine wall bit based on direction
uint8_t outVal = bool(wallVal & robot.room); // Clear all bits other than the wall robot is facing
if (outVal == 0){return false;} // If the robot is not hiting a wall outVal will equal zero
else {return true;} // and the subroutine will return false, else it returns true.
}
// Returns true if there is a wall and false if there is no wall
// on the right side of the robot
bool rightHit(myRobot_t robot){
robot.turn = 0x01; // Modify turn value to turn right
robot = turnInMaze(robot); // Call turnInMaze to turn the robot (virtually)
bool hit = hitWall(robot); // Test hit wall
return hit;
}
// Returns true if there is a wall and false if there is no wall
// on the left side of the robot
bool leftHit(myRobot_t robot){
robot.turn = 0x02; // Modify turn value to turn left
robot = turnInMaze(robot); // Call turnInMaze to turn the robot (virtually)
bool hit = hitWall(robot); // Test hit wall
return hit;
}
sensors_t readSensors(sensors_t sensors){
// Write software to read color sensors/color shield
}
// NOTE: MUST BE MODIFIED FOR EACH PROJECT, THIS IS JUST HOW I DID IT
PID_t PIDcalc(sensors_t sensors, PID_t PID){
// Calculates error
PID.present = sensors.IR0 + sensors.IR2;
PID.error = PID.present - PID.set_point;
// Calculates proportional error
PID.proportional = PID.error;
// Calculates integrated error
PID.integral += PID.error;
// Calculate change in error
PID.derivative = PID.error - PID.previous_error;
PID.previous_error = PID.error;
// Calculate compensator value
// this should be an integer from -128 to 127 which is why the casting function is used
PID.PIDvalue = int8_t(Kp*PID.proportional + Ki*PID.integral + Kd*PID.derivative);
return PID;
}
// NOTE: MUST BE MODIFIED FOR EACH PROJECT, THIS IS JUST HOW I DID IT
// errorCorrection: corrects motor speed based on PID value (limits between <50,220>)
motors_t errorCorrection(PID_t PID, motors_t motors){
motors.leftSpeed = base_speed - PID.PIDvalue;
motors.rightSpeed = base_speed + PID.PIDvalue;
// Limit the left and right motor speeds between <50,220>
if (motors.rightSpeed > 220) {motors.rightSpeed = 220;}
if (motors.leftSpeed > 220) {motors.leftSpeed = 220;}
if (motors.rightSpeed < 50) {motors.rightSpeed = 50;}
if (motors.leftSpeed < 50) {motors.leftSpeed = 50;}
return motors;
}
void readEncoders(uint16_t numCount){
// Write readEncoders for specific 400D project
}
uint8_t whichWay(MyRobot mazeBot){
// Automatically handles intersections where no decision is needed
// the rest need to be defined
// TODO: Needs recording decisions added
// TODO: Needs playback added
switch(roomType(mazeBot)){
case 0:
// 4 way Intersection
// TODO: Decision Needed!
return 0x00;
case 1:
// T-Intersection (Wall to right)
// TODO: Decision Needed!
return 0x00;
case 2:
// T-Intersection (Wall to Left)
// TODO: Decision Needed!
return 0x00;
case 3:
// Hallway Continue Forward
return 0x00;
case 4:
// T-Intersection (Wall in Front)
// TODO: Decision Needed!
return 0x00;
case 5:
// Left Turn (Turn Left)
return 0x02;
case 6:
// Right Turn (Turn Right)
return 0x01;
case 7:
// Dead End (Turn Around)
return 0x03;
}
}
}
void forward(motors_t motors){
// Write forward for specific 400D project
}
void turn_right() {
// Write turn_right for specific 400D project
}
void turn_left() {
// Write turn_left for specific 400D project
}
void turn_around() {
// Write turn_around for specific 400D project
}
void go_straight() {
digitalWrite(STBY, HIGH);
digitalWrite(AIN2, HIGH);
digitalWrite(AIN1, LOW);
digitalWrite(BIN1, LOW);
digitalWrite(BIN2, HIGH);
analogWrite(PWMA, base_speed);
analogWrite(PWMB, base_speed);
}
void stopRobot(uint16_t d) {
digitalWrite(STBY, HIGH);
digitalWrite(AIN2, HIGH);
digitalWrite(AIN1, LOW);
digitalWrite(BIN1, HIGH);
digitalWrite(BIN2, LOW);
analogWrite(PWMA, 0);
analogWrite(PWMB, 0);
delay(d);
}
Software Explanation
Structures & Variables
maze.h is where all structures, arrays and variables are defined, and they are defined as follows:
PID constants
These constants are Kp, Ki, Kd, and the motors base speed for the subroutines “PIDcalc” and “errorCorrection”.
coord_t and myRobot_t
This is where all variables are stored for the virtual instructions of the maze. These structures hold: row, column, direction, turn, room, and bees.
sensors_t, motors_t, and PID_t
sensors_t is a structure to hold values of the sensors. This can be modified for color sensors, proximity sensors, etc. motors_t is a structure for the left and right motor speeds. PID_t is a structure to hold all values for the PID calculations within the subroutine “PIDcalc”.
hit_table
This array is used in the subroutine “hitWall” to clear all bit other than the bit representing the wall that the robot is facing.
turn_table
This array is used to implement turns within “turnInMaze”.
map_table
This array is used within “stepInMaze” to modify row and column.
theMaze
This array is the virtual maze storing all room values throughout the maze.
Subroutines
subroutines.ino is where all structures, arrays and variables are defined, and they are defined as follows:
enterRoom, turnInMaze, stepInMaze, and roomInMaze
enterRoom takes the robot instance structure as a parameter and and calls turnInMaze, stepInMaze, and roomInMaze using that structure as an argument for each subroutine and then returns an updated robot instance. turnInMaze updates the direction of the robot instance based on the current direction and the turn value. stepInMaze updates row and column based on the robots direction. roomInMaze updates room and bees values using current row and column values.
roomType, hitWall, rightHit, and leftHit
roomType calls hitWall, rightHit, and leftHit to then determine the room type based on the direction of the robot. hitWall determines if the robot is hitting a wall, right and left hit determine if there is a wall on the right or left side of the robot.
PIDcalc and errorCorrection
PIDcalc uses the sensor reading to calculate a PID value which will then be used to modify the motor speeds within the errorCorrection.
whichWay
Same as in EE 346.
References
http://web.csulb.edu/~hill/ee444/Labs/
http://web.csulb.edu/~hill/ee346/Labs/CSULB%20Shield/
http://web.csulb.edu/~hill/ee346/Labs/PaperBot%203DoT/
