Colony

import java.awt.*;

import javax.swing.*;

/*

* GraphicsPanel class

* An extension of JPanel, designed for holding an image that will be repainted regularly.

*/

public class GraphicsPanel extends JPanel {

	protected Image img;

	public GraphicsPanel (Image img) {

		this(img, true);

	}

	public GraphicsPanel (Image img, boolean isDoubleBuffered) {

		super(isDoubleBuffered);

		this.img = img;

	}

	public void paint (Graphics g) {

		// Place the buffered image on the screen, inside the panel

		g.drawImage(img, 0, 0, Color.WHITE, this);

	}

}

import java.awt.*;

import java.awt.event.*;

import javax.swing.*;

/**

* Controls robot motion graphically, using a mouse-controlled adjustable "vector." 

* The controller uses the input vector to calculate a resulting velocity for each

* wheel on the robot. These velocities are also shown graphically. The vecolities

* are "normalized" internally to approximately eliminate the "dead zone" in which

* the robot applies partial power to the motors but does not move.

* @author Gregory Tress

*/

public class VectorController extends GraphicsPanel implements MouseListener, MouseMotionListener {

	// State variables

	int x, y, cx, cy;

	final Colonet colonet;

	final ColonetServerInterface csi;

	// Painting constants

	final int WIDTH, HEIGHT;

	final int SIDE;

	final int BOT_SIZE = 70;

	final int WHEEL_SIZE = 15;

	public VectorController (Image img, Colonet colonet, ColonetServerInterface csi) {

		super (img);

		WIDTH = img.getWidth(null);

		HEIGHT = img.getHeight(null);

		cx = WIDTH/2;

		cy = HEIGHT/2;

		x = cx;

		y = cy;

		if (WIDTH < HEIGHT) {

			SIDE = WIDTH;

		} else {

			SIDE = HEIGHT;

		}

		this.colonet = colonet;

		this.csi = csi;

		this.addMouseListener(this);

		this.addMouseMotionListener(this);

	}

	/**  Set the robot motion vector. The "vector" is defined as 

	* (0,0)->(x,y) where (0,0) is in the center of the targeting ring.

	* @param x The x coordinate of the point.

	* @param y The y coordinate of the point.

	*/

	public void setPoint (int x, int y) {

		if (isValidPoint(x, y)) {

			this.x = x;

			this.y = y;

		}

	}

	/**  Determines whether a point is inside the targeting ring.

	* @param x The x coordinate of the point.

	* @param y The y coordinate of the point.

	* @return True if a point is within the targeting ring.

	*/

	public boolean isValidPoint (int x, int y) {

		double xsq = Math.pow(1.0*(x - cx)/(SIDE/2), 2);

		double ysq = Math.pow(1.0*(y - cy)/(SIDE/2), 2);

		return (xsq + ysq <= 1);

	}

	/**  Notifies the controller that a MouseEvent has occurred

	* on the controller surface. This should be called when a 

	* MouseListener attached to the controller has detected 

	* that a MouseEvent has occurred that may influence the state

	* of the controller, such as mouseClicked, mouseDragged, or

	* mouseReleased events.

	*

	* @param e The MouseEvent object which contains the details of the event.

	* @param send Determines whether the motion vector established

	*    as a result of the MouseEvent should subsequently

	*    be sent to the robot(s).

	*/

	public void notifyMouseEvent (MouseEvent e, boolean send) {

		if (!isValidPoint(e.getX(), e.getY())) {

			return;

		}

		setPoint(e.getX(), e.getY());

		repaint();

		if (send) {

			Runnable r = new Runnable () {

				public void run () {

					sendToServer();

				}

			};

			(new Thread(r)).start();

		}

	}

	public void mouseExited(MouseEvent e) {

	}

	public void mouseEntered(MouseEvent e) {

	}

	public void mouseReleased(MouseEvent e) {

		notifyMouseEvent(e, true);

	}

	public void mouseClicked(MouseEvent e) {

		notifyMouseEvent(e, false);

	}

	public void mousePressed(MouseEvent e) {

	}

	public void mouseDragged(MouseEvent e) {

		notifyMouseEvent(e, false);

	}

	public void mouseMoved(MouseEvent e) {

	}

