root / trunk / code / projects / colonet / client / VectorController.java @ 637
History | View | Annotate | Download (9.19 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 |
final ColonetServerInterface csi;
|
| 18 |
|
| 19 |
// Painting constants
|
| 20 |
final int WIDTH, HEIGHT; |
| 21 |
final int SIDE; |
| 22 |
final int BOT_SIZE = 70; |
| 23 |
final int WHEEL_SIZE = 15; |
| 24 |
|
| 25 |
public VectorController (Image img, Colonet colonet, ColonetServerInterface csi) { |
| 26 |
super (img);
|
| 27 |
WIDTH = img.getWidth(null);
|
| 28 |
HEIGHT = img.getHeight(null);
|
| 29 |
cx = WIDTH/2;
|
| 30 |
cy = HEIGHT/2;
|
| 31 |
x = cx; |
| 32 |
y = cy; |
| 33 |
if (WIDTH < HEIGHT) {
|
| 34 |
SIDE = WIDTH; |
| 35 |
} else {
|
| 36 |
SIDE = HEIGHT; |
| 37 |
} |
| 38 |
this.colonet = colonet;
|
| 39 |
this.csi = csi;
|
| 40 |
|
| 41 |
this.addMouseListener(this); |
| 42 |
this.addMouseMotionListener(this); |
| 43 |
} |
| 44 |
|
| 45 |
/** Set the robot motion vector. The "vector" is defined as
|
| 46 |
* (0,0)->(x,y) where (0,0) is in the center of the targeting ring.
|
| 47 |
* @param x The x coordinate of the point.
|
| 48 |
* @param y The y coordinate of the point.
|
| 49 |
*/
|
| 50 |
public void setPoint (int x, int y) { |
| 51 |
if (isValidPoint(x, y)) {
|
| 52 |
this.x = x;
|
| 53 |
this.y = y;
|
| 54 |
} |
| 55 |
} |
| 56 |
|
| 57 |
/** Determines whether a point is inside the targeting ring.
|
| 58 |
* @param x The x coordinate of the point.
|
| 59 |
* @param y The y coordinate of the point.
|
| 60 |
* @return True if a point is within the targeting ring.
|
| 61 |
*/
|
| 62 |
public boolean isValidPoint (int x, int y) { |
| 63 |
double xsq = Math.pow(1.0*(x - cx)/(SIDE/2), 2); |
| 64 |
double ysq = Math.pow(1.0*(y - cy)/(SIDE/2), 2); |
| 65 |
return (xsq + ysq <= 1); |
| 66 |
} |
| 67 |
|
| 68 |
/** Notifies the controller that a MouseEvent has occurred
|
| 69 |
* on the controller surface. This should be called when a
|
| 70 |
* MouseListener attached to the controller has detected
|
| 71 |
* that a MouseEvent has occurred that may influence the state
|
| 72 |
* of the controller, such as mouseClicked, mouseDragged, or
|
| 73 |
* mouseReleased events.
|
| 74 |
*
|
| 75 |
* @param e The MouseEvent object which contains the details of the event.
|
| 76 |
* @param send Determines whether the motion vector established
|
| 77 |
* as a result of the MouseEvent should subsequently
|
| 78 |
* be sent to the robot(s).
|
| 79 |
*/
|
| 80 |
public void notifyMouseEvent (MouseEvent e, boolean send) { |
| 81 |
if (!isValidPoint(e.getX(), e.getY())) {
|
| 82 |
return;
|
| 83 |
} |
| 84 |
setPoint(e.getX(), e.getY()); |
| 85 |
repaint(); |
| 86 |
if (send) {
|
| 87 |
Runnable r = new Runnable () { |
| 88 |
public void run () { |
| 89 |
sendToServer(); |
| 90 |
} |
| 91 |
}; |
| 92 |
(new Thread(r)).start(); |
| 93 |
} |
| 94 |
} |
| 95 |
|
| 96 |
public void mouseExited(MouseEvent e) { |
| 97 |
} |
| 98 |
public void mouseEntered(MouseEvent e) { |
| 99 |
} |
| 100 |
public void mouseReleased(MouseEvent e) { |
| 101 |
notifyMouseEvent(e, true);
|
| 102 |
} |
| 103 |
public void mouseClicked(MouseEvent e) { |
| 104 |
notifyMouseEvent(e, false);
|
| 105 |
} |
| 106 |
public void mousePressed(MouseEvent e) { |
| 107 |
} |
| 108 |
public void mouseDragged(MouseEvent e) { |
| 109 |
notifyMouseEvent(e, false);
|
| 110 |
} |
| 111 |
public void mouseMoved(MouseEvent e) { |
| 112 |
} |
| 113 |
|
| 114 |
/** Calculates the magnitude of the vector.
|
| 115 |
* @return An int that is the truncated value of the speed of the vector.
|
| 116 |
*/
|
| 117 |
public int getSpeed () { |
| 118 |
int dx = x - cx;
|
| 119 |
int dy = y - cy;
|
| 120 |
int s = (int) Math.sqrt( Math.pow(dx, 2) + Math.pow(dy, 2) ); |
| 121 |
int maxspeed = SIDE/2; |
| 122 |
return s * 512 / SIDE; |
| 123 |
} |
| 124 |
|
| 125 |
/**
|
| 126 |
* Returns the angle of the control vector in positive degrees west of north,
|
| 127 |
* or negative degrees east of north, whichever is less than or equal to
|
| 128 |
* 180 degrees total.
|
| 129 |
*/
|
| 130 |
public int getAngle () { |
| 131 |
int dx = x - cx;
|
| 132 |
int dy = cy - y;
|
| 133 |
// find reference angle in radians
|
| 134 |
double theta = Math.atan2(Math.abs(dx), Math.abs(dy)); |
| 135 |
// transform to degrees
|
| 136 |
theta = theta * 180 / Math.PI; |
| 137 |
// adjust for quadrant
|
| 138 |
if (dx < 0 && dy < 0) |
| 139 |
theta = 90 + theta;
|
| 140 |
else if (dx < 0 && dy >= 0) |
| 141 |
theta = 90 - theta;
|
| 142 |
else if (dx >= 0 && dy < 0) |
| 143 |
theta = -90 - theta;
|
| 144 |
else
|
| 145 |
theta = -90 + theta;
|
| 146 |
return (int) theta; |
| 147 |
} |
| 148 |
|
| 149 |
private int getMotorL () { |
| 150 |
if (getSpeed() == 0) |
| 151 |
return 0; |
| 152 |
int dx = x - cx;
|
| 153 |
int dy = (cy - y) * 255 / getSpeed(); |
| 154 |
int val = 0; |
| 155 |
// Dependent on quadrant
|
| 156 |
if (dx < 0 && dy < 0) |
| 157 |
val = -255;
|
| 158 |
else if (dx < 0 && dy >= 0) |
| 159 |
val = dy * 1024 / SIDE - 255; |
| 160 |
else if (dx >= 0 && dy < 0) |
| 161 |
val = dy * 1024 / SIDE + 255; |
| 162 |
else
|
| 163 |
val = 255;
|
| 164 |
// Normalize to 0-255
|
| 165 |
return val * getSpeed() / 255; |
| 166 |
} |
| 167 |
|
| 168 |
private int getMotorR () { |
| 169 |
if (getSpeed() == 0) |
| 170 |
return 0; |
| 171 |
int dx = x - cx;
|
| 172 |
int dy = (cy - y) * 255 / getSpeed(); |
| 173 |
int val = 0; |
| 174 |
// Dependent on quadrant
|
| 175 |
if (dx < 0 && dy < 0) |
| 176 |
val = dy * 1024 / SIDE + 255; |
| 177 |
else if (dx < 0 && dy >= 0) |
| 178 |
val = 255;
|
| 179 |
else if (dx >= 0 && dy < 0) |
| 180 |
val = -255;
|
| 181 |
else
|
| 182 |
val = dy * 1024 / SIDE - 255; |
| 183 |
// Normalize to 0-255
|
| 184 |
return val * getSpeed() / 255; |
| 185 |
} |
| 186 |
|
| 187 |
private int eliminateDeadZone (int x) { |
| 188 |
final int START = 150; |
| 189 |
int val;
|
| 190 |
if (x == 0) |
| 191 |
return 0; |
| 192 |
if (x > 0) |
| 193 |
val = (int) ((1 - 1.0 * START / 255) * x + START); |
| 194 |
else
|
| 195 |
val = (int) ((1 - 1.0 * START / 255) * x - START); |
| 196 |
return val;
|
| 197 |
|
| 198 |
} |
| 199 |
|
| 200 |
public void paint (Graphics g) { |
| 201 |
// Clear image
|
| 202 |
g.setColor(Color.BLACK);
|
| 203 |
g.fillRect(0, 0, WIDTH, HEIGHT); |
| 204 |
((Graphics2D)g).setStroke(new BasicStroke(1)); |
| 205 |
|
| 206 |
// Motor indicators
|
| 207 |
int motor1 = getMotorL() * BOT_SIZE / 512; |
| 208 |
int motor2 = getMotorR() * BOT_SIZE / 512; |
| 209 |
g.setColor(Color.YELLOW);
|
| 210 |
if (motor1 < 0) |
| 211 |
g.fillRect(cx-BOT_SIZE/2 - WHEEL_SIZE, cy, WHEEL_SIZE, -motor1);
|
| 212 |
else
|
| 213 |
g.fillRect(cx-BOT_SIZE/2 - WHEEL_SIZE, cy-motor1, WHEEL_SIZE, motor1);
|
| 214 |
if (motor2 < 0) |
| 215 |
g.fillRect(cx+BOT_SIZE/2, cy, WHEEL_SIZE, -motor2);
|
| 216 |
else
|
| 217 |
g.fillRect(cx+BOT_SIZE/2, cy-motor2, WHEEL_SIZE, motor2);
|
| 218 |
|
| 219 |
// Watermark
|
| 220 |
g.setColor(Color.GRAY);
|
| 221 |
g.drawOval(cx-BOT_SIZE/2, cy-BOT_SIZE/2, BOT_SIZE, BOT_SIZE); |
| 222 |
g.drawRect(cx-BOT_SIZE/2 - WHEEL_SIZE, cy-BOT_SIZE/2, WHEEL_SIZE, BOT_SIZE); |
| 223 |
g.drawRect(cx+BOT_SIZE/2, cy-BOT_SIZE/2, WHEEL_SIZE, BOT_SIZE); |
| 224 |
|
| 225 |
// Targeting circle
|
| 226 |
g.setColor(Color.RED);
|
| 227 |
g.drawOval(cx-SIDE/2, cy-SIDE/2, SIDE, SIDE); |
| 228 |
((Graphics2D)g).setStroke(new BasicStroke(2)); |
| 229 |
|
| 230 |
// Vector Line
|
| 231 |
g.setColor(Color.GREEN);
|
| 232 |
g.drawLine(cx, cy, x, y); |
| 233 |
g.fillOval(x-3, y-3, 6, 6); |
| 234 |
} |
| 235 |
|
| 236 |
/** Set the controller to the maximum forward velocity. */
|
| 237 |
public void setMaxForward () { |
| 238 |
setPoint(cx, cy - (SIDE/2) + 1); |
| 239 |
} |
| 240 |
|
| 241 |
/** Set the controller to the maximum reverse velocity. */
|
| 242 |
public void setMaxReverse () { |
| 243 |
setPoint(cx, cy + (SIDE/2) - 1); |
| 244 |
} |
| 245 |
|
| 246 |
/** Set the controller to the maximum left velocity,
|
| 247 |
* causing to robot to turn counter-clockwise.
|
| 248 |
*/
|
| 249 |
public void setMaxLeft () { |
| 250 |
setPoint(cx - (SIDE/2) + 1, cy); |
| 251 |
} |
| 252 |
|
| 253 |
/** Set the controller to the maximum right velocity,
|
| 254 |
* causing to robot to turn clockwise.
|
| 255 |
*/
|
| 256 |
public void setMaxRight () { |
| 257 |
setPoint(cx + (SIDE/2) - 1, cy); |
| 258 |
} |
| 259 |
|
| 260 |
/** Set the controller to (0,0), the stopped state. */
|
| 261 |
public void setZero () { |
| 262 |
setPoint(cx, cy); |
| 263 |
} |
| 264 |
|
| 265 |
/** Sends the current vector to the robot(s). */
|
| 266 |
public void sendToServer () { |
| 267 |
// Determine destination ID
|
| 268 |
String dest = ColonetServerInterface.GLOBAL_DEST;
|
| 269 |
/*if (cmbRobotNum != null && cmbRobotNum.getSelectedIndex() > 0) {
|
| 270 |
dest = (String)cmbRobotNum.getSelectedItem();
|
| 271 |
}*/
|
| 272 |
|
| 273 |
if (csi == null) |
| 274 |
return;
|
| 275 |
|
| 276 |
String motor1_string;
|
| 277 |
String motor2_string;
|
| 278 |
int motor1 = eliminateDeadZone(getMotorL());
|
| 279 |
int motor2 = eliminateDeadZone(getMotorR());
|
| 280 |
|
| 281 |
if (motor1 > 0) { |
| 282 |
motor1_string = " 1 " + motor1;
|
| 283 |
} else {
|
| 284 |
motor1_string = " 0 " + (-motor1);
|
| 285 |
} |
| 286 |
if (motor2 > 0) { |
| 287 |
motor2_string = " 1 " + motor2;
|
| 288 |
} else {
|
| 289 |
motor2_string = " 0 " + (-motor2);
|
| 290 |
} |
| 291 |
|
| 292 |
csi.sendData(ColonetServerInterface.MOTOR1_SET + motor1_string, dest); |
| 293 |
csi.sendData(ColonetServerInterface.MOTOR2_SET + motor2_string, dest); |
| 294 |
|
| 295 |
/*
|
| 296 |
// Directional commands
|
| 297 |
if (x > cx && y == cy) { //move right
|
| 298 |
csi.sendData(ColonetServerInterface.MOTOR2_SET + " 0 200", dest);
|
| 299 |
csi.sendData(ColonetServerInterface.MOTOR1_SET + " 1 200", dest);
|
| 300 |
} else if (x < cx && y == cy) { //move left
|
| 301 |
csi.sendData(ColonetServerInterface.MOTOR2_SET + " 1 200", dest);
|
| 302 |
csi.sendData(ColonetServerInterface.MOTOR1_SET + " 0 200", dest);
|
| 303 |
} else if (x == cx && y > cy) { //move forward
|
| 304 |
csi.sendData(ColonetServerInterface.MOTOR2_SET + " 0 225", dest);
|
| 305 |
csi.sendData(ColonetServerInterface.MOTOR1_SET + " 0 225", dest);
|
| 306 |
} else if (x == cx && y < cy) { //move backward
|
| 307 |
csi.sendData(ColonetServerInterface.MOTOR2_SET + " 1 225", dest);
|
| 308 |
csi.sendData(ColonetServerInterface.MOTOR1_SET + " 1 225", dest);
|
| 309 |
} else if (x == cx && y == cy) { //stop!
|
| 310 |
csi.sendData(ColonetServerInterface.MOTOR2_SET + " 1 0", dest);
|
| 311 |
csi.sendData(ColonetServerInterface.MOTOR1_SET + " 1 0", dest);
|
| 312 |
}
|
| 313 |
*/
|
| 314 |
|
| 315 |
/*
|
| 316 |
// Atomic Directional commands
|
| 317 |
if (x > cx && y == cy) { //move right
|
| 318 |
csi.sendData(ColonetServerInterface.MOVE_R, dest);
|
| 319 |
} else if (x < cx && y == cy) { //move left
|
| 320 |
csi.sendData(ColonetServerInterface.MOVE_L, dest);
|
| 321 |
} else if (x == cx && y > cy) { //move forward
|
| 322 |
csi.sendData(ColonetServerInterface.MOVE_F, dest);
|
| 323 |
} else if (x == cx && y < cy) { //move backward
|
| 324 |
csi.sendData(ColonetServerInterface.MOVE_B, dest);
|
| 325 |
} else if (x == cx && y == cy) { //stop
|
| 326 |
csi.sendData(ColonetServerInterface.MOTORS_OFF, dest);
|
| 327 |
}
|
| 328 |
*/
|
| 329 |
} |
| 330 |
|
| 331 |
} |
| 332 |
|