#pragma config(ProgramType, StandaloneWithWiFi)
#pragma config(Sensor, in1, colorsense, sensorLineFollower)
#pragma config(Sensor, in2, line, sensorLineFollower)
#pragma config(Sensor, in3, eleft, sensorRotation)
#pragma config(Sensor, in4, eright, sensorRotation)
#pragma config(Sensor, in12, sonar, sensorSONAR, int1)
#pragma config(Sensor, in13, BLU, sensorTouch)
#pragma config(Sensor, in14, GRN, sensorTouch)
#pragma config(Sensor, in15, RED, sensorTouch)
#pragma config(Sensor, in16, , sensorTouch)
#pragma config(Motor, port1, right, tmotorNormal, openLoop, reversed)
#pragma config(Motor, port2, left, tmotorNormal, openLoop)
#pragma config(Motor, port3, clawleft, tmotorServoStandard, openLoop)
#pragma config(Motor, port4, clawright, tmotorServoStandard, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
//////////////////////////////////////////////////////////////////////////////////////
int nearest=50,inch=11.6,linesense_type=0;
bool override=false,init1=true;
char can_color,ground_color;
char target_can_color,target_ground_color;
bool foundcan=false,can_seek;
///////////////////////////////////////
void clawclose(); void clawopen(); void encoder_clear();
void goforward(int dist, bool length, int power);
void goback(int dist, bool length, int power);
void turnleft(int amount);
void turnright(int amount);
void scan();
char colorcheck();
char foundedge();
//////////////////////////////////////////////
//////////////////////////////////////////////
task find_can()
{
int tries=0;
if (init1==true) // Probably un-needed
{
can_seek=true;
init1=false;
}
while(can_seek==true)
{
if(foundcan == false)
{
if(nearest > 8) //If the nearest obstacle is more than 8 inches away
{
scan(); ////////////
/////
//Go forward the distance of the nearest object
goforward(((nearest)*inch-100),true,50);
wait1Msec(1000);
tries++;
}
else if(nearest <=8) //If it detects an object within 8 inches
{
goforward((nearest*inch+30),true,50); //Go to the object and grab
clawclose();
foundcan = true; //Set global variable to indicate a can is found
wait1Msec(1000);
}
if(tries>=6) //If it's tried to find the can 6 times and failed
{
turnleft(120); //Try looking somewhere else
tries=0;
}
}
else if(foundcan==true) //If a can is found
{
clawopen(); wait1Msec(250); //Get into position to check color
goback(3,true,30); //(These numbers need to be tweaked depending on ambient light)
motor[port8] = 30; //Point the color sensor at the can
wait1Msec(500);
can_color = colorcheck(); //Check the color
goforward(10,true,50); //Go grab it again
clawclose(); wait1Msec(250);
//What to do with the object now?
if(can_color == target_can_color) //If it's the right color
{
motor[port8]=-35; //Point color sensor at the ground
wait1Msec(500);
linesense_type=1; //Change the line detection mode
can_seek=false; //Stop looking for a can
tries=0;
wait1Msec(50);
}
else //Otherwise
{
clawopen(); //Release the can
wait1Msec(1000);
// FLING THE CAN AWAY >:D //
motor[left]=-100;
wait1Msec(250);
motor[clawright]=-10;
turnright(20);
goforward(100,true,70);
turnleft(20);
//////////////////
nearest = 50;
foundcan = false; // Start searching again
tries = 0;
wait1Msec(50);
}
}
}
while(can_seek==false) //When looking for the target zone
{
goforward(0,false,80); //Just go forward
}
}
////////////////////////////////////////////////
////////// LINE DETECTION "THREAD" /////////////
////////////////////////////////////////////////
task line_detect()
{
char edge;
while(linesense_type==0) //Mode when looking for a can
{
if (SensorValue[line] > 750) //The lower bound for dark line
{
override = true;
StopTask(find_can); // Halt all current activity
edge = foundedge(); // Run the edge checking function
if(edge == 'R') // If it reports the line to be on the right
{
turnleft(100); // Turn left
}
if(edge=='L') // If it reports the line to be on the left
{
turnright(100); // Turn right
}
override = false;
goback(5,true,50); // Back up a bit
StartTask(find_can); //Resume searching
}
}
///////////////////////////////
while(linesense_type==1) //Mode after a can has been found
{
if (SensorValue[line] > 700)
{
override = true;
StopTask(find_can);
wait1Msec(40);
goback(1,true,50); //BRAKE
motor[port8]=-35; // Make sure the color sensor is looking at the ground. (redundant)
wait1Msec(200);
ground_color = colorcheck(); //Check the ground color
if((ground_color==target_ground_color) && (SensorValue[line]<600))
//If the ground is the target color, and not a floor tile
{
clawopen(); //Let it go
wait1Msec(500);
goback(100,true,80);
motor[left]=-120; motor[right]=120; /// Spin for joy!!
wait1Msec(1000);
StopAllTasks(); //Shut down
}
else if((ground_color != target_ground_color) || (SensorValue[line]>700))
{
// If it's not the target color, or it's a floor tile
goback(325,true,120); wait1Msec(250); //Go way back
turnleft(120); wait1Msec(200); //Turn left and try a different direction
goforward(0,false,50); wait1Msec(1000);
StartTask(find_can);
}
}
}
}
task main() // EXECUTED AUTOMATICALLY BY THE VEX BRAIN
{
goforward(100,true,60); //Enter the arena
StartTask(line_detect); //OTHER TASKS MUST BE INITIALIZED
StartTask(find_can);
while(true) //UNLESS MAIN TASK IN MAINTAINED, EVERYTHING ELSE STOPS
{
/*SensorValue[RED/GRN/BLU] are simple jumper ports
that read '1' when a jumper is plugged in */
if(SensorValue[RED]==1) //Jumper settings for different colors
{
target_can_color = 'R';
target_ground_color = 'R';
}
else if(SensorValue[GRN]==1)
{
target_can_color = 'G';
target_ground_color = 'G';
}
else if(SensorValue[BLU]==1)
{
target_can_color = 'B';
target_ground_color = 'B';
}
else {StopAllTasks();}
wait1Msec(1000);
}
}
//////////////////////////////////////////////////
//////// MAIN ADVANCED FUNCTIONS ///////////////
//////////////////////////////////////////////////
void scan()
{
short range = 30; //Minimum Sonar range. Any less, the bot grabs the can
short delay = 30; //mS delay for increments
short increment = 2; //amount for each increment
clawopen();
wait1Msec(500);
if(override==false)
{
int oldval=50,newval=50; //intialize the values high for safety
/// Pivot left /////////////////////
for(int i=0; i<35; i++)
{
if ((SensorValue[sonar] < range) && (SensorValue[sonar]>0)) //Sensor sometimes returns -1
{
oldval = SensorValue[sonar]-5; //Set
goto END; //Stop searching
}
turnleft(increment); //Turn left a bit
newval = SensorValue[sonar]; //Read sonar value into newval
if((newval<oldval) && (newval > 0)) //if the value is any closer than the previous
{oldval = newval;} //Store the value into oldval
wait1Msec(delay); //Pause
}
wait1Msec(100); //Waiting before turning the other direction prevents skidding
////////// Repeat the same procedure for the other directions
//// Return to center ////////////////////
for(int i=0; i<40; i++)
{
if ((SensorValue[sonar] < range) && (SensorValue[sonar]>0))
{
oldval = SensorValue[sonar]-5;
turnright(5); //Correction to center the sensor
goto END;
}
turnright(increment);
newval = SensorValue[sonar];
if((newval<oldval) && (newval > 0)) //if the value is any closer than before
{oldval = newval;} //update
wait1Msec(delay);
}
/// Continue to pivot right /////////////
for(int i=0; i<40; i++)
{
if ((SensorValue[sonar] < range) && (SensorValue[sonar]>0))
{
oldval = SensorValue[sonar]-5;
turnright(5); //Correction
goto END;
}
turnright(increment);
newval = SensorValue[sonar];
if((newval<oldval) && (newval > 0)) //if the value is any closer than before
{oldval = newval;} //update
wait1Msec(delay);
}
wait1Msec(100);
/// Return to center heading ///////////////
for(int i=0; i<40; i++)
{
if ((SensorValue[sonar] < range) && (SensorValue[sonar]>0))
{
oldval = SensorValue[sonar]-5;
goto END;
}
turnleft(increment);
newval = SensorValue[sonar]; //read sonar value into newval
if((newval<oldval) & (newval > 0)) //if the value is any closer than before
{oldval = newval;} //update
wait1Msec(delay);
}
//Set the variable "nearest" to be the smallest value found by sonar
END:
nearest = oldval; ///// ***nearest is global***
}
}
//////////////////////////////////////
///////// Edge avoidance /////////////
//////////////////////////////////////
char foundedge()
{
long leftval=0,rightval=0;
int howfar=0;
goback(25,true,60); //First action when encountering edge, back up
turnleft(30); //Then turn left
encoder_clear(); //reset encoders
for(int i=0; i<35; i++) /////Check line stuff to the left
{
goforward(0,false,40); //goes forward a bit
howfar=SensorValue[eleft]; //records the distance
leftval += SensorValue[line]; //Each iteration, store the line sensor value incrementally
wait1Msec(10);
}
goback(howfar,true,40); //Back up the amount traveled
howfar=0;
turnright(60); //Position to check line stuff to the right
encoder_clear(); //reset encoders
for(int i=0; i<35; i++)
{
goforward(0,false,40); ///REPEAT
howfar=SensorValue[eleft];
rightval += SensorValue[line];
wait1Msec(10);
}
goback(howfar,true,40); //backup to starting position
howfar=0;
if(leftval<rightval) //Compare the amount of light discovered on each side
{return 'R';} //Return the value of the side that has more dark
else // which will indicate the side the line is on
{return 'L';}
}
/// Simple color checking function ////
char colorcheck()
{
char color;
for(int i=0; i<4; i++) // Check the value 4 times to eliminate error
{
if(SensorValue[colorsense]==11) // PWM converted signal for RED
{color = 'R';}
//PWM converted signal for GREEN oscillates around 340
if(SensorValue[colorsense]<400 && (SensorValue[colorsense]>300))
{color = 'G';}
//PWM converted signal for BLUE is around 670
if(SensorValue[colorsense]<700 && (SensorValue[colorsense]>600))
{color = 'B';}
//PWM converted signal for NOCOLOR is ~1024
if(SensorValue[colorsense]>950)
{color = 'W';}
wait1Msec(250);
}
return color;
}
/////////////////////////////////////////////////////
//////// OTHER BASIC FUNCTIONS /////////////////////
/////////////////////////////////////////////////////
void clawclose()
{
motor[clawright] = -15; //Tweak these to change the gripping force
motor[clawleft] = 35;
}
////////////////////
void clawopen()
{
motor[clawright] = 75; //NO LARGER THAN 80 FOR EITHER MOTOR,
motor[clawleft] = -60; //CHASSIS IN THE WAY
}
/////////////
void encoder_clear() //Zeroes the encoders (**use before moving a specific distance**)
{
SensorValue[eleft]=0;
SensorValue[eright]=0;
}
//////////////////////////////////////////////
void goforward(int dist, bool length, int power)
{
//Options to choose between specific distance, or just turn on the motor
// and how fast to move (power).
if(length == true) //If specific distance req'd
{
encoder_clear();
while((SensorValue[eleft] < dist) && (SensorValue[eright] < dist))
//while shaft encoders are less than specified distance
{
motor[right] = power; //Engage
motor[left] = power;
}
motor[right] = 0; //Turn off the motor
motor[left] = 0; //Turn off the motor
}
else if(length == false) //If just turning the motors on
{
motor[right] = power;
motor[left] = power;
}
}
//////////////////////////////////////////////
void goback(int dist, bool length, int power)
{
if(length == true)
{
encoder_clear();
while((SensorValue[eleft] < dist)&& (SensorValue[eright] < dist))
{
motor[right] = -power; // ONLY DIFFERENCE FROM ABOVE CODE
motor[left] = -power; // IS RUNNING THE MOTORS IN REVERSE
}
motor[right] = 0;
motor[left] = 0;
}
else if(length == false)
{
motor[right] = -power;
motor[left] = -power;
}
}
/////////////////////////////////////
void turnleft(int amount)
{
//Input value happens to be close to the turn angle in degrees. Nice :D
encoder_clear();
while((SensorValue[eright]<amount) && (SensorValue[eleft]<amount))
{
motor[right] = 50; //Right motor forward
motor[left] = -50; //Left motor reverse
}
motor[right] = 0; //Stop
motor[left] = 0;
}
////////////////////////////////////
void turnright(int amount)
{
encoder_clear();
while((SensorValue[eright]<amount) && (SensorValue[eleft]<amount))
{
motor[right] = -50; //Right motor reverse
motor[left] = 50; //Left motor forward
}
motor[right] = 0; //Stop
motor[left] = 0;
}
/*/////////////////////////////////////////////
--END OF LINE
*/
Saturday, February 12, 2011
Can finding robot code
Here's the final program i used on my can finding robot:
Subscribe to:
Post Comments (Atom)
No comments:
Post a Comment