private void Drive(Styles.Basic mode, float forward, float strafe, float turn)
{
/* save values for posterity */
_forward = forward;
_strafe = strafe;
_turn = turn;
float leftFrnt_throt = (forward + strafe + turn); // left front moves positive for forward, strafe-right, turn-right
float leftRear_throt = (forward - strafe + turn); // left rear moves positive for forward, strafe-left, turn-right
float rghtFrnt_throt = (forward - strafe - turn); // right front moves positive for forward, strafe-left, turn-left
float rghtRear_throt = (forward + strafe - turn); // right rear moves positive for forward, strafe-right, turn-left
/* Set control mode */
if (mode == Styles.Basic.PercentOutput)
{
_1.SetControlMode(BasicControlMode.kPercentVbus);
_2.SetControlMode(BasicControlMode.kPercentVbus);
_3.SetControlMode(BasicControlMode.kPercentVbus);
_4.SetControlMode(BasicControlMode.kPercentVbus);
}
else if (mode == Styles.Basic.Voltage)
{
_1.SetControlMode(BasicControlMode.kVoltage);
_2.SetControlMode(BasicControlMode.kVoltage);
_3.SetControlMode(BasicControlMode.kVoltage);
_4.SetControlMode(BasicControlMode.kVoltage);
}
/* Set motors */
_1.Set(leftFrnt_throt);
_2.Set(leftRear_throt);
_3.Set(rghtFrnt_throt);
_4.Set(rghtRear_throt);
}
/** Go Straight using the IMU */
public ServoHoldHeadingWithImu(PigeonImu pigeonImu, IDrivetrain driveTrain, Styles.Basic selectedStyle, ServoParameters parameters, float targetHeading, float maxOutput)
{
_pidgey = pigeonImu;
_driveTrain = driveTrain;
_selectedStyle = selectedStyle;
_targetHeading = targetHeading;
_servoParams = parameters;
_maxOutput = maxOutput;
}
/**
* Uses forward, strafe, and turn (Mecanum drive)
*
* @param forward Y direction of robot
* @param strafe X direction of robot
* @param turn twist of the robot (arch)
*/
public void Set(Styles.Basic mode, float forward, float strafe, float turn)
{
Drive(mode, forward, strafe, turn);
}
/**
* Uses forward, strafe, and turn (Mecanum drive)
*
* @param forward Y direction of robot
* @param strafe X direction of robot
*/
public void Set(Styles.Basic mode, float forward, float strafe)
{
Enable(true);
GoStraight(forward, _targetHeading);
}
/** Go Straight using the IMU */
public ServoHoldHeadingWithImu(PigeonImu pigeonImu, IDrivetrain driveTrain, Styles.Basic selectedStyle)
{
_pidgey = pigeonImu;
_driveTrain = driveTrain;
_selectedStyle = selectedStyle;
}
