//////////////////MAIN/////////////////////
task main()
{
initializeRobot();
waitForStart();
//ARM NEEDS TO BE RAISED HIGH ENOUGH THAT IT WILL BE OVER THE CRATES BUT NOT HIT THE BAR
wait1Msec(300);
if(SensorValue[IRSensor] <= 3 && SensorValue[IRSensor] != 0)
{
crate = 1;
}
else if(SensorValue[IRSensor] == 4)
{
crate = 2;
}
else if(SensorValue[IRSensor] > 4)
{
crate = 3;
}
//---Move up and left
moveStraight(90.0, 3.0,100.0);
moveStraight(25.0,27.0,100.0);
//---Rotate perpendicular to ramp
moveArc(0.0,180.0,100.0);//180 deg
moveArc(0.0,70.0,100.0);
//---Move up ramp to first crate, lift arm up
StartTask(ScoreArm);
moveStraight(0.0, 27.0 ,100.0); //move to crate 1
if(crate == 2) //this crate would have to be the closest one to us
{
moveStraight(0.0, 15.0, 100.0); //move to crate 2
}
else if(crate == 3)
{
moveStraight(0.0, 24.0, 100.0); //move to crate 3
}
boxOpen = true;
StopTask(HoldBox);
//if crate == 1 or 4, just score in crate 1
dropTheBlock();
while(true){}
}
//
// void updateState()
// Last modified: 27Aug2006
//
// Updates the state of the cell based upon the
// current states of the neighbors of the cell.
//
// Returns: <none>
// Parameters: <none>
//
void Cell::updateState()
{
Neighbor currNbr;
for (GLint i = 0; i < getNNbrs(); ++i)
{
if (!getHead(currNbr)) break;
// change formation if a neighbor has changed formation
if (getNbr(0)->formation.getFormationID() > formation.getFormationID())
changeFormation(getNbr(0)->formation, *getNbr(0));
getNbr(0)->relActual = getRelationship(currNbr.ID);
++(*this);
}
rels = getRelationships();
if(rels.getSize())
{
// reference the neighbor with the smallest gradient
// to establish correct position in formation
Neighbor *refNbr = nbrWithMinGradient();
Relationship *nbrRel = relWithID(refNbr->rels, ID);
if ((formation.getSeedID() != ID) && (refNbr != NULL) && (nbrRel != NULL))
{
// error (state) is based upon the accumulated error in the formation
nbrRel->relDesired.rotateRelative(-refNbr->rotError);
GLfloat theta = scaleDegrees(nbrRel->relActual.angle() -
(-refNbr->relActual).angle());
rotError = scaleDegrees(theta + refNbr->rotError);
transError = nbrRel->relDesired - nbrRel->relActual +
refNbr->transError;
transError.rotateRelative(-theta);
if (transError.norm() > threshold()) moveArc(transError);
else
if (abs(rotError) > angThreshold())
moveArc(0.0, degreesToRadians(-rotError));
/*if (abs(scaleDegrees(refNbr->relActual.angle() -
refNbr->relDesired.angle())) > angThreshold())
orientTo(refNbr->relActual, refNbr->relDesired.angle());*/
else moveStop();
}
else moveStop();
}
} // updateState()
task main()
{
waitForStart();
//ARM NEEDS TO BE RAISED HIGH ENOUGH THAT IT WILL BE OVER THE CRATES BUT NOT HIT THE BAR
wait1Msec(300);
if(SensorValue[IRSensor] <= 4 && SensorValue[IRSensor] != 0)
{
crate = 1;
}
else if(SensorValue[IRSensor] == 5)
{
crate = 2;
}
else if(SensorValue[IRSensor] == 0)//added in SensorValue, it caused everything to go wrong
{
crate = 3;
}
moveStraight(215.0, 30.0, 100);//---Move forward
moveStraight(90.0, 20.0, 100);//---Move towards the ramp by having servos turn and then moving forward
if(crate == 2) //this crate would have to be the closest one to us
{
moveStraight(90.0, 30.0, 100); //motors would be going at 100 power
moveArc(0.0, 90.0, 256.0);
dropTheBlock(); //drop the cube into the second crate
}
else if(crate == 3)
{
moveStraight(90.0, 40.0, 100); //motors would be going at 100 power
moveArc(0.0, 90.0, 256.0); //robot will turn 90 degrees to the right
dropTheBlock(); //drop the cube into the third crate
}
else
{
moveStraight(90.0, 15.0, 100); //Robot will go forward for 15 inches at 100 power.
moveArc(0.0, 90.0, 256.0); //robot will turn 90 degrees to the right
dropTheBlock(); //drop the cube into the first crate
}
}
