EE346/EE444 Software Translation to EE400D

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/

0 replies

Leave a Reply

Want to join the discussion?
Feel free to contribute!

Leave a Reply

Your email address will not be published. Required fields are marked *