Working cruise control

rover/control.go

@@ -58,7 +58,7 @@ func control(mode driveMode, g *gamepad.Gamepad, leftMotor, rightMotor *ev3dev.T
 			if gamepadMotors {
 				switch mode {
 				case tank:
-					if err := tankAnalogControl(e, leftMotor, &leftCruise); err != nil {
+					if err := tankAnalogControl(e, "left", leftMotor, &leftCruise); err != nil {
 						return err
 					}
 				}
@@ -67,7 +67,7 @@ func control(mode driveMode, g *gamepad.Gamepad, leftMotor, rightMotor *ev3dev.T
 			if gamepadMotors {
 				switch mode {
 				case tank:
-					if err := tankAnalogControl(e, rightMotor, &rightCruise); err != nil {
+					if err := tankAnalogControl(e, "right", rightMotor, &rightCruise); err != nil {
 						return err
 					}
 				}
@@ -151,9 +151,12 @@ func stop(motors ...*ev3dev.TachoMotor) error {
 	return nil
 }
 
-func tankAnalogControl(e gamepad.Event, motor *ev3dev.TachoMotor, cruise *int) error {
+func tankAnalogControl(e gamepad.Event, side string, motor *ev3dev.TachoMotor, cruise *int) error {
 	if e.IsAxisY() {
 		speed := -int(float64(motor.MaxSpeed()) * e.Fraction())
+		if *cruise != 0 && !(*cruise > 0 && speed > 0 && speed > *cruise) && !(*cruise < 0 && speed < 0 && speed > *cruise) {
+			speed = *cruise
+		}
 		currentSpeed, err := motor.Speed()
 		if err != nil {
 			return err
@@ -169,6 +172,33 @@ func tankAnalogControl(e gamepad.Event, motor *ev3dev.TachoMotor, cruise *int) e
 				return err
 			}
 		}
+	} else if e.IsAxisZ() {
+		if *cruise != 0 {
+			speed := int(float64(*cruise) * (1 - e.Fraction()))
+			currentSpeed, err := motor.Speed()
+			if err != nil {
+				return err
+			}
+			if speed != currentSpeed {
+				motor.SetSpeedSetpoint(speed)
+				if speed == 0 {
+					motor.Command("stop")
+				} else {
+					motor.Command("run-forever")
+				}
+				if err := motor.Err(); err != nil {
+					return err
+				}
+			}
+		}
+	} else if e.IsButtonPress() {
+		*cruise = 0
+		log.Printf("%v cruise set: %v", side, *cruise)
+		motor.SetSpeedSetpoint(*cruise)
+		motor.Command("stop")
+		if err := motor.Err(); err != nil {
+			return err
+		}
 	}
 	return nil
 }