1 package tankrunner;
  2
  3 import java.io.IOException;
  4 import java.util.Timer;
  5 import java.util.TimerTask;
  6 import lejos.nxt.Button;
  7 import lejos.nxt.LCD;
  8 import lejos.nxt.Motor;
  9 import lejos.nxt.SensorPort;
 10 import lejos.nxt.TouchSensor;
 11 import lejos.nxt.comm.Bluetooth;
 12 import lejos.nxt.remote.RemoteNXT;
 13
 14 public class TankRunner {
 15
 16     private static final int centerLeft;
 17     private static final int centerRight;
 18     private static final TimerTask poll = new TimerTask() {
 19         @Override
 20         public void run() {
 21             setSpeed();
 22             fireCheck();
 23             if (escapeCheck()) {
 24                 this.cancel();
 25                 System.exit(0);
 26             }
 27         }
 28     };
 29     private static final int rangeLeft;
 30     private static final int rangeRight;
 31     private static RemoteNXT runner;
 32     private static final TouchSensor ts1 = new TouchSensor(SensorPort.S1);
 33     private static final TouchSensor ts2 = new TouchSensor(SensorPort.S2);
 34
 35     static {
 36         int[] calVals = calibrate();
 37         centerLeft = calVals[0];
 38         centerRight = calVals[1];
 39         rangeLeft = calVals[2];
 40         rangeRight = calVals[3];
 41
 42         btConnect();
 43     }
 44
 45     public static void main(String[] args) {
 46         new Timer().schedule(poll, 0L, 100L);
 47     }
 48
 49     private static void btConnect() {
 50         //Connect to remote NXT "Runner" by Bluetooth
 51         try {
 52             System.out.println("Connecting...");
 53             runner = new RemoteNXT("Runner", Bluetooth.getConnector());
 54             LCD.clear();
 55             LCD.refresh();
 56             System.out.println("Connected");
 57         } catch (IOException e) {
 58             LCD.clear();
 59             System.out.println("Conn. failed");
 60             Button.waitForAnyPress();
 61             System.exit(1);
 62         }
 63     }
 64
 65     private static int[] calibrate() {
 66         int[] calVals = new int[4];
 67         int forwardLimitLeft, rearLimitLeft, forwardLimitRight, rearLimitRight;
 68         System.out.println("Push control\nsticks forward,"
 69                 + " then hit orange button.");
 70         Button.ENTER.waitForPressAndRelease();
 71         forwardLimitLeft = Motor.B.getTachoCount();
 72         forwardLimitRight = Motor.C.getTachoCount();
 73         LCD.clearDisplay();
 74         System.out.println("Pull control\nsticks back,"
 75                 + " then hit orange button.");
 76         Button.ENTER.waitForPressAndRelease();
 77         rearLimitLeft = Motor.B.getTachoCount();
 78         rearLimitRight = Motor.C.getTachoCount();
 79         LCD.clearDisplay();
 80         System.out.println("Calibrated:\n\nForward limits:\n"
 81                 + forwardLimitLeft + "," + forwardLimitRight
 82                 + "\nRear limits:\n" + rearLimitLeft
 83                 + "," + rearLimitRight);
 84         System.out.println("Push orange\nbutton to\ncontinue");
 85         Button.ENTER.waitForPressAndRelease();
 86         LCD.clearDisplay();
 87         calVals[0] = (forwardLimitLeft + rearLimitLeft) / 2;
 88         calVals[2] = forwardLimitLeft - rearLimitLeft;
 89         calVals[1] = (forwardLimitRight + rearLimitRight) / 2;
 90         calVals[3] = forwardLimitRight - rearLimitRight;
 91         return calVals;
 92     }
 93
 94     private static boolean escapeCheck(){
 95         boolean esc = Button.ESCAPE.isDown();
 96         if (esc) {
 97             LCD.clearDisplay();
 98             System.out.println("Breakout!");
 99             runner.B.stop();
100             runner.C.stop();
101         }
102         return esc;
103     }
104
105     private static void fireCheck() {
106         if (ts1.isPressed() && ts2.isPressed()) {
107             try {
108                 runner.B.stop();
109                 runner.C.stop();
110                 Thread.sleep(200);
111                 runner.A.setSpeed(360);
112                 runner.A.rotate(-10);
113                 Thread.sleep(100);
114                 runner.A.rotate(10);
115             } catch (InterruptedException ex) {
116
117             }
118         }
119     }
120
121     private static void setSpeed() {
122         //set remote motor speed
123         int speedLeft = tachoConvert(true);
124         int speedRight = tachoConvert(false);
125         System.out.println("L=" + speedLeft + ", R=" + speedRight);
126
127         int dirLeft = (int) Math.signum(speedLeft);
128         int dirRight = (int) Math.signum(speedRight);
129
130         //set new speed and direction for motors
131         runner.B.setSpeed(Math.abs(speedRight));
132         runner.C.setSpeed(Math.abs(speedLeft));
133
134         switch (dirLeft) {
135             case 1:
136                 runner.C.forward();
137                 break;
138             case -1:
139                 runner.C.backward();
140                 break;
141             default:
142                 runner.C.stop();
143         }
144
145         switch (dirRight) {
146             case 1:
147                 runner.B.forward();
148                 break;
149             case -1:
150                 runner.B.backward();
151                 break;
152             default:
153                 runner.B.stop();
154         }
155     }
156
157     private static int tachoConvert(boolean left) {
158         int gear;
159         int maxSpeed = 720;
160         int speedSettings = 5;
161         int speedStep = maxSpeed / speedSettings;
162
163         int center = left ? centerLeft : centerRight;
164         int range = left ? rangeLeft : rangeRight;
165         int count = left ? Motor.B.getTachoCount() : Motor.C.getTachoCount();
166         int gearWidth = range / (2 * (speedSettings + 1));
167
168         count -= center;
169
170         //This sets speed \exists \Z, -5 <= speed <= 5
171         gear = count / gearWidth;
172         gear = (Math.abs(gear) > speedSettings)
173                 ? (int) Math.signum(gear) * speedSettings
174                 : gear; //Correction if speed > +-speedSettings
175
176         return gear * speedStep;
177     }
178
179 }
180