#defineMAXPWM255//max pwm ie 2^8 = 256#defineTURNPWM255#defineDRYRUNPWM0#defineB1//Black input for sensor#defineW0/*Ideally B=1 and W=0*/#defineACHK(X)(X>=450)//for analog pins#defineAND&&#defineOR||#defineSWITCHON1#defineSWITCHOFF0#defineTRUE1#defineLEFTMOTOROFFSET0#defineFALSE0#definebot_Right_Of_The_Line_Error-8#definebot_Left_Of_The_Line_Error8//pins on Arduino for sensors#defineA014#defineA115#defineA216#defineA312#defineA411//Pins for motors#defineblme6#defineblmi15#defineblmi24#definebrme10#definebrmi19#definebrmi28/*PID*/staticunsignedlong lastTime=0;//let the bot start at t=0staticdouble newRPM;//let this be my new rpmstaticdouble Kp =30;staticdouble Ki =13;staticdouble Kd =1;//constantsstaticdouble errSum=0, lastErr=0;//function values for integration and diffstaticdouble error =0;//proportional#definevictoryLed2#defineactualRunButton0static 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;staticunsignedint turnLeft90Time =4500;staticunsignedint turnLeft135Time =2250;staticunsignedint turnRight90Time =3750;staticunsignedint turnRight135Time =1875;staticunsignedint turnRight180Time =8000;/*for st line deadend*/staticunsignedint hardBrakeTime =0;/*Dont exceed it might start to go in reverse*/staticunsignedint moveExtraInchTime =3000;/***********END_OF_FILE*************/
class Motor{shortint enable;//the enable pin shortint input1;//the first or vcc of motorshortint input2;//the 2nd input or gnd of motor//unsigned int PWM;
public:Motor(shortint enablePinNo,shortint input1PinNo,shortint input2PinNo){//constructor
enable = enablePinNo;
input1 = input1PinNo;
input2 = input2PinNo;}voidSetup(){//Goes in the setup functionpinMode(enable, OUTPUT);pinMode(input1, OUTPUT);pinMode(input2, OUTPUT);}voidforward(unsignedint PWM){//moves the bot forwardanalogWrite(enable, PWM);digitalWrite(input1, HIGH);digitalWrite(input2, LOW);}voidbackward(unsignedint PWM){/*moves the bot in reverse useful for hardBrake*/analogWrite(enable, PWM);digitalWrite(input1, LOW);digitalWrite(input2, HIGH);}voidbrake(unsignedlong 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*****************/
voidmoveBotForward(void){if(refineRPM()>=0){/*
* bot to the left of line
* reducing right motor speed
*/
backRightMotor.forward(MAXPWM - newRPM);
backLeftMotor.forward(MAXPWM - LEFTMOTOROFFSET);}elseif(refineRPM()<=0){/*
* bot to the right of line
* reducing left motor speed
*/
backLeftMotor.forward(MAXPWM - LEFTMOTOROFFSET + newRPM);
backRightMotor.forward(MAXPWM);}}voidmoveBotReverse(void){
backLeftMotor.backward(MAXPWM);
backRightMotor.backward(MAXPWM);}voidmoveBotTorqueLeft(void){
backRightMotor.forward(TURNPWM);
backLeftMotor.backward(TURNPWM);}voidmoveBotTorqueRight(void){
backLeftMotor.forward(TURNPWM);
backRightMotor.backward(TURNPWM);}voidstopBot(void){
backRightMotor.brake(0);
backLeftMotor.brake(0);}/**************END_OF_FILE*****************/
staticunsignedlong now =0;staticdouble timeChange =0;doublerefineRPM(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***************************/
voidcalcShortestPath(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");}elseif(shortestPath.equals("LBS")){
Path.replace("LBS","R");}elseif(shortestPath.equals("RBL")){
Path.replace("RBL","B");}elseif(shortestPath.equals("SBL")){
Path.replace("SBL","R");}elseif(shortestPath.equals("SBS")){
Path.replace("SBS","B");}elseif(shortestPath.equals("LBL")){
Path.replace("LBL","S");}}}/**************END_OF_FILE*****************/
staticunsignedint previousSwitchTime =0;staticunsignedint 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*****************/
扫一扫
3736
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
