Fall 2017: ModWheels Final Project Document

/in /by

By: Lucas Gutierrez (Project Manager)

Table of Contents

Project Overview

Team

Fall 2017 ModWheels:

Project Manager: Lucas Gutierrez

Mission, Systems, & Test Engineer: Andrew Yi

Electronics & Control Engineer: Matt Shellhammer

Design & Manufacturing Engineer: Natalie Arevalo

Summary

The Fall 2017 ModWheels toy car is a new project within The Robot Company.  Its modular design comes from the changeable paper overlay, allowing the user to swap out to their preferred design.  To control the robot, a 3DoT micro-controller is used to connect the user to the robot via Arxterra, a platform for easy robotics integration.  A sensor suite will enable this to navigate a 2D maze, as defined by the customer. Infrared sensors gives ModWheels the feature of line  of the maze, keeping the toy car within the confines of the maze hallways. To avoid collision, a proximity sensor  gives ModWheels the ability to detect the other robots, which allow for the robot to avoid collision within the maze.  

Mission Objective for Fall 2017

ModWheels is a toy car that will navigate a multi-colored 2D maze using autonomous capabilities as well as Arxterra to guide the toy car through the maze. The initial phase consists of having the toy car navigate the maze with user input, to which ModWheels will then memorize the route taken during the initial phase and will be able to autonomously navigate the maze for the second phase, all while being able to detect other robots and avoid collision. These are the mission objectives stated by the customer.

System Design

ModWheels is sized to fit the maze as defined by the customer for the Fall 2017 EE 400D semester.  This final document will detail the mechanical design, electronics and control, and sensor suite that made ModWheels the project that it is.

Figure 1: Fall 2017 ModWheels Prototype Used for Final Mission

Subsystem Design

Modeling Results

The use of SolidWorks helped to the ModWheels project design and model the chassis, steering mechanisms, and holders implemented onto the toy car.  Tire treads were designed, modeled, and provided by Jeff Gomes.

Figure 2: Fall 2017 ModWheels 3D Model

Fall 2017: ModWheels 3D Model

Interface Definition

To help satisfy customer requirements, a system block diagram defines the capabilities of the ModWheels electronics and control.  The interface matrix also verifies that the system block diagram and actual implementation of the ModWheels go hand in hand.

Figure 3: Fall 2017 ModWheels System Block Diagram for 3DoT v. 5.03

Figure 4: Fall 2017 ModWheels System Block Diagram for Pro Micro

Figure 5: Fall 2017 ModWheels Interface Matrix

Fall 2017: Interface Definitions

Mission Command and Control

An important aspect in fulfilling ModWheel’s mission requirements is integration with the Arxterra platform, both with the phone application and web based application.  To tailor and customize the user experience of the Arxterra applications to the ModWheels project, a few custom commands and telemetry will be incorporated. ModWheels would have implemented a 4 state custom command on the Arxterra GUI.

ModWheels Custom Command and Telemetry

Electronics Design

In the design phase, ModWheels was design to electronically and mechanically adapt to the v. 5.03 3DoT, which was to be provided by the EE 400D .  Due to complications with the fabricated 3DoT v5.03 and the limited amount of time we were required to resort to the use of the Sparkfun pro micro and motor driver for the final implementation.

Figure 6: ModWheels Cable Diagram for Pro Micro (Used on mission)

Fall 2017: ModWheels Electronics Design

Firmware

The software is one of the most important factors within the ModWheels project for maze navigation.  With the help of Matt Shellhammer (Electronics & Control Engineer for ModWheels), an adaptation for maze navigation from EE 444 to EE 400D was made.  Since the 3DoT was not used on the mission due to complications, a more simplified code was implemented on the prototyping board.

Fall 2017: ModWheels Final Software

PCB Schematic & Layout

Among the requirements set by the customer, all projects must design and fabricate a custom PCB. However, due to the nature of our project and its design, a custom PCB is not necessary. In order to get the requirement of the custom PCB waived, a cable diagram for our project was provided. The description and diagram show that the components within our design can be directly connected into the 3Dot board without being interfaced through a custom PCB.

Figure 7: ModWheels Cable Diagram for v. 5.03 3DoT

Fall 2017: ModWheels Cable Diagram

Hardware Design

The ModWheels toy robot implements a blend between 3D printed components, as well as laser cut wood.  This blend of materials helps to optimize cost and time (wood being inexpensive and quickly cut by a laser cutter) while keeping functionality (3D printed steering mechanism and engine block to hold the servo).

Figure 8: ModWheels Fall 2017 Prototype used on mission

Fall 2017: ModWheels Hardware Design

Verification & Validation Test Overview/Summary

To confirm implementation of requirements, verification and validation of  requirements of the ModWheels project was performed.  To perform verification and validation of the requirements, a verification list and a validation test procedure was created.

Fall 2017: ModWheels Verification & Validation

Project Status

Power Allocation

As requested from the customer, the ModWheels will be implementing the 3DoT micro-controller.  Due to the nature of the 3DoT, power restrictions must be made to ensure proper operation and performance. The power budget not only gives a worst case scenario of power consumption, but also helps to provide numbers to calculate mission duration, a very important factor to mission success.

ModWheels Power Budget

3D Print Times

As set by the customer, there is a limit on the time allotted for the parts that will be 3D printed for the robots. The rule for 3D printing is that total print time must not exceed six hours, with no single part taking more than two hours to print. Once the 3D models were finalized and sent to be printed, a full rundown of labor, time, and cost were provided, which gives ModWheels final print time results.

Figure 9: Final print time summary

Fall 2017: ModWheels Print Time

Cost Report

As part of the requirements stated by the customer, a budget was set for the ModWheels project for no more than $200.00 .  To help cut some costs, the EE 400D previous semester’s inventory of electronics, hardware, and materials were provided for use.  The Budget report includes total cost of the project as well as cost of the project offset by inventory.

Figure 10: Fall 2017 ModWheels Budget

Fall 2017: ModWheels Budget

Schedule

To help keep ModWheels on schedule, a project plan was created. The project planning along with the project burndown helps to give an estimation to work performed and work needed to complete the project.

Figure 11: Fall 2017 ModWheels Top Level Schedule

Figure 12: Fall 2017 ModWheels Project Burndown

Fall 2017: ModWheels Schedule

 

Future Work

As the Fall 2017 semester concludes, a list of tasks have been compiled for following semesters to help further the ModWheels project and improve the 400D class.

  1. Fix IR shield to get improved readings and improve line following (change resistor values and refabricate as well as narrowing the distance between inside IR sensors to improve line following).
  2. Implement 3DoT v5.03 within ModWheels.
  3. Improved Steering
    1. Improve steering mechanism on ModWheels chassis to improve steadiness of steering.
    2. Change screws holding steering mechanism together to clamps or something that locks together so they steering mechanism won’t unscrew as it moves and turns (similar to what holds the 3DoT into place on the ModWheels chassis).
    3. Reduce size of servo holder to have a tight fit for the servo so it won’t move as it tries to move the steering mechanism.
  4. Improve software to implement Arxterra control of the robot (implement and test software developed for 3DoT v5.03 ModWheels after v5.03 is fixed).
  5. Improve power budget (more testing with servo to determine current draw and effectiveness at different supply voltages).
  6. Design modifications to allow for software upload while 3DoT is in the ModWheels chassis.

Fall 2017: ModWheels Schedule

/in /by

By: Lucas Gutierrez (Project Manager)

Discussion

To help keep ModWheels on schedule, a project plan was created. The project planning along with the project burndown helps to give an estimation to work performed and work needed to complete the project.

Schedule

Figure 1: Fall 2017 ModWheels Schedule

Figure 2: Fall 2017 ModWheels Planning

Figure 3: Fall 2017 ModWheels Design (1 of 2)

Figure 4: Fall 2017 ModWheels Design (2 of 2)

