Improve cruise control

rover/control.go

@@ -94,26 +94,28 @@ func control(mode driveMode, g *gamepad.Gamepad, leftMotor, rightMotor *ev3dev.T
 			log.Println("y", e)
 		case e := <-g.Left:
 			if e.IsButtonPress() && gamepadMotors {
-				var err error
-				leftCruise, err = leftMotor.Speed()
-				if err != nil {
+				if err := tankSetCruise("left", leftMotor, &leftCruise); err != nil {
 					return err
 				}
-				log.Printf("left cruise set: %v", leftCruise)
 			}
 		case e := <-g.Right:
 			if e.IsButtonPress() && gamepadMotors {
-				var err error
-				rightCruise, err = rightMotor.Speed()
-				if err != nil {
+				if err := tankSetCruise("right", rightMotor, &rightCruise); err != nil {
 					return err
 				}
-				log.Printf("right cruise set: %v", rightCruise)
 			}
 		case e := <-g.Back:
-			log.Println("back", e)
+			if e.IsButtonPress() && gamepadMotors && rightCruise != 0 {
+				if err := tankSetCruiseSpeed("left", leftMotor, &leftCruise, rightCruise); err != nil {
+					return err
+				}
+			}
 		case e := <-g.Start:
-			log.Println("start", e)
+			if e.IsButtonPress() && gamepadMotors && leftCruise != 0 {
+				if err := tankSetCruiseSpeed("right", rightMotor, &rightCruise, leftCruise); err != nil {
+					return err
+				}
+			}
 		case e := <-g.Logitech:
 			if e.IsButtonPress() {
 				mode++
@@ -154,7 +156,7 @@ func stop(motors ...*ev3dev.TachoMotor) 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) {
+		if *cruise != 0 && !(*cruise > 0 && speed > 0 && speed > *cruise) && !(*cruise < 0 && speed < 0 && speed < *cruise) {
 			speed = *cruise
 		}
 		currentSpeed, err := motor.Speed()
@@ -202,3 +204,41 @@ func tankAnalogControl(e gamepad.Event, side string, motor *ev3dev.TachoMotor, c
 	}
 	return nil
 }
+
+func tankSetCruise(side string, motor *ev3dev.TachoMotor, cruise *int) error {
+	maxSpeed := motor.MaxSpeed()
+	speed, err := motor.Speed()
+	if err != nil {
+		return err
+	}
+	if speed > 0 && speed > maxSpeed {
+		*cruise = maxSpeed
+	} else if speed < 0 && speed < -maxSpeed {
+		*cruise = -maxSpeed
+	} else {
+		*cruise = speed
+	}
+	log.Printf("%v cruise set: %v", side, *cruise)
+	return nil
+}
+
+func tankSetCruiseSpeed(side string, motor *ev3dev.TachoMotor, cruise *int, speed int) error {
+	*cruise = speed
+	log.Printf("%v cruise set: %v", side, *cruise)
+	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
+		}
+	}
+	return nil
+}