#pragma config(Sensor, S1,     touch,          sensorTouch)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/* MultiManual.c
 * by Rob Seib, 2014, July
 * Written for the Lego Multigraph,
 * It allows the operator to specify what speed each of the 3 motors should run manually. */

int motorSelect = 1; //user selection of 1,2,or 3 (motor A, B, or C)
int motorAspeed = 0; //the desired speed of the motors (initially 0)
int motorBspeed = 0;
int motorCspeed = 0;
int buttonPressed = -1; //which button the user has pressed

void setMotors() //starts the motors at the desired speeds
{
	motor[motorA] = motorAspeed;
	motor[motorB] = motorBspeed;
	motor[motorC] = motorCspeed;
}

void increaseSpeed() //increases the value of motorXspeed where X is the selected motor
{
	switch (motorSelect)
	{
		case 1: motorAspeed +=1;
		break;
		case 2: motorBspeed +=1;
		break;
		case 3: motorCspeed +=1;
		break;
		default:
	}
}

void decreaseSpeed() //decreases the value of motorXspeed where X is the selected motor
{
	switch (motorSelect)
	{
		case 1: motorAspeed -=1;
		break;
		case 2: motorBspeed -=1;
		break;
		case 3: motorCspeed -=1;
		break;
		default:
	}
}

void displayValues()
{
	eraseDisplay();
	string strMotorAspeed = motorAspeed;
	string strMotorBspeed = motorBspeed;
	string strMotorCspeed = motorCspeed;
	string strMotorSelect = motorSelect;
	nxtDisplayStringAt(1,60, "Selected: ");
	nxtDisplayStringAt(80,60,strMotorSelect);
	nxtDisplayStringAt(1,50, "1 (platform): ");
	nxtDisplayStringAt(80,50,strMotorAspeed);
	nxtDisplayStringAt(1,40, "2 (left): ");
	nxtDisplayStringAt(80,40,strMotorBspeed);
	nxtDisplayStringAt(1,30, "3 (right): ");
	nxtDisplayStringAt(80,30,strMotorCspeed);
}

task main()
{
	displayValues();

	do //master loop repeats the whole control structure allowing the program to continue indefinitely
	{

		do // wait for button press
	  {
	    buttonPressed = nNxtButtonPressed;
	    if (SensorValue[touch] == 1)
	    	buttonPressed = 4;
	  }
	  while (buttonPressed == -1);

		switch(buttonPressed)//check what button was pressed and act accordingly
		{
			case 1: // if the right arrow button was pressed, increase selected motor speed
		  	increaseSpeed();
		 	break;
		 	case 2: // if the left arrow button was pressed, decrease selected motor speed
		  	decreaseSpeed();
		 	break;
		 	case 3: // if the centre button was pressed, change motor selection
		 		motorSelect ++;
		 		if (motorSelect > 3)
		 			motorSelect = 1;
		 	break;
		 	case 4: // if the touch sensor was pressed, run the motors
		 		setMotors();
		 	break;
		 	default:
		}
		displayValues();
		wait1Msec(650);

	}//end of master loop
	while (1);

}