Source

servo / sweep

#!/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,
	),
	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 $t = 0;
my $dt = 0.02;

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

	for my $servo (@servos)
	{
		my $range = $servo->{max} - $servo->{min};
		my $mid = ($servo->{max} + $servo->{min}) / 2;
		$servo->go($pos * $range/2 + $mid);
	}

	print "$pos: @pos\n";
	usleep 100000;
}