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);
}
//------ Set output routines. ----------//
public void Set(Styles.BasicStyle driveStyle, float forward, float turn)
{
/* calc the left and right demand */
float l, r;
Util.Split_1(forward, turn, out l, out r);
/* lookup control mode to match caller's selected style */
ControlMode cm = Styles.Routines.LookupCM(driveStyle);
/* apply it */
_left.Set(cm, l);
_right.Set(cm, r);
}
//---------------- Set output routines. --------------//
/**
* 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.BasicStyle driveStyle, float forward, float turn, float strafe)
{
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
/* lookup control mode to match caller's selected style */
ControlMode cm = Styles.Routines.LookupCM(driveStyle);
/* apply it */
_1.Set(cm, leftFrnt_throt);
_2.Set(cm, leftRear_throt);
_3.Set(cm, rghtFrnt_throt);
_4.Set(cm, rghtRear_throt);
}
