EV3 Robot Arm plays Towers Of Hanoi

Post your EV3 projects, project ideas, etc here!

Moderators: roger, gloomyandy, skoehler

EV3 Robot Arm plays Towers Of Hanoi

Postby saturn_at_sky » Tue Jan 21, 2014 1:56 pm

This is one of my first leJOS EV3 projects. The EV3 solves the "Towers of Hanoi" problem. See: http://www.youtube.com/watch?v=9cLztx-Y-TU.
This is the program:

Code: Select all
import lejos.hardware.Button;
import lejos.hardware.ButtonListener;
import lejos.hardware.ev3.LocalEV3;
import lejos.hardware.lcd.LCD;
import lejos.hardware.motor.NXTRegulatedMotor;
import lejos.hardware.port.Port;
import lejos.hardware.sensor.EV3TouchSensor;
import lejos.robotics.SampleProvider;
import lejos.utility.Delay;
import tool.Tool4Robot;
import tool.Tool4Throwable;


/**
 * The <CODE>Hanoi</CODE> class solves the tower of hanoi problem.
 * <P><B>title: Hanoi</B></P>
 * <P>description: Use of the motors:<br>
 * A: claw<br>
 * B: hand<br>
 * C: elbow<br>
 * D: platform.<br>
 * Use of the sensors:<br>
 * S4: touch sensor.</P>
 * <P>copyright: (c) 2014 thomas kaffka, born at 11/08/1959 in düsseldorf - germany , all rights reserved.</P>
 * @author thomas kaffka , cologne , germany.
 * @version 20.01.2014 15:14:49
 * @since 1.0
 */

public class Hanoi implements ButtonListener {

   private static final int TOWER_1 = 40;
   private static final int TOWER_2 = 90;
   private static final int TOWER_3 = 140;
   
   private static int HAND_DISK_1 = -80;
   private static int HAND_DISK_2 = -30;
   private static int HAND_DISK_3 = 70;
   private static int HAND_DISK_4 = 100;
   private static int HAND_TOP = 170;
   private static int HAND_PARK = 0;
   
   private static int ELBOW_DISK_1 = 100;
   private static int ELBOW_DISK_2 = 110;
   private static int ELBOW_DISK_3 = 70;
   private static int ELBOW_DISK_4 = 100;
   private static int ELBOW_TOP = 170;
   private static int ELBOW_PARK = 0;

   private NXTRegulatedMotor motorPlatform;
   private NXTRegulatedMotor motorElbow;
   private NXTRegulatedMotor motorHand;
   private NXTRegulatedMotor motorClaw;
   private SampleProvider touch;
   
   private int[] hand_height;
   private int[] elbow_height;
   private int[] tower_pos;
   private int[] tower;

   private Tool4Robot t4r;

   /**
    * The constructor <CODE>Hanoi</CODE> creates a new instance(object) of the class.
    * @since 1.0
    */
   public Hanoi() {
      
      try {
         
         t4r = new Tool4Robot();
         t4r.beepMessageOk();

           LCD.clear();
           LCD.drawString(t4r.LCD_center("Hanoi"), 0, 0);
         LCD.drawString(t4r.LCD_center("-----"), 0, 1);
         t4r.drawBattery(2);

         // initialize the motors (0.5.0-alpha).
           LCD.drawString("initialize motors", 0, 3);
         motorPlatform = new NXTRegulatedMotor(LocalEV3.get().getPort("D"));
         motorElbow = new NXTRegulatedMotor(LocalEV3.get().getPort("C"));
         motorHand = new NXTRegulatedMotor(LocalEV3.get().getPort("B"));
         motorClaw = new NXTRegulatedMotor(LocalEV3.get().getPort("A"));
         motorPlatform.resetTachoCount();
         motorElbow.resetTachoCount();
         motorHand.resetTachoCount();
         motorClaw.resetTachoCount();
         motorPlatform.rotateTo(0);
         motorElbow.rotateTo(0);
         motorHand.rotateTo(0);
         motorClaw.rotateTo(0);
         motorPlatform.setSpeed(40);
         motorElbow.setSpeed(150);
         motorHand.setSpeed(150);
         motorClaw.setSpeed(200);
         motorPlatform.setAcceleration(400);
         motorElbow.setAcceleration(400);
         motorHand.setAcceleration(400);
         motorClaw.setAcceleration(400);
         Delay.msDelay(8000);

           // initialize the sensors.
         // get a port instance.
           LCD.drawString("initialize touch  ", 0, 3);
         Port port = LocalEV3.get().getPort("S4");
         // get an instance of the touch EV3 sensor.
         EV3TouchSensor sensor = new EV3TouchSensor(port);
         // get an instance of this sensor in measurement mode.
         touch = sensor.getTouchMode();
           Delay.msDelay(1000);

           // initialize the buttons.
         Button.ESCAPE.addButtonListener(this);
         
         // initialize data.
         hand_height = new int[4];
         hand_height[0] = HAND_DISK_1;
         hand_height[1] = HAND_DISK_2;
         hand_height[2] = HAND_DISK_3;
         hand_height[3] = HAND_DISK_4;

         elbow_height = new int[4];
         elbow_height[0] = ELBOW_DISK_1;
         elbow_height[1] = ELBOW_DISK_2;
         elbow_height[2] = ELBOW_DISK_3;
         elbow_height[3] = ELBOW_DISK_4;

         tower = new int[4];
         tower[1] = 4;
         tower[2] = 0;
         tower[3] = 0;

         tower_pos = new int[3];
         tower_pos[0] = TOWER_1;
         tower_pos[1] = TOWER_2;
         tower_pos[2] = TOWER_3;

         LCD.drawString("ready...          ", 0, 3);
         t4r.beepMessageReady();
           Delay.msDelay(1000);

      } catch (Exception ex) {
         t4r.beepMessageError();
         System.out.println(Tool4Throwable.getCallStackOut(ex));
      }

   }

