Autonomous NXT Ballshooter W/ NXTcam

This is my robot that is programmed to hunt and kill its sworn enemy a red plastic cup it is the ballshooter build from the mindstorms NXT 2.0 set but with a little add on the NXTcam from Mindsensors.com it is a special camera for the Mindstorms brick that can return x,y coordinates and area of a pre-programmed color in this case it is Red. The program tells the robot to find the biggest red object and give me its coordinates, through that it determines whether the cup is to the left or right of the robot and moves accordingly. Once the robot has decided it is on target it adjusts for distance by backing up or going forwards. It can give a relative distance (But in no way accurate) by the objects area then once its lined  up and at the correct distance it checks to see if the object has moved again then if it hasn't it fires!

I also intend to comment my code and post it soon.

Comments

[Longboarding_Robot]

import java.awt.Point;
import java.awt.Rectangle;
import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.Sound;
import lejos.nxt.addon.NXTCam;


public class follow {


 public static void main(String[] args)throws Exception {

  int objnum = 0;
  Point LOC;
  int objA = 0;
  int width = 0;
  int height = 0;
  NXTCam cam = new NXTCam(SensorPort.S2);
  Sound.beep();
  int X0 = 0;
  int dis1 = 0;
  float dis = 0;
  long start = 0;
  long total = 0;
  Motor.A.setSpeed(200);

[Longboarding_Robot]

Motor.B.setSpeed(200);
int shots = 0;
cam.sendCommand('A');
cam.sendCommand('E');
start = System.currentTimeMillis();
while(!Button.ESCAPE.isPressed()){
Rectangle rect = cam.getRectangle(0);
objnum = cam.getNumberOfObjects();
if (objnum != 0){
width = rect.width;
height = rect.height;
objA = width * height;
LCD.drawString("x: ", 0, 0);
LOC = rect.getLocation();
X0 = (int) (LOC.x * 1);
LCD.drawInt(X0, 4, 0);
LCD.drawString("A: ", 0, 1);
LCD.drawInt(objA, 4, 1);
if (shots <= 6){
LCD.drawInt(7-shots,0 , 2);
LCD.drawString("out of 7 shots left", 2, 2);
}
else{
LCD.drawString("out of ammo", 0, 2);
}
LCD.drawString("Aprox. ",0, 4);
dis = objA / 100;
dis1 = (int) (dis * 1);
LCD.drawInt(dis1,8,4);
LCD.drawString("in. away",0,5);
if (1==1){
if (X0 == 0) {
Motor.B.stop();
Motor.A.stop();
Sound.beep();
}
else if (X0 < 90 && X0 > 75) {
Motor.B.stop();
Motor.A.stop();
if (objA < 1400 && objA > 1200){
Motor.B.stop();
Motor.A.stop();

total = System.currentTimeMillis()-start;
if (total>1000){
if (shots <= 6){
Sound.beep();
Sound.beep();
start = System.currentTimeMillis();
Motor.C.rotateTo(360);
shots = shots + 1;
Motor.C.resetTachoCount();
}
}
}
else if (objA > 1200 || objA < 1000){
start = System.currentTimeMillis();
if (objA > 1200){

Motor.B.backward();
Motor.A.backward();
}
else if (objA < 900){

Motor.A.forward();
Motor.B.forward();
}
}
}
else if (X0 > 90) {
start = System.currentTimeMillis();
Motor.B.backward();
Motor.A.forward();
}
else if (X0 < 75) {
start = System.currentTimeMillis();
Motor.B.forward();
Motor.A.backward();
}

Thread.sleep(100);
LCD.clear();
}
}
}
}
}