Zumo 32U4 Synchronize Motor

I received a recent question on how to make the Zumo drive straight.  There are a few ways to do this, but the easiest way is to synchronize the motors.  In this post, I’ll briefly discuss a simple method to slave the right motor off of the left motor.  In this case, the right motor will dynamically adjust the power so that the encoder counts on the right motor match the left motor.  As a test, you can hold the left track to slow it down, and this should slow down the right track automatically.  Note that this doesn’t work the other way around.  That would take a slightly more complicated program, and I’ll try to cover that in a later post.

The main difficulty with the Zumo not going straight is due to the fact that we are using unregulated motor commands.  No two motors are ever assembled perfectly equal.  In addition, there are minor differences in friction in the gear boxes, the axles, etc.  As a result, we tell the motors to use the same power settings, and the H-bridges deliver the correct power settings, but the motors respond slightly differently.

To overcome this, we need to measure the values of the encoders as the robot is driving, and adjust the power to the right motor to force the encoders to be equal.  The basic flow would be:

  • Reset the encoders.
  • Set the power levels of the motors to an initial guess.
  • Do the following:
    • Measure the left and right encoders values.
    • Calculate the error between the two encoders (error = left encoder – right encoder).
    • Calculate a correction factor (Kp*error where Kp is a proportional gain value).
    • Modify the right motor power value (right power = initial power + correction factor).
    • Apply the modified motor power values to the setSpeed of the motors.
    • Repeat this until an end condition is met.

However, even this may not have the Zumo go perfectly straight due to minor differences in wheel diameters and track thickness.  Therefore, I added a fudge factor for calculating the error, called STRAIGHTFACTOR.  This should be adjusted for very minor values.  Default should be 1, but you may need to tweak it somewhere between 0.9 and 1.1 depending on the robot.

#include <Zumo32U4.h>
Zumo32U4Encoders encoders;
Zumo32U4Motors motors;
Zumo32U4LCD lcd;
Zumo32U4ButtonA buttonA;

#define SPEED 200
#define DISTANCE 2000
#define Kp 1
#define STRAIGHTFACTOR 1  // Adjust this to correct for minor curve.  Should be in the 0.9 to 1.1 range

void setup() {
}

void loop() {
  int currentSpeedLeft=SPEED;
  int currentSpeedRight=SPEED;
  int error;
  int correction;

  // Start Sequence - Wait for Button
  lcd.print("Press A");
  motors.setSpeeds(0, 0);
  buttonA.waitForButton();
  lcd.clear();
  delay(200);
  
  int countsLeft = encoders.getCountsAndResetLeft();
  int countsRight = encoders.getCountsAndResetRight();

  motors.setSpeeds(currentSpeedLeft,currentSpeedRight);
  do {
    countsLeft = encoders.getCountsLeft();
    countsRight = encoders.getCountsRight();
    error = countsLeft-STRAIGHTFACTOR*countsRight;
    correction = Kp*error;
    currentSpeedRight = SPEED + correction;
    motors.setSpeeds(currentSpeedLeft,currentSpeedRight);
  }  while(countsLeft<DISTANCE&&countsRight<DISTANCE);
  motors.setSpeeds(0,0);
} 

The above example sets the left motor to a speed of 200, then modifies the speed of the right motor so that the error between the two encoders is reduced to zero.  I had the loop exit condition be 2000 encoder counts, but this can be modified to go any distance, or even use light sensors to exit the loop.  I was rather explicit with the math.  Much can be simplified to some thing like:

currentSpeed Right = SPEED + Kp*(encoders.getCountsLeft() - encoders.getCountsRight()); 

All this does is save a couple variables and a few lines of code and is not necessary.

Posted in Robotics | Tagged , , | Leave a comment

LEGO EV3 – Saving an Array to a File

I was recently asked in a forum on how to save an array from the EV3 to a file.  Imagine you are taking data with a sensor, then storing the sequential measurements in an array, and you would like to analyze this data.  You could use the data logging feature, but let’s say you wanted to include this option in a program.  This came up for one of my WRO teams in 2015 where they were trying to scan the map legend as fast as possible.  The method that they used to develop an algorithm was to read the data as fast as possible, store it in an array, then save it as a file.  After that, they downloaded the data and developed filtering and sampling methods using Excel.  This allowed them to visually look at the data as they were developing the algorithm.  Later, they left the code in the program so that they could go back and look at the data if the robot failed to interpret the data correctly.  This gave them the option to determine what went wrong, and how to fix it.

Anyway, the EV3 does not deal with arrays particularly gracefully.  Further, it deals with exporting files with about as much grace.  The method that we used was simply:

  1. Issue a delete file command for the filename you intend to use.  If you don’t, the write data to a file simply appends, and it would be appending to an old file (if one exists by the same name).
  2. Read the length of the array.  Since our measurements were based on taking as many readings as possible, the length of the array varied from run to run, and if you attempt to access an array element longer than the array is, the EV3 crashes and does wonderful unpredictable things.
  3. Setup a loop to run for a specific number of iterations.  The number of iterations is the length of the array.
  4. Within the loop, perform the following steps:
    1. Read the array at the index value of the current loop iteration.
    2. Use the file access command to write the data to a file.  Note that the file names all through this need to be the same.  This appends the data to the end of the file.
    3. Iterate the loop for the number of times that equals the length of the array.
  5. After the loop completes, close the file.

An example in EV3-G is shown below.  This was copied from a MyBlock, so it assumes that the array of data named MeasuredData already exists, and is filled with data.

Posted in Robotics | Tagged , , | Leave a comment

Motor Regulation: Part 2 – Using a P-Controller to Regulate Rotational Velocity

In the last post, we calculated the rotational velocity of the motor.  In this post, we will begin applying feedback to regulate the motor rotational velocity with a simple P-controller.  Like line following, a P, or proportional, feedback method is used to apply corrections to the power value.  As a review of P-controllers, the basic process is:

  1. Measure the value you want to control.
  2. Calculate the error (error = value – target value).
  3. Calculate the correction (correction = Kp x error).
  4. Apply the correction.

This is no different than with line following, but we will be using rotational velocity rather than the light sensor reading.  We could start at 0 power and ramp up with the rate of the P-controller, but that is a bit slow.  For the open loop condition, the rotational velocity was approximately the motor power divided by 10.  Let’s use that as the starting value.  The program below follows the basic flow of:

  1. Assumes a starting power.
  2. At a determined interval of 10 ms, it performs the following steps:
    1. Samples the velocity.
    2. Calculates the error.
    3. Calculates the correction factor.
    4. Updates the power setting.
  3. Exits the loop after a certain number of encoder counts.

Like before, I have the data of the velocity sent to the serial monitor in {{time, velocity},{…, …}} format to make plotting easier in Mathematica.  If you want to use Excel, you can simply remove the formatting and add tabs.

#include <Zumo32U4.h>
Zumo32U4Encoders encoders;
Zumo32U4Motors motors;
Zumo32U4LCD lcd;
Zumo32U4ButtonA buttonA;

#define dT_MOTOR 10
#define Kp_MOTOR 0.06
#define MOTOR_VEL 2000

void setup() {
  Serial.begin(115200); // initialize Serial communication
  while(!Serial);
  lcd.clear();
}

void loop() {
  int newPosition, oldPosition;
  int vel;
  int error;
  int newPower;
  unsigned long startTime, lastSampleTime;

  lcd.print("Press A");
  buttonA.waitForButton();
  lcd.clear();
  delay(200);
  startTime=millis();
  lastSampleTime=millis();
  newPower=MOTOR_VEL/10;
  oldPosition=encoders.getCountsAndResetLeft();
  Serial.print("{{0,");
  Serial.print("0},");
  motors.setSpeeds(newPower,0);
  do {
    if(millis()-lastSampleTime>dT_MOTOR-1) {
      newPosition=encoders.getCountsLeft();
      vel=(newPosition-oldPosition)*1000/dT_MOTOR;
      error=vel-MOTOR_VEL;
      oldPosition=newPosition;
      lastSampleTime=millis();
      newPower=newPower-Kp_MOTOR*error;
      motors.setSpeeds(newPower,0);
      Serial.print("{");
      Serial.print(millis()-startTime);
      Serial.print(",");
      Serial.print(vel);
      Serial.print("},");
    }
  } while(encoders.getCountsLeft()<2000);
  motors.setSpeeds(0,0);
} 

Using a value of Kp_MOTOR = 0.4, I got the following results:

rot-vel-kp0p4

In this case, the velocity setting of 4000 never quite reaches the target.  This is due to driving the robot with only one tread, and the other tread is rubbing against the table.  For the other values of velocity, there is a little overshoot and they settle relatively quickly.  The most exciting part about this is that I can hold the wheel and feel the power increase to attempt to maintain a constant rotational velocity.  I can also drive this at low speeds now with the robot stalling.

As another test, I increased the gain to 0.6.

rot-vel-kp0p6

The results are not very different, but the gain difference was minor.  At this point, you can see slightly faster rise times, but more overshoot and a dampened oscillation.  If I look at the actual power levels calculated in the program, I get this:

power-kp0p6

Keep in mind that anything above 400 is automatically limited to 400 in the library function for setting the speed.  At a speed of 4000, we are not able to drive the motors enough and the regulation fails.  However, at lower rotational velocity levels, you can see how the power is initially driven higher, then reduced in order to maintain a constant speed.

At this point, I could tweak the Kp value to get the best response for a given speed, but I’d rather wait until the next post and add a full PID controller.  The PID should work over a broader range of power values with a faster response and cleaner settling.

 

Posted in Robotics | Tagged , | Leave a comment

Motor Regulation: Part 1 – Calculate Rotational Velocity

The next several posts are going to deal with writing functions for regulated motor blocks.  So far, everything shown in this blog for the Zumo uses unregulated motors.  At some point, having regulated and synchronized motors will be necessary for precise navigation.  Since I have some time on my hands now, I figured I’d begin tackling it.

First of all, what is motor regulation?  In this case, I’m referring to a method of maintaining constant rotational velocity, as opposed to a constant power applied to the motors.  The actual power applied to the motor will fluctuate to maintain a specific rotational velocity.

Why do we need motor regulation?  Well, there are several reasons, and I’m only going to touch on a few.  First of all, if you look at the schematic, the H-bridges that drive the two motors operate off of the battery voltage, not the voltage after the motor regulator.  Therefore, as the battery voltage changes, the velocity of the robot changes.  Another reason is to deal with motional resistance better.  Let’s say the robot is driving at a speed of 200, then it hits an object.  The robot will naturally slow down and the power will stay at 200.  However, if using a regulated motor function, the robot measures the rotational velocity of the wheels, notices that it slows down, and increase the power to maintain a constant rotational velocity (up to the limit).  Another great reason to use regulated motor functions is for low velocity, precise movement.  If you want the robot to move very slowly, it often will stall under lower power levels.  Motor regulation will dynamically adjust the power to allow it to move precisely at low velocities.