   /**
    * The method <CODE>openClaw</CODE> opens the claw.
    * @since 1.0
    */
   public void openClaw() {
      motorClaw.rotateTo(-180);
   }
   
   /**
    * The method <CODE>closeClaw</CODE> closes the claw.
    * @since 1.0
    */
   public void closeClaw() {
      motorClaw.rotateTo(0);
   }

   /**
    * The method <CODE>moveHand</CODE> moves the hand to the current angle.
    * @param angle the angle to move to.
    * @since 1.0
    */
   public void moveHand(int angle) {
      motorHand.rotateTo(angle);
   }

   /**
    * The method <CODE>moveElbow</CODE> moves the elbow to the current angle.
    * @param angle the angle to move to.
    * @since 1.0
    */
   public void moveElbow(int angle) {
      motorElbow.rotateTo(angle);
   }

   /**
    * The method <CODE>movePlatform</CODE> moves the platform to the current angle.
    * @param angle the angle to turn.
    * @since 1.0
    */
   public void movePlatform(int angle) {
      motorPlatform.rotateTo(-angle);
   }

   /**
    * The method <CODE>movePlatformToPark</CODE> moves the platform to its park position.
    * @since 1.0
    */
   public void movePlatformToPark() {
      moveElbow(ELBOW_TOP);
      moveHand(HAND_TOP);
      motorPlatform.forward();
      float[] sample = new float[touch.sampleSize()];
      while(sample[0] == 0 && Button.ESCAPE.isUp()) {
         touch.fetchSample(sample, 0);
      }
      motorPlatform.resetTachoCount();
   }

   /**
    * The method <CODE>moveDiskFromTo</CODE> moves a disk from a tower to a tower.
    * @param tower_from move disk from tower.
    * @param height_from from tower height.
    * @param tower_to move disk to tower.
    * @param height_to to tower height.
    * @since 1.0
    */
   public void moveDiskFromTo(int tower_from, int height_from, int tower_to, int height_to) {
      if (motorPlatform.getTachoCount() > tower_from) {
          movePlatformToPark();
      }
       movePlatform(tower_from);
       openClaw();
       moveElbow(elbow_height[height_from]);
       moveHand(hand_height[height_from]);
       closeClaw();
       moveElbow(ELBOW_TOP);
       moveHand(HAND_TOP);
      if (tower_to < tower_from) {
          movePlatformToPark();
      }
       movePlatform(tower_to);
       moveElbow(elbow_height[height_to]);
       moveHand(hand_height[height_to]);
       openClaw();
       moveElbow(ELBOW_TOP);
       moveHand(HAND_TOP);
   }
   
    /**
    * The method <CODE>moveDiskCount</CODE> counts the disks of the towers.
    * @param from disk from tower.
    * @param to disk to tower.
    * @since 1.0
    */
   private void moveDiskCount(int from, int to) {
      tower[from]--;
      tower[to]++;
   }

    /**
    * The method <CODE>moveDisk</CODE> performs the towers move.
    * @param a tower.
    * @param b tower.
    * @param c tower.
    * @param n number of disks.
    * @since 1.0
    */
   private void moveDisk(int a, int b, int c, int n) {
      if (Button.ESCAPE.isDown()) {
          t4r.beepMessageEnd();
         System.exit(0);
      }

      if (n == 1) {
           moveDiskFromTo(tower_pos[a - 1], tower[a] - 1, tower_pos[c - 1], tower[c]);
         moveDiskCount(a, c);
         
      } else {
         moveDisk(a, c, b, n-1);
         moveDisk(a, b, c, 1);
         moveDisk(b, a, c, n-1);
      }
   }

   /**
    * The method <CODE>go</CODE> lets the robot move.
    * @since 1.0
    */
   public void go() {
      
      LCD.drawString("go...             ", 0, 3);
        Delay.msDelay(1000);

        try {
         
           movePlatformToPark();
           
          moveDisk(1, 2, 3, 4);

           movePlatformToPark();
           moveElbow(ELBOW_PARK);
          moveHand(HAND_PARK);
           closeClaw();
           
          t4r.beepMessageEnd();

        } catch (Exception ex) {
         t4r.beepMessageError();
         System.out.println(Tool4Throwable.getCallStackOut(ex));
      }
   }
      
   /**
    * The method <CODE>main</CODE> starts the program.
    * @param args the command line arguments.
    * @since 1.0
    */
   public static void main(String[] args) {
      Hanoi hanoi = new Hanoi();
      hanoi.go();
   }

   @Override
   public void buttonPressed(Button b) {
      t4r.beepMessageEnd();
      System.exit(0);
   }

   @Override
   public void buttonReleased(Button b) {
      // TODO Auto-generated method stub
      
   }

}

User avatar
saturn_at_sky
New User
 
Posts: 22
Joined: Wed Jan 08, 2014 1:55 pm
Location: Germany

Re: EV3 Robot Arm plays Towers Of Hanoi

Postby gloomyandy » Tue Jan 21, 2014 2:40 pm

Very nice. I like the hook/grab actuator.
User avatar
gloomyandy
leJOS Team Member
 
Posts: 3967
Joined: Fri Sep 28, 2007 2:06 pm
Location: UK


Return to EV3 Projects

Who is online

Users browsing this forum: No registered users and 1 guest

cron
more stuff