int PhoBot::control(String command) {
    // "F-100" - forward B, L, R
    String action = command.substring(0, 1);
    String speedStr = command.substring(2);
    float speed = 0;
    if (! action.equalsIgnoreCase("S")) {
        speed = speedStr.toInt() / 100.0;
    }
    if (action.equalsIgnoreCase("F")) {
        setMotor(M3, FORWARD, speed);
        setMotor(M4, FORWARD, speed);
    }
    else if (action.equalsIgnoreCase("B")) {
        setMotor(M3, BACK, speed);
        setMotor(M4, BACK, speed);
    }
    else if (action.equalsIgnoreCase("L")) {
        setMotor(M3, FORWARD, speed);
        setMotor(M4, BACK, speed);
    }
    else if (action.equalsIgnoreCase("R")) {
        setMotor(M3, BACK, speed);
        setMotor(M4, FORWARD, speed);
    }
    else if (action.equalsIgnoreCase("S")) {
        setMotor(M3, STOP);
        setMotor(M4, STOP);
    }
    return 1;
}

TSensorModes
/*
 * Spooler_10-21.c ~
 * Alex Jones for team 5105 Buffalo Wings
 *
 * Description: 	a simple unit test for the spooler that Raj has created
 *
 * Uses: 			to test the gear config on the test equipment.
 *
 * Dependancies:	NONE
 *
 * ~licensed under the MIT license included in this repositiory
*/
#include "JoystickDriver.c"
#include "../motors/SetMotor.fn"
#include "../controller/GetJoystick.fn"

task main()
{
	while(true)
	{
		getJoystickSettings(joystick);
		setMotor(spoolLeft, getJoystick(1, 1, 'y')/1.28);
		setMotor(spoolRight, getJoystick(1, 1, 'y')/1.28);
	}
}

void roboControl(void){ //For autonomous control
	setLauncherSpeed(30);
	launcherSpeed_new = 127;//Lets start this shoot speed
	goForwardFor_time(10);//Go forward 10 seconds
	goBackwardFor_time(1);
	clearTimer(T1);
	while(time1[T1] < 20000){setMotor(vertBelt,80);setMotor(hozBelt,100);updateLauncherSpeed(2);}



	/*goBackwardFor_time(1);
	turnRight(0.1);
	setMotor(hozBelt,-100);
	goForwardFor_time(.5);
	turnRight(0.1);
	goForwardFor_time(5);*/
	goForwardFor_time(7);//Go forward 7 seconds
	goBackwardFor_time(1); //Get to launching distance
	clearTimer(T1);
	while(time1[T1] < 15000){setMotor(vertBelt,80);setMotor(hozBelt,100);updateLauncherSpeed(2);}
	goBackwardFor_time(1);
	turnRight(1);
	setMotor(hozBelt,-100);
	goForwardFor_time(.5);
	turnRight(0.25);
	goForwardFor_time(5);
}

void turnLeft(int time)
{
  resetTimer();
  setMotor(MOTOR_A, -100);
  setMotor(MOTOR_B, -100);
  while (time1() < time);
}

/*Part 1*/
int ReadEncoding (){
	int encodingArr[4];
	int encodedValue = 0;
	int i =0;

	while (i<4){
		 nxtDisplayCenteredTextLine(2, "%d", SensorValue[lightSensor]);
	   if (SensorValue[lightSensor]<=BLACK_COLOUR){
	   		encodingArr[i] = 1;
	   }
	   else{
	     	encodingArr[i] = 0;
	   }
	   if(i!=3){
	   /* calibrate to 7 cm*/
	  	 setMotor(50);
	   	 wait1Msec(330);
	   }
	   setMotor(0);
	   wait1Msec(1000);
	   i++;
  }
	setMotor(0);
  i = 0;
  int increment = 8;
  while (i<4){
  	if (encodingArr[i])
  			encodedValue+= increment;

  	increment = increment/2;
  	i++;
  }
  nxtDisplayCenteredTextLine(2, "%d", encodedValue);
  return (encodedValue);
}