	/**  Calculates the magnitude of the vector. 

	* @return An int that is the truncated value of the speed of the vector.

	*/

	public int getSpeed () {

		int dx = x - cx;

		int dy = y - cy;

		int s = (int) Math.sqrt( Math.pow(dx, 2) + Math.pow(dy, 2) );

		int maxspeed = SIDE/2;

		return s * 512 / SIDE;

	}

	/**

	* Returns the angle of the control vector in positive degrees west of north,

	* or negative degrees east of north, whichever is less than or equal to

	* 180 degrees total.

	*/

	public int getAngle () {

		int dx = x - cx;

		int dy = cy - y;

		// find reference angle in radians

		double theta = Math.atan2(Math.abs(dx), Math.abs(dy));

		// transform to degrees

		theta = theta * 180 / Math.PI;

		// adjust for quadrant

		if (dx < 0 && dy < 0)

			theta = 90 + theta;

		else if (dx < 0 && dy >= 0)

			theta = 90 - theta;

		else if (dx >= 0 && dy < 0)

			theta = -90 - theta;

		else

			theta = -90 + theta;

		return (int) theta;

	}

	private int getMotorL () {

		if (getSpeed() == 0)

			return 0;

		int dx = x - cx;

		int dy = (cy - y) * 255 / getSpeed();

		int val = 0;

		// Dependent on quadrant

		if (dx < 0 && dy < 0)

			val = -255;

		else if (dx < 0 && dy >= 0)

			val = dy * 1024 / SIDE - 255;

		else if (dx >= 0 && dy < 0)

			val = dy * 1024 / SIDE + 255;

		else

			val = 255;

		// Normalize to 0-255

		return val * getSpeed() / 255;

	}

	private int getMotorR () {

		if (getSpeed() == 0)

			return 0;

		int dx = x - cx;

		int dy = (cy - y) * 255 / getSpeed();

		int val = 0;

		// Dependent on quadrant

		if (dx < 0 && dy < 0)

			val = dy * 1024 / SIDE + 255;

		else if (dx < 0 && dy >= 0)

			val = 255;

		else if (dx >= 0 && dy < 0)

			val = -255;

		else

			val = dy * 1024 / SIDE - 255;

		// Normalize to 0-255

		return val * getSpeed() / 255;

	}

	private int eliminateDeadZone (int x) {

		final int START = 150;

		int val;

		if (x == 0)

			return 0;

		if (x > 0)

			val = (int) ((1 - 1.0 * START / 255) * x + START);

		else

			val = (int) ((1 - 1.0 * START / 255) * x - START);

		return val;

	}

	public void paint (Graphics g) {

		// Clear image

		g.setColor(Color.BLACK);

		g.fillRect(0, 0, WIDTH, HEIGHT);

		((Graphics2D)g).setStroke(new BasicStroke(1));

		// Motor indicators

		int motor1 = getMotorL() * BOT_SIZE / 512;

		int motor2 = getMotorR() * BOT_SIZE / 512;

		g.setColor(Color.YELLOW);

		if (motor1 < 0)

			g.fillRect(cx-BOT_SIZE/2 - WHEEL_SIZE, cy, WHEEL_SIZE, -motor1);

		else

			g.fillRect(cx-BOT_SIZE/2 - WHEEL_SIZE, cy-motor1, WHEEL_SIZE, motor1);

		if (motor2 < 0)

			g.fillRect(cx+BOT_SIZE/2, cy, WHEEL_SIZE, -motor2);

		else

			g.fillRect(cx+BOT_SIZE/2, cy-motor2, WHEEL_SIZE, motor2);

		// Watermark

		g.setColor(Color.GRAY);

		g.drawOval(cx-BOT_SIZE/2, cy-BOT_SIZE/2, BOT_SIZE, BOT_SIZE);

		g.drawRect(cx-BOT_SIZE/2 - WHEEL_SIZE, cy-BOT_SIZE/2, WHEEL_SIZE, BOT_SIZE);

		g.drawRect(cx+BOT_SIZE/2, cy-BOT_SIZE/2, WHEEL_SIZE, BOT_SIZE);

		// Targeting circle

		g.setColor(Color.RED);

		g.drawOval(cx-SIDE/2, cy-SIDE/2, SIDE, SIDE);

		((Graphics2D)g).setStroke(new BasicStroke(2));

		// Vector Line

		g.setColor(Color.GREEN);

		g.drawLine(cx, cy, x, y);

		g.fillOval(x-3, y-3, 6, 6);

	}

