Android Robotics

package ch.aplu.raspipilot;

import ch.aplu.android.*;
import ch.aplu.android.raspi.*;
import android.graphics.Color;

public class RaspiPilot extends GameGrid
  implements ConnectionListener, GGPushButtonListener
{
  private class Blinker extends Thread
  {
    private boolean isLeft;
    private boolean isRunning;

    public Blinker(boolean isLeft)
    {
      this.isLeft = isLeft;
      isRunning = true;
      start();
    }

    public void run()
    {
      Led led;
      if (isLeft)
        led = new Led(Led.LED_LEFT);
      else
        led = new Led(Led.LED_RIGHT);
      while (isRunning)
      {
        led.setColor(Color.YELLOW);
        Tools.delay(200);
        led.setColor(Color.BLACK);
        Tools.delay(200);
      }
    }

    public void terminate()
    {
      isRunning = false;
    }
  }

  private String VERSION ="1.1";
  private Robot robot;
  private Gear gear;
  private Led led1, led2;
  private GGPushButton fasterBtn;
  private GGPushButton slowerBtn;
  private GGPushButton leftBtn;
  private GGPushButton rightBtn;
  private GGPushButton stopBtn;
  private GGTextField textField;
  private int v = 0;
  private final int vInc = 10;
  private int dir = 0;
  private final int dirInc = 5;
  private Blinker blinker = null;
  private boolean isButtonPressed = false;

  public RaspiPilot()
  {
    super(21, 21, 0);
    setScreenOrientation(PORTRAIT);
  }

  public void main()
  {
    String ipAddress = askIPAddress();
    getBg().clear(Color.BLUE);
    new GGTextField("Raspi Pilot V" + VERSION,
      new Location(0, 1), true).show();
    addButtons();

    robot = new Robot(this, ipAddress);
    if (!robot.isConnected())
    {
      new GGTextField("Connection failed", new Location(4, 19),
        true).show();
      return;
    }
    textField = new GGTextField("Connection established", new Location(3, 19),
      true);
    textField.show();
    gear = new Gear();
    gear.setSpeed(0);
    L.i("Creating LED_FRONT");
    led1 = new Led(Led.LED_FRONT);
    led1.setColor(Color.WHITE);
    led2 = new Led(Led.LED_REAR);
    led2.setColor(Color.RED);
    enableButtons();
    while (robot.isConnected())
    {
      dispatchButtons();
      Tools.delay(100);
    }
  }

  private String askIPAddress()
  {
    GGPreferences prefs = new GGPreferences(this);
    String oldName = prefs.retrieveString("IPAddress");
    String newName = null;
    while (newName == null || newName.equals(""))
    {
      newName = GGInputDialog.show(this, "Raspi", "Enter IP Address",
        oldName == null ? "192.168.0.4" : oldName);
    }
    prefs.storeString("IPAddress", newName);
    return newName;
  }

  private void addButtons()
  {
    fasterBtn = new GGPushButton("faster");
    addActor(fasterBtn, new Location(10, 5));
    slowerBtn = new GGPushButton("slower");
    addActor(slowerBtn, new Location(10, 15));
    leftBtn = new GGPushButton("left");
    addActor(leftBtn, new Location(3, 10));
    rightBtn = new GGPushButton("right");
    addActor(rightBtn, new Location(17, 10));
    stopBtn = new GGPushButton("stop");
    addActor(stopBtn, new Location(10, 10));
  }
  
  private void enableButtons()
  {
    fasterBtn.addPushButtonListener(this);
    slowerBtn.addPushButtonListener(this);
    leftBtn.addPushButtonListener(this);
    rightBtn.addPushButtonListener(this);
    stopBtn.addPushButtonListener(this);
  }


  public void onPause()
  {
    if (robot != null)
      robot.exit();
    super.onPause();
  }

  private void dispatchButtons()
  {
    if (fasterBtn.isPressed() && !isButtonPressed)
    {  
      isButtonPressed = true;
      faster();
    }
    else if (slowerBtn.isPressed())
    {  
      isButtonPressed = true;
      slower();
    }  
    else if (leftBtn.isPressed())
    {  
      isButtonPressed = true;
      turnLeft();
    }  
    else if (rightBtn.isPressed())
    {  
      isButtonPressed = true;
      turnRight();
    }  
    else if (stopBtn.isPressed())
    {  
      isButtonPressed = true;
      doStop();
    }  
    else if (!(fasterBtn.isPressed() || leftBtn.isPressed() || 
      rightBtn.isPressed() || stopBtn.isPressed()))
        isButtonPressed = false;
  }


  private void faster()
  {
    L.i("faster");
    if (v < 100)
    {
      v = v += vInc;
      doIt();
    }
  }

  private void slower()
  {
    L.i("slower");
    if (v > -100)
    {
      v = v -= vInc;
      doIt();
    }
  }

  private void doStop()
  {
    L.i("stop");
    v = 0;
    dir = 0;
    doIt();
  }

  private void turnLeft()
  {
    L.i("turnLeft");
    if (dir > -80 && v != 0)
    {
      dir -= dirInc;
      doIt();
    }
  }

  private void turnRight()
  {
    L.i("turnRight");
    if (dir < 80 && v != 0)
    {
      dir += dirInc;
      doIt();
    }
  }

  private void doIt()
  {
    double radius = toRadius(dir);
    gear.setSpeed(Math.abs(v));
    if (v > 0)
    {
      if (dir > 0)
      {
        gear.rightArc(radius);
        if (blinker != null)
          blinker.terminate();
        blinker = new Blinker(false);
      }
      else if (dir < 0)
      {
        gear.leftArc(radius);
        if (blinker != null)
          blinker.terminate();
        blinker = new Blinker(true);
      }
      else
      {
        gear.forward();
        if (blinker != null)
        {
          blinker.terminate();
          blinker = null;
        }
      }
    }
    else if (v < 0)
    {
      if (dir > 0)
      {
        gear.leftArc(-radius);
        if (blinker != null)
          blinker.terminate();
        blinker = new Blinker(false);
      }
      else if (dir < 0)
      {
        gear.rightArc(-radius);
        if (blinker != null)
          blinker.terminate();
        blinker = new Blinker(true);
      }
      else
      {
        gear.backward();
        if (blinker != null)
        {
          blinker.terminate();
          blinker = null;
        }
      }
    }
    else
    {
      gear.stop();
      if (blinker != null)
      {
        blinker.terminate();
        blinker = null;
      }
    }
  }

  private double toRadius(double direction)
  {
    double R = 0.05;
    if (direction != 0)
    {
      double r = Math.abs(R / Math.sin(Math.toRadians(direction)));
      return r;
    }
    return 0;
  }

  public void notifyConnection(boolean connected)
  {
    showToast(connected ? "Connection established" : "Connection lost", true);
    if (!connected)
    {
      textField.hide();
      new GGTextField("Connection lost", new Location(4, 19),
        true).show();
    }
  }
  
  public void buttonPressed(GGPushButton button)
  {}
  public void buttonReleased(GGPushButton button)
  {}
  public void buttonClicked(GGPushButton button)
  {}
  public void buttonRepeated(GGPushButton button)
  {}
}