Figure 5: Fall 2017 ModWheels Assembly

Figure 6: Fall 2017 ModWheels Launch

Figure 7: Fall 2017 ModWheels Project Burndown

Fall 2017: Interface Definitions

/in /by

By: Andrew Yi (Mission, System, & Test Engineer) & Matt Shellhammer (Electronics & Control Engineer)

Approved by: Lucas Gutierrez (Project Manager)

Discussion

An important factor to operating the ModWheels project is linked to the connectivity of the micro controller and the sensor suite on the ModWheels chassis.

System Block Diagram

Due to unforeseen circumstances outside of our control, ModWheels was unable to implement a v. 5.03 3DoT micro controller.  Instead, the board used for prototyping was used, the SparkFun Pro Micro.  The ProMicro microcontroller replaced the 3DoT, but uses the same chip (ATMEGA 32u4).  3DoT implementation should be simple because of the similarities of the 2 microcontrollers. The encoders are connected via analog, sending data to the microcontroller, which then transmits data to the motor driver.  1.5V AA batteries power the toy robot via breadboard connections.

Figure 1: 3DoT v. 5.03 System Block Diagram

Figure 2: Pro Micro System Block Diagram

Interface Matrix

In this interface matrix we show how all peripheral electrical components will be connected to the 3DoT v5.03. ModWheels has a total of six peripheral devices, these devices are a micro servo, two magnetic encoders, two extended shaft GM6 motors, and a IR proximity sensor. These were all to be connected to the 3DoT without a PCB, however instead we were going to use a through hole breadboard to solder headers onto and create more Vcc and GND connections. With this in mind no PCB was required and all peripheral devices have a connection to the 3DoT with extra space for two more analog inputs. Only one channel of the quadrature magnetic encoders were used since the resolution was fine enough to make turns however if there was ever a need to include the second channel for the encoders there is still the remaining two analog connections on the sensor header.

Figure 3: Interface Matrix

Fall 2017: ModWheels Verification & Validation

/in /by

By: Andrew Yi (Mission, Systems, & Test Engineer)

Approved by: Lucas Gutierrez (Project Manager)

Introduction

To confirm implementation of requirements, verification and validation of  requirements of the ModWheels must be performed.  To perform verification and validation of the requirements, a verification list and a validation test procedure was created.

Verification & Validation

ModWheels Verification & Validation Plan

Verification & Validation: List & Test Procedures

ModWheels Verification & Validation: List & Test Procedures

 

Fall 2017: ModWheels Electronics Design

/in /by

By: Matt Shellhammer (Electronics & Control Engineer)

Approved by: Lucas Gutierrez (Project Manager)

Discussion

Due to complications with the fabricated 3DoT v5.03 and the limited amount of time we were required to resort to the use of the Sparkfun pro micro and motor driver for the final implementation. The IR sensor shield was used in our design to preform line following throughout the maze to keep the car within the maze and on a straight path. The IR sensor shield was also used to detect intersections so ModWheels could monitor its location within the maze and travel the correct path (either predefined or defined with Arxterra). The servo was used in the front of our chassis within a custom built servo holder to steer the ModWheels car. It was used with a steering mechanism that was based on the Sandblaster 3D sand buggy front wheel steering mechanism. The turns were then implemented using the micro servo as well as the two GM6 motors and two magnetic Hall Effect encoders to perform electronic differential steering and electronic differential turning.

Electronic Design

Electronic Devices Used

  1. Substitute for 3DoT v5.03
    1. Sparkfun Pro Micro (3.3V/8MHz – 32U4)
    2. Sparkfun Motor Driver (TB6612FNG)
  2. IR sensor shield (designed by Charles & Muhannad)
  3. Micro Servo
  4. Two Extended Shaft GM6 Gear motors
  5. Two Magnetic Hall Effect Encoders

Figure: Wiring Diagram

Fall 2017: ModWheels Hardware Design

/in /by

By: Natalie Arevalo (Design & Manufacturing Engineer)

Approved By: Lucas Gutierrez (Project Manager)

Discussion

Various modifications were done to the original chassis to be able to complete the mission proposed the customer. Major modifications were done to the wooden chassis to make it compatible with the 5.03 3DoT board. Openings on the top part of the chassis were made so that all of the ports and headers could be accessed. In addition, another opening was made to be able to place the battery into the 3DoT board. Furthermore, an opening in between the GM6 motor placing was made to have the wires of the motors and their encoders be brought up and over the top part of the chassis to plug into the board. Another piece was cut out of the top part of the chassis to allow for the servo to be able to be raised and lowered as it was connected to the front axle. Additionally, six 3mm holes were strategically placed in which dawls would be later glued into. Three of these holes and the respective dawls were set aside for an anchoring point for the servo holder. The other three holes and their dawls were used to hold up 1.5×1.5 in platform to become a phone holder. Two more 5mm holes were also placed in the back to become zip tie holders to keep wires secured. Finally, openings were made for the motor encoders adjacent to the existing openings for the motors

The bottom part of the chassis also had several modifications done to it as well. Matching opening were made adjacent to the existing openings for the motors as it was done with the top part. Openings were also made for the Bluetooth module on the bottom of the 3DoT board and for the header for the IR/color shield. Two 5mm holes were made in the middle of the part place a zip tie that would be used to secure the wires from the IR/color shield. In addition to these holes, two 3mm holes were made in the front of the part for the placement of the front axle. Finally, on both the top and the bottom parts of the chassis, 3mm holes were made along the sides of the chassis to replace the wooden tabs with plastic spacers with screws and nuts to keep the two pieces separated.

Now, several parts were also molded and 3D printed as part of the hardware design. More specifically, the front axle and the servo holder were designed and 3D printed. The front axle was modeled after the axle of the sand buggy and was scaled down to fit our design. The front rack of this axle had tabs added to it as place to where the proximity sensor would be attached. The other parts that make up the attachment point for the wheel and then to the front rack were super glued to be over a little more than 90 degrees. The front axle was assembled together using screws and nuts and the proximity sensor was attached in the same way. To continue, the servo holder was designed to look like an engine block. The design included hollow motor heads as well as a hollow block that allowed the servo to be placed inside and then raised or lowered as needed to be attached to the front axle. On the right side of the of the servo holder, tabs were also added to hold the wires of the proximity sensor out of the way.

Some final modifications that were done in the hardware design involve the wheels and the mounting of the IR/color shield. The wheels in the front had plastic sleeve placed through their point of attachment where a screw was slid thought and then screwed into the front axle. The back wheels’ points of attachment were changed from a round circle into a ‘D’ shape to press fit into the ‘D’ shaped axle of the dual shaft motors. As for the mounting of the IR/color shield was made with a piece of female header and ribbon cable. The piece of the female header and the ribbon cable were hot glued together. This unit was then hot glued to the bottom of the chassis so that when the IR/color shield would be connected it would be as far forward as possible.

Fall 2017: ModWheels Final Software

/in /by

By: Matt Shellhammer (Electronics & Control Engineer)

Approved by: Lucas Gutierrez (Project Manager)

Table of Contents

Discussion

Due to complications with the fabricated 3DoT v5.03 and the limited amount of time we were required to resort to the use of the Sparkfun pro micro and motor driver for the final implementation. The IR sensor shield was used in our design to preform line following throughout the maze to keep the car within the maze and on a straight path. The IR sensor shield was also used to detect intersections so ModWheels could monitor its location within the maze and travel the correct path (either predefined or defined with Arxterra). The servo was used in the front of our chassis within a custom built servo holder to steer the ModWheels car. It was used with a steering mechanism that was based on the Sandblaster 3D sand buggy front wheel steering mechanism. The turns were then implemented using the micro servo as well as the two GM6 motors and two magnetic Hall Effect encoders to perform electronic differential steering and electronic differential turning.

Software

Software Implemented on Final

Final_old.ino

