Examples¶

These examples assume your motors are named "front_left_drive", "front_right_drive", "back_left_drive" and "back_right_drive". If yours have other names, change the examples to match.

Drivetrain¶

A mecanum drivetrain has four motors. The two on the left are mounted the other way round, so reverse them first:

user=> (motor/set-direction "front_left_drive" :reverse)
:reverse
user=> (motor/set-direction "back_left_drive" :reverse)
:reverse

Next, define a function that turns three numbers into four wheel powers: how fast to drive forward, strafe right and turn right, each from -1.0 to 1.0. If the three numbers add up to more than 1, all four powers are scaled down together, so the robot still moves in the direction you asked for:

(defn mecanum-drive [forward strafe rotate]
  (let [d           (max (+ (abs forward) (abs strafe) (abs rotate)) 1)
        front-left  (/ (+ forward strafe rotate) d)
        front-right (/ (- forward strafe rotate) d)
        back-left   (/ (+ (- forward strafe) rotate) d)
        back-right  (/ (- (+ forward strafe) rotate) d)]
    (motor/set-power "front_left_drive" front-left)
    (motor/set-power "front_right_drive" front-right)
    (motor/set-power "back_left_drive" back-left)
    (motor/set-power "back_right_drive" back-right)
    [front-left front-right back-left back-right]))

Paste it at the prompt.

Now drive the robot by hand, one move at a time.

First, drive forward slowly, then stop by setting everything to 0:

user=> (mecanum-drive 0.2 0 0)
[0.2 0.2 0.2 0.2]
user=> (mecanum-drive 0 0 0)
[0.0 0.0 0.0 0.0]

Each vector is what mecanum-drive returns: the power it gave each wheel, in the order front left, front right, back left, back right. Driving forward sets all four wheels to 0.2, and stopping sets them all to 0.0.

Move sideways to the right, then stop:

user=> (mecanum-drive 0 0.2 0)
[0.2 -0.2 -0.2 0.2]
user=> (mecanum-drive 0 0 0)
[0.0 0.0 0.0 0.0]

Turn right on the spot, then stop:

user=> (mecanum-drive 0 0 0.2)
[0.2 -0.2 0.2 -0.2]
user=> (mecanum-drive 0 0 0)
[0.0 0.0 0.0 0.0]

If a wheel turns the wrong way, fix its direction with motor/set-direction and try again.

Game loop¶

Instead of typing each move, let a control loop make them in order. This one drives forward, strafes right, drives backward, then strafes left, each for 3 seconds at power 0.2, so the robot ends up roughly where it started:

(defn drive-square []
  (let [start (time/elapsed)]
    (loop []
      (let [t (- (time/elapsed) start)]             ; seconds since start
        (when (< t 12)
          (cond (< t 3) (mecanum-drive 0.2 0 0)     ; forward
                (< t 6) (mecanum-drive 0 0.2 0)     ; strafe right
                (< t 9) (mecanum-drive -0.2 0 0)    ; backward
                :else   (mecanum-drive 0 -0.2 0))   ; strafe left
          (time/sleep 0.02)
          (recur))))
    (mecanum-drive 0 0 0)))                         ; stop

Pasting this only defines drive-square; the robot doesn't move yet. To drive the square, call it:

user=> (drive-square)
[0.0 0.0 0.0 0.0]

The robot drives the square, but you have to wait the full 12 seconds. The result and the next prompt appear only after the robot stops, and you can't type anything in the meantime.

To keep typing while the robot drives, use control/start instead. It runs the function in the background and returns at once:

user=> (control/start drive-square)
#<Future pending>

#<Future pending> means the work hasn't finished yet: the robot is still driving the square, and you can type the next command while it does.

Driver control¶

Now hand the robot over to the gamepad. This loop reads gamepad 1 fifty times a second: the left stick drives forward, backward and sideways, and the right stick turns:

(defn drive-with-gamepad []
  (loop []
    (let [pad     (gamepad 1)
          forward (- (:left-stick-y pad))
          strafe  (:left-stick-x pad)
          rotate  (:right-stick-x pad)]
      (mecanum-drive forward strafe rotate))
    (time/sleep 0.02)
    (recur)))

Start it with (control/start drive-with-gamepad) and end it with (control/stop). The wheels keep their last power after the loop ends, so stop them with (mecanum-drive 0 0 0). Pressing STOP on the Driver Hub stops them too.

Pushing a stick forward gives a negative :left-stick-y, so the loop negates it (see Gamepad). The directions are relative to the robot: forward is the way it faces.

Speed adjustment¶

At full power, the robot can be hard to steer in a tight space. Keep a speed factor in an atom, and multiply the forward and strafe values by it:

(defonce speed (atom 0.5))

(defn drive-with-gamepad []
  (loop []
    (let [pad     (gamepad 1)
          forward (* @speed (- (:left-stick-y pad)))
          strafe  (* @speed (:left-stick-x pad))
          rotate  (:right-stick-x pad)]
      (mecanum-drive forward strafe rotate))
    (time/sleep 0.02)
    (recur)))

Paste it, then run (control/start drive-with-gamepad) again. A running loop keeps the old definition, and control/start stops it before starting the new one.

Now change the speed at the prompt while you drive:

user=> (reset! speed 0.3)
0.3
user=> (reset! speed 1)
1.0