	/** Set the controller to the maximum forward velocity. */

	public void setMaxForward () {

		setPoint(cx, cy - (SIDE/2) + 1);

	}

	/** Set the controller to the maximum reverse velocity. */

	public void setMaxReverse () {

		setPoint(cx, cy + (SIDE/2) - 1);

	}

	/** Set the controller to the maximum left velocity, 

	* causing to robot to turn counter-clockwise.

	*/

	public void setMaxLeft () {

		setPoint(cx - (SIDE/2) + 1, cy);

	}

	/** Set the controller to the maximum right velocity, 

	* causing to robot to turn clockwise.

	*/

	public void setMaxRight () {

		setPoint(cx + (SIDE/2) - 1, cy);

	}

	/** Set the controller to (0,0), the stopped state. */

	public void setZero () {

		setPoint(cx, cy);

	}

	/** Sends the current vector to the robot(s). */

	public void sendToServer () {

		// Determine destination ID

		String dest = ColonetServerInterface.GLOBAL_DEST;

		/*if (cmbRobotNum != null && cmbRobotNum.getSelectedIndex() > 0) {

			dest = (String)cmbRobotNum.getSelectedItem();

		}*/

		if (csi == null)

			return;

		String motor1_string;

		String motor2_string;

		int motor1 = eliminateDeadZone(getMotorL());

		int motor2 = eliminateDeadZone(getMotorR());

		if (motor1 > 0) {

			motor1_string = " 1 " + motor1;

		} else {

			motor1_string = " 0 " + (-motor1);

		}

		if (motor2 > 0) {

			motor2_string = " 1 " + motor2;

		} else {

			motor2_string = " 0 " + (-motor2);

		}

		csi.sendData(ColonetServerInterface.MOTOR1_SET + motor1_string, dest);

		csi.sendData(ColonetServerInterface.MOTOR2_SET + motor2_string, dest);

		/*

		// Directional commands

		if (x > cx && y == cy) {	//move right

			csi.sendData(ColonetServerInterface.MOTOR2_SET + " 0 200", dest);

			csi.sendData(ColonetServerInterface.MOTOR1_SET + " 1 200", dest);

		} else if (x < cx && y == cy) {	 //move left

			csi.sendData(ColonetServerInterface.MOTOR2_SET + " 1 200", dest);

			csi.sendData(ColonetServerInterface.MOTOR1_SET + " 0 200", dest);

		} else if (x == cx && y > cy) {	 //move forward

			csi.sendData(ColonetServerInterface.MOTOR2_SET + " 0 225", dest);

			csi.sendData(ColonetServerInterface.MOTOR1_SET + " 0 225", dest);

		} else if (x == cx && y < cy) {	 //move backward

			csi.sendData(ColonetServerInterface.MOTOR2_SET + " 1 225", dest);

			csi.sendData(ColonetServerInterface.MOTOR1_SET + " 1 225", dest);

		} else if (x == cx && y == cy) {	//stop!

			csi.sendData(ColonetServerInterface.MOTOR2_SET + " 1 0", dest);

			csi.sendData(ColonetServerInterface.MOTOR1_SET + " 1 0", dest);

		}

		*/

		/*

		// Atomic Directional commands

		if (x > cx && y == cy) {  //move right

			csi.sendData(ColonetServerInterface.MOVE_R, dest);

		} else if (x < cx && y == cy) {	 //move left

			csi.sendData(ColonetServerInterface.MOVE_L, dest);

		} else if (x == cx && y > cy) {	 //move forward

			csi.sendData(ColonetServerInterface.MOVE_F, dest);

		} else if (x == cx && y < cy) {	 //move backward

			csi.sendData(ColonetServerInterface.MOVE_B, dest);

		} else if (x == cx && y == cy) { //stop

			csi.sendData(ColonetServerInterface.MOTORS_OFF, dest);

		}

		*/

	}

}