#define MAXPWM 255//max pwm ie 2^8 = 256
#define TURNPWM 255
#define DRYRUNPWM 0
#define B 1//Black input for sensor
#define W 0
/*Ideally B=1 and W=0*/
#define ACHK(X) (X>=450)//for analog pins
#define AND &&
#define OR ||
#define SWITCHON 1
#define SWITCHOFF 0
#define TRUE 1
#define LEFTMOTOROFFSET 0
#define FALSE 0
#define bot_Right_Of_The_Line_Error -8
#define bot_Left_Of_The_Line_Error 8
//pins on Arduino for sensors
#define A0 14
#define A1 15
#define A2 16
#define A3 12
#define A4 11
//Pins for motors
#define blme 6
#define blmi1 5
#define blmi2 4
#define brme 10
#define brmi1 9
#define brmi2 8
/*PID*/
static unsigned long lastTime=0; //let the bot start at t=0
static double newRPM; //let this be my new rpm
static double Kp = 30;
static double Ki = 13;
static double Kd = 1; //constants
static double errSum=0, lastErr=0; //function values for integration and diff
static double error = 0; //proportional
#define victoryLed 2
#define actualRunButton 0
static String Path = "";
static bool DRYRUNFINISH = 0;
/*BOT_TURNS*/
/*The turning values are for max initial speed and not zero initial speed*/
static bool intersectionDetectedFlag = 0;
static unsigned int turnLeft90Time = 4500;
static unsigned int turnLeft135Time = 2250;
static unsigned int turnRight90Time = 3750;
static unsigned int turnRight135Time = 1875;
static unsigned int turnRight180Time = 8000;/*for st line deadend*/
static unsigned int hardBrakeTime = 0;/*Dont exceed it might start to go in reverse*/
static unsigned int moveExtraInchTime = 3000;
/***********END_OF_FILE*************/
#include "A_A_PORT.h"
void glowLedOn(void);
void glowLedOff(void);
void ledAndSwitchSetup(void);
void moveBotForward(void);
void moveBotReverse(void);
void moveBotTorqueLeft(void);
void moveBotTorqueRight(void);
void stopBot(void);
void turnLeft(unsigned long interval);
void turnRight(unsigned interval);
void moveExtraInch(void);
void uTurnDeadEnd180(void);
void hardBrake(void);
void lineSensorSetup(void);
void updateSensorStatus(void);
int position_Error(void);
void intersection_Primary(void);
void out_Of_Path_Error_Check(void);
double refineRPM(void);
void intersection_Secondary(void);
void intersection_Working(void);
void calcShortestPath(void);
bool actualRunSwitchStatus(void);
void beginActualRun(void);
void glowLedOn(void){
digitalWrite(victoryLed , HIGH);
}
void glowLedOff(void){
digitalWrite(victoryLed , LOW);
}
void ledAndSwitchSetup(void){
pinMode(victoryLed , OUTPUT);
digitalWrite(victoryLed , LOW);
pinMode(actualRunButton , INPUT);
}
/**************END_OF_FILE*****************/
{
short int enable; //the enable pin
short int input1; //the first or vcc of motor
short int input2; //the 2nd input or gnd of motor
//unsigned int PWM;
public:
Motor(short int enablePinNo,short int input1PinNo,short int input2PinNo){ //constructor
enable = enablePinNo;
input1 = input1PinNo;
input2 = input2PinNo;
}
void Setup(){ //Goes in the setup function
pinMode(enable, OUTPUT);
pinMode(input1, OUTPUT);
pinMode(input2, OUTPUT);
}
void forward(unsigned int PWM){ //moves the bot forward
analogWrite(enable, PWM);
digitalWrite(input1, HIGH);
digitalWrite(input2, LOW);
}
void backward(unsigned int PWM){ /*moves the bot in reverse useful for hardBrake*/
analogWrite(enable, PWM);
digitalWrite(input1, LOW);
digitalWrite(input2, HIGH);
}
void brake(unsigned long PWM){ /*stops the bot PWM given if you want to keep any minimum bare value*/
analogWrite(enable, PWM);
digitalWrite(input1, LOW);
digitalWrite(input2, LOW);
}
};
Motor backRightMotor(brme,brmi1,brmi2);
Motor backLeftMotor(blme,blmi1,blmi2);
/**************END_OF_FILE*****************/void moveBotForward(void){
if(refineRPM()>=0){
/*
* bot to the left of line
* reducing right motor speed
*/
backRightMotor.forward(MAXPWM - newRPM);
backLeftMotor.forward(MAXPWM - LEFTMOTOROFFSET);
}
else if(refineRPM()<= 0){
/*
* bot to the right of line
* reducing left motor speed
*/
backLeftMotor.forward(MAXPWM - LEFTMOTOROFFSET + newRPM);
backRightMotor.forward(MAXPWM);
}
}
void moveBotReverse(void){
backLeftMotor.backward(MAXPWM);
backRightMotor.backward(MAXPWM);
}
void moveBotTorqueLeft(void){
backRightMotor.forward(TURNPWM);
backLeftMotor.backward(TURNPWM);
}
void moveBotTorqueRight(void){
backLeftMotor.forward(TURNPWM);
backRightMotor.backward(TURNPWM);
}
void stopBot(void){
backRightMotor.brake(0);
backLeftMotor.brake(0);
}
/**************END_OF_FILE*****************/
void turnLeft(unsigned long interval){
for(int i =0 ; i<interval ; i++){
moveBotTorqueLeft();
}
}
void turnRight(unsigned interval){
for(int i= 0 ; i< interval ; i++){
moveBotTorqueRight();
}
}
void moveExtraInch(void){
for(int i=0 ; i<moveExtraInchTime ; i++){
moveBotForward();
}
}
void uTurnDeadEnd180(void){
stopBot();
turnRight(turnRight180Time);
moveExtraInch();
}
void hardBrake(void){
for(int i=0 ; i < hardBrakeTime ; i++){
moveBotReverse();
}
}
/*****************END_OF_FILE*********************/
int sensor_Array_Pins[] = {A0,A1,A2,A3,A4,A5,A6,A7}; //pin nos for each sensor
/*l2r as in "left to right"*/
int l2r_sensor_Status[] = {'1','1','0','0','0','0','1','1'}; //array to display what each sensor outputs
int l2rs0 = l2r_sensor_Status[0];
int l2rs1 = l2r_sensor_Status[1];
int l2rs2 = l2r_sensor_Status[2];
int l2rs3 = l2r_sensor_Status[3];
int l2rs4 = l2r_sensor_Status[4];
int l2rs5 = l2r_sensor_Status[5];
int l2rs6 = l2r_sensor_Status[6];
int l2rs7 = l2r_sensor_Status[7];
void line_Sensor_Setup(void){
//setup of array pins (goes in the setup function)
for(int i=0; i<8; i++){
pinMode(sensor_Array_Pins[i], INPUT);
}
}
void update_Sensor_Status(void){
l2r_sensor_Status[0] = analogRead(sensor_Array_Pins[7]);
l2r_sensor_Status[1] = analogRead(sensor_Array_Pins[6]);
for(int i=2;i<8;i++){
l2r_sensor_Status[i] = {digitalRead(sensor_Array_Pins[7-i])}; //reading the digital value
}
l2rs0 = l2r_sensor_Status[0];
l2rs1 = l2r_sensor_Status[1];
l2rs2 = l2r_sensor_Status[2];
l2rs3 = l2r_sensor_Status[3];
l2rs4 = l2r_sensor_Status[4];
l2rs5 = l2r_sensor_Status[5];
l2rs6 = l2r_sensor_Status[6];
l2rs7 = l2r_sensor_Status[7];
}
/********END_OF_FILE********/
/*CENTER*/
#define B_B_W_W_W_W_B_B ((ACHK(l2rs0) == B) && (ACHK(l2rs1) == B) && (l2rs2 == W) && (l2rs3 == W) && (l2rs4 == W) && (l2rs5 == W) && (l2rs6 == B) && (l2rs7 == B))
#define B_B_B_W_W_B_B_B ((ACHK(l2rs0) == B) && (ACHK(l2rs1) == B) && (l2rs2 == B) && (l2rs3 == W) && (l2rs4 == W) && (l2rs5 == B) && (l2rs6 == B) && (l2rs7 == B))
/*SLIGHT*/
#define B_B_B_W_W_W_B_B ((ACHK(l2rs0) == B) && (ACHK(l2rs1) == B) && (l2rs2 == B) && (l2rs3 == W) && (l2rs4 == W) && (l2rs5 == W) && (l2rs6 == B) && (l2rs7 == B))
#define B_B_W_W_W_B_B_B ((ACHK(l2rs0) == B) && (ACHK(l2rs1) == B) && (l2rs2 == W) && (l2rs3 == W) && (l2rs4 == W) && (l2rs5 == B) && (l2rs6 == B) && (l2rs7 == B))
/*MODERATE*/
#define B_B_B_W_W_W_W_B ((ACHK(l2rs0) == B) && (ACHK(l2rs1) == B) && (l2rs2 == B) && (l2rs3 == W) && (l2rs4 == W) && (l2rs5 == W) && (l2rs6 == W) && (l2rs7 == B))
#define B_B_B_B_W_W_B_B ((ACHK(l2rs0) == B) && (ACHK(l2rs1) == B) && (l2rs2 == B) && (l2rs3 == B) && (l2rs4 == W) && (l2rs5 == W) && (l2rs6 == B) && (l2rs7 == B))
#define B_W_W_W_W_B_B_B ((ACHK(l2rs0) == B) && (ACHK(l2rs1) == W) && (l2rs2 == W) && (l2rs3 == W) && (l2rs4 == W) && (l2rs5 == B) && (l2rs6 == B) && (l2rs7 == B))
#define B_W_W_B_B_B_B_B ((ACHK(l2rs0) == B) && (ACHK(l2rs1) == W) && (l2rs2 == W) && (l2rs3 == B) && (l2rs4 == B) && (l2rs5 == B) && (l2rs6 == B) && (l2rs7 == B))
/*HEAVY*/
#define B_B_B_B_W_W_W_B ((ACHK(l2rs0) == B) && (ACHK(l2rs1) == B) && (l2rs2 == B) && (l2rs3 == B) && (l2rs4 == W) && (l2rs5 == W) && (l2rs6 == W) && (l2rs7 == B))
#define B_W_W_W_B_B_B_B ((ACHK(l2rs0) == B) && (ACHK(l2rs1) == W) && (l2rs2 == W) && (l2rs3 == W) && (l2rs4 == B) && (l2rs5 == B) && (l2rs6 == B) && (l2rs7 == B))
/*EXTREME*/
#define B_B_B_B_W_W_W_W ((ACHK(l2rs0) == B) && (ACHK(l2rs1) == B) && (l2rs2 == B) && (l2rs3 == B) && (l2rs4 == W) && (l2rs5 == W) && (l2rs6 == W) && (l2rs7 == W))
#define B_B_B_B_B_W_W_B ((ACHK(l2rs0) == B) && (ACHK(l2rs1) == B) && (l2rs2 == B) && (l2rs3 == B) && (l2rs4 == B) && (l2rs5 == W) && (l2rs6 == W) && (l2rs7 == B))
#define W_W_W_W_B_B_B_B ((ACHK(l2rs0) == W) && (ACHK(l2rs1) == W) && (l2rs2 == W) && (l2rs3 == W) && (l2rs4 == B) && (l2rs5 == B) && (l2rs6 == B) && (l2rs7 == B))
#define B_W_W_B_B_B_B_B ((ACHK(l2rs0) == B) && (ACHK(l2rs1) == W) && (l2rs2 == W) && (l2rs3 == B) && (l2rs4 == B) && (l2rs5 == B) && (l2rs6 == B) && (l2rs7 == B))
/*INSANE*/
#define B_B_B_B_B_W_W_W ((ACHK(l2rs0) == B) && (ACHK(l2rs1) == B) && (l2rs2 == B) && (l2rs3 == B) && (l2rs4 == B) && (l2rs5 == W) && (l2rs6 == W) && (l2rs7 == W))
#define W_W_W_B_B_B_B_B ((ACHK(l2rs0) == W) && (ACHK(l2rs1) == W) && (l2rs2 == W) && (l2rs3 == B) && (l2rs4 == B) && (l2rs5 == B) && (l2rs6 == B) && (l2rs7 == B))
/*INTOLERABLE*/
#define B_B_B_B_B_B_W_W ((ACHK(l2rs0) == B) && (ACHK(l2rs1) == B) && (l2rs2 == B) && (l2rs3 == B) && (l2rs4 == B) && (l2rs5 == B) && (l2rs6 == W) && (l2rs7 == W))
#define W_W_B_B_B_B_B_B ((ACHK(l2rs0) == W) && (ACHK(l2rs1) == W) && (l2rs2 == B) && (l2rs3 == B) && (l2rs4 == B) && (l2rs5 == B) && (l2rs6 == B) && (l2rs7 == B))
/*LETHAL*/
#define B_B_B_B_B_B_B_W ((ACHK(l2rs0) == B) && (ACHK(l2rs1) == B) && (l2rs2 == B) && (l2rs3 == B) && (l2rs4 == B) && (l2rs5 == W) && (l2rs6 == W) && (l2rs7 == W))
#define W_B_B_B_B_B_B_B ((ACHK(l2rs0) == W) && (ACHK(l2rs1) == W) && (l2rs2 == W) && (l2rs3 == B) && (l2rs4 == B) && (l2rs5 == B) && (l2rs6 == B) && (l2rs7 == B))
/*It is assumed that center line is zero
*
|<-->(0)<-->(1)<-->(2)<-->(3)<-->(4)<-->(5)<-->(6)<-->(7)<-->|
| represent boundaries for array of sensor
() represent sensors (individual)
sensors are x? cm apart
the vinyl strips are of 3 cm
if bot is at right error is negative if left of line error is positive*/
int position_Error(void){
/*CENTER*/
if(B_B_W_W_W_W_B_B OR B_B_B_W_W_B_B_B )
return 0 ;
/*SLIGHT*/
else if ( B_B_B_W_W_W_B_B )
return 1;
else if ( B_B_W_W_W_B_B_B )
return -1;
/*MODERATE*/
else if ( B_B_B_W_W_W_W_B OR B_B_B_B_W_W_B_B )
return 2;
else if ( B_W_W_W_W_B_B_B OR B_W_W_B_B_B_B_B )
return -2;
/*HEAVY*/
else if ( B_B_B_B_W_W_W_B )
return 3;
else if ( B_W_W_W_B_B_B_B )
return -3;
/*EXTREME*/
else if ( B_B_B_B_W_W_W_W OR B_B_B_B_B_W_W_B )
return 4;
else if ( W_W_W_W_B_B_B_B OR B_W_W_B_B_B_B_B )
return -4;
/*INSANE*/
else if ( B_B_B_B_B_W_W_W )
return 5;
else if ( W_W_W_B_B_B_B_B )
return -5;
/*INTOLERABLE*/
else if ( B_B_B_B_B_B_W_W )
return 6;
else if ( W_W_B_B_B_B_B_B )
return -6;
/*LETHAL*/
else if ( B_B_B_B_B_B_B_W )
return 7;
else if ( W_B_B_B_B_B_B_B )
return -7;
}
/**************END_OF_FILE*****************/
// junction = "left90";
#define W_W_W_W_W_W_B_B ((ACHK(l2rs0) == W) && (ACHK(l2rs1) == W) && (l2rs2 == W) && (l2rs3 == W)&& (l2rs4 == W) &&(l2rs5 == W) && (l2rs6 == B) && (l2rs7 == B))
#define W_W_W_W_W_B_B_B ((ACHK(l2rs0) == B) && (ACHK(l2rs1) == B) && (l2rs2 == W) && (l2rs3 == W)&& (l2rs4 == W) &&(l2rs5 == W) && (l2rs6 == B) && (l2rs7 == B))
// junction = "right90";
#define B_B_W_W_W_W_W_W ((ACHK(l2rs0) == B) && (ACHK(l2rs1) == B) && (l2rs2 == W) && (l2rs3 == W)&& (l2rs4 == W) &&(l2rs5 == W) && (l2rs6 == W) && (l2rs7 == W))
#define B_B_B_W_W_W_W_W ((ACHK(l2rs0) == B) && (ACHK(l2rs1) == B) && (l2rs2 == B) && (l2rs3 == W)&& (l2rs4 == W) &&(l2rs5 == W) && (l2rs6 == W) && (l2rs7 == W))
// junction = "t";
#define W_W_W_W_W_W_W_W ((ACHK(l2rs0) == W) && (ACHK(l2rs1) == W) && (l2rs2 == W) && (l2rs3 == W)&& (l2rs4 == W) &&(l2rs5 == W) && (l2rs6 == W) && (l2rs7 == W))
// junction = "deadEnd";
#define B_B_B_B_B_B_B_B ((ACHK(l2rs0) == B) && (ACHK(l2rs1) == B) && (l2rs2 == B) && (l2rs3 == B)&& (l2rs4 == B) &&(l2rs5 == B) && (l2rs6 == B) && (l2rs7 == B))
enum intersection_Library {none, left_90_Junction, right_90_Junction, t_Junction,
y_Junction, dead_End
};
enum intersection_Library intersection_Status = none;
/* |<-->(0)<-->(1)<-->(2)<-->(3)<-->(4)<-->(5)<-->(6)<-->(7)<-->|
1.5 3.1 2.15 2.15 3.1 1.5
BLACK MAT WHITE VINYL STRIPS */
void intersection_Primary(void){
update_Sensor_Status();
if ( W_W_W_W_W_W_B_B OR W_W_W_W_W_B_B_B )
intersection_Status = left_90_Junction;
else if (B_B_W_W_W_W_W_W OR B_B_B_W_W_W_W_W)
intersection_Status = right_90_Junction;
else if (W_W_W_W_W_W_W_W)
intersection_Status = t_Junction;
else if (B_B_B_B_B_B_B_B)
intersection_Status = dead_End;
else
intersection_Status = none;
}
static int error_Buffer = 0;
void out_Of_Path_Error_Check(void){
if ((position_Error() == -7) OR (position_Error() == -6))
error_Buffer = bot_Right_Of_The_Line_Error;
else if ( (position_Error() == 7) OR (position_Error() == 6))
error_Buffer = bot_Left_Of_The_Line_Error;
if(intersection_Status == dead_End)
error = error_Buffer;
}
/**************END_OF_FILE**************/
static unsigned long now = 0;
static double timeChange =0;
double refineRPM(void){
/*current time*/
now = millis();
timeChange = (double)(now - lastTime); /* del't' */
error = position_Error();
out_Of_Path_Error_Check();
if((newRPM <75)&&(newRPM > -75)){
errSum += (error * timeChange); /*integral is now assumed to be clamped*/
}
double dErr = (error - lastErr)/timeChange; /* timeChange derivative*/
newRPM = (Kp * error) + (Ki * errSum) + (Kd * dErr);
lastErr = error; /*Remember some variables for next time*/
lastTime = now;
if(newRPM > 75){
newRPM = 75;
}
if(newRPM < -75){
newRPM = -75;
}
return newRPM;
}
/**************END_OF_FILE*****************/
在这里插入代码片`在这里插入代码片`
bool right90JunctionFlag =0 ;
bool tJunctionFlag = 0;
/*called after right90 turn OR t junction *
void checkIntersection(void){
switch(intersection()){
case deadEnd : /*either all my SENSORS are showing BLACK*
/*When i have right junction*
if((right90JunctionFlag == 1 )&&(tJunctionFlag == 0)){ /*previously right turn so now move right*
turnRight(turnRight90Time);
moveExtraInch();
Path =+ "R";
if(intersection() == deadEnd)
error = botLeftOfTheLineError; /*to the left of line will *** move right *** *
while(positionError() != 0){
moveBotForward();
}
}
/*When i have T junction *
else if((right90JunctionFlag == 0)&&(tJunctionFlag == 1)){/*previously t junction hence turn left*
turnLeft(turnLeft90Time);
moveExtraInch();
Path =+ "L";
if(intersection() == deadEnd)
error = botRightOfTheLineError; /*to the right of line will *** move left *** *
while(positionError() != 0){
moveBotForward();
}
}
break;
case victory : /* Or all white*
hardBrake();
glowLedOn();
DRYRUNFINISH = 1;
break;
default : /*or we have a line*
moveExtraInch();
Path =+ "S";
break;
}
}
void intersectionWorking(void){
switch (intersection()){
case left90Junction : /*this is a pure left turn , bot should go left*
turnLeft(turnLeft90Time);/*gives me a left turn at 90 degrees at full speed*
moveExtraInch();
if(intersection() == deadEnd)
error = botRightOfTheLineError ; /*The value is fed to the PID*
Path =+ "L";
while(positionError() != 0){
moveBotForward();
}
break;
case right90Junction : /*a pure right turn bot should go straight and check*
stopBot();/*drifts forward*
right90JunctionFlag = 1;
checkIntersection();
right90JunctionFlag = 0;
break;
case tJunction : /*move extra inch to check if victory else take left*
stopBot();/*drifts forward*
tJunctionFlag = 1;
checkIntersection();
tJunctionFlag = 0;
break;
case yJunction : /*move left let 'PID' do the adjustments after extra inch *
turnLeft(turnLeft135Time);
moveExtraInch();
if(intersection() == deadEnd)
error = botRightOfTheLineError;
Path =+ "L";
while(positionError() != 0){
moveBotForward();
}
break ;
case lineDeadEnd :
uTurnDeadEnd180();
Path =+ "B";
break;
default :
break; /*should never come here*
}
}
/**********************END_OF_FILE***************************/
void calcShortestPath(void){
int index, prevCharIndex , nextCharIndex ;
String shortestPath = "";
while(Path.indexOf("B") != -1 ){/*till all B's are not eliminated*/
index = Path.indexOf("B");
prevCharIndex = index--;
nextCharIndex = index++;
shortestPath = Path.substring(prevCharIndex , nextCharIndex++);
if(shortestPath.equals("LBR")){
Path.replace("LBR","B");
}
else if(shortestPath.equals("LBS")){
Path.replace("LBS", "R");
}
else if(shortestPath.equals("RBL")){
Path.replace("RBL", "B");
}
else if(shortestPath.equals("SBL")){
Path.replace("SBL", "R");
}
else if(shortestPath.equals("SBS")){
Path.replace("SBS","B");
}
else if(shortestPath.equals("LBL")){
Path.replace("LBL", "S");
}
}
}
/**************END_OF_FILE*****************/
static unsigned int previousSwitchTime = 0;
static unsigned int switchInterval = 0;
/*
bool actualRunSwitchStatus(void){
if(digitalRead(actualRunButton) == SWITCHON){
return SWITCHON;
}
else
return SWITCHOFF;
}
static int actualRunIndex = 0 ;
static char actualRunPath = NULL;
void beginActualRun(void){
while(actualRunSwitchStatus() != SWITCHON);
glowLedOff();
while(1){
if(intersectionDetected() == TRUE){
actualRunIndex++;
}
actualRunPath = Path.charAt(actualRunIndex);
switch(actualRunPath){
case 'S' :
moveBotForward();
break;
case 'L' :
stopBot();
turnLeft(turnLeft90Time);/*gives me a left turn at 90 degrees at full speed*
moveExtraInch();
if(intersection() == deadEnd)
error = botRightOfTheLineError ; /*The value is fed to the PID*
while(positionError() != 0){
moveBotForward();
}
while(intersectionDetected() != TRUE){
moveBotForward();
}
break;
case 'R' :
stopBot();
turnRight(turnRight90Time);
moveExtraInch();
if(intersection() == deadEnd)
error = botLeftOfTheLineError; /*to the left of line will *** move right *** *
while(positionError() != 0){
moveBotForward();
}
while(intersectionDetected() != TRUE){
moveBotForward();
}
break;
}
}
}
/**************END_OF_FILE*****************/
void setup(){
ledAndSwitchSetup();
line_Sensor_Setup();
backLeftMotor.Setup();
backRightMotor.Setup();
Serial.begin(9600);
}
/**************END_OF_FILE*****************/
//void loop(){//remove comment here
/*if(intersectionDetected() == TRUE){
Path =+ 'S';
intersectionWorking();
if(DRYRUNFINISH == 1){
beginActualRun();
}
}
else{*/
// moveBotForward();
//}
//} //remove comment here*/
/**************END_OF_FILE*********
moveBotForward();
}
if (intersectionDetected() == TRUE){
//intersectionWorking();
if(DRYRUNFINISH == TRUE){
beginActualRun();
}
stopBot();
enter = 0;
}
********/