////////////////////////////////////////////////////////////////
//  Name     : ModWheels Final Project (Sparkfun Pro Micro)   //
//  Author   : Matt Shellhammer                               //
//  Date     : 11 December, 2017                              //
//  Version  : 1.0                                            //
////////////////////////////////////////////////////////////////

#define __PROG_TYPES_COMPAT__
#include <avr/pgmspace.h>
#include <Wire.h>
#include <EEPROM.h>
#include "Adafruit_TCS34725.h"
#include "Final.h"
#include <Configure.h>
#include <FuelGauge.h>
#include <Motor.h>
#include <Packet.h>
#include <Robot3DoTBoard.h>
#include <TB6612FNG.h>
#include <TelecomClass.h>
#include <Watchdog.h>
#include <SparkFun_VL6180X.h>

// Define motor driver pins
#define AIN1 14 // Pro-micro pin 14 (PB3)
#define BIN1 7  // Pro-micro pin 7  (PE6)
#define AIN2 4  // Pro-micro pin 4  (PD4)
#define BIN2 8  // Pro-micro pin 8  (PB4)
#define PWMA 5  // Pro-micro pin 5  (PC6)
#define PWMB 6  // Pro-micro pin 6  (PD7)
#define STBY 9  // Pro-micro pin 9  (PB5)
// Define Encoders
#define LME A0  // Left motor encoder (PF7)
#define RME A1  // Right motor encoder (PF6)
// Define IR proximity sensor using library
#define VL6180X_ADDRESS 0x29
VL6180xIdentification identification;
VL6180x sensor(VL6180X_ADDRESS);
// Define servo pin
#define servo_pin 10  // Pro-micro pin 10 (PB6)

void setup() {
 Serial.begin(115200); //Start Serial at 115200bps
  Wire.begin(); //Start I2C library
  delay(100); // delay .1s
  sensor.VL6180xDefautSettings(); //Load default settings for proximity IR.
  delay(1000); // delay 1s
  // DDRx & PORTx for motor drivers
  DDRD |= (1<<PD4)|(1<<PD7);
  DDRE |= (1<<PE6);
  DDRB |= (1<<PB3)|(1<<PB4)|(1<<PB5);
  DDRC |= (1<<PC6);
  // DDRx & PORTx for Encoders
  DDRF &= ~((1<<PF6)|(1<<PF7));
  PORTF |= (1<<PF6)|(1<<PF7);
  // DDRx & PORTx for Servo
  DDRB |= (1<<PB6);
  // DDRx & PORTx for IR Shield
  DDRF &= ~((1<<PF4)|(1<<PF5));
  PORTF |= (1<<PF4)|(1<<PF5);
  delay(5000);
}

void loop() {
  static bool turn_flag = false;          // turn flag for turns with ModWheels
  static uint8_t type = 0;                // initially outside of the maze
  static uint8_t count1 = 0;              // first counter used
  static uint16_t count2 = 0;             // second counter used
  static myRobot_t ModWheels;             // create an instance of myRobot_t called ModWheels
  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
  if ((sensors_inst.IR0 >= 0.92)&&(-sensors_inst.IR1 >= 0.92)){
    if (turn_flag == true){turn_flag = false;go_straight(75);}
    else{
      ModWheels = enterRoom(ModWheels);
      type = roomType(ModWheels);
      ModWheels = whichWay(type, ModWheels);
      if (ModWheels.turn != 0){turn_flag = true;}
    }
  }
}

Final.h

// PID constants
#define Kp 1600
#define Ki 0.5
#define Kd 1600
// Bias speed and angle
#define base_speed 150
#define base_angle 96

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
  uint8_t mode = 0;
};

struct sensors_t{
  float IR0; // Left IR sensor (normalized)
  float IR1; // Right IR sensor (normalized)
};

struct motors_t{
  int16_t leftSpeed = base_speed;   // Initial motor speed: 60
  int16_t rightSpeed = base_speed;  // Initial motor speed: 60
  int16_t servoAngle = base_angle;  // Initial servo angle: 95
};

// 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
};

Finalsub.ino

////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////// Physical Robot Code ////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////
void readIR(){
  uint8_t IR = 0;
  bool IRflag = false;
  IR = sensor.getDistance();
  delay(10); // delay .1s 
  while (IR < 153){          // Don't hit something
    analogWrite(PWMA, 0);
    analogWrite(PWMB, 0);
    IR = sensor.getDistance();
    delay(10); // delay .1s
    IRflag = true;
  }
  if (IRflag == true){
    delay(3000);
    IRflag = false;
  }
}

// readSensors subroutine returns normalized sensor values averaged by 10 points
sensors_t readSensors(sensors_t sensors){
  int n = 10;     // number of ADC samples
  float x_i;      // ADC input for sample i
  float A_1;      // current i running average
  float A_0;      // previous i-1 running average

  // read left IR sensor
  A_0 = 0.0;
  for (int16_t i = 1; i <= n; i++) {
    x_i = analogRead(A2)/1024.0;
    A_1 = A_0 + (x_i - A_0)/i;
    A_0 = A_1;
  }
  sensors.IR0 = A_0;

  // read right IR sensor
  A_0 = 0.0;
  for (int16_t i = 1; i <= n; i++) {
    x_i = analogRead(A3)*(-1.0/1024.0);
    A_1 = A_0 + (x_i - A_0)/i;
    A_0 = A_1;
  }
  sensors.IR1 = A_0;
  return sensors;
}

PID_t PIDcalc(sensors_t sensors, PID_t PID){
  // Calculates error
  PID.present = (sensors.IR0-0.02) + sensors.IR1;
  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;
}

// 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;
  motors.servoAngle = base_angle - (0.5*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;}
  if (motors.servoAngle > 110) {motors.servoAngle = 110;}
  if (motors.servoAngle < 80) {motors.servoAngle = 80;}
  return motors;
}

void readEncoders(uint16_t leftCount, uint16_t rightCount){
  bool LMEV; uint16_t LMEV_count = 0;
  bool RMEV; uint16_t RMEV_count = 0;
  bool LAST_LMEV = digitalRead(LME);
  bool LAST_RMEV = digitalRead(RME);
  uint16_t count = 0;
  while((LMEV_count < leftCount) && (RMEV_count < rightCount)){
    LMEV = digitalRead(LME);
    RMEV = digitalRead(RME);
    if (LMEV != LAST_LMEV){
      LMEV_count++;
    }
    if (RMEV != LAST_RMEV){
      RMEV_count++;
    }
    LAST_LMEV = LMEV;
    LAST_RMEV = RMEV;
//    count++;
//    if(count == 30){readIR();count = 0;} // Read IR sensor every 30 loops
  }
}

void forward(motors_t motors){
  digitalWrite(STBY, HIGH);
  digitalWrite(AIN2, HIGH);
  digitalWrite(AIN1, LOW);
  digitalWrite(BIN1, LOW);
  digitalWrite(BIN2, HIGH);
  analogWrite(PWMA, motors.leftSpeed);
  analogWrite(PWMB, motors.rightSpeed);
  analogWrite(servo_pin, motors.servoAngle);
}

void turn_right() {
  go_straight(180);
  digitalWrite(AIN1, LOW);
  digitalWrite(AIN2, HIGH);
  digitalWrite(BIN1, LOW);
  digitalWrite(BIN2, HIGH);
  analogWrite(PWMA, 200);
  analogWrite(PWMB, 120);
  analogWrite(servo_pin,127);
  readEncoders(1100,667);
  digitalWrite(AIN1, HIGH);
  digitalWrite(AIN2, LOW);
  digitalWrite(BIN1, HIGH);
  digitalWrite(BIN2, LOW);
  analogWrite(PWMA, 120);
  analogWrite(PWMB, 200);
  analogWrite(servo_pin,75);
  readEncoders(600,1000);
  go_back(420);
}

