in a team of 3 peoples we created Spraybot, a lego mindstorms sprayer.

Pictures

Movie

Documentations

http://www.nouser.org/WD/projects/bachelor/spraybot/flow_diagramm1.pdf

===== Code =====

NQC code of the bot

<code>
#define MOTORR OUT_A
#define MOTORC OUT_B
#define MOTORL OUT_C

// motor control
#define MOTOR_FORWARD  0
#define MOTOR_LEFT     1
#define MOTOR_RIGHT    2
#define MOTOR_REVERSE  3
#define MOTOR_STOP     4
int movementDirection = 0;
int movementTimer = 0;

// spray control
#define SPRAY          0
#define NOSPRAY        1
#define SPRAY_MOTOR_FORWARD  2
#define SPRAY_MOTOR_REVERSE  3
#define SPRAY_MOTOR_STOP     4
#define SPRAY_POS_BACK   5
#define SPRAY_POS_CENTER 6
#define SPRAY_POS_FRONT  7
int sprayPosState = SPRAY_POS_CENTER;
int sprayMotorState = SPRAY_MOTOR_STOP;
int setSprayState = NOSPRAY;
int sprayTimer = 0;        // time to spray
int sprayChangeTimer = 0;  // when changing direction we wait this time before polling touch sensor

// mind control
#define HIDE        0 // stop everything
#define WAIT        1 // waiting before starting to spray
#define CHECK       1 // equals the WAIT 
#define SEARCH      2 // search for a free spot
#define EVADE_WALL  3 // get away from the edge
#define EVADE_LEFT  4 //
#define EVADE_RIGHT 5
#define DRAW_CIRCLE 6 // spray a circle on the ground
#define DRAW_LINE   7 
#define SHAKE       8
#define STATE_ERROR_CHECK 101
int mindAction = HIDE;
int previousMindAction = -1;
int mindTimer = 0;
int currentActionTimer = 0;

// other variables
int medianWhite_S1;
int medianWhite_S3;
int medianSteps;
int lastWallTouch = 0;
int NIGHT;
int WHITE;
int BLACK;
int DAY_VAL_S1;
int DAY_VAL_S3;
int BLACK_VAL_S1;
int BLACK_VAL_S3;
int NIGHT_VAL_S1;
int NIGHT_VAL_S3;
int SPRAY_RELEASED = 0;
int loopCounter = 0;

task movement()
{
//	SetUserDisplay( movementDirection, 0 );
	// movement
	while( 1 ) {
		//SetPower( MOTORR, 6 );
		//SetPower( MOTORL, 7 );
		switch(movementDirection)
		{
			case MOTOR_FORWARD: {
				OnFwd( MOTORR );
				OnFwd( MOTORL );
				break;
			}
			case MOTOR_LEFT: {
				OnFwd( MOTORR );
				OnRev( MOTORL );
				break;
			}
			case MOTOR_RIGHT: {
				OnRev( MOTORR );
				OnFwd( MOTORL );
				break;
			}
			case MOTOR_REVERSE: {
				OnRev( MOTORR );
				OnRev( MOTORL );			
				break;
			}
			case MOTOR_STOP: {
				Off( MOTORR );
				Off( MOTORL );
				break;
			}
		}

		movementTimer -= 1;
		if (movementTimer < 0) {
			movementTimer = 0;
		}

		if (sprayMotorState == SPRAY_MOTOR_FORWARD) {
			if (sprayPosState == SPRAY_POS_BACK) {
				// we set the state to center once the sensor is inactive
				if (SENSOR_2 > 512) {
					sprayPosState = SPRAY_POS_CENTER;
				}
			}
			// if motor moving forward and position is in center
			if (sprayPosState == SPRAY_POS_CENTER) {
				// we set the state to forward once the sensor is active
				if (SENSOR_2 < 512) {
					sprayPosState = SPRAY_POS_FRONT;
				}
			}
		}
		if (sprayMotorState == SPRAY_MOTOR_REVERSE) {
			if (sprayPosState == SPRAY_POS_CENTER) {
				// we set the state to forward once the sensor is active
				if (SENSOR_2 < 512) {
					sprayPosState = SPRAY_POS_BACK;
				}
			}
			if (sprayPosState == SPRAY_POS_FRONT) {
				// we set the state to center once the sensor is inactive
				if (SENSOR_2 > 512) {
					sprayPosState = SPRAY_POS_CENTER;
				}
			}
		}

		if (setSprayState == SPRAY) {
			PlaySound( SOUND_CLICK );
			if (sprayPosState == SPRAY_POS_FRONT) {
				Off( MOTORC );
				sprayMotorState = SPRAY_MOTOR_STOP;
			}
			if (sprayPosState == SPRAY_POS_CENTER) {
				OnFwd( MOTORC );
				sprayMotorState = SPRAY_MOTOR_FORWARD;
			}
			if (sprayPosState == SPRAY_POS_BACK) {
				OnFwd( MOTORC );
				sprayMotorState = SPRAY_MOTOR_FORWARD;
			}
		}
		if (setSprayState == NOSPRAY) {
			if (sprayPosState == SPRAY_POS_FRONT) {
				OnRev( MOTORC );
				sprayMotorState = SPRAY_MOTOR_REVERSE;
			}
			if (sprayPosState == SPRAY_POS_CENTER) {
				OnRev( MOTORC );
				sprayMotorState = SPRAY_MOTOR_REVERSE;
			}
			if (sprayPosState == SPRAY_POS_BACK) {
				Off( MOTORC );
				sprayMotorState = SPRAY_MOTOR_STOP;
			}
		}
	}
}

