Master

/*NOTE TO READER OR PROGRAMMER
motorA is the X direction (the longer rack)
motorB is the Y direction (the shorter rack)
motorC ink motor*/

const int INKPOWER = 2;
const int BASEPOWER = 20;
const int ARMTHRESHOLD = 17;
const int ARMTHRESHOLD_CLOSE = 5;

void configureSensors()
{
	SensorType[S1] = sensorEV3_Ultrasonic;
	wait1Msec(50);
	SensorType[S2] = sensorEV3_Touch;
	wait1Msec(50);
	SensorType[S3] = sensorEV3_Touch;
	wait1Msec(50);
	SensorType[S4] = sensorEV3_Color;
	wait1Msec(50);
	SensorMode[S4] = modeEV3Color_Color;
	wait1Msec(50);
}

int identification()
{
	while (!getButtonPress (buttonEnter))
	{}
	while (getButtonPress (buttonEnter))
	{}

	if (SensorValue[S4] == (int) colorRed)
		return 1;
	else if (SensorValue[S4] == (int) colorBlue)
		return 2;
	else if (SensorValue[S4] == (int) colorYellow)
		return 3;
	else
		return 0;
}

float degreesToMM(int degrees)
{
	return ((float)degrees/15);
}

float rad(float deg)
{
    return deg*PI/180.0;
}

void draw(bool east, int angle, bool north, float dist, bool & worked)
{
	displayBigTextLine(4, "%f", SensorValue[S1]);
	if(SensorValue[S1] < ARMTHRESHOLD && SensorValue[S1] > ARMTHRESHOLD_CLOSE && worked)
	{
			nMotorEncoder[motorA]=0;
			nMotorEncoder[motorB]=0;
			nMotorEncoder[motorC]=0;
	    int dirX, dirY;
	    float Xdist, Ydist, powerX, powerY;

	    if(east)
	        dirX = 1;
	    else
	        dirX = -1;
	    if(north)
	        dirY = -1;
	    else
	        dirY = 1;

	    Xdist = dirX * dist * cos(rad(angle));
	    Ydist = dirY * dist * sin(rad(angle));

	    powerX = dirX * BASEPOWER*cos(rad(angle));
	    powerY = dirY * BASEPOWER*sin(rad(angle));

	    motor[motorA] = powerX;
	    motor[motorB] = powerY;

	    while((pow((degreesToMM(nMotorEncoder[motorA])),2) + pow((degreesToMM(nMotorEncoder[motorB])),2)) < dist*dist)
	    {}

			motor[motorA] = motor[motorB] = 0;
			worked = true;
	}
	else
			worked = false;
}

void inkOn (bool inkMotor)
{
	if (inkMotor == true && SensorValue[S1] < ARMTHRESHOLD && SensorValue[S1] > ARMTHRESHOLD_CLOSE)
	{
		motor[motorC]= INKPOWER;
		wait1Msec(500);
	}
	else
	{
		motor[motorC]= -5*INKPOWER;
		wait1Msec(500);
		motor[motorC]= 0;
	}

	wait1Msec(50);
}

void inkEnd()
{
	motor[motorC] = -25;
	wait1Msec(500);
	motor[motorC] = 0;
}


//This code writes UW
void drawU()
{
	motor[motorA]=motor[motorB]=motor[motorC]=0;
	bool worked = true;
	draw(true, 90, false, 40, worked);
	draw(true, 0, true, 30, worked);
	draw(true, 90, true, 7.5, worked);
	inkOn(true);
	wait1Msec(6500);

	draw(false, 90, false, 7.5, worked);
	draw(false, 45, false, 3.5, worked);
	draw(false, 0, true, 5, worked);
	draw(false, 45, true, 3.5, worked);
	draw(false, 90, true, 7.5, worked);
	draw(false, 0, true, 2.5, worked);
}

