Command.java :  » UnTagged » lego-remote-controller » lejos » lyndon » command » Android Open Source

package lejos.lyndon.command;

import lejos.lyndon.main.LeJOReceiver;
import lejos.nxt.Motor;

/**
 * 
 * @author Sniffer
 *
 */
public class Command {
  /**
   * 
   */
  private int c = 3;
  
  private int hDir;
  private int vDir;
  private int hSpeed;
  private int vSpeed;
  /**
   * byte[]string
   * @param b 
   * @return
   */
  public String getString(byte[] b){
    char[] ch = new char[b.length];
    for( int i = 0; i < b.length; i++){
      ch[i] = (char)b[i];
    }
    return String.valueOf(ch);
  }
  
  /**
   * 
   * @param d 
   * @return 
   */
  public String getCommand(String d){
    if(d.length() != 11){
      return "LengthError";
    }
    
    try{
      hDir = Integer.parseInt(d.subSequence(0, 2).toString());
      hSpeed = Integer.parseInt(d.subSequence(3, 5).toString());
      vDir = Integer.parseInt(d.subSequence(6, 8).toString());
      vSpeed = Integer.parseInt(d.subSequence(9, 11).toString());
    }catch(Exception e){
      return "FormatError";
    }
    String cmd = "";
    // 
    switch(hDir){
    case LeJOReceiver.DIRECTION_FORWARD:
      // 
      forward(getSpeed(hSpeed));
      cmd += "Forward";
      break;
    case LeJOReceiver.DIRECTION_BACK:
      // 
      back(getSpeed(hSpeed));
      cmd += "Back";
      break;
    case LeJOReceiver.DIRWCTION_STOP:
      // 
      stop();
      cmd += "Stop";
      break;
    case 15:
      // 
      return "Exit";
    }
    
    switch(vDir){
    case LeJOReceiver.DIRECTION_LEFT:
      // 
      turnLeft(vSpeed);
      return cmd + " Left";
    case LeJOReceiver.DIRECTION_RIGHT:
      // 
      turnRight(vSpeed);
      return cmd + " Right";
    case LeJOReceiver.DIRWCTION_STOP:
      return cmd;
    default:
      return "CMDError";
    }
  }
  /**
   * 
   * @param aSpeed
   * @return
   */
  private int getSpeed(int aSpeed){
    switch(aSpeed){
    case LeJOReceiver.SPEED_0:
      return 0;
    case LeJOReceiver.SPEED_1:
      return LeJOReceiver.MIN_SPEED;
    case LeJOReceiver.SPEED_2:
      return LeJOReceiver.MIDDLE_SPEED;
    case LeJOReceiver.SPEED_3:
    default:
      return LeJOReceiver.MAX_SPEED;
    }
  }
  
  /**
   * 
   * @param level 
   * @return 
   */
  private float getRound(int level){
    switch(level){
    case LeJOReceiver.SPEED_0:
      // 01
      return 1;
    case LeJOReceiver.SPEED_1:
      return LeJOReceiver.MIN_ROUND;
    case LeJOReceiver.SPEED_2:
      return LeJOReceiver.MIDDLE_ROUND;
    case LeJOReceiver.SPEED_3:
    default:
      return LeJOReceiver.MAX_ROUND;
    }
  }
  /**
   * 
   */
  private void forward(int speed){
    Motor.A.backward();
    Motor.B.backward();
    
    Motor.A.setSpeed(speed);
    Motor.B.setSpeed(speed);
  }
  
  /**
   * 
   */
  private void back(int speed){
    Motor.A.forward();
    Motor.B.forward();
    
    Motor.A.setSpeed(speed);
    Motor.B.setSpeed(speed);
  }
  
  /**
   * 
   */
  private void turnLeft(int level){
    // 
    if( hDir != LeJOReceiver.DIRECTION_FORWARD && hDir != LeJOReceiver.DIRECTION_BACK ){
      return;
    }
    // 
    int sp = Motor.A.getSpeed();
    // 
    Motor.A.setSpeed(Math.round(sp * getRound(level)));
  }
  
  /**
   * 
   */
  private void turnRight(int level){
    // 
    if( hDir != LeJOReceiver.DIRECTION_FORWARD && hDir != LeJOReceiver.DIRECTION_BACK ){
      return;
    }
    // 
    int sp = Motor.B.getSpeed();
    // 
    Motor.B.setSpeed(Math.round(sp * getRound(level)));
  }
  /**
   * 
   */
  private void stop(){
    Motor.A.flt();
    Motor.B.flt();
  }
}