task main()
{
	// init sensors
	SetSensor( SENSOR_1, SENSOR_LIGHT );
	SetSensor( SENSOR_3, SENSOR_LIGHT );
	SetSensor( SENSOR_2, SENSOR_TOUCH );
	SetSensorMode( SENSOR_2, SENSOR_MODE_RAW );

	// init motors
	SetPower( MOTORC, 7 );
	SetPower( MOTORR, 7 );
	SetPower( MOTORL, 4 );

	// set display output
	SetUserDisplay( mindAction, 0 );

	//int movementDirection = 0;
	movementDirection = MOTOR_FORWARD;

	start movement;

	// triggervalue to use as daylight
	int DAY_VAL_S1 = SENSOR_1 - 1;
	int DAY_VAL_S3 = SENSOR_3 - 1;
	// triggervalue to use as nightlight
	int NIGHT_VAL_S1 = DAY_VAL_S1 - 3;
	int NIGHT_VAL_S3 = DAY_VAL_S3 - 3;
	// triggervalue to use as black ground
	int BLACK_VAL_S1 = NIGHT_VAL_S1 - 5;
	int BLACK_VAL_S3 = NIGHT_VAL_S3 - 5;

	/*// SENSOR 1 DAY    WEISS 58, GRAY 50, BLACK 40
	// SENSOR 1 SHADOW WEISS 42, GRAY 39, BLACK 29

	// READ BLACK & WHITE VALUE
	if (SENSOR_1 > SENSOR_3) {
		WHITE = SENSOR_1-1;
		BLACK = SENSOR_3-1;
	} else if (SENSOR_3 > SENSOR_1) {
		WHITE = SENSOR_3-1;
		BLACK = SENSOR_1-1;
	}
	NIGHT = WHITE - 5; // BLACK + ((WHITE - BLACK) / 2);



	WHITE = (SENSOR_1 + SENSOR_3) / 2;
	//NIGHT = 42;
	NIGHT = WHITE - 4;
	//BLACK = 34;
	BLACK = NIGHT - 4;
	// triggervalue to use as daylight
	DAY_VAL_S1 = WHITE;
	DAY_VAL_S3 = WHITE;
	// triggervalue to use as nightlight
	NIGHT_VAL_S1 = NIGHT;
	NIGHT_VAL_S3 = NIGHT;
	// triggervalue to use as black ground
	BLACK_VAL_S1 = BLACK;
	BLACK_VAL_S3 = BLACK;*/


	// main loop	
	while( 1 ) {
		switch (mindAction) 
		{
			case HIDE: {
				// wait for a specified time
				movementDirection = MOTOR_STOP;
				if (currentActionTimer > 240) {
					mindAction = CHECK;
				}
				break;
			}
			case CHECK: {
				// if starting to wait
				if (currentActionTimer == 0) {
					loopCounter += 1;
					//SetSensorMode( SENSOR_1, SENSOR_MODE_RAW );
					//SetSensor( SENSOR_1, SENSOR_MODE_RAW );
					//SetSensorMode( SENSOR_3, SENSOR_MODE_RAW );
					//SetSensor( SENSOR_3, SENSOR_MODE_RAW );
					movementDirection = MOTOR_LEFT;
				}
				else if (currentActionTimer == 10) {
					medianWhite_S1 = SENSOR_1;
					medianWhite_S3 = SENSOR_3;
					medianSteps = 1;
				}
				// check light : rotate around on spot for 40 steps
				else if (currentActionTimer < 55) {
					medianWhite_S1 += SENSOR_1;
					medianWhite_S3 += SENSOR_3;
					medianSteps += 1;
				} 
				// calculate median colorvalues and compare to day/night/black
				else if (currentActionTimer == 55) {
					movementDirection = MOTOR_STOP;
					//SetSensor( SENSOR_1, SENSOR_LIGHT );
					//SetSensor( SENSOR_3, SENSOR_LIGHT );

					medianWhite_S1 /= medianSteps;
					medianWhite_S3 /= medianSteps;

					/*// if both sensors think it's day
					if ((medianWhite_S1 > DAY_VAL_S1) && (medianWhite_S3 > DAY_VAL_S3)) {
						mindAction = HIDE;
					} 
					// if both sensors think it's day
					else if ((medianWhite_S1 > NIGHT_VAL_S1) && (medianWhite_S3 > NIGHT_VAL_S3)) {
						mindAction = SEARCH;
					} 
					// both sensors think they see black
					else if ((medianWhite_S1 > BLACK_VAL_S1) && (medianWhite_S3 > BLACK_VAL_S1)) {
						mindAction = HIDE;
					} 
					// else
					else {
						mindAction = HIDE;
					}*/

					// override sensor settings
					if ((loopCounter > 0) && (loopCounter <= 5)) {
						mindAction = HIDE;
					}
					if (loopCounter > 5) {
						mindAction = SEARCH;
					}
				}
				break;
			}
			// move around searching for a free spot
			case SEARCH: {
				// both sensors are activated
				if( (SENSOR_1 < BLACK_VAL_S1) && (SENSOR_3 < BLACK_VAL_S3) ) {
					mindAction = EVADE_WALL;
				} 
				// left sensor is activated
				else if( SENSOR_3 < BLACK_VAL_S3 ) {
					mindAction = EVADE_LEFT;
				} 
				// right sensor is activated
				else if( SENSOR_1 < BLACK_VAL_S1 ) {
					mindAction = EVADE_RIGHT;
				}
				// no sensor is activated
				else if (currentActionTimer > (120)) {
					mindAction = SHAKE;
					/*if (currentActionTimer > (Random(120)+120)) {
						mindAction = DRAW;
					}*/
				}
				// usually move forward
				else {
					movementDirection = MOTOR_FORWARD;
				}
				// from time to time we start a check session
				/*if (Random(50) == 1) {
					mindAction = CHECK;
				}*/
				lastWallTouch += 1;
				break;
			}
			// move away from a wall (is called when both sensors are activated
			case EVADE_WALL: {
				// reverse for 15 steps
				if (currentActionTimer == 0) {
					movementDirection = MOTOR_REVERSE;
				} 
				// choose a random rotation and execute for 10 steps
				else if (currentActionTimer == 15) {
					if (Random(1)) {
						movementDirection = MOTOR_LEFT;
					} else {
						movementDirection = MOTOR_RIGHT;
					}
				} 
				// restore previous action
				else if (currentActionTimer == 25) {
					mindAction = SEARCH;
				}
				// after 20 steps start checking for collisions again
				if (currentActionTimer > 15) {
					// if any sensor activated call complex evade actions
					if ( (SENSOR_1 < BLACK_VAL_S1) || (SENSOR_3 < BLACK_VAL_S3) ) {
						mindAction = SEARCH;
					}
				}
				break;
			}
			case EVADE_LEFT: {
				if (currentActionTimer == 0) {
					movementDirection = MOTOR_LEFT;
				}
				// after 10 steps start SEARCH action
				else if (currentActionTimer == 10) {
					mindAction = SEARCH;
					movementDirection = MOTOR_STOP;
				}

				// after 5 steps start checking for collisions
				if (currentActionTimer > 5) {
					// if any sensor activated call complex evade actions
					if ( (SENSOR_1 < BLACK_VAL_S1) || (SENSOR_3 < BLACK_VAL_S3) ) {
						mindAction = SEARCH;
					}
				} 
				break;
			}
			case EVADE_RIGHT: {
				if (currentActionTimer == 0) {
					movementDirection = MOTOR_RIGHT;
				}
				// after 10 steps start SEARCH action
				else if (currentActionTimer == 10) {
					mindAction = SEARCH;
					movementDirection = MOTOR_STOP;
				}

				// after 5 steps start checking for collisions
				if (currentActionTimer > 5) {
					// if any sensor activated call complex evade actions
					if ( (SENSOR_1 < BLACK_VAL_S1) || (SENSOR_3 < BLACK_VAL_S3) ) {
						mindAction = SEARCH;
					}
				} 
				break;
			}
			case SHAKE: {
				if ((currentActionTimer % 4) == 0) {
					movementDirection = MOTOR_RIGHT;
				} else if ((currentActionTimer % 2) == 0) {
					movementDirection = MOTOR_LEFT;
				}
				if (currentActionTimer > 40) {
					if (Random(1)) {
						mindAction = DRAW_CIRCLE;
					} else {
						mindAction = DRAW_LINE;
					}

				}
				// if we hit an obstacle, we stop shaking and search a new place
				if ( (SENSOR_1 < BLACK_VAL_S1) || (SENSOR_3 < BLACK_VAL_S3) ) {
					mindAction = SEARCH;
				}
				break;
			}
			// draw a tag on the floor
			case DRAW_CIRCLE: {
				// randomly change direction to rotate
				if (currentActionTimer == 0) {
					if (Random(1)) {
						movementDirection = MOTOR_LEFT;
					} else {
						movementDirection = MOTOR_RIGHT;
					}
				}
				// start spraying after 5 steps
				else if (currentActionTimer == 5) {
					setSprayState = SPRAY;
				} 
				// stop spraying after 50 steps
				else if (currentActionTimer == 55) {
					setSprayState = NOSPRAY;
				} 
				// after 15 steps we start searching for a new place
				else if (currentActionTimer == 70) {
					setSprayState = NOSPRAY;
					mindAction = CHECK;
				}
				// if we hit an obstacle, we stop spraying and search a new place
				if ( (SENSOR_1 < BLACK_VAL_S1) || (SENSOR_3 < BLACK_VAL_S3) ) {
					setSprayState = NOSPRAY;
					mindAction = SEARCH;
				}
				break;
			}
			case DRAW_LINE: {
				if (currentActionTimer == 0) {
					movementDirection = MOTOR_FORWARD;
					setSprayState = SPRAY;
				}
				if ((currentActionTimer % 5) == 0) {
					if (Random(1)) {
						movementDirection = MOTOR_LEFT;
					} else {
						movementDirection = MOTOR_RIGHT;
					}
				} else if ((currentActionTimer % 5) == 1) {
					movementDirection = MOTOR_FORWARD;
				}
				if (currentActionTimer == 40) {
					mindAction = SEARCH;
					setSprayState = NOSPRAY;
				}
				// if we hit an obstacle, we stop spraying and search a new place
				if ( (SENSOR_1 < BLACK_VAL_S1) || (SENSOR_3 < BLACK_VAL_S3) ) {
					mindAction = SEARCH;
					setSprayState = NOSPRAY;
				}
			}
			/*default: {
				// should only happen when a error occurs
				PlaySound( SOUND_UP );
				mindAction = HIDE;
			}*/
		}

		// do general stuff
		mindTimer += 1;
		currentActionTimer += 1;
		if (previousMindAction != mindAction) {
			currentActionTimer = 0;
		}
		previousMindAction = mindAction;
	}
}