//////////////////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 } }