Movement converter - convert high level movement commands into absolute motor positions.

simple_move_cvtr.pm

package simple_move_cvtr;

use stream_cache;


use strict;
use warnings;

use List::Util qw(min max);

use Data::Dumper;

sub new{

    my $settings = shift;

    my @copy_settings = qw( movementSource  currentMotorPositions  axisTopSpeeds timePerFrame ); #getNextMovement

    my $self={};

    for my $name(@copy_settings){
        $self->{$name} = $settings->{$name};
    }
    $self->{lastMovementDelta}=[0,0,0];
    $self->{lastMoveIdCompleted}=-1;
    $self->{fractionOfNextMoveAlreadyCompleted}=0;
    $self->{lastMovementEndingPosition}=$self->{currentMotorPositions};

    my $cache = stream_cache::new(sub{return $self->{movementSource}->{getNextMovement}->()});
    $self->{cache}=$cache;
    $self->{getMovementById} = sub{stream_cache::get_chunk($self->{cache},shift())};

    return $self;
}


sub get_next_frame{
    my $self = shift;

    my $next_frame = getBestNextFrame (
        $self->{lastMoveIdCompleted},
        $self->{lastMovementEndingPosition},
        $self->{currentMotorPositions},
        $self->{getMovementById},
        $self->{fractionOfNextMoveAlreadyCompleted},
        $self->{axisTopSpeeds},
        $self->{timePerFrame}
    );
    #warn Dumper $next_frame;

    $self->{lastMoveIdCompleted} = $next_frame->{lastMoveIdCompleted};
    $self->{currentMotorPositions} = $next_frame->{currentMotorPositions};
    $self->{fractionOfNextMoveAlreadyCompleted} = $next_frame->{fractionOfNextMoveAlreadyCompleted};
    $self->{lastMovementEndingPosition} = $next_frame->{lastMovementEndingPosition};

    return $self->{currentMotorPositions};
}

sub round {
  $_[0] > 0 ? int($_[0] + .5) : -int(-$_[0] + .5)
}


