Source

servo / sweep

Full commit
#!/usr/bin/perl
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.usbmodemfd1211 1 == 4 wrist twist, 2 == 3 forearm
# /dev/tty.usbmodemfd1221 1 == 5 wrist tilt, 2 == 6 tool spin

my @filenames = qw(
	/dev/tty.usbmodemfd1231
	/dev/tty.usbmodemfd1211
	/dev/tty.usbmodemfd1221
);

my @devs = map { Servo::RobotEQ->new($_) } @filenames;


my @servos = (
	Servo::Axis->new(
		-dev		=> $devs[0],
		-channel	=> 1,
	),
	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[2],
		-channel	=> 1,
		-min		=> -300,
		-max		=> +300,
	),
	Servo::Axis->new(
		-dev		=> $devs[2],
		-channel	=> 2,
	),
);

for my $i (0..5)
{
	my $servo = $servos[$i];
	$servo->home();
}

my $t = 0;
my $dt = 0.005;

while (1)
{
	my @pos;
	my $pos = sin($t += $dt);

	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($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 20000;
}