VIDEO: Fast & Furious: Tokytoybox Drift

For the Tidy the Toys challenge, we've got wheels and a forklift, but we're having trouble staying on target.

We've watched some videos of other people's robots spotting the colourful cubes from a metre away... but unfortunately our Lego Mindstorms/EV3 kit don't have anywhere near that level of visual accuracy.

The standard Lego fare includes a colour/light sensor, but it works mostly on reflected light, which means that it can only really detect the colour when it's less than a centimetre away from the lens.

Lego also includes a Gyro sensor... which I'd never played with until now, and it's not half bad.

The usage is simple... request the sensor's details:

gyroStatus = BP.get_sensor(BP.PORT_1)

...and it returns an array.

print(gyroStatus)
[165, -5]

The first number is the angle representing the absolute direction the gyro is pointing. When the gyro first switches on, this number is zero - it's not connected to a compass or anything. This number doesn't stick between 0 and 360 either; if we keep spinning it can go into the thousands, or the negative thousands.

The second number is the difference in angle since the last reading. In the above example, the last reading must have been at 170, and this reading is 165, so the second number reads -5.

So, to keep the robot driving on the straight and narrow, we wrote three defs...

findZero()

This one just remembers which way we are facing when the program starts.

def findZero():
 sensorIsWorking = False
 z = 0
 while not sensorIsWorking:
  try:
   gyroStatus = BP.get_sensor(BP.PORT_1)
   z = gyroStatus[0] % 360
   sensorIsWorking = True
  except:
   print(“Still finding Zero”)
 return z

driveStraight()

This one takes a direction (degrees), distance (cm), speed (okay, it's actually motor power between 0 and 100), and then keeps us on course a little like a line follower.

Looking at the bolded lines below... if we start to drift, we give more power to one side and less to the other so it gets back on course.

def driveStraight(direction, distance, speed):
 driveTime = distance / (speed * 0.8)
 gyroZero = 0
 seconds = 0.0
 while seconds < driveTime:
  try:
   gyroStatus = BP.get_sensor(BP.PORT_1)
   facing = gyroStatus[0] % 360
   BP.set_motor_power(BP.PORT_B, speed+((facing-direction)/2))
   BP.set_motor_power(BP.PORT_C, speed-((facing-direction)/2))
   time.sleep(0.1)
   seconds = seconds+0.1
  except:
   print(“Trying to read Gyro”)
 BP.set_motor_power(BP.PORT_B+BP.PORT_C, 0)

turn()

This one spins the two wheels until we're facing the desired direction:

def turn(direction):
 speed = 20
 facingDirection = False
 while not facingDirection:
  gyroStatus = BP.get_sensor(BP.PORT_1)
  facing = gyroStatus[0] % 360  
  if facing > direction+1:
   BP.set_motor_power(BP.PORT_B, speed)
   BP.set_motor_power(BP.PORT_C, -speed)
   time.sleep(0.1)
  elif facing < direction-1:
   BP.set_motor_power(BP.PORT_B, -speed)
   BP.set_motor_power(BP.PORT_C, speed)
   time.sleep(0.1)
  else:
   BP.set_motor_power(BP.PORT_B + BP.PORT_C, 0)
   time.sleep(0.3)
   facingDirection = True

Finally, in the main body of the program:

• we assign the zero direction to a constant called SOUTH

• we assign WEST, NORTH and EAST relative to the SOUTH gyro direction (+90, +180, +270).

• and from there, we can control the robot with simple, easy-to-read commands:

  • turn(NORTH)

  • driveStraight(NORTH, 100, 25)

  • turn(WEST)

  • driveStraight(WEST, 30, 25)

  • etc

Here's a run of Tidy The Toys with the gyro sensor code... the forklift needs work, but the driving is spot on! Vin Diesel couldn't have done better himself.