void turnRight(int time)
{
  resetTimer();
  setMotor(MOTOR_A, 100);
  setMotor(MOTOR_B, 100);
  while (time1() < time);
}

// Großer Wert = Zu
void curtain_setPosition(uint8_t pos)
{
    // Do not accept new positions if the module is currently reseting to the start position
    // or the sensor is not working or pos is an invalid value
    if (state == SensorNotWorkingState || state == SensorCalibratingState)
        return;

    // apply maximum value
    if (pos > CURTAIN_COMPLETE_SENSOR_CHANGES)
        pos = CURTAIN_COMPLETE_SENSOR_CHANGES;

    setMotor(0, DirectionDown);
    movetoposition = currentposition;

    // Down: high value
    if (pos > currentposition) {
        state = DownState;
        movetoposition = pos;
        setMotor(255, DirectionDown);
    } else if (pos < currentposition) { // Up: low value
        movetoposition = pos;
        if (currentposition-pos>2) {
            state = FastUpState;
            setMotor(255, DirectionUp);
        }
        else {
            state = UpState;
            setMotor(60, DirectionUp);
        }
    }
}

void IntakeSystem::intake(InputMethod* input) {
	//auto intake button
	if(input->intakeToggle()){
		setMotor(1);
		open();
		isToggled = true;
	}else if(isToggled && !input->intakeToggle()){
		setMotor(0);
		close();
		isToggled = false;
	}

	//manual control of motors
	if (!isToggled) {
		if (input->intake())
			setMotor(1);
		else if (input->outtake())
			setMotor(-1);
		else
			setMotor(0);
	}

	//catch ball
	if(!catchToggle && input->catchBall()){
		if(lift->Get() == Relay::kForward){
			lift->Set(Relay::kReverse);
		}else{
			lift->Set(Relay::kForward);
		}
	}
	catchToggle = input->catchBall();
}

void OrbitPlanet(int planetValue)
{
	if (planetValue == 13){
		// Find the planet
		PlanetSearch(1);
		wait1Msec(100); //// REMOVE THIS AFTER
		// Do the square orbit
		SquareOrbit(-1);

	}
	else if (planetValue == 9){

		PlanetSearch(-1);
				wait1Msec(100); //// REMOVE THIS AFTER
		SquareOrbit(1);
	}
	else{
		SearchSaturn();
		motor[motorB] = 10;
		motor[motorC] = -10;
		wait1Msec(2000);
			while(SensorValue[sonarSensor] > 60){
				// Keep rotating
		}
		setMotor(0);
		// Now pointing right at the planet, approach the planet
		while(SensorValue[sonarSensor] > 15){
			setMotor(50);
		}
		setMotor(0);
		SquareOrbit(1);
	}

}

void __attribute__((__interrupt__, auto_psv)) _IC2Interrupt(void)
{
    _IC2IF = 0; // Clear the interrupt flag

    //-------------------------------------------------------------------------
    //  If we are looking at the beginning (rising edge) of the pulse...
    //-------------------------------------------------------------------------
    if (IC2_PULSE_STATE == RISE)
    {
        IC2_PULSE_STATE    = FALL;     // Next interrupt occurs on falling edge
    }
    //-------------------------------------------------------------------------
    //  ... else we are looking at the end (falling edge) of the pulse.
    //-------------------------------------------------------------------------
    else
    {
        IC2_PULSE_STATE    = RISE;     // Next interrupt occurs on rising edge
        ++IC2_COUNT;       // increase the sample count
        if(!useCompass){
            if(IC2_COUNT>=IC_GO_COUNTS || IC1_COUNT>=IC_GO_COUNTS){
                currentMotorSetting=0;
                setMotor(currentMotorSetting);
            }//endif
        }//endif
        else{
            if(f_Heading){
                currentMotorSetting=0;
                setMotor(currentMotorSetting);
            }//endif
        }//endelse
    }
}//endic2

