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