void turn_left() {
  go_straight(100);
  digitalWrite(AIN1, LOW);
  digitalWrite(AIN2, HIGH);
  digitalWrite(BIN1, LOW);
  digitalWrite(BIN2, HIGH);
  analogWrite(PWMA, 120);
  analogWrite(PWMB, 200);
  analogWrite(servo_pin,75);
  readEncoders(840,1400);
  digitalWrite(AIN1, HIGH);
  digitalWrite(AIN2, LOW);
  digitalWrite(BIN1, HIGH);
  digitalWrite(BIN2, LOW);
  analogWrite(PWMA, 200);
  analogWrite(PWMB, 120);
  analogWrite(servo_pin,120);
  readEncoders(1300,780);
  go_back(400);
}

void turn_around(){
  digitalWrite(AIN1, LOW);
  digitalWrite(AIN2, HIGH);
  digitalWrite(BIN1, LOW);
  digitalWrite(BIN2, HIGH);
  analogWrite(PWMA, 200);
  analogWrite(PWMB, 120);
  analogWrite(servo_pin,120);
  readEncoders(1100,611);
  digitalWrite(AIN1, HIGH);
  digitalWrite(AIN2, LOW);
  digitalWrite(BIN1, HIGH);
  digitalWrite(BIN2, LOW);
  analogWrite(PWMA, 120);
  analogWrite(PWMB, 200);
  analogWrite(servo_pin,75);
  readEncoders(650,1150);
}

void stopRobot(uint16_t d) {
  digitalWrite(STBY, HIGH);
  digitalWrite(AIN1, LOW);
  digitalWrite(AIN2, HIGH);
  digitalWrite(BIN1, LOW);
  digitalWrite(BIN2, HIGH);
  analogWrite(PWMA, 0);
  analogWrite(PWMB, 0);
  delay(d);
}

void go_straight(uint16_t count){
  digitalWrite(STBY, HIGH);
  digitalWrite(AIN2, HIGH);
  digitalWrite(AIN1, LOW);
  digitalWrite(BIN1, LOW);
  digitalWrite(BIN2, HIGH);
  analogWrite(PWMA, base_speed);
  analogWrite(PWMB, base_speed);
  analogWrite(servo_pin, base_angle);
  readEncoders(count,count);
}

void go_back(uint16_t count){
  digitalWrite(STBY, HIGH);
  digitalWrite(AIN2, LOW);
  digitalWrite(AIN1, HIGH);
  digitalWrite(BIN1, HIGH);
  digitalWrite(BIN2, LOW);
  analogWrite(PWMA, base_speed);
  analogWrite(PWMB, base_speed);
  analogWrite(servo_pin, base_angle);
  readEncoders(count,count);
}


////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////// Virtual Robot Code ///////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////
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;
}

// Should be modified to create a permanent solution to putting the 32U4 to sleep
// Also, could possibly do something before sleeping.
void inForest(myRobot_t robot){
  if (robot.maze.row == 0xFF){
    bool sleep = true;
    stopRobot(300000);
//    sleep();
  }
}

myRobot_t whichWay(uint8_t RoomType, myRobot_t robot){ // Update for shortest path
  stopRobot(500);
  if ((RoomType == 0)||(RoomType == 1)||(RoomType == 5)){go_straight(55);robot.turn = 0b00;}
  else if (RoomType == 3){turn_left();robot.turn = 0b10;}
  else if (RoomType == 6){turn_right();robot.turn = 0b01;}
  else if (RoomType == 7){turn_around();robot.turn = 0b11;}
  else if (RoomType == 4){turn_right();robot.turn = 0b01;}
  return robot;
}

Software developed for final implementation with 3DoT and Arxterra (Untested)

Final_3DoT_503.ino

////////////////////////////////////////////////////////////////
//  Name     : ModWheels Final Project (Sparkfun Pro Micro)   //
//  Author   : Matt Shellhammer                               //
//  Date     : 11 December, 2017                              //
//  Version  : 1.0                                            //
////////////////////////////////////////////////////////////////

#define __PROG_TYPES_COMPAT__
#include <avr/pgmspace.h>
#include <Wire.h>
#include <EEPROM.h>
#include "Adafruit_TCS34725.h"
#include "Final.h"
#include <Configure.h>
#include <FuelGauge.h>
#include <Motor.h>
#include <Packet.h>
#include <Robot3DoTBoard.h>
#include <TB6612FNG.h>
#include <TelecomClass.h>
#include <Watchdog.h>
#include <SparkFun_VL6180X.h>

// Define motor driver pins
#define AIN1 12
#define AIN2 4
#define PWMA 6
#define BIN1 9
#define BIN2 5
#define PWMB 10
#define STBY 8
// Define Encoders
#define LME A0
#define RME A1
// Define IR proximity sensor using library
#define VL6180X_ADDRESS 0x29
VL6180xIdentification identification;
VL6180x sensor(VL6180X_ADDRESS);
// Define servo pin
#define servo_pin SERVO_11

// Custom commands configuration
// Setup command addresses
#define MODE    0x40
#define ALLOW   0x41

// Define 3DoT Robot
Robot3DoTBoard Robot3DoT;
motors_t ArxterraMotors;
myRobot_t ArxterraRobot;
uint16_t EEPROM_Idx = 0x0000; // first EEPROM index is 0x0000

// Setup Custom 3Dot Commands
void moveHandler (uint8_t cmd, uint8_t param[], uint8_t n);
void modeHandler (uint8_t cmd, uint8_t param[], uint8_t n);
void allowHandler (uint8_t cmd, uint8_t param[], uint8_t n);
const uint8_t CMD_LIST_SIZE = 3;
Robot3DoTBoard::cmdFunc_t onCommand[CMD_LIST_SIZE] = {{MODE,modeHandler}, {MOVE,moveHandler}, {ALLOW,allowHandler}};

void setup() {
  Robot3DoT.begin();
  Robot3DoT.setOnCommand(onCommand, CMD_LIST_SIZE);
  Serial.begin(115200); //Start Serial at 115200bps
  Wire.begin(); //Start I2C library
  delay(100); // delay .1s
  sensor.VL6180xDefautSettings(); //Load default settings to get started.
  delay(1000); // delay 1s
  // Set the PIN Mode for motor drivers
  pinMode(PWMA, OUTPUT);
  pinMode(AIN1, OUTPUT);
  pinMode(AIN2, OUTPUT);
  pinMode(PWMB, OUTPUT);
  pinMode(BIN1, OUTPUT);
  pinMode(BIN2, OUTPUT);
  pinMode(STBY, OUTPUT);
  // Set the PIN Mode for Encoders
  pinMode(LME, INPUT);
  pinMode(RME, INPUT);
  // Set the PIN Mode for Servo
  pinMode(servo_pin, OUTPUT);
  asm("in r24, 0x35");
  asm("ori r24, 0x80");
  asm("out 0x35, r24");
  asm("out 0x35, r24");
  delay(5000);
}

