Fall 2017: ModWheels Final Project Document
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
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
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.
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)
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.
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.
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
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.
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.
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
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
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
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.
- 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).
- Implement 3DoT v5.03 within ModWheels.
- Improved Steering
- Improve steering mechanism on ModWheels chassis to improve steadiness of steering.
- 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).
- 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.
- Improve software to implement Arxterra control of the robot (implement and test software developed for 3DoT v5.03 ModWheels after v5.03 is fixed).
- Improve power budget (more testing with servo to determine current draw and effectiveness at different supply voltages).
- Design modifications to allow for software upload while 3DoT is in the ModWheels chassis.
Fall 2017: ModWheels Schedule
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
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
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
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
- Substitute for 3DoT v5.03
- Sparkfun Pro Micro (3.3V/8MHz – 32U4)
- Sparkfun Motor Driver (TB6612FNG)
- IR sensor shield (designed by Charles & Muhannad)
- Micro Servo
- Two Extended Shaft GM6 Gear motors
- Two Magnetic Hall Effect Encoders
Figure: Wiring Diagram
Fall 2017: ModWheels Hardware Design
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
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
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
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);
}