root / trunk / code / projects / colonet / client / VectorController.java @ 776
History | View | Annotate | Download (7.4 KB)
| 1 | import java.awt.*;
|
|---|---|
| 2 | import java.awt.event.*;
|
| 3 | import javax.swing.*;
|
| 4 | |
| 5 | /**
|
| 6 | * Controls robot motion graphically, using a mouse-controlled adjustable "vector." |
| 7 | * The controller uses the input vector to calculate a resulting velocity for each |
| 8 | * wheel on the robot. These velocities are also shown graphically. The vecolities |
| 9 | * are "normalized" internally to approximately eliminate the "dead zone" in which |
| 10 | * the robot applies partial power to the motors but does not move. |
| 11 | * @author Gregory Tress |
| 12 | */ |
| 13 | public class VectorController extends GraphicsPanel implements MouseListener, MouseMotionListener { |
| 14 | // State variables
|
| 15 | int x, y, cx, cy;
|
| 16 | final Colonet colonet;
|
| 17 | |
| 18 | // Painting constants
|
| 19 | final int WIDTH, HEIGHT; |
| 20 | final int SIDE; |
| 21 | final int BOT_SIZE = 70; |
| 22 | final int WHEEL_SIZE = 15; |
| 23 | |
| 24 | public VectorController (Image img, Colonet colonet) {
|
| 25 | super (img);
|
| 26 | WIDTH = img.getWidth(null);
|
| 27 | HEIGHT = img.getHeight(null);
|
| 28 | cx = WIDTH/2;
|
| 29 | cy = HEIGHT/2;
|
| 30 | x = cx; |
| 31 | y = cy; |
| 32 | if (WIDTH < HEIGHT) {
|
| 33 | SIDE = WIDTH; |
| 34 | } else {
|
| 35 | SIDE = HEIGHT; |
| 36 | } |
| 37 | this.colonet = colonet;
|
| 38 | |
| 39 | this.addMouseListener(this); |
| 40 | this.addMouseMotionListener(this); |
| 41 | } |
| 42 | |
| 43 | /** Set the robot motion vector. The "vector" is defined as
|
| 44 | * (0,0)->(x,y) where (0,0) is in the center of the targeting ring. |
| 45 | * Does nothing if the specified point is outside the ring. |
| 46 | * @param x The x coordinate of the point. |
| 47 | * @param y The y coordinate of the point. |
| 48 | */ |
| 49 | public void setPoint (int x, int y) { |
| 50 | if (isValidPoint(x, y)) {
|
| 51 | this.x = x;
|
| 52 | this.y = y;
|
| 53 | } |
| 54 | } |
| 55 | |
| 56 | /** Determines whether a point is inside the targeting ring.
|
| 57 | * @param x The x coordinate of the point. |
| 58 | * @param y The y coordinate of the point. |
| 59 | * @return True if a point is within the targeting ring. |
| 60 | */ |
| 61 | public boolean isValidPoint (int x, int y) { |
| 62 | double xsq = Math.pow(1.0*(x - cx)/(SIDE/2), 2); |
| 63 | double ysq = Math.pow(1.0*(y - cy)/(SIDE/2), 2); |
| 64 | return (xsq + ysq <= 1); |
| 65 | } |
| 66 | |
| 67 | /** Notifies the controller that a MouseEvent has occurred
|
| 68 | * on the controller surface. This should be called when a |
| 69 | * MouseListener attached to the controller has detected |
| 70 | * that a MouseEvent has occurred that may influence the state |
| 71 | * of the controller, such as mouseClicked, mouseDragged, or |
| 72 | * mouseReleased events. |
| 73 | * |
| 74 | * @param e The MouseEvent object which contains the details of the event. |
| 75 | * @param send Determines whether the motion vector established |
| 76 | * as a result of the MouseEvent should subsequently |
| 77 | * be sent to the robot(s). |
| 78 | */ |
| 79 | public void notifyMouseEvent (MouseEvent e, boolean send) { |
| 80 | if (!isValidPoint(e.getX(), e.getY())) {
|
| 81 | return;
|
| 82 | } |
| 83 | setPoint(e.getX(), e.getY()); |
| 84 | repaint(); |
| 85 | if (send) {
|
| 86 | Runnable r = new Runnable () {
|
| 87 | public void run () { |
| 88 | sendToServer(); |
| 89 | } |
| 90 | }; |
| 91 | (new Thread(r)).start();
|
| 92 | } |
| 93 | } |
| 94 | |
| 95 | public void mouseExited(MouseEvent e) { |
| 96 | } |
| 97 | public void mouseEntered(MouseEvent e) { |
| 98 | } |
| 99 | public void mouseReleased(MouseEvent e) { |
| 100 | notifyMouseEvent(e, true);
|
| 101 | } |
| 102 | public void mouseClicked(MouseEvent e) { |
| 103 | notifyMouseEvent(e, false);
|
| 104 | } |
| 105 | public void mousePressed(MouseEvent e) { |
| 106 | } |
| 107 | public void mouseDragged(MouseEvent e) { |
| 108 | notifyMouseEvent(e, false);
|
| 109 | } |
| 110 | public void mouseMoved(MouseEvent e) { |
| 111 | } |
| 112 | |
| 113 | /** Calculates the magnitude of the vector.
|
| 114 | * @return An int that is the truncated value of the speed of the vector. |
| 115 | */ |
| 116 | public int getSpeed () { |
| 117 | int dx = x - cx;
|
| 118 | int dy = y - cy;
|
| 119 | int s = (int) Math.sqrt( Math.pow(dx, 2) + Math.pow(dy, 2) ); |
| 120 | int maxspeed = SIDE/2; |
| 121 | return s * 512 / SIDE; |
| 122 | } |
| 123 | |
| 124 | /**
|
| 125 | * Returns the angle of the control vector in positive degrees west of north, |
| 126 | * or negative degrees east of north, whichever is less than or equal to |
| 127 | * 180 degrees total. |
| 128 | */ |
| 129 | public int getAngle () { |
| 130 | int dx = x - cx;
|
| 131 | int dy = cy - y;
|
| 132 | // find reference angle in radians
|
| 133 | double theta = Math.atan2(Math.abs(dx), Math.abs(dy));
|
| 134 | // transform to degrees
|
| 135 | theta = theta * 180 / Math.PI;
|
| 136 | // adjust for quadrant
|
| 137 | if (dx < 0 && dy < 0) |
| 138 | theta = 90 + theta;
|
| 139 | else if (dx < 0 && dy >= 0) |
| 140 | theta = 90 - theta;
|
| 141 | else if (dx >= 0 && dy < 0) |
| 142 | theta = -90 - theta;
|
| 143 | else
|
| 144 | theta = -90 + theta;
|
| 145 | return (int) theta; |
| 146 | } |
| 147 | |
| 148 | private int getMotorL () { |
| 149 | if (getSpeed() == 0) |
| 150 | return 0; |
| 151 | int dx = x - cx;
|
| 152 | int dy = (cy - y) * 255 / getSpeed(); |
| 153 | int val = 0; |
| 154 | // Dependent on quadrant
|
| 155 | if (dx < 0 && dy < 0) |
| 156 | val = -255;
|
| 157 | else if (dx < 0 && dy >= 0) |
| 158 | val = dy * 1024 / SIDE - 255; |
| 159 | else if (dx >= 0 && dy < 0) |
| 160 | val = dy * 1024 / SIDE + 255; |
| 161 | else
|
| 162 | val = 255;
|
| 163 | // Normalize to 0-255
|
| 164 | return val * getSpeed() / 255; |
| 165 | } |
| 166 | |
| 167 | private int getMotorR () { |
| 168 | if (getSpeed() == 0) |
| 169 | return 0; |
| 170 | int dx = x - cx;
|
| 171 | int dy = (cy - y) * 255 / getSpeed(); |
| 172 | int val = 0; |
| 173 | // Dependent on quadrant
|
| 174 | if (dx < 0 && dy < 0) |
| 175 | val = dy * 1024 / SIDE + 255; |
| 176 | else if (dx < 0 && dy >= 0) |
| 177 | val = 255;
|
| 178 | else if (dx >= 0 && dy < 0) |
| 179 | val = -255;
|
| 180 | else
|
| 181 | val = dy * 1024 / SIDE - 255; |
| 182 | // Normalize to 0-255
|
| 183 | return val * getSpeed() / 255; |
| 184 | } |
| 185 | |
| 186 | private int eliminateDeadZone (int x) { |
| 187 | final int START = 150; |
| 188 | int val;
|
| 189 | if (x == 0) |
| 190 | return 0; |
| 191 | if (x > 0) |
| 192 | val = (int) ((1 - 1.0 * START / 255) * x + START); |
| 193 | else
|
| 194 | val = (int) ((1 - 1.0 * START / 255) * x - START); |
| 195 | return val;
|
| 196 | |
| 197 | } |
| 198 | |
| 199 | public void paint (Graphics g) { |
| 200 | // Clear image
|
| 201 | g.setColor(Color.BLACK); |
| 202 | g.fillRect(0, 0, WIDTH, HEIGHT); |
| 203 | ((Graphics2D)g).setStroke(new BasicStroke(1)); |
| 204 | |
| 205 | // Motor indicators
|
| 206 | int motor1 = getMotorL() * BOT_SIZE / 512; |
| 207 | int motor2 = getMotorR() * BOT_SIZE / 512; |
| 208 | g.setColor(Color.YELLOW); |
| 209 | if (motor1 < 0) |
| 210 | g.fillRect(cx-BOT_SIZE/2 - WHEEL_SIZE, cy, WHEEL_SIZE, -motor1);
|
| 211 | else
|
| 212 | g.fillRect(cx-BOT_SIZE/2 - WHEEL_SIZE, cy-motor1, WHEEL_SIZE, motor1);
|
| 213 | if (motor2 < 0) |
| 214 | g.fillRect(cx+BOT_SIZE/2, cy, WHEEL_SIZE, -motor2);
|
| 215 | else
|
| 216 | g.fillRect(cx+BOT_SIZE/2, cy-motor2, WHEEL_SIZE, motor2);
|
| 217 | |
| 218 | // Watermark
|
| 219 | g.setColor(Color.GRAY); |
| 220 | g.drawOval(cx-BOT_SIZE/2, cy-BOT_SIZE/2, BOT_SIZE, BOT_SIZE); |
| 221 | g.drawRect(cx-BOT_SIZE/2 - WHEEL_SIZE, cy-BOT_SIZE/2, WHEEL_SIZE, BOT_SIZE); |
| 222 | g.drawRect(cx+BOT_SIZE/2, cy-BOT_SIZE/2, WHEEL_SIZE, BOT_SIZE); |
| 223 | |
| 224 | // Targeting circle
|
| 225 | g.setColor(Color.RED); |
| 226 | g.drawOval(cx-SIDE/2, cy-SIDE/2, SIDE, SIDE); |
| 227 | ((Graphics2D)g).setStroke(new BasicStroke(2)); |
| 228 | |
| 229 | // Vector Line
|
| 230 | g.setColor(Color.GREEN); |
| 231 | g.drawLine(cx, cy, x, y); |
| 232 | g.fillOval(x-3, y-3, 6, 6); |
| 233 | } |
| 234 | |
| 235 | /** Set the controller to the maximum forward velocity. */
|
| 236 | public void setMaxForward () { |
| 237 | setPoint(cx, cy - (SIDE/2) + 1); |
| 238 | } |
| 239 | |
| 240 | /** Set the controller to the maximum reverse velocity. */
|
| 241 | public void setMaxReverse () { |
| 242 | setPoint(cx, cy + (SIDE/2) - 1); |
| 243 | } |
| 244 | |
| 245 | /** Set the controller to the maximum left velocity,
|
| 246 | * causing to robot to turn counter-clockwise. |
| 247 | */ |
| 248 | public void setMaxLeft () { |
| 249 | setPoint(cx - (SIDE/2) + 1, cy); |
| 250 | } |
| 251 | |
| 252 | /** Set the controller to the maximum right velocity,
|
| 253 | * causing to robot to turn clockwise. |
| 254 | */ |
| 255 | public void setMaxRight () { |
| 256 | setPoint(cx + (SIDE/2) - 1, cy); |
| 257 | } |
| 258 | |
| 259 | /** Set the controller to (0,0), the stopped state. */
|
| 260 | public void setZero () { |
| 261 | setPoint(cx, cy); |
| 262 | } |
| 263 | |
| 264 | /** Sends the current vector to the robot(s). */
|
| 265 | public void sendToServer () { |
| 266 | // Determine destination ID
|
| 267 | String dest = ColonetServerInterface.GLOBAL_DEST; |
| 268 | ColonetServerInterface csi = colonet.getCSI(); |
| 269 | if (csi == null) |
| 270 | return;
|
| 271 | int motor1 = eliminateDeadZone(getMotorL());
|
| 272 | int motor2 = eliminateDeadZone(getMotorR());
|
| 273 | String motor1_string = (motor1 > 0) ? " 1 " + motor1 : " 0 " + (-motor1); |
| 274 | String motor2_string = (motor2 > 0) ? " 1 " + motor2 : " 0 " + (-motor2); |
| 275 | |
| 276 | csi.sendData(ColonetServerInterface.MOVE + motor1_string + motor2_string, dest); |
| 277 | } |
| 278 | |
| 279 | } |
| 280 |