void loop() {
////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////// Variable Declaration ///////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////
  static bool decision;                   // create a variable called decision
  static bool turn_flag = false;          // turn flag for turns with ModWheels
  static uint8_t type = 0;                // initially outside of the maze
  static uint8_t count1 = 0;              // first counter used
  static uint16_t count2 = 0;             // second counter used
  static myRobot_t ModWheels;             // create an instance of myRobot_t called ModWheels
  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
////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////// Shortest Path (Autonomous) /////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////
  if(ArxterraRobot.mode == 0){
    Robot3DoT.loop();
    if(resetRobot == true){
      myRobot_t ModWheels;
      resetRobot = false;
    }
    count2++;
    if(count2 == 30){readIR();count2 = 0;} // Read IR proximity sensor every 30 loops
    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 in forest
    inForest(ModWheels);
  
    // Check if at an intersection
    if ((sensors_inst.IR0 >= 0.95)&&(-sensors_inst.IR1 >= 0.95)){
      if (turn_flag == true){turn_flag = false;go_straight(75);}
      else{
        ModWheels = enterRoom(ModWheels);
        type = roomType(ModWheels);
        ModWheels = whichWay(type, ModWheels);
        if (ModWheels.turn != 0){turn_flag = true;}
      }
    }
  }


////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////// Write to EEPROM (Arxterra control) /////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////
  if(ArxterraRobot.mode == 1){
    Robot3DoT.loop();
    // No decision to be made when in a Hallway or outside of the maze (go straight)
    if ((type == 0)||(type == 5)){decision = false;ArxterraRobot.turn = 0x00;}
    // No decision to be made when in a left corner (turn left)
    if (type == 3){decision = false;ArxterraRobot.turn = 0x10;}
    // No decision to be made when in a right corner (turn right)
    if (type == 6){decision = false;ArxterraRobot.turn = 0x01;}
    // No decision to be made when at a dead end (turn around)
    if (type == 7){decision = false;ArxterraRobot.turn = 0x11;}
    else{decision = true;}

    // Call write data to EEPROM when a decision is made on the Arxterra App at a decision point
    if ((decision == true)&&(EEPROM_Idx < 0x400)){
      // Call Arxterra custom command to request a turn value (update ArxterraRobot.turn)
      // Store dir facing and turn value
      EEPROM_write(EEPROM_Idx, ArxterraRobot.dir);EEPROM_Idx++;
      EEPROM_write(EEPROM_Idx, ArxterraRobot.turn);EEPROM_Idx++;
    }
    ArxterraRobot = enterRoom(ArxterraRobot); // Update ArxterraRobot
    type = roomType(ArxterraRobot);           // Determine room type
  }


////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////// Read from EEPROM (Autonomous playback) /////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////
  if(ArxterraRobot.mode == 2){
    Robot3DoT.loop();
    // No decision to be made when in a Hallway or outside of the maze (go straight)
    if ((type == 0)||(type == 5)){decision = false;ArxterraRobot.turn = 0x00;}
    // No decision to be made when in a left corner (turn left)
    if (type == 3){decision = false;ArxterraRobot.turn = 0x10;}
    // No decision to be made when in a right corner (turn right)
    if (type == 6){decision = false;ArxterraRobot.turn = 0x01;}
    // No decision to be made when at a dead end (turn around)
    if (type == 7){decision = false;ArxterraRobot.turn = 0x11;}
    else{decision = true;}

    // Call read data to EEPROM when at a decision point
    if ((decision == true)&&(EEPROM_Idx < 0x400)){
      // Call Arxterra custom command to request a turn value
      // Store dir facing and turn value
      uint8_t temp_dir = EEPROM_read(EEPROM_Idx);EEPROM_Idx++;
  //    if (temp_dir != ArxterraRobot.dir){//ERROR: Do something}
      ArxterraRobot.turn = EEPROM_read(EEPROM_Idx);EEPROM_Idx++;
    }
      ArxterraRobot = enterRoom(ArxterraRobot); // Update robot_inst
      type = roomType(ArxterraRobot);           // Determine room type
  }


////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////// RC mode (Outside of the maze) ////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////
  if(ArxterraRobot.mode == 3){
    Robot3DoT.loop();
  }
}

Final.h

// PID constants
#define Kp 4000
#define Ki 0.8
#define Kd 4500
// Bias speed and angle
#define base_speed 150
#define base_angle 96

// Global reset robot flag
bool resetRobot = false;

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
  uint8_t mode = 0;
};

struct sensors_t{
  float IR0; // Left IR sensor (normalized)
  float IR1; // Right IR sensor (normalized)
};

struct motors_t{
  int16_t leftSpeed = base_speed;   // Initial motor speed: 60
  int16_t rightSpeed = base_speed;  // Initial motor speed: 60
  int16_t servoAngle = base_angle;  // Initial servo angle: 95
};

// 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
};

Finalsub.ino

////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////// Physical Robot Code ////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////
void readIR(){
  uint8_t IR = 0;
  bool IRflag = false;
  IR = sensor.getDistance();
  delay(10); // delay .1s 
  while (IR < 153){          // Don't hit something
    analogWrite(PWMA, 0);
    analogWrite(PWMB, 0);
    IR = sensor.getDistance();
    delay(10); // delay .1s
    IRflag = true;
  }
  if (IRflag == true){
    delay(3000);
    IRflag = false;
  }
}

// readSensors subroutine returns normalized sensor values averaged by 10 points
sensors_t readSensors(sensors_t sensors){
  int n = 10;     // number of ADC samples
  float x_i;      // ADC input for sample i
  float A_1;      // current i running average
  float A_0;      // previous i-1 running average

  // read left IR sensor
  A_0 = 0.0;
  for (int16_t i = 1; i <= n; i++) {
    x_i = analogRead(A2)/1024.0;
    A_1 = A_0 + (x_i - A_0)/i;
    A_0 = A_1;
  }
  sensors.IR0 = A_0;

  // read right IR sensor
  A_0 = 0.0;
  for (int16_t i = 1; i <= n; i++) {
    x_i = analogRead(A3)*(-1.0/1024.0);
    A_1 = A_0 + (x_i - A_0)/i;
    A_0 = A_1;
  }
  sensors.IR1 = A_0;
  return sensors;
}

PID_t PIDcalc(sensors_t sensors, PID_t PID){
  // Calculates error
  PID.present = (sensors.IR0) + sensors.IR1;
  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;
}

// 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;
  motors.servoAngle = base_angle - (0.6*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;}
  if (motors.servoAngle > 110) {motors.servoAngle = 110;}
  if (motors.servoAngle < 80) {motors.servoAngle = 80;}
  return motors;
}

void readEncoders(uint16_t leftCount, uint16_t rightCount){
  bool LMEV; uint16_t LMEV_count = 0;
  bool RMEV; uint16_t RMEV_count = 0;
  bool LAST_LMEV = digitalRead(LME);
  bool LAST_RMEV = digitalRead(RME);
  uint16_t count = 0;
  while((LMEV_count < leftCount) && (RMEV_count < rightCount)){
    LMEV = digitalRead(LME);
    RMEV = digitalRead(RME);
    if (LMEV != LAST_LMEV){
      LMEV_count++;
    }
    if (RMEV != LAST_RMEV){
      RMEV_count++;
    }
    LAST_LMEV = LMEV;
    LAST_RMEV = RMEV;
//    count++;
//    if(count == 30){readIR();count = 0;} // Read IR sensor every 30 loops
  }
}

void forward(motors_t motors){
  digitalWrite(STBY, HIGH);
  digitalWrite(AIN2, HIGH);
  digitalWrite(AIN1, LOW);
  digitalWrite(BIN1, LOW);
  digitalWrite(BIN2, HIGH);
  analogWrite(PWMA, motors.leftSpeed);
  analogWrite(PWMB, motors.rightSpeed);
  analogWrite(servo_pin, motors.servoAngle);
}

void turn_right() {
  go_straight(270);
  digitalWrite(AIN1, LOW);
  digitalWrite(AIN2, HIGH);
  digitalWrite(BIN1, LOW);
  digitalWrite(BIN2, HIGH);
  analogWrite(PWMA, 200);
  analogWrite(PWMB, 120);
  analogWrite(servo_pin,120);
  readEncoders(1100,611);
  digitalWrite(AIN1, HIGH);
  digitalWrite(AIN2, LOW);
  digitalWrite(BIN1, HIGH);
  digitalWrite(BIN2, LOW);
  analogWrite(PWMA, 120);
  analogWrite(PWMB, 200);
  analogWrite(servo_pin,75);
  readEncoders(650,1150);
  go_back(370);
}