void roboControl(void){ //For autonomous control	//This is test code
	setLauncherSpeed(30);
	launcherSpeed_new = 127;//Lets start this
	goForwardFor_time(5.3,3);//Go forward 6.1 seconds
	clearTimer(T1);
	while(time1[T1] < 20000){setMotor(vertBelt,80);setMotor(hozBelt,100);
		updateLauncherSpeed(2);
	}
}

void PicoBorgRev::setMotors(float power)
{
	if (power < 0) {
		power = power * -1;
		setMotor(PBR_COMMAND_SET_ALL_REV, power);
	} else {
		setMotor(PBR_COMMAND_SET_ALL_FWD,power);
	}
}

/*lint -save  -e970 Disable MISRA rule (6.3) checking. */
int main(void)
/*lint -restore Enable MISRA rule (6.3) checking. */
	{
  /* Write your local variable definition here */
	uint16 countServo;
	int i;
  /*** Processor Expert internal initialization. DON'T REMOVE THIS CODE!!! ***/
	PE_low_level_init();
	ADC0_CFG2 |= ADC_CFG2_MUXSEL_MASK;
  /*** End of Processor Expert internal initialization.                    ***/
	
	
  /* Write your code here */
  /* For example: for(;;) { } */
	
	
	
/*
   * Motores são setados com valores de 0 a 255, 2o argumento resolve o sentido.
   */
	cameraSIBit_ClrVal();
	sMachine_Init();
	Serial_Init();
	enableMotor();
	setMotor(0,1,0);
	setMotor(1,1,0);
	servoPWM_Enable(servoPWM_DeviceData);
	countServo = 19000;
	servoPWM_SetDutyUS(servoPWM_DeviceData,countServo);
	cameraCLK_Enable();
	cameraCLK_EnableEvent();
	cameraCLKBit_ClrVal();
	while(1){
			for (i=0; i < 128; i++){
				vetorCamera[i] = getCamPixel(i);
			}
			for (i=0; i < 128; i++){
				itoa(vetorCamera[i], buffer0);
				Send_String(buffer0);
				Send_String(" ");
				}
			Send_String("\r\n");
  }
  
  /*** Don't write any code pass this line, or it will be deleted during code generation. ***/
  /*** RTOS startup code. Macro PEX_RTOS_START is defined by the RTOS component. DON'T MODIFY THIS CODE!!! ***/
  #ifdef PEX_RTOS_START
    PEX_RTOS_START();                  /* Startup of the selected RTOS. Macro is defined by the RTOS component. */
  #endif
  /*** End of RTOS startup code.  ***/
  /*** Processor Expert end of main routine. DON'T MODIFY THIS CODE!!! ***/
  for(;;){}
  /*** Processor Expert end of main routine. DON'T WRITE CODE BELOW!!! ***/
} /*** End of main routine. DO NOT MODIFY THIS TEXT!!! ***/

/*
void goForwardFor_time(int time){ //Goes forward for a set ammount of time
setMotor(leftMotor,autoSpeed);//Use -8 to go forward
setMotor(rightMotor,(autoSpeed-5));
clearTimer(T1);
while(time1[T1] < time* 1000){updateLauncherSpeed();setMotor(hozBelt,100);}
stopMotor(leftMotor);
stopMotor(rightMotor);
}
*/
void goForwardFor_time(int time, int launcherSpeedInc){ //Goes forward for a set ammount of time
	setMotor(leftMotor,autoSpeed);
	setMotor(leftMotor2,autoSpeed);
	setMotor(rightMotor,autoSpeed);
	setMotor(rightMotor2,autoSpeed);//Use -8 to go forward
	setRightMotors(autoSpeed-20);
	clearTimer(T1);
	while(time1[T1] < time* 1000){updateLauncherSpeed();setMotor(hozBelt,100);}
	stopLeftMotors();
	wait(.8);
	stopRightMotors();
}

