public static byte[] ParseUpdateWheelVelocity(WheelVelocity wheelVelocity, byte setWheelVelocityCode, int wheelClicks)
{
int minValue = 7;
var velocity = wheelVelocity;
//1050 ex 588 clicks por tempo de amostragem
var leftVelocity = -velocity.LeftVelocity;
var rightVelocity = velocity.RightVelocity;
if (leftVelocity != 0 && Math.Abs(leftVelocity) < minValue)
{
leftVelocity = (Math.Sign(leftVelocity) * minValue);
}
if (rightVelocity != 0 && Math.Abs(rightVelocity) < minValue)
{
rightVelocity = (Math.Sign(rightVelocity) * minValue);
}
//Trace.WriteLine(leftVelocity + " " + rightVelocity);
var command = new byte[5];
command[0] = setWheelVelocityCode;
command[1] = (byte)((short)rightVelocity >> 8);
command[2] = (byte)(short)rightVelocity;
command[3] = (byte)((short)leftVelocity >> 8);
command[4] = (byte)(short)leftVelocity;
return(command);
}
public void Actuate()
{
var leftWeight = (Math.Abs(currentVelocity.LeftVelocity) - Math.Abs(previousVelocity.LeftVelocity) > 0) ? accelerationWeight : breakingWeight;
if (Math.Sign(currentVelocity.LeftVelocity) != Math.Sign(previousVelocity.LeftVelocity))
{
leftWeight = breakingWeight;
}
if (previousVelocity.LeftVelocity == 0)
{
leftWeight = accelerationWeight;
}
var rightWeight = (Math.Abs(currentVelocity.RightVelocity) - Math.Abs(previousVelocity.RightVelocity) > 0) ? accelerationWeight : breakingWeight;
if (Math.Sign(currentVelocity.RightVelocity) != Math.Sign(previousVelocity.RightVelocity))
{
rightWeight = breakingWeight;
}
if (previousVelocity.RightVelocity == 0)
{
rightWeight = accelerationWeight;
}
var left = previousVelocity.LeftVelocity + (leftWeight * (currentVelocity.LeftVelocity - previousVelocity.LeftVelocity));
var right = previousVelocity.RightVelocity + (rightWeight * (currentVelocity.RightVelocity - previousVelocity.RightVelocity));
previousVelocity = new WheelVelocity(left, right);
var command = ParseUpdateWheelVelocity(previousVelocity, SetWheelVelocityCode, WheelClicks);
manager.CommandAsync(command, 0, this);
}
public DifferentialSteeringBoard(ICommunicationManager communicationManager, double wheelRadius, int wheelClicks, float accelerationWeight, float breakingWeight)
{
previousVelocity = new WheelVelocity();
currentVelocity = new WheelVelocity();
manager = communicationManager;
CommandChecksum = from command in Observable.FromEventPattern <CommandCompletedEventArgs>(manager, "CommandCompleted")
where command.EventArgs.UserState == this
select ParseMotorActuatorResponse(command.EventArgs.Response);
WheelRadius = wheelRadius;
WheelClicks = wheelClicks;;
this.accelerationWeight = accelerationWeight;
this.breakingWeight = breakingWeight;
}
public DifferentialSteeringResponse(WheelVelocity velocity)
{
wheelVelocity = velocity;
}
public void UpdateWheelVelocity(WheelVelocity wheelVelocity)
{
currentVelocity = wheelVelocity;
}
