//
you're reading...
How-to's

HTTYR: Training by demonstration

record

This post is part of a series on how to train your robot.

In this post, we are going to train a robot to make a series of movements, by manually moving its motors and recording the movement. We can then play back the movements. The played back movements are done at the default speed settings for the motors, not at the speed they were manually moved at.

If you do this with a robot arm, it would work in a similar way to the Baxter robot. But you can use the same technique with any robot, including a wheeled robot. With a wheeled robot, you press the key to start recording and then push your robot around a course you want it to travel. You then press a key to finish the recording, move the robot to the start of the course and press the replay key. The robot will then follow the course.

Here is the program:

import java.util.ArrayList;

import lejos.hardware.Button;
import lejos.hardware.motor.EV3LargeRegulatedMotor;
import lejos.hardware.port.MotorPort;
import lejos.hardware.port.TachoMotorPort;
import lejos.robotics.RegulatedMotor;
import lejos.utility.Delay;

public class Record {

  private static final int PERIOD = 100;
  private static final int NUM_MOTORS = 2;

  private TachoMotorPort[] ports = new TachoMotorPort[] {
      MotorPort.A.open(TachoMotorPort.class),
      MotorPort.B.open(TachoMotorPort.class),
      MotorPort.C.open(TachoMotorPort.class),
      MotorPort.D.open(TachoMotorPort.class) };

  private RegulatedMotor[] motors = new RegulatedMotor[] {
      new EV3LargeRegulatedMotor(ports[0]),
      new EV3LargeRegulatedMotor(ports[1]),
      new EV3LargeRegulatedMotor(ports[2]),
      new EV3LargeRegulatedMotor(ports[3]) };

  private ArrayList<Step> steps = new ArrayList<Step>();

  public static void main(String[] args) {
    new Record().run();
  }

  /*
   * Loop waiting for record or replay requests
   */
  public void run() {
    // Float all motors to make moving them easy
    flt();

    System.out.println("ENTER to Record");
    System.out.println("Right to Replay");

    // Loop waiiting for button presses
    for (;;) {
      int b = Button.waitForAnyPress();

      if (b == Button.ID_ENTER) {
        record();
        System.out.println("Record finished");
      } else if (b == Button.ID_RIGHT) {
        replay();
        System.out.println("Replay finished");
      } else if (b == Button.ID_ESCAPE) {
        break;
      }
    }
    // Close all the motors and ports
    close();
  }

  /*
   * Record manual motor movements
   */
  public void record() {
    int[] tachoCount = new int[NUM_MOTORS];
    int[] state = new int[NUM_MOTORS];
    int[] stepCount = new int[NUM_MOTORS];
    boolean[] forward = new boolean[NUM_MOTORS];
    boolean[] backward = new boolean[NUM_MOTORS];

    clearScreen();
    System.out.println("Recording");

    reset();
    steps.clear();

    for (int i = 0; i < NUM_MOTORS; i++) {
      tachoCount[i] = motors[i].getTachoCount();
      state[i] = 0;
      forward[i] = false;
      backward[i] = false;
      stepCount[i] = tachoCount[i];
    }

    while (!Button.DOWN.isDown() && !Button.ESCAPE.isDown()) {
      Delay.msDelay(PERIOD);
      int[] oldCount = new int[NUM_MOTORS];
      int[] oldState = new int[NUM_MOTORS];
      boolean changed = false;

      for (int i = 0; i < NUM_MOTORS; i++) {
        oldCount[i] = tachoCount[i];
        tachoCount[i] = motors[i].getTachoCount();
        forward[i] = (tachoCount[i] > oldCount[i]);
        backward[i] = (tachoCount[i] < oldCount[i]);
        oldState[i] = state[i];

        // Calculate new states
        if (forward[i])
          state[i] = 1;
        else if (backward[i])
          state[i] = 2;
        else
          state[i] = 0;

        if (state[i] != oldState[i])
          changed = true;
      }

      if (changed) {
        // Something has changed
        Step step = new Step();

        for (int i = 0; i < NUM_MOTORS; i++) {
          step.state[i] = oldState[i];
          step.count[i] = (tachoCount[i] - stepCount[i]);
          step.current[i] = tachoCount[i];
        }

        steps.add(step);
        step.display();

        for (int i = 0; i < NUM_MOTORS; i++) {
          stepCount[i] = tachoCount[i];
        }
      }
    }
  }