static void cmdMotor(BaseSequentialStream *chp, int argc, char *argv[])
{
    (void)argv;

    uint8_t motor;
    motor_direction_t direction;
    motor_speed_t speed;

    if (argc > 0) {
        if (strcmp(argv[0], "on") == 0) {
            setMotor(MOTOR_LEFT, motor_forward, 100);
            setMotor(MOTOR_RIGHT, motor_forward, 100);
            return;
        } else if (strcmp(argv[0], "off") == 0) {
            setMotor(MOTOR_LEFT, motor_stop, 0);
            setMotor(MOTOR_RIGHT, motor_stop, 0);
            return;
        } else if ((argc > 1)) {
            motor = atoi(argv[0]);
            speed = 100;

            switch (argv[1][0]) {
                case 'f':
                    direction = motor_forward;
                    break;
                case 'r':
                    direction = motor_reverse;
                    break;
                default:
                    direction = motor_stop;
            }

            if (argc > 2) {
                speed = atoi(argv[2]);
            }

            if (motor < 2) {
                chprintf(chp, "Set motor %d to direction %d and speed %d\r\n", motor, direction, speed);
                setMotor(motor, direction, speed);
                return;
            }
        }
    }

    // Usage
    chprintf(chp, "Usage: motor N <f|b|s> [speed]\r\n");
    chprintf(chp, "       motor 0 forward 50\r\n");
    chprintf(chp, "       motor 1 b\r\n");
    chprintf(chp, "       motor off\r\n");
    chprintf(chp, "where N is motor number, 0-1\r\n");
    chprintf(chp, "and direction f, b or s (forward, backward, stop)\r\n");
    chprintf(chp, "and speed is percentage 0-100\r\n");
}

void driveForward(unsigned char speed_setting)
{
    unsigned char speed;
    switch (speed_setting) {
        case MAX_SPEED_SETTING:
            speed = MAX_SPEED;
            break;
        default:
            speed = 0;
    }
    setMotor(leftMotorPort, speed + OFFSET);
    setMotor(rightMotorPort, -speed + OFFSET);
}

void turnRight(int time){
	setMotor(leftMotor,autoSpeed);
	setMotor(leftMotor2,autoSpeed);
	clearTimer(T1);
	wait(time);
	//while(time1[T1] < time * 1000){updateLauncherSpeed();}
	//By calling to update the wheel speed, we lose accuracy.
	//This is why I have changed it to a wait. Though we do not manage the speed control, 
	//we only lose it for a few seconds.
	wait(time);
	stopLeftMotors();
	stopRightMotors();
}

void goForwardFor_time(int time){ //Goes forward for a set ammount of time
	int driftCorrection = 40;
	//setMotor(leftMotor,127);
	setMotor(leftMotor2,127);
	//setMotor(rightMotor,100);
	setMotor(rightMotor2,127);
	//setMotor(rightMotor,autoSpeed - driftCorrection);
	//setMotor(rightMotor2,autoSpeed - driftCorrection);//Use -8 to go forward
	clearTimer(T1);
	while(time1[T1] < time* 1000){updateLauncherSpeed();setMotor(hozBelt,100);}
	stopRightMotors();
	stopLeftMotors();
}

void MoveForward (){

/* Black is less than 25*/
nxtDisplayCenteredTextLine(2, "%d", SensorValue[lightSensor]);

	while (SensorValue[lightSensor] > BLACK_COLOUR){
		nxtDisplayCenteredTextLine(2, "%d", SensorValue[lightSensor]);
		setMotor(50);
		wait1Msec(50);
	}
	//found BLACK
	setMotor(0);
	wait1Msec(1000);
}

void motor(void)
{
    initialize();
    wait1Msec(200);

    //Drive Forward
    setMotor(MOTOR_A, (50*0.95));
    setMotor(MOTOR_B, 50);
    wait1Msec(30000);

    //Test Right Motor
    setMotor(MOTOR_A, 50);
    setMotor(MOTOR_B, 0);
    wait1Msec(3000);

    //Test Left Motor
    setMotor(MOTOR_A, 0);
    setMotor(MOTOR_B, 50);
    wait1Msec(3000);

    //Turn Motors Off
    setMotor(MOTOR_A, 0);
    setMotor(MOTOR_B, 0);

}