public YAxis(IDriveSettingRepository driveSettingRepository, IEmergencyStop emergencyStop, IGpioControllerFacade gpioControllerFacade)
: base(driveSettingRepository, emergencyStop)
{
var drivePins = new DrivePins
{
EnablePin = 22,
DirectionPin = 5,
StepPin = 6,
ReferenceSwitchPin = 21
};
_stepperDriveControl = new StepperDriveControl(drivePins, gpioControllerFacade);
_defaultDriveSetting = new DriveSetting
{
DriveName = Name,
MaxSpeed = new Speed(80, SpeedUnit.MillimeterPerSecond),
Acceleration = new Acceleration(80, AccelerationUnit.MillimeterPerSecondSquared),
SpindlePitch = new Length(8, LengthUnit.Millimeter),
StepsPerRevolution = 400,
SoftwareLimitMinus = new Length(0.1, LengthUnit.Millimeter),
SoftwareLimitPlus = new Length(141.1, LengthUnit.Millimeter),
ReverseDirection = false,
ReferenceDrivingDirection = DrivingDirection.Negative,
ReferencePosition = new Length(0, LengthUnit.Millimeter),
ReferenceDrivingSpeed = new Speed(3.5, SpeedUnit.MillimeterPerSecond)
};
}
public async Task <ActionResult <DriveSetting> > Put(string driveName, [FromBody] DriveSetting driveSetting)
{
if (!_axis.TryGetValue(driveName, out IAxis selectedAxis))
{
return(new NotFoundResult());
}
await selectedAxis.UpdateSettingsAsync(driveSetting);
return(new OkObjectResult(driveSetting));
}
private async Task GetDriveSettings()
{
DriveSetting = await _driveSettingRepository.GetDriveSetting(Name);
if (DriveSetting == null)
{
await _driveSettingRepository.Create(DefaultDriveSetting);
DriveSetting = DefaultDriveSetting;
}
_lengthOfOneStep = 1.ToDistance(DriveSetting);
}
public async Task <ActionResult <DriveSetting> > GetTest()
{
var test = new DriveSetting
{
DriveName = "X",
Acceleration = new Acceleration(20, AccelerationUnit.MillimeterPerSecondSquared),
MaxSpeed = new Speed(200, SpeedUnit.MillimeterPerSecond),
SpindlePitch = new Length(8, LengthUnit.Millimeter),
StepsPerRevolution = 400,
ReverseDirection = false
};
await _driveSettingsRepo.Create(test);
return(new ObjectResult(test));
}
public static int ToSteps(this Length distance, DriveSetting driveSetting)
{
var steps = distance.Millimeters * driveSetting.StepsPerRevolution / driveSetting.SpindlePitch.Millimeters;
return(Convert.ToInt32(steps));
}
public static Length ToDistance(this int steps, DriveSetting driveSetting)
{
var distanceInMillimeters = steps * driveSetting.SpindlePitch.Millimeters / driveSetting.StepsPerRevolution;
return(new Length(distanceInMillimeters, LengthUnit.Millimeter));
}
public static double ToAccelerationPerSecondSquare(this Acceleration acceleration, DriveSetting driveSetting)
{
return(acceleration.MillimetersPerSecondSquared * driveSetting.StepsPerRevolution / driveSetting.SpindlePitch.Millimeters);
}
public static double ToStepsPerSecond(this Speed speed, DriveSetting driveSetting)
{
return(speed.MillimetersPerSecond * driveSetting.StepsPerRevolution / driveSetting.SpindlePitch.Millimeters);
}
public async Task UpdateSettingsAsync(DriveSetting setting)
{
await _driveSettingRepository.Update(setting);
await GetDriveSettings();
}
public async Task <ActionResult <DriveSetting> > Post([FromBody] DriveSetting driveSetting)
{
await _driveSettingsRepo.Create(driveSetting);
return(new OkObjectResult(driveSetting));
}
private static Position GetNextFastestReachablePosition(Position currentPosition, List <Position> positionsToTravelTo, DriveSetting xAxis, DriveSetting yAxis)
{
bool firstIteration = true;
Duration fastestWay = default(Duration);
Position nextPosition = null;
foreach (var position in positionsToTravelTo)
{
var xDistance = currentPosition.X - position.X;
var yDistance = currentPosition.Y - position.Y;
var timeX = MotionCalculatorHelper.GetTimeForDistance(xDistance.Millimeters, xAxis.Acceleration.MillimetersPerSecondSquared, xAxis.MaxSpeed.MillimetersPerSecond);
var timeY = MotionCalculatorHelper.GetTimeForDistance(yDistance.Millimeters, yAxis.Acceleration.MillimetersPerSecondSquared, yAxis.MaxSpeed.MillimetersPerSecond);
double timeForTravelinSeconds = timeX > timeY ? timeX : timeY;
var timeForTravel = new Duration(timeForTravelinSeconds, UnitsNet.Units.DurationUnit.Second);
if (timeForTravel < fastestWay ||
firstIteration)
{
firstIteration = false;
fastestWay = timeForTravel;
nextPosition = position;
}
}
return(nextPosition);
}
public static List <Position> GetFastestWayToGetDrunk(Position currentPosition, List <Position> positionsToTravelTo, DriveSetting xAxis, DriveSetting yAxis)
{
List <Position> getDrunkFastWayToTravel = new();
while (positionsToTravelTo.Any())
{
var nextPosition = GetNextFastestReachablePosition(currentPosition, positionsToTravelTo, xAxis, yAxis);
currentPosition = nextPosition;
positionsToTravelTo.Remove(currentPosition);
getDrunkFastWayToTravel.Add(currentPosition);
}
return(getDrunkFastWayToTravel);
}
