Browse Source

Use A and B buttons on the gmaepad to enable/disable motor control

Douglas Thrift 5 years ago
parent
commit
ecfbc4e56d
1 changed files with 37 additions and 11 deletions
  1. 37 11
      rover/control.go

+ 37 - 11
rover/control.go

@@ -49,28 +49,43 @@ func control(mode driveMode, g *gamepad.Gamepad, leftMotor, rightMotor *ev3dev.T
 		return err
 	}
 
+	gamepadMotors := false
+
 	for {
 		select {
 		case e := <-g.LeftAnalog:
-			switch mode {
-			case tank:
-				if err := tankAnalogControl(e, leftMotor); err != nil {
-					return err
+			if gamepadMotors {
+				switch mode {
+				case tank:
+					if err := tankAnalogControl(e, leftMotor); err != nil {
+						return err
+					}
 				}
 			}
 		case e := <-g.RightAnalog:
-			switch mode {
-			case tank:
-				if err := tankAnalogControl(e, rightMotor); err != nil {
-					return err
+			if gamepadMotors {
+				switch mode {
+				case tank:
+					if err := tankAnalogControl(e, rightMotor); err != nil {
+						return err
+					}
 				}
 			}
 		case e := <-g.DPad:
 			log.Println("dpad", e)
 		case e := <-g.A:
-			log.Println("a", e)
+			if e.IsButtonPress() {
+				gamepadMotors = true
+				log.Print("gamepad motor control enabled")
+			}
 		case e := <-g.B:
-			log.Println("b", e)
+			if e.IsButtonPress() {
+				gamepadMotors = false
+				log.Print("gamepad motor control disabled")
+				if err := stop(leftMotor, rightMotor); err != nil {
+					return err
+				}
+			}
 		case e := <-g.X:
 			log.Println("x", e)
 		case e := <-g.Y:
@@ -109,6 +124,17 @@ func setPolarity(polarity ev3dev.Polarity, motors ...*ev3dev.TachoMotor) error {
 	return nil
 }
 
+func stop(motors ...*ev3dev.TachoMotor) error {
+	for _, motor := range motors {
+		motor.SetSpeedSetpoint(0)
+		motor.Command("stop")
+		if err := motor.Err(); err != nil {
+			return err
+		}
+	}
+	return nil
+}
+
 func tankAnalogControl(e gamepad.Event, motor *ev3dev.TachoMotor) error {
 	if e.IsAxisY() {
 		speed := int(float64(motor.MaxSpeed()) * e.Fraction())
@@ -123,7 +149,7 @@ func tankAnalogControl(e gamepad.Event, motor *ev3dev.TachoMotor) error {
 			} else {
 				motor.Command("run-forever")
 			}
-			if err = motor.Err(); err != nil {
+			if err := motor.Err(); err != nil {
 				return err
 			}
 		}