/// <summary>
/// executes command to drive at certain speed. correctedSpeed can be null.
/// </summary>
/// <param name="ddi"></param>
private void Drive(DifferentialDriveInputs ddi)
{
if (ddi == null)
{
ddi = new DifferentialDriveInputs(); // practically a stop command
}
// the board accepts integers in the range -100...+100. Dead zone around -20...20 (see MotorsDeadZone)
// we operate in ints here, as we consider a change in power by at least 1 to be significant. Otherwise we ignore the change.
int correctedTrimmedSpeedLeft = VelocityToMotorSpeed(ddi.velocityLeftWheel);
int correctedTrimmedSpeedRight = VelocityToMotorSpeed(ddi.velocityRightWheel);
//Debug.WriteLine("Drive: " + ddi.ToString() + " L=" + correctedTrimmedSpeedLeft + " R=" + correctedTrimmedSpeedRight);
bool leftSpeedChanged = correctedSpeedLastLeft != correctedTrimmedSpeedLeft;
bool rightSpeedChanged = correctedSpeedLastRight != correctedTrimmedSpeedRight;
bool speedChanged = leftSpeedChanged || rightSpeedChanged;
// only command to the motors if speed changes:
if (speedChanged)
{
differentialMotorController.LeftMotorSpeed = correctedTrimmedSpeedLeft; // == 0 ? correctedTrimmedSpeedLeft : (correctedTrimmedSpeedLeft - 1);
differentialMotorController.RightMotorSpeed = correctedTrimmedSpeedRight; // == 0 ? correctedTrimmedSpeedRight : (correctedTrimmedSpeedRight - 1);
differentialMotorController.DriveMotors(); // this call commands the Hardware Brick (i.e. Element board) via the serial line and takes bandwidth. Avoid unneccessary calls.
correctedSpeedLastLeft = correctedTrimmedSpeedLeft;
correctedSpeedLastRight = correctedTrimmedSpeedRight;
}
}
/// <summary>
/// apply driveInputs to drive - command the motors.
/// </summary>
public override void Drive()
{
DifferentialDriveInputs di = this.driveInputs as DifferentialDriveInputs;
Debug.Assert(di != null);
di.Compute(this); // turn Unicycle into Differential Drive
this.Drive(di);
}
/// <summary>
/// executes command to drive at certain speed. correctedSpeed can be null.
/// </summary>
/// <param name="ddi"></param>
private void Drive(DifferentialDriveInputs ddi)
{
if (ddi == null)
{
ddi = new DifferentialDriveInputs(); // practically a stop command
}
// the board accepts integers in the range -100...+100. Dead zone around -20...20 (see MotorsDeadZone)
// we operate in ints here, as we consider a change in power by at least 1 to be significant. Otherwise we ignore the change.
int correctedTrimmedSpeedLeft = VelocityToMotorSpeed(ddi.velocityLeftWheel);
int correctedTrimmedSpeedRight = VelocityToMotorSpeed(ddi.velocityRightWheel);
//Debug.WriteLine("Drive: " + ddi.ToString() + " L=" + correctedTrimmedSpeedLeft + " R=" + correctedTrimmedSpeedRight);
bool leftSpeedChanged = correctedSpeedLastLeft != correctedTrimmedSpeedLeft;
bool rightSpeedChanged = correctedSpeedLastRight != correctedTrimmedSpeedRight;
bool speedChanged = leftSpeedChanged || rightSpeedChanged;
// only command to the motors if speed changes:
if (speedChanged)
{
differentialMotorController.LeftMotorSpeed = correctedTrimmedSpeedLeft; // == 0 ? correctedTrimmedSpeedLeft : (correctedTrimmedSpeedLeft - 1);
differentialMotorController.RightMotorSpeed = correctedTrimmedSpeedRight; // == 0 ? correctedTrimmedSpeedRight : (correctedTrimmedSpeedRight - 1);
differentialMotorController.DriveMotors(); // this call commands the Hardware Brick (i.e. Element board) via the serial line and takes bandwidth. Avoid unneccessary calls.
correctedSpeedLastLeft = correctedTrimmedSpeedLeft;
correctedSpeedLastRight = correctedTrimmedSpeedRight;
}
}