sub sum {
    my $t=0;
    for my $x(@_){
        $t += $x;
    }
    return $t;
}
sub diff{
    my $a=shift;
    my $b=shift;
    return map {$a->[$_] - $b->[$_]} (0..2);
}
sub add{
    my $a=shift;
    my $b=shift;
    return map {$a->[$_] + $b->[$_]} (0..2);
}
sub smul{
    my $a=shift;
    my $b=shift;
    return map {$a->[$_] * $b} (0..2);
}
sub dot{
    my $a=shift;
    my $b=shift;
    my @c = map {$a->[$_]*$b->[$_]} (0 ..$#{$a});
    return sum @c;
}
sub mag{
    my @parts = @_;
    @parts = map {$_*$_} @parts;
    my $sum = sqrt(sum @parts);
    return $sum;
}
sub normalize{
    my @v=shift;
    return smul(\@v,1/mag(@v));
}
sub interpolate{
    my $a=shift;
    my $b=shift;
    my $t=shift;
    return add($a,[smul([diff($b,$a)],$t)]);
}
sub between{#Is A between B and C?
    my $a=shift;
    my $b=shift;
    my $c=shift;
    return 0 if $a < $b;
    return 0 if $a > $c;
    return 1;
}
sub isPointInRange{
    my $point = shift;
    my $range = shift;
    my @notInRange = map {!between($point->[$_],@{$range->[$_]})} (0..2);
    return 0 if grep {$_} @notInRange;
    return 1;
}
sub isPointWithinToleranceOfRange{# May occasionally give false positives. Includes corner points, where they should be rounded.
    my $point = shift;
    my $range = shift;
    my $axisTolerances = shift;
    my $expandedRange = [map {[$range->[$_][0] - $axisTolerances->[$_],$range->[$_][0] + $axisTolerances->[$_]]} (0..2)];
    return isPointInRange($point, $expandedRange);
}

sub okayToAskForMoreFrames{
	my $self=shift;
	return (($self->{movementSource}->{canProvideMoreMovements}->()) or (!doINeedAnotherMovementToContinue($self)));
}

sub doINeedAnotherMovementToContinue{
	my $self = shift;
	
	if($self->{fractionOfNextMoveAlreadyCompleted} == 0){
		return 1;
	}
	#current move not completed
	return 0;
	
}

sub getBestNextFrame {
    my $lastMoveIdCompleted= shift;
    my $lastMovementEndingPosition = shift;
    my $currentMotorPositions = shift;
    my $getMovementById = shift;
    #my $lastMovementDelta = shift;
    #my $getReachableRanges = shift;
    my $fractionOfNextMoveAlreadyCompleted = shift;
    #my $nFramesToLookAhead = shift;
    #my $axisTolerances = shift;
    my $axisTopSpeeds = shift;#true speeds, steps per second
    my $timePerFrame = shift;

    my $nextMoveId = $lastMoveIdCompleted+1;



    #my $lastPoint = $getMovementById->($lastMoveIdCompleted);#Where you started, minus the fraction already completed.
    my $nextPoint = $getMovementById->($lastMoveIdCompleted+1);#Where you want to go, ideally.
    #warn Dumper $lastPoint;
    #warn Dumper $nextPoint;
    #warn $fractionOfNextMoveAlreadyCompleted;

	my $minimumTimeToComplete = (1 - $fractionOfNextMoveAlreadyCompleted)*$nextPoint->{minimumTimeToComplete};
	
	
	
    my $startingTheoreticalPosition=[interpolate($lastMovementEndingPosition,$nextPoint->{position},$fractionOfNextMoveAlreadyCompleted)];
    print "theoretical too far from actual previous position\n" if mag(diff($startingTheoreticalPosition,$currentMotorPositions))>2;
    my $maxTOnLineForFeedrate;
    if($minimumTimeToComplete>0.000000001){
		$maxTOnLineForFeedrate=$timePerFrame/$minimumTimeToComplete;
    }else{
    	$maxTOnLineForFeedrate=1;
    }
    

    my @axisDeltaAbs = map {abs} diff($startingTheoreticalPosition,$nextPoint->{position});

    my @maxTsForAxisSpeeds = map { ($axisDeltaAbs[$_]==0)?100000000000: $axisTopSpeeds->[$_]*$timePerFrame/$axisDeltaAbs[$_]} (0..2);

    my $maxT = min(1,$maxTOnLineForFeedrate,@maxTsForAxisSpeeds);

    my $motorPositions = [map {round $_} (positionOnLine($startingTheoreticalPosition,$nextPoint->{position},$maxT))];
    $fractionOfNextMoveAlreadyCompleted = $fractionOfNextMoveAlreadyCompleted + $maxT*(1-$fractionOfNextMoveAlreadyCompleted);

    if ($maxT > 0.999999){
        $lastMoveIdCompleted++;
        $fractionOfNextMoveAlreadyCompleted =0;
        $lastMovementEndingPosition = $nextPoint->{position};
    }

    return {
        lastMoveIdCompleted=> $lastMoveIdCompleted,
        fractionOfNextMoveAlreadyCompleted=>$fractionOfNextMoveAlreadyCompleted,
        currentMotorPositions=> $motorPositions,
        lastMovementEndingPosition => $lastMovementEndingPosition
    };

}

sub terminateCurrentMove{
	my $self=shift;
	$self->{fractionOfNextMoveAlreadyCompleted} =1;
}
sub setCurrentPosition{
	my $self=shift;
	my $pos=shift;
	$self->{lastMovementEndingPosition}=$pos;
	$self->{currentMotorPositions}=$pos;
}

sub isPointWithinTolerance {
    my $point = shift;
    my $axisTolerances = shift;

    my @parts = map {$point->[$_]/$axisTolerances->[$_]} (0..2);
    my $sum = mag @parts;
    $sum *= $sum;
    return 1 if $sum <= 1;
    return 0;
}

sub randomPointInsideSphere{
    while(1){
        my @v = map {rand()*2-1} (0..2);
        return @v if mag(@v) <= 1;
    }
}

sub positionOnLine{
    my $x1=shift;#p1
    my $x2=shift;#p2
    my $t=shift;
    return map {$x1->[$_]+($x2->[$_] - $x1->[$_])*$t} (0..2);
}

sub closestTToLine{
    my $x0=shift;
    my $x1=shift;#p1
    my $x2=shift;#p2
    my $top = dot([diff($x1,$x0)],[diff($x2,$x1)]);
    my $bot = mag(diff($x2,$x1));
    $bot *= $bot;
    my $mint = -$top/$bot;
    return 0 if $mint <=0;
    return 1 if $mint >=1;
    return $mint;

}

sub closestPointToLine{
    my $x0=shift;
    my $x1=shift;#p1
    my $x2=shift;#p2
    return positionOnLine($x1,$x2,closestTToLine($x0,$x1,$x2));
}

sub divTolerance{
    my $point = shift;
    my $axisTolerances = shift;
    return map {$point->[$_]/$axisTolerances->[$_]} (0..2)
}
sub mulTolerance{
    my $point = shift;
    my $axisTolerances = shift;
    return map {$point->[$_]*$axisTolerances->[$_]} (0..2)
}

1;