This topic will be split over several posts.  The content is written primarily for my middle school robotics kids.  People who understand control theory will think it’s too easy, people using the Zumo after using the EV3 may think it’s overly complicated.  If you have any questions, please send me a note through the “Contact Me” tab and I’d be happy to help you out.

This post will deal first with setting a power level, and measuring the rotational velocity.  In all cases, I’m leaving the velocity in terms of encoder counts on my 75:1 Zumo robot.  Velocity is simply the change of encoder counts divided by the change in time.  I can control the change in time by setting how fast I sample the measurements.  For reasons that will become clear later the regulated motor topic, we don’t want to sample too fast.  For now, let’s sample at about 10 ms.

The code below turns the motors on at a power of 300 for a short distance, calculates the rotational velocity, and outputs the velocity vs. time to the serial monitor.  I used the output format of “{time,velocity}” to be compatible with Mathematica, which I use for plotting the data.

#include <Zumo32U4.h>

Zumo32U4Encoders encoders;
Zumo32U4Motors motors;
Zumo32U4LCD lcd;
Zumo32U4ButtonA buttonA;

#define dT_MOTOR 10       //Sample time for velocity calculation
#define MOTOR_POWER 300   //Motor power
void setup() {
  Serial.begin(115200); // initialize Serial communication
  while(!Serial);
  lcd.clear();
}

void loop() {
  int newPosition, oldPosition;
  int vel;
  unsigned long startTime, lastSampleTime;

  lcd.print("Press A");
  buttonA.waitForButton();
  lcd.clear();
  delay(200);

  startTime=millis();                              //Record the start time
  lastSampleTime=millis();                         //Initiallize the time of the last sample
  oldPosition=encoders.getCountsAndResetLeft();
  Serial.print("{{0,");
  Serial.print("0},");
  motors.setSpeeds(MOTOR_POWER,MOTOR_POWER);
  do {
    if(millis()-lastSampleTime>dT_MOTOR-1) {       //If current time-lastSample >= sample time....
      newPosition=encoders.getCountsLeft();
      vel=(newPosition-oldPosition)*1000/dT_MOTOR;
      oldPosition=newPosition;
      lastSampleTime=millis();
      Serial.print("{");
      Serial.print(millis()-startTime);
      Serial.print(",");
      Serial.print(vel);
      Serial.print("},");
    }
  } while(encoders.getCountsLeft()<500);
  motors.setSpeeds(0,0);
} 

The chart below shows typical data for various power levels.  Note that there is some ripple from due to rounding at the sampling times.  If I were to run the same conditions, but had the robot pushing an object, the robot would slow down considerably.  Also, notice in the plot how the rise time for the slower speeds appears fairly slow to settle.  Regulation should speed that up considerably by applying more power at the start to overcome the robot inertia.

rot-vel-from-power

In the next post, we will put the motor in a proportional feedback loop to provide a low order of regulation.

Posted in Robotics | Tagged , | Leave a comment

Zumo 32U4 – Using the Buttons the Select a Start Move

This post discusses one way to handle setting up the Zumo to perform one of three initial moves prior to entering the main body of the program.  If you go back a couple of posts, we discussed the simple state machine for a sumo competition.  In the setup, there was a section for the initial move.  The initial move is the first move you want your robot to make prior to entering the state machine and following a preset algorithm.  This could be a fixed move, in which case, you can ignore this post.  However, you may decide you want to have your robot do a different initial move based on how your competitor is set up, or in the case of BottleSumo, where the bottle is located.  In this case, you often don’t know these conditions until you are set up at the table, ready to go.

This post describes a way that you can use the three buttons to select a particular first move.  So far, we’ve only used the buttons to wait until you press a button.  In this case, we need to record the action we want to take, wait for a button, then take that action.

The basic flow is:

  1. Check to see if any of the three buttons are pressed.
  2. If a button is pressed, record which one.
  3. Go to step 1 if no button is pressed, if not, proceed to step 4.
  4. Wait until button A is pressed to signal the start of the match.
  5. Execute the option selected from step 2.
  6. Proceed to normal state machine.

To make it easier, I like to use the enum function to define a data type.  You cold simply use an integer to represent button A, B, and C, but why not use the actually button name?  Defining the datatype with enum is done as below:

enum ButtonChoice       // Create datatype for the button label.
{
  A,
  B,
  C
}; 

This creates a datatype  named ButtonChoice that allows values of A, B, and C.  The next step is to continuously check the buttons until one is pressed.  For this, you will need a Boolean variable to store whether or not a button has been pressed.  It gets initialized to false, as a button has not been pressed at the beginning.  As usual, I always think it is a good idea to give the user feedback by prompting on the LCD and providing beeps to know if you have pressed a button.

bool buttonPressed = false;       // Boolean whether or not a button has been pressed.
ButtonChoice buttonSelected;      // variable to store which button was pressed.

// Prompt User and Wait for Button.  Record which button is pressed.
  lcd.clear();
  lcd.gotoXY(1, 0);
  lcd.print("Press:");
  lcd.gotoXY(0, 1);
  lcd.print("A  B  C");
  do {
    if (buttonA.isPressed()) {
      buttonSelected = A;
      buttonPressed = true;
      buzzer.playFrequency(300, 200, 15);
    }
    if (buttonB.isPressed()) {
      buttonSelected = B;
      buttonPressed = true;
      buzzer.playFrequency(440, 200, 15);
    }
    if (buttonC.isPressed()) {
      buttonSelected = C;
      buttonPressed = true;
      buzzer.playFrequency(600, 200, 15);
    }
  } while (!buttonPressed); 

At this point, you’ve selected with variant you want to run, you just need to wait till you press the button to start, and run the first move.  for this example, I made the first move options very simple.  If A is pressed, the robot moves forward, if B is pressed, the robot moves backwards, and if C is pressed, the robot turns.  In practice, you probably want something more specific, like targeting a region of the table, moving out of the way, etc.

// Wait for button and execute the selection.
lcd.clear();
lcd.print("Press A");
lcd.gotoXY(0, 1);
lcd.print("to Go!");
buttonA.waitForButton();
buzzer.playFrequency(440, 50, 15);
lcd.clear();

switch (buttonSelected) {
  case A:
    lcd.print("Case A");
    motors.setSpeeds(200, 200);
    delay(1000);
    motors.setSpeeds(0, 0);
    break;
  case B:
    lcd.print("Case B");
    motors.setSpeeds(-200, -200);
    delay(1000);
    motors.setSpeeds(0, 0);
    break;
  case C:
    lcd.print("Case C");
    motors.setSpeeds(-200,200);
    delay(1000);
    motors.setSpeeds(0,0);
    break;
}  

When putting this into your sumo state machine, you will clearly have to add other items such as the initial state to start in, light sensor calibrations, initializations, etc.  Putting the whole example program together:

#include <Zumo32U4.h>
Zumo32U4LCD lcd;
Zumo32U4Motors motors;
Zumo32U4ButtonA buttonA;
Zumo32U4ButtonB buttonB;
Zumo32U4ButtonC buttonC;
Zumo32U4Buzzer buzzer;

enum ButtonChoice       // Create datatype for the button label.
{
  A,
  B,
  C
};

void setup() {
  bool buttonPressed = false;       // Boolean whether or not a button has been pressed.
  ButtonChoice buttonSelected;      // variable to store which button was pressed.

  // Prompt User and Wait for Button.  Record which button is pressed. 
  lcd.clear(); 
  lcd.gotoXY(1, 0); 
  lcd.print("Press:"); 
  lcd.gotoXY(0, 1); 
  lcd.print("A  B  C"); 
  do { 
    if (buttonA.isPressed()) { 
      buttonSelected = A; 
      buttonPressed = true; 
      buzzer.playFrequency(300, 200, 15); 
    } 
    if (buttonB.isPressed()) { 
      buttonSelected = B; 
      buttonPressed = true; 
      buzzer.playFrequency(440, 200, 15); 
    } 
    if (buttonC.isPressed()) { 
      buttonSelected = C; 
      buttonPressed = true; 
      buzzer.playFrequency(600, 200, 15); 
    } 
  } while (!buttonPressed); 
  delay(500);   
  
  // Wait for button and execute the selection. 
  lcd.clear(); 
  lcd.print("Press A"); 
  lcd.gotoXY(0, 1); 
  lcd.print("to Go!"); 
  buttonA.waitForButton(); 
  buzzer.playFrequency(440, 50, 15); 
  lcd.clear(); 
  
  switch (buttonSelected) { 
    case A: 
      lcd.print("Case A"); 
      motors.setSpeeds(200, 200); 
      delay(1000); 
      motors.setSpeeds(0, 0); 
      break; 
    case B: 
      lcd.print("Case B"); 
      motors.setSpeeds(-200, -200); 
      delay(1000); 
      motors.setSpeeds(0, 0); 
      break;     
    case C: 
      lcd.print("Case C"); 
      motors.setSpeeds(-200,200); 
      delay(1000); 
      motors.setSpeeds(0,0); 
      break; 
  } 
} 

void loop() { 
  // Put the rest of the state machine here. 
} 
 In addition to the three options, you could make the menu go deeper into other options.  It might be something like A means go forward, then have another menu for how far forward.  This would essentially allow you to call an audible at the start time to keep your opponents guessing what your robot will do.
Posted in Robotics | Tagged , , | Leave a comment

Read Data from the VL53L0X ToF Sensor Mounted on the Zumo 32U4

In this post, we will write a short program to read the data from the VL53L0X and display the data to the Serial Monitor.  As usual, we will need the Zumo32U4 library, but we will also need the I2C library (Wire.h) and the VL53L0X library (VL53L0X.h).  Wire should be built included with the Arduino distribution.  However, you will probably need to download and install VL53L0X from the Manage Libraries menu.  In addition to including the libraries, we need to create an instance of the time of flight sensor, similar to how we do the same for the Zumo buzzer, buttons, motors, etc.  The code for the libraries and creating an instance of the ToF sensor object are shown below:

#include <Zumo32U4.h>
#include <Wire.h>
#include <VL53L0X.h>

VL53L0X tofsensor;
Zumo32U4ButtonA buttonA;
Zumo32U4Buzzer buzzer; 

Within the setup(), we need to initialize a few things.  The first thing we need to do is to initialize the I2C bus.  Without telling the Zumo that we are going to use the I2C bus, the microcontroller won’t know how to communicate with the sensor.  This is simply done with:

Wire.begin(); 

The next step is to initialize the sensor, and tell the sensor that we want to use it in continuous operation (sensor stays active and doesn’t restart and calibrate every measurement).

tofsensor.init();
tofsensor.startContinuous(); 

At this point, the sensor is ready and we just need to issue the command to take the measurement with:

tofsensor.readRangeContinuousMillimeters(); 

