/** Go Straight using the IMU */
public ServoHoldHeadingWithImu(PigeonIMU pigeonImu, IDrivetrain driveTrain, Styles.BasicStyle selectedStyle, ServoParameters parameters, float targetHeading, float maxOutput)
{
_pidgey = pigeonImu;
_driveTrain = driveTrain;
_selectedStyle = selectedStyle;
_targetHeading = targetHeading;
_servoParams = parameters;
_maxOutput = maxOutput;
}
//------ 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);
}
/**
* Uses forward, strafe, and turn (Mecanum drive)
*
* @param forward Y direction of robot
* @param strafe X direction of robot
*/
public void Set(Styles.BasicStyle mode, float forward, float strafe)
{
Enable(true);
GoStraight(forward, _targetHeading);
}
/** Go Straight using the IMU */
public ServoHoldHeadingWithImu(PigeonIMU pigeonImu, IDrivetrain driveTrain, Styles.BasicStyle selectedStyle)
{
_pidgey = pigeonImu;
_driveTrain = driveTrain;
_selectedStyle = selectedStyle;
}
/**
* Uses forward, strafe, and turn (Mecanum drive)
*
* @param forward Y direction of robot
* @param strafe X direction of robot
*/
public void Set(Styles.BasicStyle driveStyle, float forward, float turn)
{
Set(driveStyle, forward, turn, 0);
}
