Esempio n. 1
0
  // Parses integer messages
  private static void parseMessage(int message) {
    int reset = message & 1;
    int kick = (message >>> 1) & 1;
    int motor_dleft = (message >>> 2) & 7;
    int motor_dright = (message >>> 5) & 7;
    int motor_sleft = (message >>> 8) & 511;
    int motor_sright = (message >>> 17) & 511;

    ControlCentre.setKickState((kick != 0));
    ControlCentre.setTargetSteeringAngleLeft(motor_sleft);
    ControlCentre.setTargetSteeringAngleRight(motor_sright);
    ControlCentre.setTargetDriveLeftVal(motor_dleft);
    ControlCentre.setTargetDriveRightVal(motor_dright);
  }
Esempio n. 2
0
  public void run() {
    motor_left.resetTachoCount();
    motor_left.regulateSpeed(true);
    Movement.motor_left.smoothAcceleration(true);
    int previousCommandCount = -1;

    while (true) {
      if (Movement.getCommandCount() == previousCommandCount) {
        try {
          Thread.sleep(10);
        } catch (InterruptedException e) {
        }
        continue;
      }

      previousCommandCount = Movement.getCommandCount();
      setToAngle(ControlCentre.getTargetSteeringAngleLeft());
      int new_angle = getToAngle();
      if (new_angle < 10) LCD.drawString("  ", 8, 1);
      else if (new_angle < 100) LCD.drawString(" ", 9, 1);
      LCD.drawString(Integer.toString(new_angle), 7, 1);
      LCD.drawString("R", 11, 1);

      int cur_angle = getCurrentSteeringAngle();
      double delta = new_angle - cur_angle;
      final double C = Movement.rotConstant;
      double turn_angle = 0;

      if (Math.abs(delta) < thresholdAngle / 2.0) {
        continue;
      } else if (Math.abs(delta) >= thresholdAngle / 2.0 && Math.abs(delta) < thresholdAngle) {
        delta = thresholdAngle * delta / Math.abs(delta);
      }
      setCurrentSteeringAngle((int) (cur_angle + delta) % 360);

      if (delta != 0 && Math.abs(delta) < 180) {
        turn_angle = C * delta;
      } else if (delta >= 180 && delta < 360) {
        turn_angle = -C * (360 - delta);
      } else if (delta <= -180) {
        turn_angle = C * (360 + delta);
      } else {
          /* No turning needed */
        continue;
      }

      motor_left.rotate((int) Math.round(turn_angle));
    }
  }
Esempio n. 3
0
 public void run() {
   while (true) {
     boolean kick = ControlCentre.getKickState();
     if (kick) {
       LCD.drawString("K,", 0, 1);
       Movement.motor_kick.setSpeed(900);
       Movement.motor_kick.rotate((-120 * (5 / 3)));
       Movement.motor_kick.rotate((120 * (5 / 3)));
     } else {
       LCD.drawString("_,", 0, 1);
     }
     try {
       Thread.sleep(100);
     } catch (InterruptedException e) {
     }
   }
 }
Esempio n. 4
0
  public void run() {
    Multiplexor chip = new Multiplexor(SensorPort.S4);
    while (true) {

      int targetLeft = ControlCentre.getTargetDriveLeftVal();
      LCD.drawString(Integer.toString(targetLeft) + ",", 2, 1);

      switch (targetLeft) {
        case 0:
          chip.setMotors(0, 0, 0);
          break;
        case 4:
          chip.setMotors(0, 0, 0);
          break;
        case 1:
          chip.setMotors(1, 1, 0);
          break;
        case 2:
          chip.setMotors(1, 2, 0);
          break;
        case 3:
          chip.setMotors(1, 3, 0);
          break;
        case 5:
          chip.setMotors(-1, 1, 0);
          break;
        case 6:
          chip.setMotors(-1, 2, 0);
          break;
        case 7:
          chip.setMotors(-1, 3, 0);
          break;
      }

      int targetRight = ControlCentre.getTargetDriveRightVal();
      LCD.drawString(Integer.toString(targetRight) + " L", 4, 1);
      switch (targetRight) {
        case 0:
          chip.setMotors(0, 0, 1);
          break;
        case 4:
          chip.setMotors(0, 0, 1);
          break;
        case 1:
          chip.setMotors(1, 1, 1);
          break;
        case 2:
          chip.setMotors(1, 2, 1);
          break;
        case 3:
          chip.setMotors(1, 3, 1);
          break;
        case 5:
          chip.setMotors(-1, 1, 1);
          break;
        case 6:
          chip.setMotors(-1, 2, 1);
          break;
        case 7:
          chip.setMotors(-1, 3, 1);
          break;
      }

      try {
        Thread.sleep(10);
      } catch (InterruptedException e) {
      }
    }
  }