public void TurboDriveTest()
{
var driveHelper = new DriveHelper(this.communicationHelper, this.controlSettings);
driveHelper.TurboModeOn = true;
int leftSpeed;
int rightSpeed;
driveHelper.CalculateMotorsSpeed(0, 1, out leftSpeed, out rightSpeed);
driveHelper.GenerateMotorCommands(leftSpeed, rightSpeed, out this.leftMotorCommand, out this.rightMotorCommand);
Assert.AreEqual("L00FF", this.leftMotorCommand);
Assert.AreEqual("R00FF", this.rightMotorCommand);
driveHelper.CalculateMotorsSpeed(0, 0.5, out leftSpeed, out rightSpeed);
driveHelper.GenerateMotorCommands(leftSpeed, rightSpeed, out this.leftMotorCommand, out this.rightMotorCommand);
Assert.AreEqual("L00DD", this.leftMotorCommand);
Assert.AreEqual("R00DD", this.rightMotorCommand);
driveHelper.CalculateMotorsSpeed(0, -1, out leftSpeed, out rightSpeed);
driveHelper.GenerateMotorCommands(leftSpeed, rightSpeed, out this.leftMotorCommand, out this.rightMotorCommand);
Assert.AreEqual("LFF01", this.leftMotorCommand);
Assert.AreEqual("RFF01", this.rightMotorCommand);
driveHelper.CalculateMotorsSpeed(0, -0.5, out leftSpeed, out rightSpeed);
driveHelper.GenerateMotorCommands(leftSpeed, rightSpeed, out this.leftMotorCommand, out this.rightMotorCommand);
Assert.AreEqual("LFF23", this.leftMotorCommand);
Assert.AreEqual("RFF23", this.rightMotorCommand);
}
public void RotationTest1()
{
var driveHelper = new DriveHelper(this.communicationHelper, this.controlSettings);
driveHelper.TurboModeOn = true;
int leftSpeed;
int rightSpeed;
driveHelper.CalculateMotorsSpeed(1, 0, out leftSpeed, out rightSpeed);
driveHelper.GenerateMotorCommands(leftSpeed, rightSpeed, out this.leftMotorCommand, out this.rightMotorCommand);
Assert.AreEqual("L00FF", this.leftMotorCommand);
Assert.AreEqual("R0000", this.rightMotorCommand);
driveHelper.CalculateMotorsSpeed(0.5, 0, out leftSpeed, out rightSpeed);
driveHelper.GenerateMotorCommands(leftSpeed, rightSpeed, out this.leftMotorCommand, out this.rightMotorCommand);
Assert.AreEqual("L00DD", this.leftMotorCommand);
Assert.AreEqual("R0000", this.rightMotorCommand);
driveHelper.CalculateMotorsSpeed(-1, 0, out leftSpeed, out rightSpeed);
driveHelper.GenerateMotorCommands(leftSpeed, rightSpeed, out this.leftMotorCommand, out this.rightMotorCommand);
Assert.AreEqual("L0000", this.leftMotorCommand);
Assert.AreEqual("R00FF", this.rightMotorCommand);
driveHelper.CalculateMotorsSpeed(-0.5, 0, out leftSpeed, out rightSpeed);
driveHelper.GenerateMotorCommands(leftSpeed, rightSpeed, out this.leftMotorCommand, out this.rightMotorCommand);
Assert.AreEqual("L0000", this.leftMotorCommand);
Assert.AreEqual("R00DD", this.rightMotorCommand);
}
public void HalfRightBackward()
{
var driveHelper = new DriveHelper(this.communicationHelper, this.controlSettings);
driveHelper.TurboModeOn = true;
int leftSpeed;
int rightSpeed;
driveHelper.CalculateMotorsSpeed(0.7071, -0.7071, out leftSpeed, out rightSpeed);
driveHelper.GenerateMotorCommands(leftSpeed, rightSpeed, out this.leftMotorCommand, out this.rightMotorCommand);
Assert.AreEqual("LFF01", this.leftMotorCommand);
Assert.AreEqual("RFFB6", this.rightMotorCommand);
driveHelper.CalculateMotorsSpeed(0.3536, -0.3536, out leftSpeed, out rightSpeed);
driveHelper.GenerateMotorCommands(leftSpeed, rightSpeed, out this.leftMotorCommand, out this.rightMotorCommand);
Assert.AreEqual("LFF23", this.leftMotorCommand);
Assert.AreEqual("RFFC9", this.rightMotorCommand);
}
public void HalfRightForward()
{
var driveHelper = new DriveHelper(this.communicationHelper, this.controlSettings);
driveHelper.TurboModeOn = true;
int leftSpeed;
int rightSpeed;
driveHelper.CalculateMotorsSpeed(0.7071, 0.7071, out leftSpeed, out rightSpeed);
driveHelper.GenerateMotorCommands(leftSpeed, rightSpeed, out this.leftMotorCommand, out this.rightMotorCommand);
Assert.AreEqual("L00FF", this.leftMotorCommand);
Assert.AreEqual("R004A", this.rightMotorCommand);
driveHelper.CalculateMotorsSpeed(0.3536, 0.3536, out leftSpeed, out rightSpeed);
driveHelper.GenerateMotorCommands(leftSpeed, rightSpeed, out this.leftMotorCommand, out this.rightMotorCommand);
Assert.AreEqual("L00DD", this.leftMotorCommand);
Assert.AreEqual("R0037", this.rightMotorCommand);
}
public void BackwardTest()
{
var driveHelper = new DriveHelper(this.communicationHelper, this.controlSettings);
driveHelper.TurboModeOn = false;
int leftSpeed;
int rightSpeed;
driveHelper.CalculateMotorsSpeed(0, -1, out leftSpeed, out rightSpeed);
driveHelper.GenerateMotorCommands(leftSpeed, rightSpeed, out this.leftMotorCommand, out this.rightMotorCommand);
Assert.AreEqual("L" + MessageHelper.IntToMessageValue(-this.controlSettings.DriveModeNormalMaxSpeed), this.leftMotorCommand);
Assert.AreEqual("R" + MessageHelper.IntToMessageValue(-this.controlSettings.DriveModeNormalMaxSpeed), this.rightMotorCommand);
driveHelper.CalculateMotorsSpeed(0, -0.5, out leftSpeed, out rightSpeed);
driveHelper.GenerateMotorCommands(leftSpeed, rightSpeed, out this.leftMotorCommand, out this.rightMotorCommand);
int speed = Convert.ToInt32(255.0 * 0.5);
speed = DriveHelper.NonlinearSpeedCorrection(speed);
speed = driveHelper.TurboModeOn ? speed : speed * this.controlSettings.DriveModeNormalMaxSpeed / 255;
Assert.AreEqual("L" + MessageHelper.IntToMessageValue(-speed), this.leftMotorCommand);
Assert.AreEqual("R" + MessageHelper.IntToMessageValue(-speed), this.rightMotorCommand);
}