Putting everything together and outputting the data to the serial monitor:

#include <Zumo32U4.h>
#include <Wire.h>
#include <VL53L0X.h>

VL53L0X tofsensor;
Zumo32U4ButtonA buttonA;
Zumo32U4Buzzer buzzer;

void setup()
{
  Serial.begin(115200);
  Wire.begin();
  tofsensor.init();
  buttonA.waitForButton();
  buzzer.playFrequency(440, 200, 15);
  tofsensor.startContinuous();
}

void loop()
{
  int distance;
  distance=tofsensor.readRangeContinuousMillimeters();
  Serial.println(distance);
  delay(20);
} 

Note that this uses the default settings for the ToF sensor.  There are a lot of settings that can be tweaked to either maximize range, accuracy, or measurement time.  In default mode, I get about 800 mm reliably with consistent accuracy and 33 ms read times.  I’ll try to write a post soon regarding the various options you can choose to configure the sensor.

Another item to note is that the datasheet does not specify the beamwidth of the sensor system (combined VCSEL and detection APD).  However, one of the prints shows a 25 degree cone.  This would make it not usable for a robot close to the ground like a Zumo.  I took some measurements and I’m seeing about 3-4 degrees, which is quite narrow.
Posted in Robotics | Tagged , | Leave a comment

Attaching a VL53L0X Breakout Board to Zumo for Accurate Range Finding

The IR sensors on the Zumo 32U4 left us wanting with a desire for a lot more range.  We came across the VL53L0X Time-of-Flight range finding module.  The VL53L0X is capable of up to 2 m range with mm level resolution, and boasts that it is insensitive to ambient lighting conditions, angle of target object, and reflectivity of target object.  Seems too good to be true, so we tried it out.  This post discusses how to hook up a breakout board (manufactured by Pololu) to the Zumo 32U4 and how to take data.

The time of flight sensor uses a VCSEL diode (a type of laser diode) to transmit a pulse of IR light.  The light hits an object, a small portion is reflected back, and the module uses an avalanche photodiode to detect and amplify the pulse.  The module then compares the time difference between when the pulse was sent out and when it was received to determine the range.  It’s a pretty neat module, especially considering that the speed of light is 3e8 m/s, and 1 mm resolution would mean that it can sense time differences in the 1 ps range.

0j7226-600x480

