Commits

Trammell Hudson committed 077b487 Draft

use the home process

Comments (0)

Files changed (2)

 package Servo::Axis;
 use warnings;
 use strict;
+use Time::HiRes 'usleep';
 
 sub new
 {
 }
 
 
+sub home
+{
+	my $self = shift;
+	my $eps = 400;
+	my $destination;
+
+	while (1)
+	{
+		my @counts = $self->{dev}->counters()
+			or next;
+
+		my $cur = $counts[$self->{channel} - 1];
+
+		if (defined $destination)
+		{
+			$destination *= 0.99;
+		} else {
+			# Convert for scale factor from counters
+			$destination = $cur/20;
+		}
+
+		last if (-$eps < $cur and $cur < $eps);
+
+		print "$self->{channel}: $cur\n";
+
+		$self->go($destination);
+		usleep 10000;
+	}
+}
+		
+
+
 "0, but true";
 __END__
 	Servo::Axis->new(
 		-dev		=> $devs[0],
 		-channel	=> 2,
+		-min		=> -900,
+		-max		=> +900,
+	),
+	Servo::Axis->new(
+		-dev		=> $devs[1],
+		-channel	=> 2,
 	),
 	Servo::Axis->new(
 		-dev		=> $devs[1],
 		-channel	=> 1,
 	),
 	Servo::Axis->new(
-		-dev		=> $devs[1],
-		-channel	=> 2,
-	),
-	Servo::Axis->new(
 		-dev		=> $devs[2],
 		-channel	=> 1,
 		-min		=> -300,
 	),
 );
 
+for my $i (0..5)
+{
+	my $servo = $servos[$i];
+	$servo->home();
+}
+
 my $t = 0;
-my $dt = 0.02;
+my $dt = 0.005;
 
 while (1)
 {
 	my @pos;
 	my $pos = sin($t += $dt);
 
-	for my $servo (@servos)
+	for my $i (0..5)
 	{
+		my $servo = $servos[$i];
+		my $p = $i == 1 ? $pos : $pos;
 		my $range = $servo->{max} - $servo->{min};
 		my $mid = ($servo->{max} + $servo->{min}) / 2;
-		$servo->go($pos * $range/2 + $mid);
+		$servo->go($p * $range/2 + $mid);
 	}
 
+	$servos[0]->{dev}->{dev}->write("?s\r?c\r?a\r");
+	$servos[2]->{dev}->{dev}->write("?s\r?c\r?a\r");
+	$servos[4]->{dev}->{dev}->write("?s\r?c\r?a\r");
+
 	print "$pos: @pos\n";
-	usleep 100000;
+	usleep 20000;
 }