  /*
   * String representation of a state
   */
  public String getState(int s) {
    switch (s) {
    case 0:
      return "s";
    case 1:
      return "f";
    case 2:
      return "b";
    default:
      return "i";
    }
  }

  /*
   * Replay the last recorded set of movements
   */
  public void replay() {
    clearScreen();
    System.out.println("Replaying");

    reset();

    for (Step step : steps) {
      int[] oldSpeed = new int[NUM_MOTORS];
      float longest = 0;

      for (int i = 0; i < NUM_MOTORS; i++) {
        oldSpeed[i] = motors[i].getSpeed();
        if (longest < Math.abs(step.count[i]))
          longest = Math.abs(step.count[i]);
        if (step.state[i] != 0)
          System.out.println(((char) ('A' + i)) + " pre : "
              + motors[i].getTachoCount());
      }

      for (int i = 0; i < NUM_MOTORS; i++) {
        if (step.state[i] != 0) {
          step.display(i);
          if (longest > 0)
            motors[i].setSpeed((int) (oldSpeed[i] * (step.count[i] / longest)));
          motors[i].rotate(step.count[i], true);
        }
      }

      while (isMoving())
        ;

      for (int i = 0; i < NUM_MOTORS; i++) {
        motors[i].setSpeed(oldSpeed[i]);
        if (step.state[i] != 0)
          System.out.println(((char) ('A' + i)) + " post : "
              + motors[i].getTachoCount());
      }
    }

    flt();
  }

  /*
   * Float all motors
   */
  public void flt() {
    for (RegulatedMotor m : motors) {
      m.flt();
    }
  }

  /*
   * Close all motors and motor ports
   */
  public void close() {
    for (RegulatedMotor m : motors) {
      m.close();
    }
    for (TachoMotorPort p : ports) {
      p.close();
    }
  }

  /*
   * Reset all motors and motor ports
   */
  public void reset() {
    for (int i = 0; i < NUM_MOTORS; i++) {
      ports[i].resetTachoCount();
      motors[i].resetTachoCount();
    }
  }

  /*
   * Return true iff any motor is moving
   */
  public boolean isMoving() {
    for (RegulatedMotor m : motors) {
      if (m.isMoving())
        return true;
    }
    return false;
  }

  /*
   * Clear the scrolling text screen
   */
  public void clearScreen() {
    System.out.println('\f');
  }

  /*
   * Class to represent a single synchronized step of all motors
   */
  class Step {
    public int[] state = new int[NUM_MOTORS];
    public int[] count = new int[NUM_MOTORS];
    public int[] current = new int[NUM_MOTORS];

    void display() {
      for (int i = 0; i < NUM_MOTORS; i++)
        display(i);
    }

    void display(int i) {
      System.out.println(((char) ('A' + i)) + " : " + getState(state[i])
          + " : " + count[i] + " : " + current[i]);
    }
  }
}

Advertisements

Discussion

4 thoughts on “HTTYR: Training by demonstration

  1. Actually it seems some characters are missing in this program probably due to copy/paste? Is it possible to show me where the source code is? Thanks.

    Posted by Danbo Liu | 2016/01/06, 06:17
    • I have recopied the source. I think it is OK now. If you still have problems, I will put the source in the repository.
      I haven’t run this program for a very long time. I hope it still works on the latest release of leJOS.

      Posted by Lawrie Griffiths | 2016/01/06, 10:01
  2. please where should i type this code exactly

    Posted by mizouboss | 2016/02/29, 18:41
    • It is best to ask questions on the leJOS forums. This post, like the others, assumes you have a leJOS system set up in Eclipse. You would then create a leJOS EV£ project in Eclipse, create a class with the same name as the one in the code and paste the code into that class.

      Posted by Lawrie Griffiths | 2016/03/02, 09:40

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s

About leJOS News

leJOS News keeps you up-to-date with leJOS. It features latest news, explains cool features, shows advanced techniques and highlights amazing projects. Be sure to subscribe to leJOS News and never miss an article again. Best of all, subscription is free!
Follow leJOS News on WordPress.com
%d bloggers like this: