Commits

Trammell Hudson committed 5255f1c Draft

sweep works with Servo::Axis

Comments (0)

Files changed (2)

 sub new
 {
 	my $class = shift;
-	my @args = (
-		.min		= -800,
-		.max		= +800,
+	my %args = (
+		-min		=> -700,
+		-max		=> +700,
 		@_
 	);
 
 use warnings;
 use strict;
 use Servo::RobotEQ;
+use Servo::Axis;
 use Time::HiRes 'usleep';
 
 # /dev/tty.usbmodemfd1231 1 == 0 spin, 2 == 1 shoulder
 	/dev/tty.usbmodemfd1221
 );
 
+my @devs = map { Servo::RobotEQ->new($_) } @filenames;
 
-my @limits = (
-	[-800, +800],  # 1 == spin
-	[-800, +800],  # 2 == shoulder
-	[-800, +800],  # 3 == forearm
-	[-800, +800],  # 4 == wrist twist
-	[-400, +400],  # 5 == wrist tilt
-	[-800, +800],  # 6 == tool spin
+
+my @servos = (
+	Servo::Axis->new(
+		-dev		=> $devs[0],
+		-channel	=> 1,
+	),
+	Servo::Axis->new(
+		-dev		=> $devs[0],
+		-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,
+		-max		=> +300,
+	),
+	Servo::Axis->new(
+		-dev		=> $devs[2],
+		-channel	=> 2,
+	),
 );
 
-my @devs = map { Servo::RobotEQ->new($_) } @filenames;
-
 my $t = 0;
-my $dt = 0.01;
+my $dt = 0.02;
 
 while (1)
 {
 	my @pos;
-	my $pos = int(800 * sin($t += $dt));
+	my $pos = sin($t += $dt);
 
-	for my $dev (@devs)
+	for my $servo (@servos)
 	{
-		for my $channel (1..2)
-		{
-			$dev->position($channel, $pos);
-		}
-
-		push @pos, $dev->counters();
+		my $range = $servo->{max} - $servo->{min};
+		my $mid = ($servo->{max} + $servo->{min}) / 2;
+		$servo->go($pos * $range/2 + $mid);
 	}
 
 	print "$pos: @pos\n";