Programming we found is the most complex aspect of all our task.
Our mechanics and sensors can work 100%, but without competent programming codes; nothing works.
As I have mentioned before. The robot can seek out and find the can. What about the rest?
It turns out the rest is a lot more work than we have time for.
The scan for can finds the can but we still need an approach the can routine. The robot can't just rush to the can and hope to hold it in upright. The robot needs to cautiously creep up to the can so it maybe able to capture or contain it in the upright position for color reading.
Even if it was able to approach the can close enough for containment, the arm needs to capture or secure the can gently so it doesn't knock over. A lower the arm subroutine was needed to actuate a competent secure the can routine that will always capture the can upright and not knock it over. Once the color is read, the robot needs to make decision base on the result of the reading. If it is not the correct color of choice, the can needs to be rejected and kept out of the robot's way. The ferris wheel will flip the other way and knock the top of the can from behind and push it out and knocking it down at the same time. If it is the correct color of choice, the robot keeps the can and work the subroutine for placing the can in its proper color region. We have a subroutine that does a random 90 degree turns when it lines up with any black lines. When ever we hit the black line, we use the ground color scan routine to check for the correct color over the black line--our arm is made to do such scan at convenience. No matter what can or what can color spot, we will find the place the can.
We did not have the arena to test this until the competition. ( we hadn't tweek and finish this part of the program in class ) but here's what we got the day before the competition along with post dated competition video.
We must admit that though we have much help from our professor concerning programming, we still have lots of work to do.
#pragma config(Sensor, in1, LeftLineSensor, sensorLineFollower)
#pragma config(Sensor, in2, RightLineSensor, sensorLineFollower)
#pragma config(Sensor, in3, Range, sensorReflection)
#pragma config(Sensor, in4, Color, sensorReflection)
#pragma config(Sensor, in5, Switch, sensorTouch)
#pragma config(Sensor, in6, LeftWheel, sensorRotation)
#pragma config(Sensor, in7, RightWheel, sensorRotation)
#pragma config(Motor, port1, LeftMotor, tmotorNormal, openLoop)
#pragma config(Motor, port2, RightMotor, tmotorNormal, openLoop, reversed)
#pragma config(Motor, port3, Servo, tmotorServoStandard, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
int can_color = 3; //1 = RED,2 = Green, 3 = Blue
int foundcolor = 0; //This is the color found by the color sensor
void goforward(int time)
{
motor[port2] = 50;
motor[port1] = 50;
wait1Msec(time);
}
void gobackward(int time)
{
motor[port2] = -50;
motor[port1] = -50;
wait1Msec(time);
}
void TurnLeft(int time)
{
motor[port2] = 55;
motor[port1] = -53;
wait10Msec(time);
}
void TurnRight(int time)
{
motor[port2] = -55;
motor[port1] = 55;
wait10Msec(time);
}
void StopBot(int time)
{
motor[port2] = 0;
motor[port1] = 0;
wait10Msec(time);
}
void SlowArmDown ()
{
motor[port3] = -60;
wait10Msec(50);
motor[port3] = -40;
wait10Msec(50);
motor[port3] = -30;
wait10Msec(50);
motor[port3] = -20;
wait10Msec(50);
motor[port3] = -10;
wait10Msec(50);
motor[port3] = -0;
wait10Msec(50);
motor[port3] = -10;
wait10Msec(50);
motor[port3] = -20;
wait10Msec(50);
motor[port3] = -30;
wait10Msec(50);
motor[port3] = -40;
wait10Msec(50);
motor[port3] = -50;
wait10Msec(50);
motor[port3] = -60;
wait10Msec(50);
motor[port3] = -70;
wait10Msec(50);
motor[port3] = -80;
wait10Msec(50);
}
void gotocolor(int destcolor) //1 is red, 2 is blue, 3 is green
{ //Go to a line
while ((SensorValue[RightLineSensor] <>
{
goforward(1);
}
//Now we are on a line
if (SensorValue[RightLineSensor]>800)
{
StopBot(100);
while(SensorValue[LeftLineSensor] <>
{
TurnLeft(1);
}
StopBot(1000);
motor[port3] = -60;
wait10Msec(20);
motor[port3] = 0;
wait10Msec(20);
//Now the other sensor is over the line so the cart is straight on the line
// Now turn 90 degrees
TurnLeft(150);
StopBot(100);
motor[port3] = -60;
wait10Msec(20);
motor[port3] = 0;
wait10Msec(20);
}
else if (SensorValue[LeftLineSensor]>800)
StopBot(100);
{ while(SensorValue[RightLineSensor] <>
{
TurnRight(1);
}
StopBot(100);
motor[port3] = -60;
wait10Msec(20);
motor[port3] = 0;
wait10Msec(20);
//Now the other sensor is over the line so the cart is straight on the line
// Now turn 90 degrees
TurnRight(150);
StopBot(100);
motor[port3] = -60;
wait10Msec(20);
motor[port3] = 0;
wait10Msec(20);
}
}
//Check the color of the can. Right now there is just signalling IE pecking to indicate the color found
//for debugging purposes.
void colorscan()
{
foundcolor = SensorValue[Color];
if (foundcolor <>
{
StopBot(10);
motor[port3] = -60;
wait10Msec(20);
motor[port3] = -50;
wait10Msec(20);
motor[port3] = -60;
wait10Msec(20);
motor[port3] = -50;
wait10Msec(20);
motor[port3] = -60;
wait10Msec(20);
motor[port3] = -50;
wait10Msec(20);
gotocolor(1);
}
if (foundcolor > 500 && foundcolor <>
{
StopBot(10);
motor[port3] = -60;
wait10Msec(100);
motor[port3] = -50;
wait10Msec(100);
motor[port3] = -60;
wait10Msec(100);
motor[port3] = -50;
wait10Msec(50);
gotocolor(2);
}
if (foundcolor <> 100) //Green Peck 1 times
{
StopBot(10);
motor[port3] = -60;
wait10Msec(20);
motor[port3] = -50;
wait10Msec(20);
gotocolor(3);
}
}
//Check the color of the GROUND. Right now there is just signalling IE pecking to indicate the color found
//for debugging purposes.
void Groundcolorscan()
{
foundcolor = SensorValue[Color];
if (foundcolor <>
{
StopBot(10);
motor[port3] = -60;
wait10Msec(20);
motor[port3] = -50;
wait10Msec(20);
motor[port3] = -60;
wait10Msec(20);
motor[port3] = -50;
wait10Msec(20);
motor[port3] = -60;
wait10Msec(20);
motor[port3] = -50;
wait10Msec(20);
}
if (foundcolor > 500 && foundcolor <>
{
StopBot(10);
motor[port3] = -60;
wait10Msec(100);
motor[port3] = -50;
wait10Msec(100);
motor[port3] = -60;
wait10Msec(100);
motor[port3] = -50;
wait10Msec(50);
}
if (foundcolor <> 100) //Green Peck 1 times
{
StopBot(10);
motor[port3] = -60;
wait10Msec(20);
motor[port3] = -50;
wait10Msec(20);
}
}
void approachCan()
{
while (SensorValue[Range]>50 && SensorValue[Range]<200)
{
//CheckLine();
goforward(10);
StopBot(1);
//colorscan();
}
while (SensorValue[Range]>200 && SensorValue[Range]<400)
{
//CheckLine();
goforward(10);
StopBot(1);
//colorscan();
}
if (SensorValue[Range]>400) //If the can is close bring it all the way in.
{ //Bring the can all the way in
SlowArmDown ();
}
//colorscan();
gotocolor(1);
}
// First stop the robot, then move the sensor arm up and down to indicate that a can is found.
//Next call approach can to move toward the can
void grabcan()
{
StopBot(100);
motor[port3] = -50;
wait10Msec(100);
motor[port3] = -60;
wait10Msec(100);
approachCan();
}
//Approach the can until it is close, continually checking the color of the can.
//Once the can is close, move the sensor arm to bring the can into the robot.
void ScanforCanDelay(int times)
{
for (int i=0;i <>
{
wait10Msec(1);
if (SensorValue(Range) > 100)
{
grabcan();
}
}
}
void ScanforCan()
{
if (SensorValue(Range) > 100)
{
grabcan();
}
}
void CheckLine()
{
if (SensorValue[RightLineSensor] > 800) //If we see the line on the right then go back turn and scan
{
gobackward(1000);
TurnLeft(1);
ScanforCanDelay(150);
}
else if (SensorValue[LeftLineSensor] > 800) //If we see the line on the left then go back turn and scan
{
gobackward(1000);
TurnRight(1);
ScanforCanDelay(150);
}
}
/*
void candetection //
{
motor[port3] = 50;
if ((abs((sensorvalue(ColorSensor)-FloorColor))) > 100)
{
do
{
motor[port2] = 30;
motor[port1] = 30;
while (sensorvalue(Switch)=0)
stopbot(100);
motor[port2] = 50;
motor[port1] = 50;
if (SensorValue(Rightv
do
{
motor[port1] = 40;
motor[port2] = 0;
while (SensorValue(LefttLineSensor) <>
motor(port3) = 50
motor[port1] = -40;
motor[port2] = -40;
wait10Msec(150)
goto maingoal;
}
else if (SensorValue(LefttLineSensor) > 800)
{
stopbot(100);
do
{
motor[port1] = 0;
motor[port2] = 40;
while (SensorValue(RighttLineSensor) <>
motor(port3) = 50
motor[port1] = -40;
motor[port2] = -40;
wait10Msec(150)
goto maingoal;
}
else if ((abs((sensorvalue(ColorSensor)-FloorColor))) <>
}
}
void CanMission
{
motor[port3] = 30; //Close the arm
do
{
goforward(10);
while ((sensorvalue(LeftLineSensor) <>
}
if (sensorvalue(LeftLineSensor) > 800)
{
do
{motor[port2] = 30;
motor[port1] = 00;
while (sensorvalue(RightLineSensor) <>
}
stopbot(100);
goforward(50);
if sensorvalue
if
}
}
*/
task main()
{
motor[port3] = 10;
int FloorColor = SensorValue(Color); //First find the color of the floor
wait1Msec(3000);
Groundcolorscan();
// TurnRight(1000);
// StopBot(100);;
goforward(800); //ON startup move forward to get out of gate
//In the main loop, the robot moves forward 1 cm, checks to see if it hit the black line.(CheckLine)
//If it hit the line, it moves backward and turns.
//The the robot scans for the can. If it finds the can, it runs grab can
//If it doesn't find the can it repeats this process of moving forward, checking for the line and scanning for the can.
while (true)
{
//Maneuver Code
goforward(1);
CheckLine(); //If the the bot does not hit a black line
ScanforCan(); //This scans for the can one time
}
No comments:
Post a Comment