void turn_left() {
  go_straight(140);
  digitalWrite(AIN1, LOW);
  digitalWrite(AIN2, HIGH);
  digitalWrite(BIN1, LOW);
  digitalWrite(BIN2, HIGH);
  analogWrite(PWMA, 120);
  analogWrite(PWMB, 200);
  analogWrite(servo_pin,75);
  readEncoders(760,1370);
  digitalWrite(AIN1, HIGH);
  digitalWrite(AIN2, LOW);
  digitalWrite(BIN1, HIGH);
  digitalWrite(BIN2, LOW);
  analogWrite(PWMA, 200);
  analogWrite(PWMB, 120);
  analogWrite(servo_pin,120);
  readEncoders(1100,611);
  go_back(500);
}

void turn_around(){
  digitalWrite(AIN1, LOW);
  digitalWrite(AIN2, HIGH);
  digitalWrite(BIN1, LOW);
  digitalWrite(BIN2, HIGH);
  analogWrite(PWMA, 200);
  analogWrite(PWMB, 120);
  analogWrite(servo_pin,120);
  readEncoders(1100,611);
  digitalWrite(AIN1, HIGH);
  digitalWrite(AIN2, LOW);
  digitalWrite(BIN1, HIGH);
  digitalWrite(BIN2, LOW);
  analogWrite(PWMA, 120);
  analogWrite(PWMB, 200);
  analogWrite(servo_pin,75);
  readEncoders(650,1150);
}

void stopRobot(uint16_t d) {
  digitalWrite(STBY, HIGH);
  digitalWrite(AIN1, LOW);
  digitalWrite(AIN2, HIGH);
  digitalWrite(BIN1, LOW);
  digitalWrite(BIN2, HIGH);
  analogWrite(PWMA, 0);
  analogWrite(PWMB, 0);
  delay(d);
}

void go_straight(uint16_t count){
  digitalWrite(STBY, HIGH);
  digitalWrite(AIN2, HIGH);
  digitalWrite(AIN1, LOW);
  digitalWrite(BIN1, LOW);
  digitalWrite(BIN2, HIGH);
  analogWrite(PWMA, base_speed);
  analogWrite(PWMB, base_speed);
  analogWrite(servo_pin, base_angle);
  readEncoders(count,count);
}

void go_back(uint16_t count){
  digitalWrite(STBY, HIGH);
  digitalWrite(AIN2, LOW);
  digitalWrite(AIN1, HIGH);
  digitalWrite(BIN1, HIGH);
  digitalWrite(BIN2, LOW);
  analogWrite(PWMA, base_speed);
  analogWrite(PWMB, base_speed);
  analogWrite(servo_pin, base_angle);
  readEncoders(count,count);
}


////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////// Virtual Robot Code ///////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////
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;
}

// Should be modified to create a permanent solution to putting the 32U4 to sleep
// Also, could possibly do something before sleeping.
void inForest(myRobot_t robot){
  if (robot.maze.row == 0xFF){
    bool sleep = true;
    stopRobot(300000);
//    sleep();
  }
}

myRobot_t whichWay(uint8_t RoomType, myRobot_t robot){ // Update for shortest path
  stopRobot(2000);
  if ((RoomType == 0)||(RoomType == 1)||(RoomType == 5)){go_straight(75);robot.turn = 0b00;}
  else if (RoomType == 3){turn_left();robot.turn = 0b10;}
  else if (RoomType == 6){turn_right();robot.turn = 0b01;}
  else if (RoomType == 7){turn_around();robot.turn = 0b11;}
  else if (RoomType == 4){turn_right();robot.turn = 0b01;}
  return robot;
}


////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////// Arxterra Custom Commands ///////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////
/* moveHandler: RC mode inside the maze */
void moveHandler(uint8_t cmd, uint8_t param[], uint8_t n){
  // Set the turn direction and record it, (ONLY ACCEPT AT INTERSECTIONS)
  if(ArxterraRobot.turn == 4){ // at intersection for turn command
    // 01 2B 01 2B == Foward
    if (param[0] == 0x01 && param[2] == 0x01){
      ArxterraRobot.turn = 0x00;
    // 02 2B 02 2B == Backward
    }else if(param[0] == 0x02 && param[2] == 0x02){
      ArxterraRobot.turn = 0x11;
    // 01 00 02 00 == Right
    }else if(param[0] == 0x01 && param[2] == 0x02){
      ArxterraRobot.turn = 0x01;
    // 02 00 01 00 == Left
    }else if(param[0] == 0x02 && param[2] == 0x01){
      ArxterraRobot.turn = 0x10;
    }
  }
}

/* moveHandler: RC mode outside of maze */
/*void moveHandler(uint8_t cmd, uint8_t param[], uint8_t n){
  digitalWrite(STBY, HIGH);
  digitalWrite(AIN2, HIGH);
  digitalWrite(AIN1, LOW);
  digitalWrite(BIN1, LOW);
  digitalWrite(BIN2, HIGH);
  // 01 2B 01 2B == Foward (0 to 255 over 5 ticks (increments of 51))
  if (param[0] == 0x01 && param[2] == 0x01){
    if((param[1] == 0x2B)||(param[1] == 0x55)||(param[1] == 0x80)||(param[1] == 0xAA)||(param[1] == 0xD5)){
     if (ArxterraMotors.leftSpeed <= 204){ArxterraMotors.leftSpeed += 51;}
      if (ArxterraMotors.rightSpeed <= 204){ArxterraMotors.rightSpeed += 51;}
    }
    analogWrite(PWMA, ArxterraMotors.leftSpeed);
    analogWrite(PWMB, ArxterraMotors.rightSpeed);
    analogWrite(servo_pin,60);
  }
  // 02 2B 02 2B == Stop (255 to 0 over 5 ticks (increments of 51))
  else if(param[0] == 0x02 && param[2] == 0x02){
    if((param[1] == 0x2B)||(param[1] == 0x55)||(param[1] == 0x80)||(param[1] == 0xAA)||(param[1] == 0xD5)){
      if (ArxterraMotors.leftSpeed >= 51){ArxterraMotors.leftSpeed -= 51;}
      if (ArxterraMotors.rightSpeed >= 51){ArxterraMotors.rightSpeed -= 51;}
    }
    analogWrite(PWMA, ArxterraMotors.leftSpeed);
    analogWrite(PWMB, ArxterraMotors.rightSpeed);
    analogWrite(servo_pin,60);
  }
  // 01 00 02 00 == Right (120 to 60 over 5 ticks (increments of 12 degrees))
  else if(param[0] == 0x01 && param[2] == 0x02){
    if((param[1] == 0x2B)||(param[1] == 0x55)||(param[1] == 0x80)||(param[1] == 0xAA)||(param[1] == 0xD5)){
      if (ArxterraMotors.servoAngle >= 72){ArxterraMotors.servoAngle -= 12;}
    }
    analogWrite(servo_pin,ArxterraMotors.servoAngle);
  }
  // 02 00 01 00 == Left (60 to 120 over 5 ticks (increments of 12 degrees))
  else if(param[0] == 0x02 && param[2] == 0x01){
    if((param[1] == 0x2B)||(param[1] == 0x55)||(param[1] == 0x80)||(param[1] == 0xAA)||(param[1] == 0xD5)){
      if (ArxterraMotors.servoAngle <= 108){ArxterraMotors.servoAngle += 12;}
    }
    analogWrite(servo_pin,ArxterraMotors.servoAngle);
  }
//  // 04 00 04 00 return servo to center
//  else if(param[0] == 0x04 && param[2] == 0x04){
//    analogWrite(servo_pin,90);
//  }
}*/