Above is an image from the Pololu website for the module and breakout board (https://www.pololu.com/product/2490).  At about $14, it is hard to beat.  The module uses a 5V I2C protocol, which makes it very easy to interface with the Zumo 32U4.  The image above shows the external connections to the board.  However, all we need to really worry about is Vin, Gnd, SDA, and SCL.

Vin is the input voltage to the module.  This particular board has a voltage regulator that can take a range of input voltages, and regulated it down to the correct level for the ToF module.  Gnd is the ground reference.  SDA and SCL are the I2C pins for Serial Data and Serial Clock.  To go into the workings of I2C is beyond the scope of this post, but all you really need to understand for now is that we need to hook these connections up to the equivalent pins on the Zumo.

zumo-i2c-connections

The above image is taken from the Zumo 32U4 pinout document from the Pololu website (https://www.pololu.com/file/0J864/zumo-32u4-pinout.pdf).  Note that the right side expansion port has ground, 5V, SDA, and SCL thru-holes that we can attached pins to.  How convenient!

img_1698

I soldered some 0.1″ pins into these thru-hole pads.  There is a nice access window in the chassis where the batteries go to allow access to the solder pads from the underside.  I also added some 2-56 stand-offs to allow me to attach a thin plywood platform to mount the sensor to the robot.

img_1699

I then built up a 4-wire harness to connect the Zumo connections to the ToF sensor pins.  You can use 6″ female-to-female jumper wire as well if you don’t want to build a custom wire harness.

img_1701

The picture above shows the ToF sensor mounted to the wood platform with the wire harness attached.  The sensor breakout board has 2 mounting holes where I used 2-56 socket head screws to mount the sensor to the platform.  Note that I could not use a washer as it interferes with some of the components on the board.

The next post will discuss how to write a program to read from the sensor as well as optional methods of configuring the sensor.

Posted in Robotics | Tagged , | Leave a comment

Simplified Sumo State Machine for Zumo

This post will talk about a simple state machine that could be used for robot sumo.  The target here is the Robofest Senior BottleSumo rules, so the state machine is adapted for long searching on large tables.  If you want to do international sumo rules, the state diagram would look a bit different.

A state machine is a device that can be in a specific condition based on previous conditions and the current value of its inputs.  One of the easiest ways to describe the behavior of a state machine is through a state diagram.  A state diagram depicts each state with a circle, then connects the states with arrows to show how the device can transfer between different states, and the conditions required for the device to change states.  You can think of it a bit like a Furby where each behavior the Furby has is a particular state.  There are things you can do to the Furby that will cause it to go from one state to another.

A very basic BottleSumo state diagram is shown below.  Note that this is just the beginning.  Personally, I’d add many other states and transitions between states to optimize performance.

 

simplestatediagram

A description of each state found below:

  • Start – Initialize global variables, initialize sensors, perform a light sensor calibration, etc.  This would be the stuff done in the setup().
  • Initial Move – The BottleSumo rules have a specific start condition.  This state would perform the unknown start condition, and maneuver the robot to a point on the table where it can really begin the state machine sequence.  Since this is performed only once, it also would be in the setup().
  • Scan – Pretty much what it says.  Look around for targets.  If the robot sees a target, transition to Rotate to Target.  If the robot sees nothing, the robot should go for a walk.
  • Rotate to Target – This assumes that a target is in site, this state rotates to face the target.  After this is complete, the robot should charge the target.
  • Charge the Target – Ramming Speed!  You are currently facing the target, now is the time to charge it.  This state drives the robot hard until it sees the edge of the table.
  • Recover from Edge – The only way that the robot enters this state is having driven to an edge of the table.  At this point, you could square to the edge, backup, rotate, then go into Scan again.
  • Wander – Basically just going for a walk around the table.  This diagram assumes you are wandering at a slower speed, and monitoring the downward facing light sensors and proximity sensors.  If you wander to an edge of the table, you Recover from Edge.  If you see a target while wandering, you Rotate to Target.

As I said above, this is really just the beginning.  You can add a lot more complexity as you see fit.  To create a state machine, it is very useful to create a datatype that defines the states.  You could simply assign each state a number and refer to the states as integers.  However, I’m old…. It is much easier to create a datatype that uses the state name.  To do this, you need to use the enum command.

enum State
{
  stateScanning,            // Robot scans from static position.
  stateWander,              // Robot wanders around.  
  stateRotate2Target,       // Assumes target is found, will rotate to largest prox value.
  stateChargeTarget,        // Charges a target.  
  stateEdgeRecovery         // Recovers from finding an edge.
}; 

This creates a new datatype called “State” that can consist of only the values listed.  This allows us to declare a variable of this datatype later that will store the current value of the state.  You can also create variables that can store previous values of the state as well.   Note that I did not include the states for Start and Initial Movement.  These are simple enough and can be handled in the setup() individually.

The next thing we need to do is to create the variable of datatype State, and store the first state of the robot that we want to execute once the loop() begins.  We can do this with the following statement:

State currentState = stateScanning;        // Declare and initialize the state variable of type State 

This command creates a variable named “currentState” that is type State (uppercase s), and we are assigning that variable the value of stateScanning, which is the state we want to be in when we enter loop() for the first time.

You would then write the setup() function, which does all the normal stuff we would do such as initialize sensors, light sensor calibrate, etc, but then we would add the start condition and the first move.  After the setup is complete, we begin the loop().  Each time through the loop, the robot needs to look at the value of the variable state, execute the commands associated with the value, change states, then begin the loop again (which should be under a new state).  This can be done easily with the switch case command.  Details for this particular command can be found at:

https://www.arduino.cc/en/Reference/SwitchCase

Below is an example, with a lot of the functional code for each state removed, for the basic state machine described above.

#include <Zumo32U4.h>
Zumo32U4LCD lcd;
Zumo32U4ProximitySensors proxSensors;
Zumo32U4ButtonA buttonA;
Zumo32U4Motors motors;
Zumo32U4Encoders encoders;
Zumo32U4Buzzer buzzer;
Zumo32U4LineSensors lineSensors;

// setup possible values of the robot states
enum State
{
  stateScanning,            // Robot scans from static position.  
  stateWander,              // Robot wanders around.  
  stateRotate2Target,       // Assumes target is found, will rotate to largest prox value
  stateChargeTarget,        // Charges a target. 
  stateEdgeRecovery         // Recovers from finding an edge.
};

// Global Variables
unsigned int lineSensorValues[3];   // line sensor values.  Array of [left, middle, right]
State currentState = stateScanning;        // Declare and initialize the state variable of type State

void setup() {
  proxSensors.initThreeSensors();
  lineSensors.initThreeSensors();
  calLightSensors(5);
  
  // Wait for Start
  lcd.clear();
  lcd.print("Press A");
  buttonA.waitForButton();
  lcd.clear();
  // Add Code for Start Condition Here
  // Add Code for the First Move Here
}

void loop() {

  switch (currentState) {
    case stateScanning:
      {
        // Add code for the scanning step
        if(ADD CONDITIONAL FOR FINDING A TARGET) {
          currentState=stateRotate2Target;
        } else {
          currentState=stateWander;
        }
      }
      break;

   case stateRotate2Target:
      {
        //Add code for rotating to face a target  
        currentState=stateChargeTarget;
      }
      break;
    case stateChargeTarget:
      {
        // Add code for charging a target
        currentState=stateEdgeRecovery;
      }
      break;

    case stateEdgeRecovery:
      {
        // Add code for recovering from hitting the edge of the table.
        currentState=stateScanning;
      }
      break;

    case stateWander:
      {
        // Add code for wander routine
        if(ADD CONDITION FOR IF THE ROBOT IS AT THE EDGE OF TABLE) {
          currentState=stateEdgeRecovery;
        } else if(ADD CONDITION FOR FINDING A TARGET) {
          currentState=stateRotate2Target;
        } 
      }
      break;
  } // This ends the switch case block
} 

There are a lot of commented out sections that should be filled in for the behavior or each state.  Also, the conditions for some of the state changes are clearly not detailed.  However, this should give you the starting point for putting stuff together.  I would encourage you to make separate test programs for the various states prior to putting them in the full state machine.  This allows you to create and test each state by themselves prior to dealing with transitioning between many states.

Posted in Robotics | Tagged , , | 1 Comment

Zumo 32U4 Unregulated, Unsynchronized Motor Steer Block

This post brings us back to motion and navigation.  One of the goals was to make the Zumo behave a bit more like a LEGO EV3.  The EV3 has these very useful MoveSteer block.  While this seems like a very simplistic block when you use it, there is a lot of information under the hood of the block that the user doesn’t see.  A few highlights of the MoveSteer block are:

  • The block automatically calculates the power distribution to each motor for values of steering between -100 and +100, increments of 1.
  • The block counts the encoder pulses, and automatically determines to use the left encoder when turning right and the right encoder when turning left in order to select the encoder the with most amount of travel (maximizes accuracy).
  • It uses a PID control to regulate the rotational velocity of the under any load within valid range.  This is a very complicated process with a lot of calculations.  Basically, the value that you enter for power is actually proportional to rotational velocity.  The brick dynamically measures the rotational velocity and adjusts the power to each motor to maintain a constant velocity.
  • Wheel synchronization.  The brick calculates the differences in rotation between the two motors and adjusts the power to attempt to keep the motors in synchronization.

The purpose of this post is to look at the first two items in this list.  We will hopefully get to the other two, but that will be the topics of later posts.  First of all, let’s talk about the power distribution to the motors.  After logging the power values to the motors under several conditions, I mapped the relative power to each motor for adjusting the turn value from -100 to 100.  This is shown in the plot below.

motor-power-values

As expected, if the turn value is 0, both motors are at the same power.  If the turn value is -100, the right motor is at the power value entered, and the left motor is at the negative value of the power level entered, and the opposite for the value of 100.  Of course, there are all of the values in between as well.

Another way of saying this is:

  • If the turn value is less than 0, then the right motor is the target speed, and the left motor is the target speed times (turn value + 50)/50, or for those that like it in slope/intercept, it is 1/50*(turn value)+1.
  • If the turn value is 0 or greater, then the left motor is the target speed, and the right motor is the target speed times (turn value-50)/(-50), or -1/50*(turn value)+1.

In Arduino, it is simply:

if(turnDirection>0) {
  targetSpeedLeft=targetSpeed;
  targetSpeedRight=targetSpeed*(turnDirection-50)/(-50.0);
}
else {
  targetSpeedRight=targetSpeed;
  targetSpeedLeft=targetSpeed*(turnDirection+50)/(50.0);
} 

Note that we do not have to actually evaluate if turnDirection<0 as it will be the default case if turnDirection is not greater than 0.   Once the weighting of the speed it done, you can set the motors:

motors.setSpeeds(targetSpeedLeft,targetSpeedRight); 

You can drive the motors based on the encoder counts just like we previously did in the post regarding how to read the encoders. However, they will be different if the robot is turning. Like the EV3, we would like to base the movement on the encoder that moves the most, or the one on the outside of the turn. This is shown below:

if (turnDirection>=0) 
  {
    do {
      countsLeft = encoders.getCountsLeft();
    } while (abs(countsLeft)<abs(enCounts));
  } else {
    do {
      countsRight = encoders.getCountsRight();      
    } while (abs(countsRight)<abs(enCounts));
  } 

The complete function for driving a specific number of encoder pulses for a specific turn direction is given below:

void unRegMoveSteer(int targetSpeed, int turnDirection, int enCounts) {
  int countsLeft; 
  int countsRight;
  int targetSpeedLeft;
  int targetSpeedRight;
 
  if(turnDirection>0) {
    targetSpeedLeft=targetSpeed;
    targetSpeedRight=targetSpeed*(turnDirection-50)/(-50.0);
  }
  else {
    targetSpeedRight=targetSpeed;
    targetSpeedLeft=targetSpeed*(turnDirection+50)/(50.0);
  }
  motors.setSpeeds(targetSpeedLeft,targetSpeedRight);
  if (turnDirection>=0) 
  {
    do {
      countsLeft = encoders.getCountsLeft();
    } while (abs(countsLeft)<abs(enCounts));
  } else {
    do {
      countsRight = encoders.getCountsRight();      
    } while (abs(countsRight)<abs(enCounts));
  }    
} 

You can use the function in the program as shown below:

#include <Zumo32U4.h> 

Zumo32U4Encoders encoders;
Zumo32U4Motors motors;
Zumo32U4ButtonA buttonA;
Zumo32U4Buzzer buzzer;

void setup() {
}

void loop() {
  
  motors.setSpeeds(0,0);
  buttonA.waitForButton();   
  buzzer.playFrequency(440, 200, 15);
  delay(200);

  unRegMoveSteer(150, 0, 1000);
  motors.setSpeeds(0,0); 
} 

This will make the robot move forward at a speed of 150, turn direction of 0 (straight forward) for 1000 encoder pulses.  You can try other values and make sure that the function works under all conditions.  Note that negative encoder counts won’t actually work as we use the absolute values as the end condition for the movement.

 

 

Posted in Robotics | Tagged , , | Leave a comment

Zumo 32U4 Sensor Read Times

Last post had the Zumo rotate and face a target.  I’ve been working on driving towards the target in an attempt to push it off of a table like done in Sumo competitions with great success with the rare occasion that the Zumo thinks it can fly and goes right off the table.  While driving towards the target, the program performs light sensor reads and proximity sensor reads as fast as possible while driving.  I know that it won’t fall off the table with only the light sensor reads based on a previous post.  Therefore, it is hypothesized that the proximity sensor reads are fairly long operations.  This post tests that theory out.

The program below uses the Zumo timer, millis(), to time how long it takes to perform 1000 light sensor reads, and 1000 proximity sensor reads.  The time for each read is calculated by simply dividing that amount of time by 1000, and displays on the Serial Monitor.

#include <Zumo32U4.h>

Zumo32U4ProximitySensors proxSensors;
Zumo32U4ButtonA buttonA;
Zumo32U4Motors motors;
Zumo32U4Buzzer buzzer;
Zumo32U4LineSensors lineSensors;

unsigned int lineSensorValues[3];

void setup() {
  proxSensors.initThreeSensors();
  lineSensors.initThreeSensors();
  Serial.begin(115200);
}

void loop() {
  unsigned long startTime;
  unsigned long runTime;
  Serial.println("Light Sensor 1k Test");
  startTime=millis();
  for(int i=1;i<1000;i++) {
    lineSensors.read(lineSensorValues);
  }
  runTime=millis()-startTime;
  Serial.print("Time per Read: ");
  Serial.print(runTime/1000.0);
  Serial.println(" ms");

  Serial.println("Proximity Sensor 1k Test");
  startTime=millis();
  for(int i=1;i<1000;i++) {
    proxSensors.read();
  }
  runTime=millis()-startTime;
  Serial.print("Time per Read: ");
  Serial.print(runTime/1000.0);
  Serial.println(" ms");

  buttonA.waitForButton();
  buzzer.playFrequency(440, 200, 15);
} 

When I first ran this, I thought that the proximity sensor read section hung up.  It took a little over 13 s to run 1k proximity sensor reads!

Here is my output on the Serial Monitor:

Light Sensor 1k Test
Time per Read: 2.53 ms
Proximity Sensor 1k Test
Time per Read: 13.03 ms 

I was a little surprised at the light sensor reads as analog reads on an Arduino are in the order of 0.2 ms.  However, it is attempting to read three sensor and may perform some averaging.  2.5 ms is not a problem given the speed of the robot.  However, 13 ms for a proximity sensor read is way too long.  At a motor power of 400, the robot moves at 5000 encoder counts per second for a 75:1 gear ratio.  There are 909.7 counts per 360 degree of wheel rotation making the rotational velocity 1980 degrees per second.  The wheel and track is 38 mm in diameter.  That means for every 360 of rotation, the robot moves almost 120 mm.  That puts the robot velocity at 660 mm/s!  If it takes 2.53 ms to take a light sensor reading, then the robot could travel 1.7 mm during the measurement.  However, if the robot is trying to take both a light sensor array measurement and a proximity sensor measurement, the robot could move an additional 11 mm before the robot could possibly respond to the sensor input.  Add in the fact that the robot cannot instantly stop, but needs some time to turn off the motor power and let the momentum of the robot slow down, and you could potentially be off of the table!  So…. when charging an object to push it off of the table, we may want to not reading the proximity sensors, or find another way to sensor distance.

Posted in Robotics | Tagged , , , | Leave a comment

Zumo 32U4 – Using the Proximity Sensors to Point Towards the Closest Object

Assuming you want to use the cute little Zumo for a Robot Sumo competition, you need to make the Zumo turn in the direction of the highest proximity.  This may be a competing robot, or in the case of Robofest BottleSumo, it may be a 2L bottle.  Either way, the faster you can identify and turn towards the closest object, the faster you can do something about it.  In this post, I discuss a simple way to turn to point to the closest object within the range of the proximity sensors.  There are lots of ways to do this, including the example that comes with the Zumo library.  The example is very elegant, but slightly abstract for my target audience of these blog posts.  For this post, I’ll take a very simple procedural approach.

The flow is:

  • If the left proximity sensor is the strongest, rotate counter-clockwise until the front sensor with the left LED is the strongest.
  • Else if the right proximity sensor is the strongest, rotate clockwise until the front sensor with the right LED is the strongest.
  • At this point, we know that the left or right sensor are lower than the front sensor with either LED.  Therefore, if the front with the left LED is stronger, rotate counter clockwise until the front has the same returned signal for the left and right LED’s.  Similarly, if the front with the right is stronger, rotate clockwise until the front returns the same level with both the left and the right LED’s.

The easiest way to define the last step is to use a variable named “error” which is the difference of the front with right LED minus the front with left LED.  The function for this is shown below:

void turnToStrongestProx() {
  int left,frontLeft,frontRight,right;
  int error;

  proxSensors.read();
  displayProximity();
  left=proxSensors.countsLeftWithLeftLeds();
  frontLeft=proxSensors.countsFrontWithLeftLeds();
  frontRight=proxSensors.countsFrontWithRightLeds();
  right=proxSensors.countsRightWithRightLeds();
  if(left>frontLeft&&left>frontRight&&left>=right) {
    motors.setSpeeds(-TURN_SPEED,TURN_SPEED);
    do {
      proxSensors.read();
      displayProximity();
    } while (proxSensors.countsFrontWithLeftLeds()<left);
  }  else if(right>frontLeft&&right>frontRight&&right>left) {
    motors.setSpeeds(TURN_SPEED,-TURN_SPEED);
    do {
      proxSensors.read();
      displayProximity();
    } while (proxSensors.countsFrontWithRightLeds()<right);
  }
  do {
    proxSensors.read();
    displayProximity();
    error=proxSensors.countsFrontWithRightLeds()-proxSensors.countsFrontWithLeftLeds();
    if(error>0){
      motors.setSpeeds(TURN_SPEED,-TURN_SPEED);
    } else if(error<0) {
      motors.setSpeeds(-TURN_SPEED,TURN_SPEED);
    }
  } while (error!=0);
  buzzer.playFrequency(440, 200, 15);
  motors.setSpeeds(0,0);
} 

This isn’t the most elegant way of achieving this, but it gets the job done.  If there is no signal, the robot does nothing.  This function uses a constant that needs to be defined in the main program by

#define TURN_SPEED 200

You can adjust the value of TURN_SPEED to change the speed of rotation.  The function also calls a function named displayProximity().  This function simply displays the values of the proximity sensor on the LCD.

void displayProximity() {
  lcd.clear();
  lcd.gotoXY(0,1);
  lcd.print(proxSensors.countsLeftWithLeftLeds() );
  lcd.gotoXY(2,0);
  lcd.print(proxSensors.countsFrontWithLeftLeds() );
  lcd.gotoXY(5,0);
  lcd.print(proxSensors.countsFrontWithRightLeds() );
  lcd.gotoXY(7,1);
  lcd.print(proxSensors.countsRightWithRightLeds() );
} 

The rest of the program (minus the two functions described above) is:

#include <Zumo32U4.h>

Zumo32U4LCD lcd;
Zumo32U4ProximitySensors proxSensors;
Zumo32U4ButtonA buttonA;
Zumo32U4Motors motors;
Zumo32U4Buzzer buzzer;

#define TURN_SPEED 200

void setup() {
  proxSensors.initThreeSensors();
}

void loop() {
  lcd.clear();
  lcd.print("Press A");
  buttonA.waitForButton();
  lcd.clear();
  turnToStrongestProx();
}
Posted in Robotics | Tagged , , | 2 Comments

Zumo 32U4 – Squaring to an Edge or Line

This post talks about a common step for any robotics competition, namely, squaring the robot to a line or an edge.  This builds off of previous post where you worked on having the robot drive to an edge.  The program for the driving to an edge can be found at:

Zumo 32U4 Drive to Edge

For this step, we are just going to build off of that program and I assume that you already have that sketch completed.

There are many ways to square to a line.  You could drive the left side until the light sensor sees the line, then drive the right side.  However, unless the sensors are in line with the wheel base, this results in an error that requires multiple passes.  I like the use the proportional method.  The PID method is even better, but adds considerable complexity.  For this case, we will consider only the proportional.  A proportional squaring method is very similar to a proportional line follow that has been written about quite a bit for any line-following tutorial.  I’m writing this assuming that you understand the principles of proportional feedback.

In this case, we need two proportional feedback paths, one for the left motor and left light sensor, and one for the right motor and right light sensor.  As with a line follow, we calculate the error for each side, multiply each side by a gain to create a left and a right correction factor, then apply the correction factor to the motor power.  Unlike a line follower where we are adjusting the steering, we need to adjust the power of each motor so that the light sensors reach the edge of the table.  The pseudo-code looks something like this:

  1. Read the value of the left and right light sensors.
  2. Calculate the error of the left and right light sensors.
  3. Calculate the correction factor of the left and right (correction is error times gain).
  4. Apply the correction factor to the motor power.
  5. Repeat as long as the left and right errors are larger than a certain tolerance.

There really is nothing difficult about this with respect to the code and there really are no new concepts.  Let’s create a function called square2line() that is type void with an input parameter of the tolerance (which should be an integer).  The tolerance is a way of calculating how close to square is close enough.  We will need some local variables, errorLeft, errorRight, corrLeft, and corrRight where corr is short for correction.  Everything else just builds off what we’ve already covered and is placed inside a do… while loop.  The condition to keep running the loop is if either |errorLeft|> tolerance or |errorRight|>tolerance.  Otherwise, the loop should end and the motors stopped.

void square2line(int tol) {
  int errorLeft, errorRight,corrLeft, corrRight;

  do {
    lineSensors.readCalibrated(lineSensorValues);
    errorLeft=lineSensorValues[0]-500;
    errorRight=lineSensorValues[2]-500;
    corrLeft=-errorLeft*KPsq;
    corrRight=-errorRight*KPsq;
    motors.setLeftSpeed(corrLeft);
    motors.setRightSpeed(corrRight);
  } while (abs(errorLeft)>tol||abs(errorRight)>tol);
  motors.setSpeeds(0,0);
} 

The value of the gain, KPsq, is not defined within this function.  It is a constant that is defined at the very beginning of the program.  Constants can be defined with a compiler directive called #define.  This tells the compile to define one term to be something else.  At the beginning of the program, I have:

#define KPsq 0.6

Which tells the compiler to use 0.6 everywhere I have KPsq in the program.  You could simply just use 0.6 directly in the program, but this is a value that should be tweaked, adjusted, optimized, and played with until perfect.  Putting it at the top of the program makes it easy to do this.

Here is the entire program:

#include <Zumo32U4.h>
Zumo32U4LCD lcd;
Zumo32U4ButtonA buttonA;
Zumo32U4Motors motors;
Zumo32U4LineSensors lineSensors;
Zumo32U4Buzzer buzzer;

#define FORWARD_SPEED 400
#define EDGE_THRESHOLD 500
#define EDGE_TOLERANCE 50
#define KPsq 0.6

unsigned int lineSensorValues[3];

void setup() {
  lineSensors.initThreeSensors();
  calLightSensors(5);
}

void loop() {
  lcd.print("Press A");
  motors.setSpeeds(0, 0);
  buttonA.waitForButton();
  lcd.clear();
  delay(200);

  drive2line(FORWARD_SPEED);
  square2line(EDGE_TOLERANCE);
}

void calLightSensors(int calTime) {
  lcd.clear();
  lcd.gotoXY(0,0);
  lcd.print("Press A");
  lcd.gotoXY(0,1);
  lcd.print("to Cal");
  buttonA.waitForButton();
  buzzer.playFrequency(440, 200, 15);
  lcd.clear();
  lcd.print("Cal\'ing");
  for (int i = 20*calTime; i > 0; i--) {
    lcd.gotoXY(0, 1);
    if(i%20==0) {
      lcd.print(i/20);
      lcd.print(" ");
      buzzer.playFrequency(440, 50, 15);
    }
    lineSensors.calibrate();
    delay(20);
  }
  buzzer.playFrequency(600, 200, 15);
  lcd.clear();
  lcd.print("Cal Done");
  delay(1000);
}

void drive2line(int motorSpeed) {
  motors.setSpeeds(motorSpeed,motorSpeed);
  do {
    lineSensors.readCalibrated(lineSensorValues);
  } while (lineSensorValues[0]<EDGE_THRESHOLD && lineSensorValues[2]<EDGE_THRESHOLD);
  motors.setSpeeds(0,0);
}

void square2line(int tol) {
  int errorLeft, errorRight, corrLeft, corrRight;

  do {
    lineSensors.readCalibrated(lineSensorValues);
    errorLeft=lineSensorValues[0]-500;
    errorRight=lineSensorValues[2]-500;
    corrLeft=-errorLeft*KPsq;
    corrRight=-errorRight*KPsq;
    motors.setLeftSpeed(corrLeft);
    motors.setRightSpeed(corrRight);
  } while (abs(errorLeft)>tol||abs(errorRight)>tol);
  motors.setSpeeds(0,0);
}  

I also used a few other values as constants in order to make tweaking the program simpler.  There is a little bit of overshoot in the squaring to the line that is easily fixed with a PID algorithm, which will be the topic for a later post.  If everything is done correctly, the robot should go through a light sensor calibration, pause for the A button to be pressed, drive the an edge of the table, square to the edge, then pause until the button is pressed again.

Posted in Robotics | Tagged , , | Leave a comment

Display of the Zumo IR and Light Sensors on the Serial Monitor

In this post, we are going to measure both the downward facing light sensors and the proximity sensors, and display their data.  The previous several posts discussed the reading of the light and proximity sensors, so we will not spend any time discussing those aspects of the task.  However, the small LCD display does not have enough characters available to display the 7 values of interest.  Note that there is a very good example program in the Zumo Examples that covers how to do this using custom characters to create a very nice bar chart on the LCD.  I found that trying to explain this to new programmers was more difficult that it was worth.  Fundamentally, we are just interested in reading the values so we can set some thresholds within a larger program to use these sensors as tools, and we are not too concerned with making nice bar charts on the display.

In order to display all of the data, we are going to use the Arduino Serial Monitor.  In the upper right corner of the Arduino IDE, there is an icon that looks like a magnifying glass.  This is the serial monitor.  It is an interface that allows us to receive data from the Arduino directly to our computer over a USB connection.  Note that for this to work, we need to keep the USB cable plugged in to receive the data.

In order to use the Serial Monitor, you first need to set up the interface.  Serial simply means that the information is transmitted bit-by-bit over the USB.  In Arduino, we need to specify how fast the bits are coming to the computer.  You would do that with the following command placed in the Setup() function as this command is run only once.

Serial.begin(115200); 

What this command does is that it simply initialized the serial interface, and says that we want to transmit the data from the Arduino at a rate of 115,200 bits per second.  Each character is approximately 8 bits (1 byte), so the characters and data is flying by pretty fast.  Once the program runs, the Arduino doesn’t normally wait for the user to open the Serial Monitor.  Personally, I like to have the monitor open prior to the robot sending me information so that I don’t miss anything that is important.  The way to force the program to pause and wait for the Serial Monitor to be opened is with the following command:

while(!Serial); 

This is short for saying, “do nothing while there is no serial communication.”  When sending information to the Serial Monitor, it can be done with the Serial.print() function.  This is very similar to the lcd.print() functions that we have been using to write to the LCD.  Like with the LCD, the Serial.print() does not send a carriage return at the end to specify that we are starting a new line.  To force a print with a new line, you would use the Serial.println() function.  println means “Print Line” which does force a carriage return at the end.  If we want the data to line up in a nice set of columns, we also want to use “\t” which is the format code for Tab.

The complete program is below:

#include <Zumo32U4.h>
Zumo32U4LineSensors lineSensors;
Zumo32U4ProximitySensors proxSensors;
Zumo32U4LCD lcd;
Zumo32U4ButtonA buttonA;
Zumo32U4Buzzer buzzer;

unsigned int lineSensorValues[3];

void setup() {
  lineSensors.initThreeSensors();
  proxSensors.initThreeSensors();
  calLightSensors(5);
  Serial.begin(115200);
  lcd.clear();
  lcd.gotoXY(0,0);
  lcd.print("Connect");
  lcd.gotoXY(0,1);
  lcd.print("Serial");
  // Wait Until Serial Connection Open
  while(!Serial);
  lcd.clear();
  Serial.println("Begin Serial Communication");
  delay(500);
  lcd.clear();
  lcd.gotoXY(0,0);
  lcd.print("Taking");
  lcd.gotoXY(0,1);
  lcd.print("Data");
}

void loop() {
  lineSensors.readCalibrated(lineSensorValues);
  proxSensors.read();
  displaySerial();
  delay(50);
}

void calLightSensors(int calTime) {
  lcd.clear();
  lcd.gotoXY(0,0);
  lcd.print("Press A");
  lcd.gotoXY(0,1);
  lcd.print("to Cal");
  buttonA.waitForButton();
  buzzer.playFrequency(440, 200, 15);
  lcd.clear();
  lcd.print("Cal\'ing");
  for (int i = 20*calTime; i > 0; i--)
  {
    lcd.gotoXY(0, 1);
    if(i%20==0) {
      lcd.print(i/20);
      lcd.print(" ");
      buzzer.playFrequency(440, 50, 15);
    }
    lineSensors.calibrate();
    delay(20);
  }
buzzer.playFrequency(600, 200, 15);
lcd.clear();
lcd.print("Cal Done");
delay(1000);
}

void displaySerial() {
  Serial.print("ProxSensors:\t");
  Serial.print(proxSensors.countsLeftWithLeftLeds() );
  Serial.print("\t");
  Serial.print(proxSensors.countsFrontWithLeftLeds() );
  Serial.print("\t");
  Serial.print(proxSensors.countsFrontWithRightLeds() );
  Serial.print("\t");
  Serial.print(proxSensors.countsRightWithRightLeds() );
  Serial.print("\tLineSensors:\t");
  Serial.print(lineSensorValues[0]);
  Serial.print("\t");
  Serial.print(lineSensorValues[1]);
  Serial.print("\t");
  Serial.println(lineSensorValues[2]);
} 

Like with so many things, there are shorter ways to display the data on the serial monitoring. However, this requires a better understanding of formatted string output. If I get time, I’ll have a post on this later.

Posted in Robotics | Tagged , , | Leave a comment

Reading the Zumo 32U4 Proximity Sensors

There are three IR proximity sensors on the Zumo.  All of them are on the lower sensor board that contains the downward facing light sensors.  In addition to the sensors, there are four IR emitters.  There are two front IR emitters that are in standard thru-hole LED packages on the front of the robot at the top of the metal blade.  There are also two side facing surface mount emitters that are on the sides where the circuit board extends through the tracks.  The left front and the left side emitters operate in parallel, and the right front and right side operating in parallel.

When you issue the command to read the sensors with the command:

proxSensors.read(); 

The robot sends our 6 IR pulses on the left side, and 6 pulses on the right side.  The pulses are at 38 kHz, which is also a standard frequency for digital pulses for IR remote control applications like TV remote controls and IR headphones.  So, there is always a potential for interference.  Each of the six pulses that are transmitted are at a different amplitude level.  After the IR pulses are emitted, then may find an object to reflect off of and get received by the IR receivers of the robot.  As the pulses travel, their amplitude drops off rather quickly (1/r^2 for each way).  The further away an object is, the lower the amplitude of the received pulse.  The IR receiver simply counts the number of pulses that it can measure.  Let’s say an object is a medium distance away.  The very low amplitude pulses can not be received with sufficient amplitude.  However, medium and high power pulses would be detected, so the number of pulses would be somewhere from 3-4.  If the object is very far away, maybe only the highest power pulse would be received, and the number of pulses would be 1 (or 0 if no pulses were received).

The proximity sensor read command takes care of sending the 6 pulses on both the left and the right, as well as measuring the 3 IR sensors for both the left and right sides.  However, we aren’t terribly interested in the left sensor receiving from the right side or the right sensor receiving from the left side.  The following functions return integers ranging from 0 to 6 depending on the amount of IR light that is reflected back.

proxSensors.countsLeftWithLeftLeds()
proxSensors.countsLeftWithRightLeds()
proxSensors.countsFrontWithLeftLeds()
proxSensors.countsFrontWithRightLeds()
proxSensors.countsRightWithLeftLeds()
proxSensors.countsRightWithRightLeds()

As previously discussed, reading one side with the opposite side LED doesn’t really provide us with a lot of information.  However, the front sensor with the left LED vs the right LED does give us some useful information regarding the direction an object may be.

Our goal of this post is simply to read the proximity sensors and display the values on the LCD.  As usual, we need to include the Zumo32U4 library, rename the LCD and Proximity Sensor classes to something more usable, initialize the proximity sensors, read the values, and write the values to the display.  The only item that is really new is how to read the sensors, and how to get the values.  It should be fairly straight forward from the example program below.

#include <Zumo32U4.h>
Zumo32U4LCD lcd;
Zumo32U4ProximitySensors proxSensors;

void setup() {
  proxSensors.initThreeSensors();
}

void loop() {
  proxSensors.read();
  lcd.clear();
  lcd.gotoXY(0,1);
  lcd.print(proxSensors.countsLeftWithLeftLeds() );
  lcd.gotoXY(2,0);
  lcd.print(proxSensors.countsFrontWithLeftLeds() );
  lcd.gotoXY(5,0);
  lcd.print(proxSensors.countsFrontWithRightLeds() );
  lcd.gotoXY(7,1);
  lcd.print(proxSensors.countsRightWithRightLeds() );
  delay(100);
}
Posted in Robotics | Tagged , , | Leave a comment

Zumo 32U4 Drive to Edge

This post discusses a common issue with any sumo event or competition that involves navigating to a line.  Our goal with this post is simply to drive the robot placed on a light colored surface until it reaches a dark color.  In the case of my office, I have a light colored desk, and I want to drive it to the edge of the table and stop (preferably without going over the edge!).

This uses the light sensor calibration function from the previous post, so if you don’t have that, you would want to go back and look at it.  My main program looks like this:

#include <Zumo32U4.h>

Zumo32U4LCD lcd;
Zumo32U4ButtonA buttonA;
Zumo32U4Motors motors;
Zumo32U4LineSensors lineSensors;
Zumo32U4Buzzer buzzer;

#define FORWARD_SPEED 200
#define EDGE_THRESHOLD 500

unsigned int lineSensorValues[3];

void setup() {
  lineSensors.initThreeSensors();
  calLightSensors(10);
}

void loop() {
  lcd.print("Press A");
  motors.setSpeeds(0, 0);
  buttonA.waitForButton();
  lcd.clear();
  delay(200);
  drive2line(FORWARD_SPEED);
} 

The #define statements at the beginning define constants used in the program.  If there is a constant that you use through the program, it often is best to define it at the beginning so you only need to change it in one place if you every want to adjust it.

The function that is new in the program is drive2line(speed), which we need to create.  The basic flow of this is to turn the motors on, monitor the light sensors (take repeated readings) until either light sensor sees a value greater than EDGE_THRESHOLD, then turn the motors off.  Setting the speed and turning the speed to 0 is easy, and we did that in previous posts.  Reading the light sensors is also something that we’ve already done.  We’ve also already done something very similar by reading the encoders until they reach a certain value.  All we need to do is to put together what we already know.

We will be reading the light sensors inside a do… while statement.  The condition to continue the loop will simply be that both of the outer light sensors see a light colored surface (low value on reflected light).  We want to stay in the loop while the left sensor sees white and the right sensor sees white.  If either one sees a dark color, we want to exit and turn the motors off.  This can be accomplished with the following:

void drive2line(int motorSpeed) {
  motors.setSpeeds(motorSpeed,motorSpeed);
  do {
    lineSensors.readCalibrated(lineSensorValues);
  } while (lineSensorValues[0]<EDGE_THRESHOLD && lineSensorValues[2]<EDGE_THRESHOLD);
  motors.setSpeeds(0,0);
} 

Remember, the && symbol is a logical AND operation.  Assuming that you add the drive2line(speed) function, and copy the light sensor calibration function from the previous post, the robot should run the light sensor cal, then wait for the user to press the button, drive at a speed of 200 until it sees the edge of the table (or a black line), stop, then wait for the button and repeat.  Try increasing the speed and have some fun.  Just always be ready to catch the robot so it doesn’t go over the edge.

Posted in Robotics | Tagged , | Leave a comment

Zumo 32U4 Downward Light Sensors, Part 2 – Calibration

This post will discuss how to calibrate the downward facing light sensors, and to read the calibrated values.  I went a little over the top on the calibration function, but it makes it that much nicer.  You can certainly do something a little less fancy and simplify, but I like things looking nice.
For the calibration, I added some button functions and the buzzer.  I’ll have a post later discussing more regarding the buttons and buzzer, but for now, you just need to know a couple of things.  First of all, button A and the buzzer have their own class, and we should rename it like we did with the motors, LCD, and line sensor array.  We also need the Zumo32U4 library file.  The beginning of the program should look like this:
#include <Zumo32U4.h>
Zumo32U4LineSensors lineSensors; 
Zumo32U4LCD lcd; 
Zumo32U4ButtonA buttonA; 
Zumo32U4Buzzer buzzer;
We also need to declare our 3 element array for the light sensors.  We will use it in multiple spots in the program without passing parameters, so it should be a global variable.  Therefore, let’s put it next:
unsigned int lineSensorValues[3];
Before we get to the rest of the program, let’s talk a little bit about functions, and the purpose of the setup() and loop() portion of the program.  The setup() and   loop() are meant to be high level portions of the program, and easily readable without distraction.  We’ve been using basic commands in these portions of the program in previous posts simply because the programs have been very simple.  In more complicated programs, these sections should be very short with high level function calls.  All of the nitty-gritty details should be done in various functions.  My rule of thumb is that if the setup and main cannot be basically understood by someone not familiar with the program, it should be simplified.  Therefore, we should probably write a separate function for the light sensor calibration.
The light sensor calibration function lineSensors.calibrate() takes a light sensor reading on all active light sensors and stores it.  It doesn’t matter if it is high reflectivity, low reflectivity, or anywhere in between.  You can run this function many times.  Each time you run it, it looks to see if the new value is larger or smaller than any of the previous values.  Unlike the EV3, where you need to specify if you are measuring a minimum or maximum, this function takes care of it for you.  As a result, you could continuously run the calibration for a period of time, and scan the robot across an area that has a wide range of reflectance.  Below is my version of the light sensor calibration function:
void calLightSensors(int CalTime) {
  lcd.clear();
  lcd.gotoXY(0,0);
  lcd.print("Press A");
  lcd.gotoXY(0,1);
  lcd.print("to Cal");
  buttonA.waitForButton();
  buzzer.playFrequency(440, 200, 15);
  lcd.clear();
  lcd.print("Cal\'ing");
  for (int i = 20*CalTime; i > 0; i--) {
    lcd.gotoXY(0, 1);
    if(i%20==0) {
      lcd.print(i/20);
      lcd.print(" ");
      buzzer.playFrequency(440, 50, 15);
    }
    lineSensors.calibrate();
    delay(20);
  }
  buzzer.playFrequency(600, 200, 15);
  lcd.clear();
  lcd.print("Cal Done");
  delay(1000);
}
The overall flow of the function is pretty simple, but there is a lot added to make it more user friendly.  The basic flow is
  1. Wait for the button to be pressed.
  2. Run the loop a lot of times.
    1. As the loop is running, display approximate time left during the calibration.
    2. Run a cal each time through the loop.
  3. End and let the user know the cal is done.

Starting at the beginning, the function name is calLightSensors and it does not return a value, which is why the function is defined as void.  The function accepts one input parameter, which is an integer.  The input parameter is the amount of time you want to give the function to light cal.  In some cases, you want the cal to go quick, other times, you may want a lot of time to push the Zumo around the table to profile as much of the different lighting conditions possible.

The next step is to display a message to the user to press the A button to begin the calibration, then the program runs the command buttonA.waitForButton().  This function does exactly what it says.  It pauses the program until button A is pressed.  Whenever pressing a button, I like to have a little feedback, usually in the form of a beep, to tell me that the button was pressed.  The function

buzzer.playFrequency(440, 200, 15); 

plays a tone at 440 Hz for 200 ms at a volume level of 15 (15 is the highest).  I’ll get into more functions of the buzzer in a later post, but for the time being, all I want to do is play a beep, and this accomplishes it.

The next section is a for loop.  A for loop is a type of loop that is used for running a section of code repeatedly for a specific number of times.  The syntax for a for loop is given below:

for (initialization; condition; increment) {
  //statement(s);
}

The very first time through the for loop, it initializes a counter.  The loop runs until the counter reaches a certain condition, and finally, the counter is either incremented or decremented.  Assuming that the counter meets the condition, everything with the { } is executed.  For my case, the for loop reads:

for (int i = 20*CalTime; i > 0; i--) {
  // statements to be executed
} 

For the initialization, the statement int i = 20*CalTime says that I’m initializing integer i to be 20 times the amount of time I’d like to calibration for.  Remember that CalTime was an input parameter to this function.  If we assume 10 s, then we are initializing i to be 200.  The condition here is i>0.  That is, we want to execute the contents of this for loop as long as i is greater than 0.  The increment is i--, which is shorthand for i=i-1.  Assuming that CalTime is 10, this loop would execute 200 times as i changes from 200 to 0, decrementing by a value of 1 each time through the loop.

We want to run a calibration each time through the loop, and have a slight delay.  We could simply run the loop as:

for (int i = 20*CalTime; i > 0; i--) {
  lineSensors.calibrate();
  delay(20);
} 

However, this provide no information to the user about how much time they have left on the calibration.  By the way, the delay time was adjusted to provide approximately the correct amount of time for CalTime to work out to seconds.  The calibration function takes a little time to run, and I knew I wanted about 20 calibration readings per second.  It took a little trial and error and is not exact.  What I’d like to have now is a countdown so I know how much time I have left.  The easiest way to do that is to use the variable i.  If I printed the value of i directly on the display, it would count down from 200 to 0 changing values about 20 times a second (50 ms).  That isn’t terribly useful.  I’d like to display in seconds.  Every time i becomes a multiple of 20, it would represent a 1 second increment in time.  If you take i and divide by 20, you can tell if i is a multiple of 20 if the remainder is 0.  Arduino has an arithmetic operation called the modulo (or modulus), which displays the remainder of a division operation.  The modulo operator is the percent sign (%).  i%20 would give the remainder of i divided by 20.  If this value is 0, then we know we are at a multiple of 20, and we are at a 1 second increment.

if(i%20==0) {
  lcd.print(i/20);
  lcd.print(" ");
  buzzer.playFrequency(440, 50, 15);
} 

The statement above says that if the modulus of i/20 is 0 (the == is the logical statement for equal to – note that this is very different to a single equal sign which means we are assigning a value), the we execute the statements within the { }.  In this case, we are writing the number of seconds (i/20) to the display and playing a beep.  Why do you think writing the ” ” (writing a space) is necessary?

After the calibration and countdown, the function simply tells the user that the calibration is done.

Now that the function calLightSensors() is written, it can be called in the setup() part of the program as we only want to execute this once.

In order to read the calibrated values of the light sensors, you can use the following command:

lineSensors.readCalibrated(lineSensorValues); 

Displaying the values to the LCD is identical to the previous blog posting.  However, to do this a more correct way, we should really make the display commands into a function called displayLCD().  The complete version of the program is below:

#include <Zumo32U4.h>
Zumo32U4LineSensors lineSensors;
Zumo32U4LCD lcd;
Zumo32U4ButtonA buttonA;
Zumo32U4Buzzer buzzer;
unsigned int lineSensorValues[3];

void setup() { 
  lineSensors.initThreeSensors(); 
  calLightSensors(10); 
  lcd.clear(); 
  lcd.gotoXY(0,0); 
  lcd.print("Press A"); 
  lcd.gotoXY(0,1); 
  lcd.print("to Cont."); 
  buttonA.waitForButton();   
  buzzer.playFrequency(440, 200, 15); 
  lcd.clear(); 
} 

void loop() { 
  lineSensors.readCalibrated(lineSensorValues); 
  displayLCD(); 
  delay(50); 
}

void calLightSensors(int CalTime) { 
  lcd.clear(); 
  lcd.gotoXY(0,0); 
  lcd.print("Press A"); 
  lcd.gotoXY(0,1); 
  lcd.print("to Cal"); 
  buttonA.waitForButton(); 
  buzzer.playFrequency(440, 200, 15); 
  lcd.clear(); 
  lcd.print("Cal\'ing"); 
  for (int i = 20*CalTime; i > 0; i--) { 
    lcd.gotoXY(0, 1); 
    if(i%20==0) { 
      lcd.print(i/20); 
      lcd.print(" "); 
      buzzer.playFrequency(440, 50, 15); 
    } 
    lineSensors.calibrate(); 
    delay(20); 
  } 
  buzzer.playFrequency(600, 200, 15); 
  lcd.clear(); 
  lcd.print("Cal Done"); 
  delay(1000); 
} 

void displayLCD () { 
  lcd.clear(); 
  lcd.gotoXY(2, 0); 
  lcd.print(lineSensorValues[1]); 
  lcd.gotoXY(0, 1); 
  lcd.print(lineSensorValues[0]); 
  lcd.gotoXY(4, 1); 
  lcd.print(lineSensorValues[2]); 
} 

Note that the functions can simply be placed after the loop() function.  You can actually place the functions anywhere you want, but it is typical to have setup() and loop() first, then any functions after that.  Another option, which is what I typically do, is to place groups of functions as separate tabs in the Arduino IDE.  If you run this program, and move the robot around to a full range of reflectance values during the calibration period including the minimum reflectance and the maximum reflectance, you should have displayed values ranging between 0 and 1000 with 0 being the most reflective and 1000 being the least reflective.  All three light sensors should also be more equal if they are on an object of similar reflectivity from one another.

The light sensor calibration stores the calibration factors in what’s called volatile memory.  That is, when the robot is turned off or reset, it forgets the calibration.  Therefore, if you want calibrated values, you need to calibrate once every time the robot is turned on.

Posted in Robotics | Tagged , | Leave a comment

Reading the Zumo 32U4 Downward Facing Light Sensors, Part 1

The Zumo 32U4 has 5 downward facing light sensors on the front light sensor array.  The light sensor array board also has 3 IR sensors used for proximity sensors or IR receivers for remote control.  However, two of the light sensors share data sensor lines with two of the IR proximity sensors.  For the time being, we are only going to look at using 3 of the 5 downward facing light sensors.  We will discuss more about how to switch to 5 sensors later when we talk about line following.

As usual, we need to include the library and rename the classes that we are going to use to something shorter.  In this program, we will need the Zumo32U4LCD class and the Zumo32U4LineSensors class.  The beginning of the program should look like this:

#include <Zumo32U4.h>
Zumo32U4LCD lcd;
Zumo32U4LineSensors lineSensors;

The library functions for reading the lights sensors wants to put the data into a 3 element array of unsigned integers.  An unsigned integer is nothing more than an integer that can only be positive.  An array is simply a list of values, like in the EV3.  However, reading and writing to an array in C is much easier than in EV3.  A particular element of an array is referred to by the array index inside square brackets.  If we wanted to access index 2 of array myArray, we would refer to it as myArray[2].  Like all variables in C, we need to declare them.  We are going to declare the line sensor value array at the very beginning to make it a global variable.  The line declare the array looks like this:

unsigned int lineSensorValues[3]; 

In this statement, we are declaring that the variable lineSensorValues is a 3 element array of type unsigned int.  In order to use the line sensor array, we need to run an initialization function.  This basically turns the sensors on and gets them ready for use.  We only need to run this once, so let’s put it in the setup().  The function to initialize the sensors is:

lineSensors.initThreeSensors(); 

To read the sensors, we use the function:

lineSensors.read(lineSensorValues,QTR_EMITTERS_ON); 

This stores the three uncalibrated light sensor values of each light sensor in the array lineSensorValues that we declared up above.  The “QTR_EMITTERS_ON” option forces the emitters to be on to measure the reflected light intensity.  Note that is not necessary as the default condition has the emitters turned on.  However, it is useful to include.  The left sensor value is stored in lineSensorValues[0], the middle sensor is stored in lineSensorValues[1], and the right sensor value is stored in lineSensorValues[2].  We will discussed calibrating the light sensors in a later post.

We just need to display these values, and we should be set.  In the “Hello World” program, we wrote a string to the display (a portion of text between quotes).  To write the value of a variable is done in much the same way, but no quotes.

lcd.print(lineSensorValues[0]); 

In order to fit all three values on the display, we need to stagger the position on the 2×8 element LCD.  The complete program is below:

#include <Zumo32U4.h>
Zumo32U4LCD lcd;
Zumo32U4LineSensors lineSensors;

unsigned int lineSensorValues[3];

void setup() {
  lineSensors.initThreeSensors();
}

void loop() {
  lineSensors.read(lineSensorValues,QTR_EMITTERS_ON);
  lcd.clear();
  lcd.gotoXY(2, 0);
  lcd.print(lineSensorValues[1]);
  lcd.gotoXY(0, 1);
  lcd.print(lineSensorValues[0]);
  lcd.gotoXY(4, 1);
  lcd.print(lineSensorValues[2]);
  delay(20);
} 

I also added a delay of 20 ms between updates.  This program executes each time through the loop in less than 1 ms, and the display looks like noise.  The 20 ms delay slows it down enough that one can almost read it.  The light sensor values are uncalibrated and range from a low value (100ish) to a value of about 2000 when completely saturated by an external light.  On my light colored table, I get about 110-200 depending on the sensor, and I get between 1100-1200 if the robot is leaning off the table.

One thing to try is to change QTR_EMITTERS_ON to QTR_EMITTERS_OFF.  This turns off the emitters and measures the background light.  You will probably get a value of 2000 on all of the sensors.  If you shine a light on an emitter, the value will quickly drop.  One of the start conditions of a sumo competition that we participate in often uses a light bulb to signal the start of the match.  Turning the emitters off would allow one to easily sense the light bulb.

Posted in Robotics | Tagged , , | Leave a comment

Zumo 32U4 Simple LCD Display Test

I realized that before we can work on reading the light sensors, we probably should cover an easy way to display the data.  There are a couple of ways to display data on the Zumo.  One, which we will cover later, is to use the serial monitor.  This is a method of communicating between the Arduino and your computer over USB to send data.  This is a great method, and we will get to it later.  However, another method is to simply write the information to the LCD on the robot.  This can be useful as it allows you to see the data without being tethered to a computer.
In this post, we will simply just write the customary “Hello World” statement to the display.  Like previous posts, we need to include the Zumo32U4 library, and it is always helpful to create a shorten name for the class that we are using.  The class for driving the LCD is Zumo32U4LCD, and let’s create an instance of it and call it simply “lcd.”  Both of these steps are done in the lines:
#include <Zumo32U4.h>
Zumo32U4LCD lcd; 
 Since we are just displaying once, we will put everything in the setup().  In this case, loop will be empty.  The LCD displays the items that you tell it, then it holds the display and does not need to be refreshed until you want to do something different with it.  There are a couple of commands that you will need.  The first is:
lcd.clear();
 This simply clears the display.  In a lot of cases, the display is already clear, but sometimes, it has remnants from previous programs.  It is always a good idea to clear the display.  The next step is that we need to tell the display where to write our message.  The display has 2 lines of 8 characters.  We cannot write the entire display to a single line, so let’s split it on two lines.  We need to specify that we want to start the first line at the far left position.  The LCD positions start at 0.  The first line, left most position would be (0,0).  We can specify that we want to start there with the following command:
lcd.gotoXY(0,0);
Then to print the word “Hello,” we would use:
lcd.print("Hello");
Writing the word “World” is the same method, but let’s start this on the second line, third character location, or (1,2).  The entire program is then:
#include <Zumo32U4.h>
Zumo32U4LCD lcd;
void setup() {
  lcd.clear();
  lcd.gotoXY(0,0);
  lcd.print("Hello");
  lcd.gotoXY(1,2);
  lcd.print("World");
}

void loop() {
  // put your main code here, to run repeatedly:
}

 

Posted in Robotics | Tagged , | 2 Comments

Zumo 32U4 Motor Encoders

In the previous post, we walked through how to turn the motors on for a specific amount of time.  In this post, we will turn the motors on for a specific amount of encoder pulses.  Like the EV3, encoders count fractions of a rotation.  The EV3 uses an optical encoder where it shines a light through a rotating disk.  The zumo uses what is called a quadrature magnetic encoder.  It senses changes in magnetic field and eddy currents to measure 1/12 of a rotation.  For the EV3, it was simple in that each encoder pulse equated to 1 degree of rotation after the gearing of the motor.  For the Zumo, it is similar, but the numbers don’t quite match.  Each motor rotation produces 12 encoder pulses.  However, the gear ratio after the motor is ultimately what determines the number of pulses per rotation.  My Zumo has a gear ratio of 75.81:1.  That is, for every 75.81 rotations of the motor, the wheel would make 1 rotation.  Since each encoder produces 12 pulse for each motor rotation, that means that the encoder measures 75.81 x 12 pulses for each wheel rotation, or about 909.7 pulses.

Anyway, let’s demonstrate how to read the encoders, and how to uses them to make the robot move forward a certain distance, then backwards a certain distance.  For the sake of this demonstration, let’s do the following steps:

  1. Delay 1 second.
  2. Move forward at a speed of 200 for 2000 encoder counts (a little more than two wheel rotations).
  3. Stop the Motors and pause for 1 second.
  4. Move backwards at a speed of 200 for 2000 encoder counts.
  5. Stop, then repeat.

Like the previous program, we need to include some objects from the Zumo32u4 library.  We are creating an instance of the object Zumo32U4Motors, and calling it “motors.”  We are also using the Zumo32U4Encoders class, so we need to create an instance of the Zumo32U4Encoders class and call it “encoders.”  Therefore, the section before the main body of the program should look something like this:

  #include <Zumo32U4.h>
  Zumo32U4Encoders encoders;
  Zumo32U4Motors motors; 

The next section is the Setup().  Like the previous program, there really is nothing to set up for this simple task.

After the setup(), we need the loop() function.  One really big difference between C and EV3-G is that all variables have to be declared.  In EV3-G, we would just insert a variable anywhere we wanted one, and all variables were global.  In C (which Arduino is based from), we have to declare our variables where they will be used, and assign a type to the variable.  In this case, we would like to create a variable for the left encoder, and one for the right encoder.  The type of variable is “int,” which is short for integer.  You declare the variables like below:

  int countsLeft;
  int countsRight; 

Once they are declared, we can use them.  The very first operation we should do is measure and reset the encoders.  This is done with the following:

  countsLeft = encoders.getCountsAndResetLeft();
  countsRight = encoders.getCountsAndResetRight(); 

The command getCountsAndResetLeft() is a member of the class of Zumo32U4Encoders, which we renamed to encoders, and it returns an integer.

The next steps should be similar to the previous post.  We need to delay for 1 s, then set the motors to forward at a power of 200.

  delay(1000);
  motors.setSpeeds(200,200); 

Once the motors are set, we need to continuously measure the encoder values until they reach a value of 2000 counts.  This is similar to the EV3 loop block.  For the Zumo, it is a “do {….   } while ();” statement.  This statement does all the commands within the { } while the condition within ( ) is met.  We need to put the encoder measurements with the { } brackets.  We want to perform this act while both the left and right encoders are less than 2000.

  do {
    countsLeft = encoders.getCountsLeft();
    countsRight = encoders.getCountsRight();
  }  while(countsLeft<2000&&countsRight<2000); 

The above section of code performs these actions.  The loop will run as long as the left and right encoders are less than 2000.  The “&&” is the logic AND operation.  Once the condition is met, we need to turn the motors off and delay for a second.

  motors.setSpeeds(0,0);
  delay(1000);  

Moving the robot in reverse is simply the opposite.  You could leave the encoders where they are (at about 2000) and drive while they are greater than 0.  I like to go for simplicity and just reset the encoders.  To move backwards the code is:

  countsLeft = encoders.getCountsAndResetLeft();
  countsRight = encoders.getCountsAndResetRight();
  motors.setSpeeds(-200,-200);
  do {
    countsLeft = encoders.getCountsLeft();
    countsRight = encoders.getCountsRight();
  }  while(countsLeft>-2000&&countsRight>-2000);
  motors.setSpeeds(0,0);  

Putting the whole program together:

#include <Zumo32U4.h>

Zumo32U4Encoders encoders;
Zumo32U4Motors motors;

void setup() {
}

void loop() {
  int countsLeft;
  int countsRight;
 
  countsLeft = encoders.getCountsAndResetLeft();
  countsRight = encoders.getCountsAndResetRight();
 
  delay(1000);
  motors.setSpeeds(200,200);
  do {
    countsLeft = encoders.getCountsLeft();
    countsRight = encoders.getCountsRight();
  }  while(countsLeft<2000&&countsRight<2000);   
  motors.setSpeeds(0,0);   
  delay(1000);     

  countsLeft = encoders.getCountsAndResetLeft();   
  countsRight = encoders.getCountsAndResetRight();   
  motors.setSpeeds(-200,-200);   
  do {     
    countsLeft = encoders.getCountsLeft();     
    countsRight = encoders.getCountsRight();   
  }  while(countsLeft>-2000&&countsRight>-2000);
  motors.setSpeeds(0,0);
} 

The next post will probably be on reading the downward facing light sensors.

Posted in Robotics | Tagged , , | Leave a comment

Zumo Simple Driving Program

These blog posts on the Zumo will start out very simple.  This is assuming the reader has little to no Arduino experience.  The goal of this post is simply to teach the reader how to write a program that will make the Zumo robot move forward for 1 second, pause, move backwards for 1 second, pause, then repeat.

When using the Zumo 32U4, you need to include the Zumo 32U4 library file.  This includes all of the basic commands to interface with the robot.  This is done by adding the line:

include <Zumo32U4.h>

to the program.  This should be the first line in the program (after any comments you might add).  The second line you will want to add is:

Zumo32U4Motors motors;

Zumo32U4Motors is what’s called a Class.  A Class is a way of defining an object.  We can discuss Classes later, but what you need to understand is that we need to create an object using the Class Zumo32U4Motors, and call it “motors.”  This is done using the line above.  From here on out, we can refer to our instance of the class simply as motors.

Now that we got that stuff out of the way, every Arduino program has to have two sections.  When you create a new program, the Arduino IDE will create these two sections for you.  It should look something like:

void setup() {
  // put your setup code here, to run once:
}
void loop() {
  // put your main code here, to run repeatedly:
} 

These are actually both functions, one named “setup” and the other named “loop.”  Traditionally, functions return values, and the type of value that they return needs to be defined before the function name.  However, these functions do not return anything.  Therefore, their type is call void.  When the Arduino executes a program, it runs the setup function once, then runs the loop command over and over and over, until the power is turned off or the master reset is pressed.  When it runs a function, it executes everything between the { }.  In this case, there are only comments.  Whenever you have “//” it tells the compile to skip the rest of the line when compiling.  These are comments only for the users to read and the Zumo never sees them.

In the case of our simple program, we have nothing to set up, so just leave the setup function as it is.  Don’t delete it, as the compiler needs to have the function present, but you don’t need to type anything inside it.

For the loop function, you will need some delays and motor commands.  The first command we want is a 1 second delay.  This can be accomplished with the delay command.  This is a function that tells the robot not to do anything different for a certain number of milliseconds.  To delay for 1 second, you would need:

  delay(1000);

Note that the command requires a semicolon after it.  All commands other than compiler directives (like the include statement above) require a semicolon after them.

The next command that we will need is a motor command.  The command belongs to the class Zumo32U4Motors, but remember we created an object for this called motors.  When writing a command that is part of a class, we need to follow the format of classname.command().  The command is setSpeeds().  This command takes two arguments, namely, the left motor speed and the right motor speed.  Zumo speeds range from -400 to +400 and are always integer values.  To make the robot move forward at full speed, we would need:

  motors.setSpeeds(400, 400);

If we want to go forward for 1 second, we would need to follow this with another delay.  To stop the robot, you would then need:

  motors.setSpeeds(0, 0);

Going backwards is the same, but use speeds of -400.

Putting everything together, the program would be:

#include <Zumo32U4.h>
Zumo32U4Motors motors;

void setup() {
}

void loop() {
  delay(1000);
  motors.setSpeeds(400, 400);
  delay(1000);
  motors.setSpeeds(0, 0);
  delay(1000);
  motors.setSpeeds(-400, -400);
}  

You can modify this to do any sort of blind movement you would like.  However, keep in mind that everything is based on time, which makes it hard for precision navigation.  Comparing to the LEGO EV3, this would be equivalent to using unregulated motor blocks based on time.  If you want raw speed, it may work.  If you want precise navigation, not a good idea….  We can touch on better methods of driving and navigation in later posts.

The next post will be learning how to read the motor encoders.

Posted in Robotics | Tagged , | Leave a comment