void drawW()
{
	bool worked = true;
	draw(false, 76, false, 10, worked);
	draw(false, 63, true, 5.5, worked);
	inkEnd();
	draw(false, 63, false, 5.5, worked);
	draw(false, 76, true, 10, worked);
}
	//Space between letters is 0.5cm however the O finishes at the bottom left corner and must travel 10 mm + 5mm

// Draws a cube
void drawCube()
{
	bool worked = true;
	draw(true, 90, false, 40, worked);
	draw(true, 0, true, 30, worked);
	inkOn (true);
	wait1Msec(7000);

	draw(false, 0 , true, 10, worked);
	draw(false, 90 , true , 10, worked);
	draw(true, 0 , false, 10, worked);
	draw(true, 90 , false, 10, worked);

	draw(true, 45, true, 5, worked);
	draw(true, 90, true, 10, worked);
	draw(false, 45, false, 5, worked);
	inkEnd();
	draw(true, 45, true, 5, worked);

	draw(false, 0, false, 10, worked);
	draw(false, 45, false, 5, worked);
	
	motor[motorA] = motor[motorB] = motor[motorC] = 0;
}

void drawArrow()
{
	bool worked = true;
	draw(true, 90, false, 20, worked);

	inkOn(true);
	wait1Msec(8000);
	draw(true, 0, true, 40, worked);
	draw(false, 25, false, 6, worked);
	inkEnd();
	draw(true, 25, true, 6, worked);
	draw(false, 25, true, 6, worked);

	motor[motorA] = motor[motorB] = motor[motorC] = 0;
	
}

void drawSquare()
{
	bool worked = true;
	draw(true, 90, false, 40, worked);
	draw(true, 0, true, 30, worked);
	inkOn (true);
	wait1Msec(6500);

	draw(false, 0 , true, 10, worked);
	draw(false, 90 , true , 10, worked);
	inkEnd();
	draw(true, 0 , false, 10, worked);
	draw(true, 90 , false, 10, worked);
}

void zeroBot()
{
	// reset bot to 0 in x
	motor[motorA] = -10;
	while (SensorValue[S2] == 0)
	{}
	motor[motorA] = 0;

	// reset bot to 0 in y
	motor[motorB] = -10;
	while (SensorValue[S3] == 0)
	{}
	motor[motorB] = 0;
}

void shutdownProcedure()
{
	inkOn(false);
	zeroBot();
	displayString(6,"Shutdown");
	wait1Msec(5000);
}

task main ()
{
	configureSensors();
	zeroBot();
	int userInt = 0;
	string userStr = "";

	displayString (1, "Place your colour ID card");
	displayString (2, "and press ENTER button.");
	userInt = identification();

	if (userInt == 1)
		userStr = "Professor";
	else if (userInt == 2)
		userStr = "TA";
	else if(userInt == 3)
		userStr = "student";
	else
		userStr = "";

	displayString (3, "Welcome %s", userStr);
	wait1Msec(3000);

	eraseDisplay();

	displayString(1, "PRESS LEFT FOR CUBE");
	displayString(2, "PRESS RIGHT FOR UW");
	displayString(3, "PRESS UP FOR ARROW");
	displayString(4, "PRESS DOWN FOR SQUARE");


	bool waitForPress = true;

	time1[T1] = 0;

	while (waitForPress && time1[T1] < 25000)
	{
		if (getButtonPress(buttonLeft))
		{
			eraseDisplay();
			while (getButtonPress(buttonLeft)) {}
			{
				drawCube();
				waitForPress = false;
			}

		}
		else if (getButtonPress(buttonRight))
		{
			eraseDisplay();
			while (getButtonPress(buttonRight)) {}
			{
				drawU();
				drawW();
				waitForPress = false;
			}
		}

		else if (getButtonPress(buttonDown))
		{
			eraseDisplay();
			while (getButtonPress(buttonDown))
			{
				drawSquare();
				waitForPress = false;
			}
		}

		else if (getButtonPress(buttonUp))
		{
			eraseDisplay();
			while (getButtonPress(buttonUp))
			{
				drawArrow();
				waitForPress = false;
			}
		}
	}
	shutdownProcedure();
}