void modeHandler(uint8_t cmd, uint8_t param[], uint8_t n){
 //Changes the set mode 
 // Mode 0 = Predetermined Route
 // Mode 1 = Record Mode (RC in the maze)
 // Mode 2 = Playback Mode (RC in the maze)
 // Mode 3 = RC out of the Maze
  if(param[0] == 0 && ArxterraRobot.mode != 0){
    // change mode and reset needed paramters
    ArxterraRobot.mode = 0;
    resetRobot = true;
    //teleMState.sendPacket(MODESTATE, &data, 1);
  }
  if(param[0] == 1 && ArxterraRobot.mode != 1){
    // change mode and reset needed paramters
    ArxterraRobot.mode = 1;
    EEPROM_Idx = 0x0000;      // first EEPROM index is 0x0000
    //teleMState.sendPacket(MODESTATE, 0x0001);
  }
  if(param[0] == 2 && ArxterraRobot.mode != 2){
    // change mode and reset needed paramters
    ArxterraRobot.mode = 2;
    EEPROM_Idx = 0x0000;      // first EEPROM index is 0x0000
    //teleMState.sendPacket(MODESTATE, 0x0002);
  }
}

void allowHandler(uint8_t cmd, uint8_t param[], uint8_t n){
// // Turns the motors on or off (Stopping the bot) 
// // 0 = Stop!
// // 1 = Start moving again
// if(param[0] == 0){
//    // Stop motors
//    remoteStop = true;
//    robotMovement("Stop");
//    LEDDirection("X");
//  
//  if(param[0] == 1){
//    // Movement True (GO!)
//    remoteStop = false;
//    robotMovement("Go");
//    LEDDirection("Foward");
//  }
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////// Read/Write EEPROM Subroutines ////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
 * Write data to EEPROM, NOTE: interrupts are disabled while writing
 * @param uiAddress 16 bit interger pointing to the address of the data to write
 * @param ucData 8 bit value signifying the data being written
 */
void EEPROM_write(uint16_t uiAddress, uint8_t ucData) {
  /*Store SREG value before we disable Interrupts*/
  char SREG_save = SREG;
  noInterrupts();
  /* Wait for completion of any Flash Write
        Note:Only necessary if Flash Memory Manipulation is taking place */
  while(SPMCSR &(1<<SPMEN));
  /* Wait for completion of previous write */
  while(EECR & (1<<EEPE));
  /* Set up address and Data Registers */
  EEAR = uiAddress;
  EEDR = ucData;
  /* Write logical one to EEMPE */
  EECR |= (1<<EEMPE);
  /* Start eeprom write by setting EEPE */
  EECR |= (1<<EEPE);
  /*Restore the SREG value*/
  SREG = SREG_save;
}

/*
 * Read data from EEPROM, NOTE: interrupts are disabled while writing
 * @param uiAddress 16 bit interger pointing to the address of the data to read
 * @return 8 bit value signifying the data that was read
 */
uint8_t EEPROM_read(uint16_t uiAddress) {
  /*Store SREG value before we disable Interrupts*/
  char SREG_save = SREG;
  noInterrupts();
  /* Wait for completion of any Flash Write
        Note:Only necessary if Flash Memory Manipulation is taking place */
  while(SPMCSR &(1<<SPMEN));
  /* Wait for completion of previous write */
  while(EECR & (1<<EEPE));
  /* Set up address register */
  EEAR = uiAddress;
  /* Start eeprom read by writing EERE */
  EECR |= (1<<EERE);
  /*Restore the SREG value*/
  SREG = SREG_save;
  /* Return data from Data Register */
  return EEDR;
}

 

Fall 2017: ModWheels Budget

/in /by

By: Lucas Gutierrez (Project Manager)

 

Discussion

As part of the requirements stated by the customer, a budget was set for the ModWheels project for no more than $200.00 .  To help cut some costs, the EE 400D previous semester’s inventory of electronics, hardware, and materials were provided for use.  The Budget report includes total cost of the project as well as cost of the project offset by inventory.

Budget

Figure: Fall 2017 ModWheels Final Budget

ModWheels Electronic Breadboarding

/in /by

By: Matt Shellhammer (Electronics & Control Engineer)

Approved by: Lucas Gutierrez (Project Manager)

Introduction

To create a baseline design that can be used for testing and software development an electronic breadboarding of ModWheels was constructed. All components were breadboarded using some components that will not be used on the day of the final that are being used for prototyping and testing. This allows us to do testing on all of our components and ensure that our wiring diagrams are correct as well as confirming that all of our components work together properly and the software is developed appropriately for the electronic components in use.

Methodology

To perform this electronic breadboarding a Sparkfun Pro Micro and a Sparkfun TB6612FNG motor driver was used to imitate the 3DoT v5.03 board. Then all external components were connected to these two central components.

Components breadboarded:

  • I2C Multiplexer (TCA9548A)
  • Two Color sensors (TCS34725) (Through I2C Mux)
  • IR Proximity sensor (VL6180) (Through I2C Mux)
  • Two GM6 Extended Shaft Motors
  • Two Magnetic Quadrature Hall Effect Encoders
  • Micro Servo

The breadboarding was first done without the ModWheels chassis to test the breadboarding and software first. This was done connected printing data to the Serial print monitor from the Arduino IDE. The breadboarding done without the ModWheels chassis and serial print monitor is shown below in Figures 1 & 2. The motors were programed to stop when the proximity sensor was lower than 6 inches (~153 mm) but otherwise to continue spinning.

Figure 1. Electronic breadboarding without ModWheels chassis.

 

Figure 2. Serial print monitor showing sensor readings (color sensors, encoders, IR proximity sensor)

 

 This breadboarding was then implemented within the ModWheels chassis to test the motors and proximity sensor together. Since it is programmed to stop within 6 inches of an object the car should stop and then continue when that object is moved. The breadboarding can be seen below in Figure 3 and the testing can be seen in Video 1.

Figure 3. Electronic breadboarding with ModWheels chassis.

 

Video 1. Testing of IR proximity sensor with GM6 motors and ModWheels chassis.

Software

ElectronicBreadboarding.ino

////////////////////////////////////////////////////////////////
//  Name     : Electronic Breadboarding                       //
//  Author   : Matt Shellhammer                               //
//  Date     : 3 December, 2017                               //
//  Version  : 1.0                                            //
////////////////////////////////////////////////////////////////

#define __PROG_TYPES_COMPAT__
#include <avr/pgmspace.h>
#include <Wire.h>
#include "Adafruit_TCS34725.h"
#include "EBB.h"
#include <Configure.h>
#include <FuelGauge.h>
#include <Motor.h>
#include <Packet.h>
#include <Robot3DoTBoard.h>
#include <TB6612FNG.h>
#include <TelecomClass.h>
#include <Watchdog.h>
#include <SparkFun_VL6180X.h>

// Define motor driver pins
#define AIN1 14 // Pro-micro pin 14 (PB3)
#define BIN1 7  // Pro-micro pin 7  (PE6)
#define AIN2 4  // Pro-micro pin 4  (PD4)
#define BIN2 8  // Pro-micro pin 8  (PB4)
#define PWMA 5  // Pro-micro pin 5  (PC6)
#define PWMB 6  // Pro-micro pin 6  (PD7)
#define STBY 9  // Pro-micro pin 9  (PB5)

// Address for I2C multiplexer
#define TCAADDR 0x70

// Define Color sensors using library
Adafruit_TCS34725 colorSensor = Adafruit_TCS34725();

// Define IR proximity sensor using library
#define VL6180X_ADDRESS 0x29
VL6180xIdentification identification;
VL6180x sensor(VL6180X_ADDRESS);

// Define Servo
int8_t pos = 115;     // servo position
#define servo_pin 10  // (PB6)

// Define Encoders
#define LME A0  // Left motor encoder
#define RME A1  // Right motor encoder

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  // DDRx & PORTx for motor drivers
  DDRD |= (1<<PD4)|(1<<PD7);
  DDRE |= (1<<PE6);
  DDRB |= (1<<PB3)|(1<<PB4)|(1<<PB5);
  DDRC |= (1<<PC6);
  // Pinmode for Servo
  DDRB |= (1<<PB6);

//  if(sensor.VL6180xInit() != 0){
//    Serial.println("FAILED TO INITALIZE"); //Initialize device and check for errors
//  };
//  sensor.VL6180xDefautSettings(); //Load default settings to get started.
}

