void testEncoder(int daisychain, int motornumber)
{
	int encoderposition = I2C_GetEncoderPosition(S1, daisychain, motornumber);

	// if it's not 0 it must have moved sometime earlier, indicating that the encoder is working
	if (encoderposition != 0) return;

	I2C_SetMotorSpeed(S1, daisychain, motornumber, 10);
	Sleep(150);
	I2C_SetMotorSpeed(S1, daisychain, motornumber, 0);
	if (encoderposition == I2C_GetEncoderPosition(S1, daisychain, motornumber))
	{
		textToAdd = "FATAL: encoder test failed. aborting.";
		addLog();
		StopAllTasks();
	}
	textToAdd = "success: encoder test passed. continuing.";
	addLog();
}
예제 #2
0
void Arm_Update()
{
	Arm_Encoder = I2C_GetEncoderPosition(S1, 1, 1);
	//if the arm isn't initialized yet
	if(!Arm_Initialized)
	{
		//set the speed to 10
		I2C_SetMotorSpeed(S1, 1, 1, 25);

		//if the lower limit is triggered, set the upper and lower encoder bounds
		if(SensorValue[S4] == 1)
		{
			Arm_Initialized = true;
			Arm_BottomEncoderLimit = Arm_Encoder + (-.2 * 1440); //the amount of distance from the buttom press down. This lets it go below the button press.
			Arm_TopEncoderLimit = Arm_Encoder - (10 * 1440);
		}
	}

	//if the arm is initialized
	else
	{
		//if the arm is with in the encoder range
		if(Arm_Encoder > Arm_TopEncoderLimit && Arm_Encoder < Arm_BottomEncoderLimit)
		{
			I2C_SetMotorSpeed(S1, 1, 1, Arm_Speed);
		}

		//if the arm is below the bottom encoder and trying to go up
		else if(Arm_Encoder > Arm_BottomEncoderLimit && Arm_Speed < 0)
		{
			I2C_SetMotorSpeed(S1, 1, 1, Arm_Speed);
		}

		//if the arm is above the top encoder and trying to go down
		else if(Arm_Encoder < Arm_TopEncoderLimit && Arm_Speed > 0)
		{
			I2C_SetMotorSpeed(S1, 1, 1, Arm_Speed);
		}

		//if the arm is out of the encoder range and trying to go the wrong direction
		else
		{
			I2C_SetMotorSpeed(S1, 1, 1, 0);
		}
	}
}