/*********************************************************************************/ /*********************************************************************************/ /***************************A Program for the MuscleBot***************************/ /*********************************************************************************/ /*********************************************************************************/ /****************************Set Integers and Variables***************************/ int value_1; //a variable for preforming sensor one's calculations int value_3; //a variable for preforming sensor three's calculations int highthreshold1; //sensor one's reading on the board int highthreshold3; //sensor three's reading on the board int lowthreshold1; //sensor one's reading off the board int lowthreshold3; //sensor three's reading off the board int gate_distance; //the distance from the intersection to the gate /********************************Create Subroutines*******************************/ //This subroutine creates a single light sensor board follower that uses hysteresis //and floats the motors to drive about 150 percent faster than the normal single //light sensor line follower. Due to its programming it sacrifices almost no //accuracy. We use it when the robot is on a long straight. sub followboard_fast () { //light sensor is half on and half off the board (gray area) if ((SENSOR_1 > lowthreshold1 && SENSOR_1 < highthreshold1)) { SetPower(OUT_A+OUT_C,3); //increasing this power level increases speed OnFwd (OUT_A+OUT_C); //but decreases accuracy significantly } //light sensor is over the board if (SENSOR_1 > highthreshold1) { SetPower(OUT_A+OUT_C,7); OnFwd (OUT_C); //turn slightly to the left Float (OUT_A); } //light sensor is off the board if (SENSOR_1 < lowthreshold1) { SetPower(OUT_A+OUT_C,7); OnFwd (OUT_A); //turn slightly to the right Float (OUT_C); } } //This subroutine is for a single light sensor board follower that uses hysteresis //to follow the board slightly faster than the normal single light sensor line //follower. It is used when we need to follow the board tightly without straying //too much from the edge of the board sub followboard_tight () { //light sensor is half on and half off the board (gray area) if ((SENSOR_1 > lowthreshold1 && SENSOR_1 < highthreshold1)) { OnFwd (OUT_A+OUT_C); //drive forwards } //light sensor is over the board if (SENSOR_1 > highthreshold1) { OnFwd (OUT_C); //turn to the left Off (OUT_A); } //light sensor is off the board if (SENSOR_1 < lowthreshold1) { OnFwd (OUT_A); //turn to the right Off (OUT_C); } } task main () { //initialize sensors SetSensor (SENSOR_1, SENSOR_LIGHT); SetSensor (SENSOR_2, SENSOR_ROTATION); SetSensor (SENSOR_3, SENSOR_LIGHT); //start the auxiliary lights for the light sensors start aux_light; Wait (25); /**********************Automatic Light Sensor Calibrater*********************/ //The human player holds the robot over the darkest and brightest areas of the //playing field before the match starts. This algorithm then uses some of the //RCX's variables to store the highest and lowest light readings from the //field. It then automatically calibrates the light sensors based on the //readings taken. This method allows the robot to operate in a wide spectrum //of light readings. /*store light readings to calibrate light sensors*/ //take the lowest light sensor reading for both sensors lowthreshold1 = SENSOR_1; //store sensor one's reading off the board lowthreshold3 = SENSOR_3; //store sensor three's reading off the board //pause to let human player move robot over board PlaySound (SOUND_DOUBLE_BEEP); until (SENSOR_1 > 90); until (SENSOR_1 < 90); PlaySound (SOUND_DOUBLE_BEEP); //take the highest light sensor reading for both sensors highthreshold1 = SENSOR_1; //store sensor one's reading on the board highthreshold3 = SENSOR_3; //store sensor three's reading on the board //convert the results to seperate variables value_1 = highthreshold1; value_3 = highthreshold3; /*self calibrate both light sensors*/ //subtract the low reading from the high reading value_1 -= lowthreshold1; value_3 -= lowthreshold3; //divide the difference of the high and low readings by three value_1 /= 3; value_3 /= 3; //subtract the quotient from the brightest light //reading to end up with the highest gray value highthreshold1 -= value_1; highthreshold3 -= value_3; //add the quotient to the darkest light //reading to end up with the lowest gray value lowthreshold1 += value_1; lowthreshold3 += value_3; //start sequence PlaySound (SOUND_DOUBLE_BEEP); //signal that robot has finished calibration Wait (500); until (SENSOR_1 > 80); //wait for touch sensor to be pressed PlaySound (SOUND_DOUBLE_BEEP); until (SENSOR_1 < 80); //wait for touch sensor to be released Wait (25); //drive forwards past line marking robot one's base OnFwd (OUT_A+OUT_C); until (SENSOR_1 > highthreshold1); Off (OUT_A+OUT_C); //reset sensors ClearSensor (SENSOR_2); //drive forwards to prepare for turn OnFwd (OUT_A+OUT_C); //drive forwards enough so that a 90 until (SENSOR_2 > 13); //degree turn points the robot down Off (OUT_A+OUT_C); //the center of the straightaway Wait (10); //reset sensors ClearSensor (SENSOR_2); //spin to prepare to follow board to gate SetPower (OUT_A+OUT_C,7); OnFwd (OUT_C); OnRev (OUT_A); until (SENSOR_2 < -13); Off (OUT_A+OUT_C); Wait (15); //reset rotation sensor ClearSensor (SENSOR_2); //forwards to prepare to follow board OnFwd (OUT_A+OUT_C); until (SENSOR_2 > 15); Off (OUT_A+OUT_C); //reset rotation sensor ClearSensor (SENSOR_2); //align robot to follow board SetPower (OUT_A+OUT_C,2); while (SENSOR_2 < 20) { //use the slower of the two board followboard_tight (); //following subroutines to align robot } Float (OUT_A+OUT_C); //reset timer one ClearTimer (1); //follow board to gate SetPower (OUT_A+OUT_C,7); while (SENSOR_1 < 95) { //use the faster of the two followboard_fast (); //board following subroutines //check to make sure robot is not jammed against gate if (Timer(1) > 50) //Once this timer reaches five seconds { //we know the sensor to detect the gate Off (OUT_A+OUT_C); //is not functioning for an unknown PlaySound (SOUND_DOUBLE_BEEP); //reason, as the robot should be at the goto at_gate; //gate by now. The code then moves on } //to the next checkpoint and assumes } //the robot has grabbed onto the gate. Off (OUT_A+OUT_C); //store the distance the robot traveled to the gate in a variable at_gate: gate_distance = SENSOR_2 - 10; //reset rotation sensor ClearSensor (SENSOR_2); //forwards to have claw grab onto gate SetPower (OUT_A+OUT_C,3); OnFwd (OUT_A+OUT_C); Wait (45); Off (OUT_A+OUT_C); Wait (40); //wait to ensure claw has locked into position //reset rotation sensor ClearSensor (SENSOR_2); //back up and prepare to spin SetPower (OUT_A,7); SetPower (OUT_C,6); OnRev (OUT_A+OUT_C); until (SENSOR_2 < -30); Off (OUT_A+OUT_C); //reset rotation sensor ClearSensor (SENSOR_2); //spin 90 degrees to drop gate over the side SetPower (OUT_A+OUT_C,7); OnFwd (OUT_C); OnRev (OUT_A); until (SENSOR_2 <= -19); //robot senses 90 degree turn Off (OUT_A+OUT_C); //drop gate over the edge of the board SetPower (OUT_A+OUT_C,2); OnFwd (OUT_A+OUT_C); until (SENSOR_3 < lowthreshold3); Off (OUT_A+OUT_C); //algorithm to ensure that the robot is dropping the board correctly if (SENSOR_1 > highthreshold1) { SetPower (OUT_C,3); OnFwd (OUT_A); OnRev (OUT_C); until (SENSOR_1 < lowthreshold1); Off (OUT_A+OUT_C); } //back up into the center of the board SetPower (OUT_A+OUT_C,7); OnRev (OUT_A+OUT_C); until (SENSOR_3 > lowthreshold3); Off (OUT_A+OUT_C); //reset rotation sensor ClearSensor (SENSOR_2); //back up slightly more to ensure turn is precise OnRev (OUT_A+OUT_C); until (SENSOR_2 < -5); Off (OUT_A+OUT_C); //reset rotation sensor ClearSensor (SENSOR_2); //spin another 180 degrees prepare to follow line back to base SetPower (OUT_A+OUT_C,5); OnFwd (OUT_C); OnRev (OUT_A); until (SENSOR_2 < -11); until (SENSOR_3 > lowthreshold3); until (SENSOR_1 < highthreshold1); //robot senses complete 180 degree spin Off (OUT_A+OUT_C); //robot stops //partially align robot with board OnFwd (OUT_A); until (SENSOR_1 > highthreshold1); Off (OUT_A); SetPower (OUT_A+OUT_C,7); //reset rotation sensor ClearSensor (SENSOR_2); //align robot to follow board SetPower (OUT_A+OUT_C,5); while (SENSOR_2 < 10) { //use the slower of the two board followboard_tight (); //following subroutine to align robot } Float (OUT_A+OUT_C); //follow line back to intersection SetPower (OUT_A+OUT_C,7); while (SENSOR_2 < gate_distance) { //use the faster of the two followboard_fast (); //board following subroutines } Off (OUT_A+OUT_C); //signal that robot is on final stretch PlaySound (SOUND_DOUBLE_BEEP); //reset rotation sensor ClearSensor (SENSOR_2); //forwards into intersection SetPower (OUT_A+OUT_C,2); OnFwd (OUT_A+OUT_C); until (SENSOR_3 < lowthreshold3); //stop at the edge of the board Off (OUT_A+OUT_C); //reset rotation sensor ClearSensor (SENSOR_2); //back up to prepare for spin SetPower (OUT_A+OUT_C,7); OnRev (OUT_A+OUT_C); until (SENSOR_2 < -8); Off (OUT_A+OUT_C); Wait (10); //reset rotation sensor ClearSensor (SENSOR_2); //turn to prepare to drive into base OnFwd (OUT_A); OnRev (OUT_C); until (SENSOR_2 > 12); until (SENSOR_1 < lowthreshold1); until (SENSOR_1 > highthreshold1); //robot senses 90 degree spin Off (OUT_A+OUT_C); //reset rotation sensor ClearSensor (SENSOR_2); //drive into base SetPower (OUT_A+OUT_C,6); while (SENSOR_2 < 15) { //use the slower of the two board followboard_tight (); //following subroutine to align robot } Off (OUT_A+OUT_C); //follow board all the way to the end of the base SetPower (OUT_A+OUT_C,2); while (SENSOR_3 > lowthreshold3) { //use the slower of the two followboard_tight (); //board following subroutines } Off (OUT_A+OUT_C); //reset rotation sensor ClearSensor (SENSOR_2); //back up to ensure that robot is completely within base OnRev (OUT_A+OUT_C); until (SENSOR_2 < -10); Off (OUT_A+OUT_C); //reset rotation sensor ClearSensor (SENSOR_2); //spin around 180 degrees SetPower (OUT_A,6); SetPower (OUT_C,7); OnFwd (OUT_C); OnRev (OUT_A); until (SENSOR_2 < -25); until (SENSOR_1 < highthreshold1); //robot senses 90 degree spin Off (OUT_A+OUT_C); //reset rotation sensor ClearSensor (SENSOR_2); //spin slightly more than 180 degrees to prepare to leave base OnFwd (OUT_C); OnRev (OUT_A); until (SENSOR_2 < -2); Off (OUT_A+OUT_C); stop aux_light; } //end of program //Task to activate auxiliary lights for light sensors. //The auxiliary lights help to increase the signal to noise ratio for the light //sensors, which greatly improves reliability. task aux_light () { OnFwd (OUT_B); }