void loop() {
  static bool IRflag = false;
  static color_t colorL;    // create an instance of color_t called colorL (left color sensor)
  static color_t colorR;    // create an instance of color_t called colorR (right color sensor)
  static uint8_t IR = 0;    // create variable for IR proximity sensor
  static bool LMEV;  // Left motor Encoder value
  static bool RMEV;  // Right motor Encoder value
  static uint16_t LMEV_count = 0;  // Left motor Encoder value count
  static uint16_t RMEV_count = 0;  // Right motor Encoder value count

  // Read Sensors
  colorL = readColor(0);
//  Serial.print("Left Red: "); Serial.print(colorL.r); Serial.print(" ");
//  Serial.print("Left Green: "); Serial.print(colorL.g); Serial.print(" ");
//  Serial.print("Left Blue: "); Serial.print(colorL.b); Serial.print(" ");
//  Serial.print("Left Clear: "); Serial.print(colorL.c, DEC); Serial.print(" ");Serial.println();
  colorR = readColor(1);
//  Serial.print("Right Red: "); Serial.print(colorR.r); Serial.print(" ");
//  Serial.print("Right Green: "); Serial.print(colorR.g); Serial.print(" ");
//  Serial.print("Right Blue: "); Serial.print(colorR.b); Serial.print(" ");
//  Serial.print("Right Clear: "); Serial.print(colorR.c, DEC); Serial.print(" ");Serial.println();
  IR = readIR(2);
//  Serial.print("Distance measured (mm) = ");Serial.println(IR);Serial.println("");

  while (IR < 153){          // Don't hit something
    analogWrite(PWMA, 0);
    analogWrite(PWMB, 0);
    IR = readIR(2);
    IRflag = true;
  }
  if (IRflag == true){
    delay(3000);
    IRflag = false;
  }
  // Move motors/servo
  forward();
  LMEV = digitalRead(LME);
  RMEV = digitalRead(RME);
  if (LMEV == true){LMEV_count++;}
  if (RMEV == true){RMEV_count++;}
//  Serial.print("LMEV_count: ");Serial.println(LMEV_count);
//  Serial.print("RMEV_count: ");Serial.println(RMEV_count);Serial.println("");
//  uint16_t count = 40000;
//  while(count > 0){
//    LMEV = digitalRead(LME);
//    RMEV = digitalRead(RME);
//    if (LMEV == true){LMEV_count++;}
//    if (RMEV == true){RMEV_count++;}
//    count--;
//  }
}

EBB.h

struct color_t{
  uint16_t r = 0; // Color sensor Red
  uint16_t g = 0; // Color sensor Green
  uint16_t b = 0; // Color sensor Blue
  uint16_t c = 0; // Color sensor Clear
};

EBBsub.ino

void tcaselect(uint8_t i) {
  //Selecting an address on I2C Mutiplexer
  if (i > 7) return;
  Wire.beginTransmission(TCAADDR);
  Wire.write(1 << i);
  Wire.endTransmission();
}

color_t readColor(uint8_t index) {
  color_t colors; // create an instance of color_t called colors
  // Read value from color sensor and determine reading
  tcaselect(index);
  colorSensor.begin();
  colorSensor.getRawData(&colors.r, &colors.g, &colors.b, &colors.c);
  return colors;
}

uint8_t readIR(uint8_t index) {
  // Read value from color sensor and determine reading
  tcaselect(index);
  uint8_t IR = sensor.getDistance();
  return IR;
}

void forward(){
  digitalWrite(STBY, HIGH);
  digitalWrite(AIN2, LOW);
  digitalWrite(AIN1, HIGH);
  digitalWrite(BIN1, HIGH);
  digitalWrite(BIN2, LOW);
  analogWrite(PWMA, 150);
  analogWrite(PWMB, 150);
  analogWrite(servo_pin,60);
}

void turn_right() {
  digitalWrite(STBY, HIGH);
  digitalWrite(AIN2, LOW);
  digitalWrite(AIN1, HIGH);
  digitalWrite(BIN1, HIGH);
  digitalWrite(BIN2, LOW);
  analogWrite(PWMA, 100);
  analogWrite(PWMB, 150);
  analogWrite(servo_pin,30);
}

void turn_left() {
  digitalWrite(STBY, HIGH);
  digitalWrite(AIN2, LOW);
  digitalWrite(AIN1, HIGH);
  digitalWrite(BIN1, HIGH);
  digitalWrite(BIN2, LOW);
  analogWrite(PWMA, 150);
  analogWrite(PWMB, 100);
  analogWrite(servo_pin,90);
}

void turn_around() {
}

void stopRobot(uint16_t d) {
  digitalWrite(STBY, HIGH);
  digitalWrite(AIN2, LOW);
  digitalWrite(AIN1, HIGH);
  digitalWrite(BIN1, HIGH);
  digitalWrite(BIN2, LOW);
  analogWrite(PWMA, 0);
  analogWrite(PWMB, 0);
  analogWrite(servo_pin,115);
  delay(d);
}

Fall 2017: ModWheels Cable Diagram

/in /by

 

By: Natalie Arevalo (Design & Manufacturing Engineer)

Approved by: Lucas Gutierrez (Project Manager)

Introduction

Among the requirements set by the Robot Company, all projects must design and fabricate a custom PCB. However, due to the nature of our project and its design, a custom PCB is not necessary. In order to get the requirement of the custom PCB waived, the cable diagram for our project must be provided. The following is the cable diagram for our robot accompanied by a description. The description and diagram show that the components within our design can be directly connected into the 3Dot board without being interfaced through a custom PCB.  

Cable Diagram

The electrical components implemented in our design include one 3DoT board, one generic color shield, two motors each with an encoder, one servo, and one proximity sensor. The majority of the components listed can be directly connected into the 3DoT board. For instance, the generic color shield will be directly connected into the color shield header on the bottom side of the board. Additionally, the servo used in the design is also directly connected into the male servo header on the 3DoT board. Furthermore, the SDA and SCL connections on the proximity sensor are directly connected into the SDA and SCL female connections on the board. Also, the A channel connections on both encoders are directly connected into the A0 and A1 female connectors on the 3DoT. Then, the M1 and M2 connections of both encoders are placed into the female connectors for motor drivers B and A  on the board. The only connections placed indirectly into the 3DoT board are the ground and VCC connections for both of the encoders and the proximity sensor. These are placed into the 3.3 V and ground female connectors on the 3DoT board through some minor breadboarding. The two encoders on the motors, as well as the proximity sensor, have ground connections that are soldered in series on a through hole breadboard. A lead coming off of this breadboard, connected in series with the other ground connections, is then connected directly into the ground female connector on the 3DoT board. This set-up is then repeated for the 3.3 V connection on the proximity sensor and both VCC connections on the encoders. The lead coming off of their respective through hole breadboard is connected into the 3.3 V female connector on the 3DoT board. As shown below and detailed above, the majority of the connections needed for our components can be directly placed into the  board. The only exceptions are the VCC and ground connections for the encoders and proximity sensors. This is resolved by soldering these connections in series on a through hole breadboard and then connecting a lead off of that directly into the respective female connectors on the 3DoT board. With this small design implementation, all components are eventually connected directly into the board thus eliminating the need for a custom PCB.

Figure 1: Cable Diagram

Update

Due to unforeseen complications, 3DoT boards will not be available to be incorporated into our design. Instead, we will be executing the mission by incorporating break out boards that are connected on a breadboard. We will be using a ProMicro, a motor driver, as well as an HM 10 bluetooth module in place of the 3DoT board. We will also be using an IR shield instead of a color shield because there were complications with the fabrications of these as well. As a result, the cable diagram for our design has changed drastically. The new components and their incorporation onto the breadboard are shown in the updated cable diagram below.

Figure 2: Updated Cable Diagram