A start for cruise control

rover/control.go

@@ -45,11 +45,12 @@ func (d *driveMode) Set(value string) error {
 func control(mode driveMode, g *gamepad.Gamepad, leftMotor, rightMotor *ev3dev.TachoMotor) error {
 	log.Printf("control loop started (drive mode: %v)", mode)
 
-	if err := setPolarity(ev3dev.Inversed, leftMotor, rightMotor); err != nil {
+	if err := setPolarity(ev3dev.Normal, leftMotor, rightMotor); err != nil {
 		return err
 	}
 
 	gamepadMotors := false
+	var leftCruise, rightCruise int
 
 	for {
 		select {
@@ -57,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); err != nil {
+					if err := tankAnalogControl(e, leftMotor, &leftCruise); err != nil {
 						return err
 					}
 				}
@@ -66,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); err != nil {
+					if err := tankAnalogControl(e, rightMotor, &rightCruise); err != nil {
 						return err
 					}
 				}
@@ -85,15 +86,30 @@ func control(mode driveMode, g *gamepad.Gamepad, leftMotor, rightMotor *ev3dev.T
 				if err := stop(leftMotor, rightMotor); err != nil {
 					return err
 				}
+				leftCruise, rightCruise = 0, 0
 			}
 		case e := <-g.X:
 			log.Println("x", e)
 		case e := <-g.Y:
 			log.Println("y", e)
 		case e := <-g.Left:
-			log.Println("left", e)
+			if e.IsButtonPress() && gamepadMotors {
+				var err error
+				leftCruise, err = leftMotor.Speed()
+				if err != nil {
+					return err
+				}
+				log.Printf("left cruise set: %v", leftCruise)
+			}
 		case e := <-g.Right:
-			log.Println("right", e)
+			if e.IsButtonPress() && gamepadMotors {
+				var err error
+				rightCruise, err = leftMotor.Speed()
+				if err != nil {
+					return err
+				}
+				log.Printf("right cruise set: %v", rightCruise)
+			}
 		case e := <-g.Back:
 			log.Println("back", e)
 		case e := <-g.Start:
@@ -135,9 +151,9 @@ func stop(motors ...*ev3dev.TachoMotor) error {
 	return nil
 }
 
-func tankAnalogControl(e gamepad.Event, motor *ev3dev.TachoMotor) error {
+func tankAnalogControl(e gamepad.Event, motor *ev3dev.TachoMotor, cruise *int) error {
 	if e.IsAxisY() {
-		speed := int(float64(motor.MaxSpeed()) * e.Fraction())
+		speed := -int(float64(motor.MaxSpeed()) * e.Fraction())
 		currentSpeed, err := motor.Speed()
 		if err != nil {
 			return err