As one can see the range sensor is not working still not as well. however i noticed that when i worked only with line sensor i had a high chance of getting a can. Rangi wanted me to really start working on the line sensor and the color sensor today instad of the range sensor. His line sensor was working great on the first try with no problems. when we worked on the color sensor though we noticed that it had a hard time find out the difference between blue and white. it was not until that he and i looked at the code that we found the problem.
INSERT CODE HERE
when we finished with that our color sensor was working great. we were able to distinguish between different colors from about 2 inches away which was a nice advantage compared to other teams.
we also latter found that we had to adjust the color sensor in the ENDGAME phase because the color sesnor had to be at certian positoons to detected the different values of the target color floor. if it had to detect a green value it had to be about 20 degrees up. if it was red it had to be about -10 degress.
#pragma config(Sensor, in1, LineDetector, sensorLineFollower)
#pragma config(Sensor, in2, colorSensor, sensorReflection)
#pragma config(Sensor, in5, SonarSensor, sensorSONAR, int1)
#pragma config(Sensor, in7, LeftEncoder, sensorRotation)
#pragma config(Sensor, in8, RightEncoder, sensorRotation)
#pragma config(Motor, port1, LeftMotor, tmotorNormal, openLoop)
#pragma config(Motor, port2, RightMotor, tmotorNormal, openLoop)
#pragma config(Motor, port3, hand, tmotorNormal, openLoop)
#pragma config(Motor, port4, colorMotor, tmotorNormal, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
/* THIS IS THE SIMPLE TRACKER PROGRAM THAT USES THE LINE TRACKER MAINLY TO FIND THE CANS. IF THE CAN IS
IS WITHIN THE CERTIAN DISNCE OF OUR SENSOR(FOUR INCHES) THEN THE COLOR SENSOR WILL CHECK THAT
VALE TO MAKE SURE IT WAS THE ONE THAT IT WENT FOR IN THE TARGETING PHASE. AFTER ITS FOUND ITS
CAN IT WILL LOOK ON THE GROUND AND TRY TO FIND ITS CORRECT COLOR THEN STOP.
*/
void goForward(int distance);
void goForward();
void goLeft(int distance);
void goRight(int distance);
void goStop(int distance);
void goback (int distance);
void grab();
void letGo();
void colorReject();
int colorPositionServo(int targetColor);
int colorCheckVal(int targetColor);
task main
{
bMotorReflected[port2] = 1; //Reflects the direction of the motor on port2
SensorValue[RightEncoder] = 0; //Set the encoder so that it starts counting at 0
SensorValue[LeftEncoder] = 0;
int lineCheck = 0; //Line Sensor values
int targetColor = 0; //Target color
int colorCheck=0; //Color indicator for collecting cans
int colorCheck2 = 0;
int colorPosition = 0; //FOR THE MOTORS OF THE SEARCH LINGHT WHEN WE ARE TRYING TO FIND THE END GAME IT IS
//(-50) = GREEN (50) = RED (ANY) = BLUE
int range = 0; //Used to hold sonar values
int groundColor = 0;
int colorNumber = 0; //1 = red 2 = blue 3 = green
bool go = true; //While true the loop to find the cans will be true
bool endGame = true;
int count =0; //count is used to simply check if we have crossed the line 5 times
//if count has more than five it will go forward
//*!!ALL CODE ABOVE WILL BE DECLARTIONS AND BELOW CODE WILL BE START OF GAME !!*//
wait10Msec(200); //Waits 2 seconds to be placed down
motor[port4] = 50; //Raises the color sensor to find target color.
wait10Msec(500); //waits 5 seconds so the color will be clearly indicated
motor[port4] = 0;
targetColor = SensorValue(colorSensor);
colorNumber = colorCheckVal(targetColor);//COLOR indicates what can it is
if (colorNumber ==4) //DOUBLE CHECK if out of range
{
motor[port4] = 50; //Raises the color sensor to find target color.
colorNumber = 0;
wait10Msec(500); //waits 5 seconds so the color will be clearly indicated
targetColor = SensorValue(colorSensor);
motor[port4] = 0;
colorNumber = colorCheckVal(targetColor);
}
colorPosition = colorPositionServo(targetColor); //we pass in the value of the target sensor and now the color position is
//going to hold whatever value
//Now going into SEARCH and END GAME MODE
while (endGame)
{
while(go) //GO keeps going until we find the can
{
lineCheck = SensorValue(LineDetector); //set lineCheker to the line detector value during the search loop
range = SensorValue(SonarSensor); //set Range as what the sonar will see
if (lineCheck < 400) //If the line is 450 or less we keep moving
{
goForward();
if (range < 4 && range>0) //if the range is less than 4 or greater than zero will stop and grab the can
{
goStop(200); //STOPPP!!!! we are in range!!!
grab();
wait10Msec(200); //Wait 2 seconds to get a good reading
colorCheck = SensorValue(colorSensor); //CAN COLOLR THAT YOU HAVE!!!!
colorCheck2 = colorCheckVal(colorCheck);//COLOR indicates what can it is
if (colorNumber == 4) //DOUBLE CHECK if out of setRange
{
colorNumber = 0;
colorCheck2 = 0;
motor[port4] = 50; //Raises the color sensor to find target color.
wait10Msec(500); //waits 5 seconds so the color will be clearly indicated
colorCheck = SensorValue(colorSensor);
colorCheck2 = colorCheckVal(colorCheck);
}//end of doubleChecking if statement (still inside the range else and not outside the line
wait10Msec(200);
motor[port4] = 0; //go to default location for the color sensor
count = 0; //reset count if used before like all other instance of count == 0
if (colorCheck2 == colorCheck)
{
go = false;
}//second if end and also end of tracking if statement (still inside the range if)
else
{
goback(20);
colorReject();
letGo();
count = 0;
}
}//end of range if
else
{
goForward();
}
}//END OF IF LINE IS LESS THATN 950
else
{
if(count == 5)
{
goForward(200);
}
else //THIS IS THE MOVEMENT WHEN WE HIT A LINE
{
goStop(20);
goback(50);
goStop(20);
goLeft(200);
goStop(20);
count++;
}
}
}//END OF SEARCH(SINGLE TARGET)
lineCheck = SensorValue(LineDetector); //Checking if we don't go out of line
motor[port4] = colorPosition; //move the motor to the position for the desiered target
groundColor = SensorValue(colorSensor); //search whatever our ground color is
goForward();
if(lineCheck <350)
{
goForward();
count = 0;
}
if(lineCheck>350 && groundColor == targetColor)
{
goForward(30);
letGo();
goForward(20);
endGame = false;
count = 0;
}
if(lineCheck>350 && groundColor <1000)
{
goback(20);
goLeft(200);
count = 0;
}
else //THIS IS THE MOVEMENT WHEN WE HIT A LINE
{
goStop(20);
goback(50);
goStop(20);
goLeft(200);
goStop(20);
count++;
}
}//end of ENDGAME
}//END OF MAIN
void goForward(int distance)
{
motor[port1] = 35;//rightSide
motor[port2] = 30;//LeftSide
wait10Msec(distance);
}
void goForward()
{
motor[port1] = 35;//rightSide
motor[port2] = 30;//LeftSide
}
void goLeft(int distance)
{
motor[port1] = 30;
motor[port2] = -40;
wait10Msec(distance);
}
void goRight(int distance)
{
motor[port1] = 40;
motor[port2] = -30;
wait10Msec(distance);
}
void goStop(int distance)
{
motor[port1] = 0;
motor[port2] = 0;
wait10Msec(distance);
}
void goback (int distance)
{
motor [port1]=-50;
motor [port2]=-53;
wait10Msec(distance);
}
void grab()
{
motor [port3]= -80; //close the hand to a tight fit (for our servo it was -80)
wait10Msec(50);
motor[port4] = 50; //raise the hand to check the value
wait10Msec(50);
}
void letGo()
{
motor [port3]= 0;
wait10Msec(50);
}
void colorReject()
{
goLeft(200);
goForward(20);
letGo();
goback(20);
goRight(200);
}
int colorPositionServo(int targetColor)
{
int colorPos;
if(targetColor < 100)
//If the color sensor reads less than 100 (red) it will set the position
//else it will go to blue
{
colorPos = 50;
}
else
{
colorPos = -50;
}
return colorPos;
}
int colorCheckVal(int targetColor)
{
int colorNum=0;
if(targetColor<100)//RED
{
colorNum = 1;
}
if(targetColor<400 && targetColor>200)//GREEN
{
colorNum = 2;
}
if(targetColor<900 && targetColor>500)//BLUE
{
colorNum = 3;
}
if(targetColor>900) //out of range
{
colorNum = 4;
}
return colorNum;
}
This is the final code that we will use at the compotition
No comments:
Post a Comment