Hey everyone,
I am in the final stages of coding for my arduino sumobot for a class project.
I have to have it follow a line and enter an arena to fight another robot.
I am having a difficult time trying to figure out how to code it so that once it finished following the line to the arena, that it will go over the circular arena edge instead of trying to follow it. I am using a pollolu QTR-8-RC sensor using 6 of the available eight. It follows lines beautifully, but I just cant figure out hot to tell the arduino when all 6 sensors see the edge to continue to go straight. Any help or insight would be greatly appreciated.
Code:
#include <QTRSensors.h>
#include <AFMotor.h>
#define MIDDLE_SENSOR 3
#define NUM_SENSORS 5
// Defines the amount of sensors used
#define TIMEOUT 2500
//Sets a time of 2.5 seconds to the QTR sesnors call.
#define EMITTER_PIN 14
// Defines which pin will be used for turning on and off the IR led's
#define KP 0.2
#define KD 5.05
// Defines the constants for the PID Control
#define M1_REG_SPEED 100
#define M2_REG_SPEED 100
// Defines the default speeds for the robots motors
#define M1_MAX_SPEED 125
#define M2_MAX_SPEED 125
// Defines the max speed for the robots motors
#include <stdio.h>
// #include <robot_stuff.h> // add your special header files here
#include <arduino.h>
//**********************************************************************************************************************************
unsigned int sensorValues[NUM_SENSORS];
// Sets the incoming sensor values to be unsigned integers.
QTRSensorsRC qtrrc((unsigned char[]) { 15,16,17,18,19} ,NUM_SENSORS, TIMEOUT, EMITTER_PIN);
// Sets up the QTR reflectance sensor using the QTR sensor library, sets the pins as inputs and reads them. Digital pins 14-19 or analog 0-5.
//********************************************************************************************************************************
int analogPin = A0;
int sensorValue = 0;
//*******************************************************************************************************************************
AF_DCMotor motor1(1, MOTOR12_8KHZ);
AF_DCMotor motor2(2, MOTOR12_8KHZ);
//*******************************************************************************************************************************
void setup(void){
Serial.begin(9600);
}
void loop(void){
enterArena();
sensorValue = analogRead(analogPin);
if (sensorValue < 700 && sensorValue > 350)
{
motor1.setSpeed(100);
motor2.setSpeed(100);
motor1.run(FORWARD);
motor2.run(FORWARD);
}
if (sensorValue >700 || sensorValue < 350)
{ motor1.setSpeed(75);
motor2.setSpeed(75);
motor1.run(BACKWARD);
motor2.run(FORWARD);
}
}
void enterArena()
{
int lastError = 0;
int last_proportional = 0;
int integral = 0;
int motor1speed;
int motor2speed;
unsigned int sensors[5];
// tells
float position = qtrrc.readLine(sensors);
// sets the received position from the QTR sensors as a float value, and stores them.
float error = 2000-position ;
// Sets the error to be 2500-position, and stores it as a float value.
float motorSpeed = KP * error + KD * (error - lastError);
lastError = error;
/* This is the heart of the PID algorithm. This sets the main motor speed equal to the proportional multiplied by the error and adds the derivitive multiplied by the error minus last error. This basically tells the robot how to figure out the
error in the line and where its position is, allowing it to fine tune the motor control for whatever direction it needs to go.*/
float leftMotorSpeed = M1_REG_SPEED + motorSpeed;
// Sets the left motor to be the default speed plus whatever the speed the algorithm determines to adjust it to, and sets it as a float value.
float rightMotorSpeed = M2_REG_SPEED - motorSpeed;
// Sets the right motor to be the default speed minus whatever the speed th algorith has determined to adjust it to, and sets it as a float value.
/* The reason one is adding the value and one is subtracting the value is because the wheels need to spin the same way, not the opposite.*/
if (motor1speed > M1_MAX_SPEED ) motor1speed = M1_MAX_SPEED;
// Basically this limits the top speed so we can't get any error values for motor 1.
if (motor2speed > M2_MAX_SPEED ) motor2speed = M2_MAX_SPEED;
// this one limits the top speed for motor 2 so that we can't get any error values.
if (motor1speed < 0) motor1speed = 0;
// This makes it so that we can't have a negative motor speed value and get an error.
if (motor2speed < 0) motor2speed = 0;
// This makes it so that we can't have a negative motor speed value and get an error.
motor1.setSpeed(motor1speed);
// Sets the speed of motor 1 to the value obtained from motor1speed.
motor2.setSpeed(motor2speed);
// Sets the speed of motor 2 to the value obtained from motor2speed.
motor1.run(FORWARD);
// Instructs motor 1 to run forwards
motor2.run(FORWARD);
//instructs motor two to run forwards.
}