1: #define WATCHDOG_TIME_LIMIT_CS 1000
  2: #define PEN_PERIOD 30
  3: #define BLACK_THRESH 40
  4: #define DRIVE_SPEED 1
  5: #define DRIVE_MM_PER_CS 2
  6: #define TURN_SPEED 1
  7: #define TURN_CDEG_PER_CS 15
  8: 
  9: task dropPen()
 10: {
 11: 	SetDirection(OUT_A, OUT_REV);
 12: 	SetPower(OUT_A, 7);	
 13: 	OnFor(OUT_A,PEN_PERIOD);
 14: }
 15: 
 16: task raisePen()
 17: {
 18: 	SetDirection(OUT_A, OUT_FWD);
 19: 	SetPower(OUT_A, 7);	
 20: 	OnFor(OUT_A,PEN_PERIOD);
 21: }
 22: 
 23: // Positive drive will move so as to drag pen
 24: void drive(int dist_mm) 
 25: {
 26: 	if (dist_mm > 0) {
 27: 		SetDirection(OUT_B + OUT_C, OUT_FWD);
 28: 	} else {
 29: 		SetDirection(OUT_B + OUT_C, OUT_REV);
 30: 		dist_mm *= -1;
 31: 	}
 32: 	SetPower(OUT_B + OUT_C, DRIVE_SPEED);
 33: 	OnFor(OUT_B + OUT_C, dist_mm / DRIVE_MM_PER_CS);
 34: }
 35: 
 36: // Positive turn will rotate clockwise
 37: void turn(int angle_cdeg) 
 38: {
 39: 	if (angle_cdeg > 0) {
 40: 		SetDirection(OUT_B, OUT_FWD);
 41: 		SetDirection(OUT_C, OUT_REV);
 42: 	} else {
 43: 		SetDirection(OUT_B, OUT_REV);
 44: 		SetDirection(OUT_C, OUT_FWD);
 45: 		angle_cdeg *= -1;
 46: 	}
 47: 	SetPower(OUT_B + OUT_C, TURN_SPEED);
 48: 	OnFor(OUT_B + OUT_C, angle_cdeg / TURN_CDEG_PER_CS);
 49: }
 50: 
 51: // Drive in a straight line until the sensor threshold is triggered
 52: void driveUntil()
 53: {
 54: 	SetSensorType(SENSOR_1, SENSOR_TYPE_LIGHT);
 55: 	SetDirection(OUT_B + OUT_C, OUT_FWD);
 56: 	SetPower(OUT_B + OUT_C, DRIVE_SPEED);
 57: 	SetOutput(OUT_B + OUT_C, OUT_ON);
 58: 	while (SENSOR_1 < 53);
 59: 	while (SENSOR_1 > 52);
 60: 	SetOutput(OUT_B + OUT_C, OUT_FLOAT);
 61: }
 62: 
 63: // Turn in place until the sensor threshold is triggered
 64: void turnUntil()
 65: {
 66: 	SetSensorType(SENSOR_1, SENSOR_TYPE_LIGHT);
 67: 	SetDirection(OUT_B, OUT_REV);
 68: 	SetDirection(OUT_C, OUT_FWD);
 69: 	SetPower(OUT_B + OUT_C, TURN_SPEED);
 70: 	SetOutput(OUT_B + OUT_C, OUT_ON);
 71: 	while (SENSOR_1 < 53);
 72: 	while (SENSOR_1 > 52);
 73: 	SetOutput(OUT_B + OUT_C, OUT_FLOAT);
 74: }
 75: 
 76: // Rattle around inside a polygon (tested with a box)
 77: void stayInBox()
 78: {
 79: 	while (1) {
 80: 		driveUntil();
 81: 		Wait(10); // Allow inertia to get you over the line
 82: 		turnUntil();
 83: 	}
 84: }
 85: 
 86: // Stops the robot after a period of time has elapsed
 87: task watchdog()
 88: {
 89: 	Wait(WATCHDOG_TIME_LIMIT_CS);
 90: 	SetOutput(OUT_A + OUT_B + OUT_C, OUT_FLOAT);
 91: 	StopAllTasks();
 92: }
 93: 
 94: // Should stop the fool thing when it runs off the board
 95: task carpetminder()
 96: {
 97: 	while (SENSOR_1 > 48);
 98: 	SetOutput(OUT_A + OUT_B + OUT_C, OUT_FLOAT);
 99: 	StopAllTasks();
100: }
101: 
102: 
103: task main()
104: {
105: 	start watchdog;
106: 	//start dropPen;
107: 	//Wait(100);
108: 	//start raisePen;	
109: 	//drive(-200);
110: 	//turn(1800);
111: 	stayInBox();
112: 	SetOutput(OUT_A + OUT_B + OUT_C, OUT_FLOAT);
113: }