Hack hack hack

components/controller/controller.go

@@ -2,7 +2,6 @@ package controller
 
 import (
 	"io"
-	"math"
 	"time"
 
 	"github.com/Sirupsen/logrus"
@@ -24,8 +23,8 @@ const (
 	// Distance to adjust the clearance each time Up or Down is pressed.
 	clearanceStep = 10
 
-	// Minimum pressure needed to trigger the dpad.
-	dpadMinPressure = 10
+	// Minimum pressure needed to trigger a button press.
+	minButtonPressure = 10
 
 	// Maximum angle (in degrees) to bank to the left or right using the
 	// orientation of the controller.
@@ -34,6 +33,10 @@ const (
 	// Maximum angle (in degrees) to pitch forwards or backwards using the
 	// orientation of the controller.
 	pitchScale = 15.0
+
+	// TODO: Document what 'offset' is, here and in the legs.
+	xOffsetScale = 40.0
+	zOffsetScale = 40.0
 )
 
 type Controller struct {
@@ -43,9 +46,14 @@ type Controller struct {
 
 	// Keep track of whether various buttons were being pressed during the
 	// previous tick, to avoid key repeat.
-	upLatch   Latch
-	downLatch Latch
-	psLatch   Latch
+	upLatch    Latch
+	downLatch  Latch
+	leftLatch  Latch
+	rightLatch Latch
+	psLatch    Latch
+
+	// Track select + button options, which change states.
+	selectTriangle Latch
 
 	// Enable target orientation mode, where the target bank/pitch (x/y) are set
 	// using the controller orientation. Press the PS button to toggle. Defaults
@@ -100,30 +108,39 @@ func (c *Controller) Tick(now time.Time, state *hexapod.State) error {
 	// If target orientation mode is enabled, set the target XZ orientation to
 	// match the controller. (Note that the axes are different and inverted.)
 	if c.setTargetOrientation {
-		state.Target.Pitch = -quantize(c.sa.Orientation.Y(), 0.05) * pitchScale
-		state.Target.Bank = -quantize(c.sa.Orientation.X(), 0.05) * bankScale
+		state.Target.Pitch = -c.sa.Orientation.Y() * pitchScale
+		state.Target.Bank = -c.sa.Orientation.X() * bankScale
 	} else {
 		state.Target.Pitch = 0
 		state.Target.Bank = 0
 	}
 
-	// Use the right stick to set the focal point, which the head aims at. Note
-	// that (a) we discard the pitch+bank orientation of the hex pose, so that
-	// our focal point is "forwards" relative to the ground rather than the
-	// chassis, and (b) that the Y axis is inverted from the pull-down-to-look-
-	// up scheme often used in games. This is all very silly, but looks cool.
-	fp := state.Pose.Add(math3d.Pose{
-		Pitch: -state.Pose.Pitch,
-		Bank:  -state.Pose.Bank,
-	}).Add(math3d.Pose{
-		Position: math3d.Vector3{
-			X: (float64(c.sa.RightStick.X) / 127.0 * horizontalLookScale) + focalHorizontalOffset,
-			Y: (float64(c.sa.RightStick.Y*-1) / 127.0 * verticalLookScale) + focalVerticalOffset,
-			Z: focalDistance,
-		},
-		Heading: 0,
-	}).Position
-	state.LookAt = &fp
+	// Set offset using the right stick while R1 is held down.
+	if c.sa.R1 > minButtonPressure {
+		state.Offset = math3d.Vector3{
+			X: (float64(c.sa.RightStick.X) / 127.0 * xOffsetScale),
+			Z: (float64(c.sa.RightStick.Y*-1) / 127.0 * zOffsetScale),
+		}
+	} else {
+
+		// Use the right stick to set the focal point, which the head aims at. Note
+		// that (a) we discard the pitch+bank orientation of the hex pose, so that
+		// our focal point is "forwards" relative to the ground rather than the
+		// chassis, and (b) that the Y axis is inverted from the pull-down-to-look-
+		// up scheme often used in games. This is all very silly, but looks cool.
+		fp := state.Pose.Add(math3d.Pose{
+			Pitch: -state.Pose.Pitch,
+			Bank:  -state.Pose.Bank,
+		}).Add(math3d.Pose{
+			Position: math3d.Vector3{
+				X: (float64(c.sa.RightStick.X) / 127.0 * horizontalLookScale) + focalHorizontalOffset,
+				Y: (float64(c.sa.RightStick.Y*-1) / 127.0 * verticalLookScale) + focalVerticalOffset,
+				Z: focalDistance,
+			},
+			Heading: 0,
+		}).Position
+		state.LookAt = &fp
+	}
 
 	// Toggle target orientation mode by pressing PS.
 	if c.psLatch.Run(c.sa.PS) {
@@ -132,20 +149,34 @@ func (c *Controller) Tick(now time.Time, state *hexapod.State) error {
 	}
 
 	// Increase clearance by pressing Up
-	if c.upLatch.Run(c.sa.Up > dpadMinPressure) {
+	if c.upLatch.Run(c.sa.Up > minButtonPressure) {
 		c.clearance += clearanceStep
 		log.Infof("clearance=%v", c.clearance)
 	}
 
 	// Decrease clearance by pressing Down
-	if c.downLatch.Run(c.sa.Down > dpadMinPressure) {
+	if c.downLatch.Run(c.sa.Down > minButtonPressure) {
 		c.clearance -= clearanceStep
 		log.Infof("clearance=%v", c.clearance)
 	}
 
-	return nil
-}
+	// Increase speed by pressing right
+	if c.rightLatch.Run(c.sa.Right > minButtonPressure) {
+		state.Speed += 1
+		log.Infof("Speed=%v", state.Speed)
+	}
+
+	// Decrease speed by pressing left
+	if c.leftLatch.Run(c.sa.Left > minButtonPressure) {
+		state.Speed -= 1
+		log.Infof("Speed=%v", state.Speed)
+	}
 
-func quantize(val, interval float64) float64 {
-	return math.Trunc(val/interval) * interval
+	// Cycle through gaits by pressing select + triangle
+	if c.selectTriangle.Run(c.sa.Select && c.sa.Triangle > minButtonPressure) {
+		state.GaitIndex += 1
+		log.Infof("GaitIndex=%v", state.GaitIndex)
+	}
+
+	return nil
 }

components/head/head.go

@@ -9,6 +9,7 @@ import (
 	"github.com/adammck/dynamixel/servo"
 	"github.com/adammck/hexapod"
 	"github.com/adammck/hexapod/math3d"
+	"github.com/adammck/hexapod/servos"
 	"github.com/adammck/hexapod/utils"
 )
 
@@ -65,9 +66,6 @@ func (h *Head) Boot() error {
 		if err != nil {
 			return fmt.Errorf("%s (while setting torque limit)", err)
 		}
-
-		// Buffer all moves until end of tick.
-		s.SetBuffered(true)
 	}
 
 	return nil
@@ -96,7 +94,7 @@ func (h *Head) Tick(now time.Time, state *hexapod.State) error {
 
 	// Update servos every tick.
 	// TODO: Maybe only update if the x/y has changed.
-	h.h.MoveTo(x)
-	h.v.MoveTo(y)
+	servos.RegMoveTo(h.h, x)
+	servos.RegMoveTo(h.v, y)
 	return nil
 }

components/legs/gait/gait_default.go

@@ -4,9 +4,9 @@ import (
 	"math"
 )
 
-func TheGait(ticksPerStep int) Gait {
-	ticksPerStepCycle := ticksPerStep * 3
-	cc := curveCenters(2, ticksPerStepCycle)
+func TheGait(groupSize int, ticksPerStep int) Gait {
+	ticksPerStepCycle := ticksPerStep * (6 / groupSize)
+	cc := curveCenters(groupSize, ticksPerStepCycle)
 
 	var legs [numLegs]Frames
 	for i := 0; i < numLegs; i += 1 {