Colony

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;

        // 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) {

                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.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.

        * Does nothing if the specified point is outside the 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;

                ColonetServerInterface csi = colonet.getCSI();

                if (csi == null)

                        return;

                int motor1 = eliminateDeadZone(getMotorL());

                int motor2 = eliminateDeadZone(getMotorR());

                String motor1_string = (motor1 > 0) ? " 1 " + motor1 : " 0 " + (-motor1);

                String motor2_string = (motor2 > 0) ? " 1 " + motor2 : " 0 " + (-motor2);

                csi.sendData(ColonetServerInterface.MOVE + motor1_string + motor2_string, dest);

        }

}