Revision 590
Robot vector controller works, with motor status diagram
Colonet.java | ||
---|---|---|
23 | 23 |
final int RADIUS = 30; |
24 | 24 |
|
25 | 25 |
// Used for the robot controller |
26 |
final int VECTOR_CONTROLLER_HEIGHT = 200;
|
|
27 |
final int VECTOR_CONTROLLER_WIDTH = 330;
|
|
26 |
final int VECTOR_CONTROLLER_HEIGHT = 190;
|
|
27 |
final int VECTOR_CONTROLLER_WIDTH = 320;
|
|
28 | 28 |
|
29 | 29 |
// Connection |
30 | 30 |
JTextField txtHost; |
... | ... | |
429 | 429 |
} |
430 | 430 |
|
431 | 431 |
public void disconnect () { |
432 |
dataUpdater.interrupt(); |
|
432 |
try { |
|
433 |
dataUpdater.interrupt(); |
|
434 |
} catch (Exception e) { |
|
435 |
} |
|
433 | 436 |
csi = null; |
434 | 437 |
Runnable r = new Runnable() { |
435 | 438 |
public void run () { |
... | ... | |
805 | 808 |
* |
806 | 809 |
*/ |
807 | 810 |
class DataUpdater extends Thread { |
808 |
final int DATAUPDATER_DELAY = 250; |
|
809 |
|
|
811 |
final int DATAUPDATER_DELAY = 500; |
|
810 | 812 |
public DataUpdater () { |
811 | 813 |
super("Colonet DataUpdater"); |
812 | 814 |
} |
... | ... | |
1062 | 1064 |
*/ |
1063 | 1065 |
class VectorController extends GraphicsPanel implements MouseListener, MouseMotionListener { |
1064 | 1066 |
int x, y, cx, cy; |
1065 |
int width, height; |
|
1066 |
int side; |
|
1067 |
final int WIDTH, HEIGHT; |
|
1068 |
final int SIDE; |
|
1069 |
final int BOT_SIZE = 70; |
|
1070 |
final int WHEEL_SIZE = 15; |
|
1067 | 1071 |
|
1068 | 1072 |
public VectorController (Image img) { |
1069 | 1073 |
super (img); |
1070 |
width = img.getWidth(null);
|
|
1071 |
height = img.getHeight(null);
|
|
1072 |
cx = img.getWidth(null)/2;
|
|
1073 |
cy = img.getHeight(null)/2;
|
|
1074 |
WIDTH = img.getWidth(null);
|
|
1075 |
HEIGHT = img.getHeight(null);
|
|
1076 |
cx = WIDTH/2;
|
|
1077 |
cy = HEIGHT/2;
|
|
1074 | 1078 |
x = cx; |
1075 | 1079 |
y = cy; |
1076 |
if (width < height) {
|
|
1077 |
side = width;
|
|
1080 |
if (WIDTH < HEIGHT) {
|
|
1081 |
SIDE = WIDTH;
|
|
1078 | 1082 |
} else { |
1079 |
side = height;
|
|
1083 |
SIDE = HEIGHT;
|
|
1080 | 1084 |
} |
1081 | 1085 |
this.addMouseListener(this); |
1082 | 1086 |
this.addMouseMotionListener(this); |
... | ... | |
1090 | 1094 |
} |
1091 | 1095 |
|
1092 | 1096 |
public boolean isValidPoint (int x, int y) { |
1093 |
double xterm = Math.pow(1.0*(x - cx)/(side/2), 2);
|
|
1094 |
double yterm = Math.pow(1.0*(y - cy)/(side/2), 2);
|
|
1095 |
return (xterm + yterm <= 1);
|
|
1097 |
double xsq = Math.pow(1.0*(x - cx)/(SIDE/2), 2);
|
|
1098 |
double ysq = Math.pow(1.0*(y - cy)/(SIDE/2), 2);
|
|
1099 |
return (xsq + ysq <= 1);
|
|
1096 | 1100 |
} |
1097 | 1101 |
|
1098 | 1102 |
public void notifyMouseEvent (MouseEvent e, boolean send) { |
... | ... | |
1102 | 1106 |
setPoint(e.getX(), e.getY()); |
1103 | 1107 |
repaint(); |
1104 | 1108 |
if (send) { |
1105 |
sendToServer(); |
|
1109 |
Runnable r = new Runnable () { |
|
1110 |
public void run () { |
|
1111 |
sendToServer(); |
|
1112 |
} |
|
1113 |
}; |
|
1114 |
(new Thread(r)).start(); |
|
1106 | 1115 |
} |
1107 | 1116 |
} |
1108 | 1117 |
|
... | ... | |
1127 | 1136 |
public int getSpeed () { |
1128 | 1137 |
int dx = x - cx; |
1129 | 1138 |
int dy = y - cy; |
1130 |
int v = (int) Math.sqrt( Math.pow(dx, 2) + Math.pow(dy, 2) ); |
|
1131 |
return v; |
|
1139 |
int s = (int) Math.sqrt( Math.pow(dx, 2) + Math.pow(dy, 2) ); |
|
1140 |
int maxspeed = SIDE/2; |
|
1141 |
return s * 512 / SIDE; |
|
1132 | 1142 |
} |
1133 | 1143 |
|
1134 | 1144 |
/** |
... | ... | |
1154 | 1164 |
theta = -90 + theta; |
1155 | 1165 |
return (int) theta; |
1156 | 1166 |
} |
1167 |
|
|
1168 |
public int getMotorL () { |
|
1169 |
if (getSpeed() == 0) |
|
1170 |
return 0; |
|
1171 |
int dx = x - cx; |
|
1172 |
int dy = (cy - y) * 255 / getSpeed(); |
|
1173 |
int val = 0; |
|
1174 |
// Dependent on quadrant |
|
1175 |
if (dx < 0 && dy < 0) |
|
1176 |
val = -255; |
|
1177 |
else if (dx < 0 && dy >= 0) |
|
1178 |
val = dy * 1024 / SIDE - 255; |
|
1179 |
else if (dx >= 0 && dy < 0) |
|
1180 |
val = dy * 1024 / SIDE + 255; |
|
1181 |
else |
|
1182 |
val = 255; |
|
1183 |
return val * getSpeed() / 255; |
|
1184 |
} |
|
1185 |
|
|
1186 |
public int getMotorR () { |
|
1187 |
if (getSpeed() == 0) |
|
1188 |
return 0; |
|
1189 |
int dx = x - cx; |
|
1190 |
int dy = (cy - y) * 255 / getSpeed(); |
|
1191 |
int val = 0; |
|
1192 |
// Dependent on quadrant |
|
1193 |
if (dx < 0 && dy < 0) |
|
1194 |
val = dy * 1024 / SIDE + 255; |
|
1195 |
else if (dx < 0 && dy >= 0) |
|
1196 |
val = 255; |
|
1197 |
else if (dx >= 0 && dy < 0) |
|
1198 |
val = -255; |
|
1199 |
else |
|
1200 |
val = dy * 1024 / SIDE - 255; |
|
1201 |
return val * getSpeed() / 255; |
|
1202 |
} |
|
1157 | 1203 |
|
1158 | 1204 |
public void paint (Graphics g) { |
1159 | 1205 |
// Clear image |
1160 | 1206 |
g.setColor(Color.BLACK); |
1161 |
g.fillRect(0, 0, width, height);
|
|
1207 |
g.fillRect(0, 0, WIDTH, HEIGHT);
|
|
1162 | 1208 |
((Graphics2D)g).setStroke(new BasicStroke(1)); |
1163 | 1209 |
|
1210 |
// Motor indicators |
|
1211 |
int motor1 = getMotorL() * BOT_SIZE / 512; |
|
1212 |
int motor2 = getMotorR() * BOT_SIZE / 512; |
|
1213 |
g.setColor(Color.YELLOW); |
|
1214 |
if (motor1 < 0) |
|
1215 |
g.fillRect(cx-BOT_SIZE/2 - WHEEL_SIZE, cy, WHEEL_SIZE, -motor1); |
|
1216 |
else |
|
1217 |
g.fillRect(cx-BOT_SIZE/2 - WHEEL_SIZE, cy-motor1, WHEEL_SIZE, motor1); |
|
1218 |
if (motor2 < 0) |
|
1219 |
g.fillRect(cx+BOT_SIZE/2, cy, WHEEL_SIZE, -motor2); |
|
1220 |
else |
|
1221 |
g.fillRect(cx+BOT_SIZE/2, cy-motor2, WHEEL_SIZE, motor2); |
|
1222 |
|
|
1164 | 1223 |
// Watermark |
1165 | 1224 |
g.setColor(Color.GRAY); |
1166 |
g.drawOval(cx-side/2 + 30, cy-side/2 + 30, side - 60, side - 60);
|
|
1167 |
g.drawRect(cx-side/2 + 30 - 30, cy-side/2 + 30, 30, side - 60);
|
|
1168 |
g.drawRect(cx+side/2 - 30 - 30, cy-side/2 + 30, 30, side - 60);
|
|
1225 |
g.drawOval(cx-BOT_SIZE/2, cy-BOT_SIZE/2, BOT_SIZE, BOT_SIZE);
|
|
1226 |
g.drawRect(cx-BOT_SIZE/2 - WHEEL_SIZE, cy-BOT_SIZE/2, WHEEL_SIZE, BOT_SIZE);
|
|
1227 |
g.drawRect(cx+BOT_SIZE/2, cy-BOT_SIZE/2, WHEEL_SIZE, BOT_SIZE);
|
|
1169 | 1228 |
|
1170 | 1229 |
// Targeting circle |
1171 | 1230 |
g.setColor(Color.RED); |
1172 |
g.drawOval(cx-side/2, cy-side/2, side, side);
|
|
1231 |
g.drawOval(cx-SIDE/2, cy-SIDE/2, SIDE, SIDE);
|
|
1173 | 1232 |
((Graphics2D)g).setStroke(new BasicStroke(2)); |
1174 | 1233 |
|
1175 | 1234 |
// Vector Line |
... | ... | |
1179 | 1238 |
} |
1180 | 1239 |
|
1181 | 1240 |
public void setMaxForward () { |
1182 |
setPoint(cx, cy - (side/2) + 1);
|
|
1241 |
setPoint(cx, cy - (SIDE/2) + 1);
|
|
1183 | 1242 |
} |
1184 | 1243 |
|
1185 | 1244 |
public void setMaxReverse () { |
1186 |
setPoint(cx, cy + (side/2) - 1);
|
|
1245 |
setPoint(cx, cy + (SIDE/2) - 1);
|
|
1187 | 1246 |
} |
1188 | 1247 |
|
1189 | 1248 |
public void setMaxLeft () { |
1190 |
setPoint(cx - (side/2) + 1, cy);
|
|
1249 |
setPoint(cx - (SIDE/2) + 1, cy);
|
|
1191 | 1250 |
} |
1192 | 1251 |
|
1193 | 1252 |
public void setMaxRight () { |
1194 |
setPoint(cx + (side/2) - 1, cy);
|
|
1253 |
setPoint(cx + (SIDE/2) - 1, cy);
|
|
1195 | 1254 |
} |
1196 | 1255 |
|
1197 | 1256 |
public void setZero () { |
... | ... | |
1199 | 1258 |
} |
1200 | 1259 |
|
1201 | 1260 |
public void sendToServer () { |
1202 |
//System.out.println("Attempting to send angle = " + getAngle() + ", speed = " + getSpeed() + "");
|
|
1261 |
// Determine destination ID
|
|
1203 | 1262 |
String dest = ColonetServerInterface.GLOBAL_DEST; |
1204 | 1263 |
if (cmbRobotNum != null && cmbRobotNum.getSelectedIndex() > 0) { |
1205 | 1264 |
dest = (String)cmbRobotNum.getSelectedItem(); |
1206 | 1265 |
} |
1207 | 1266 |
|
1208 |
if (csi != null) { |
|
1209 |
/* |
|
1210 |
csi.sendData(ColonetServerInterface.MOVE + " " + getSpeed() + " " + getAngle(), dest); |
|
1211 |
*/ |
|
1212 |
/* |
|
1213 |
//Directional commands |
|
1214 |
if (x > cx && y == cy) { //move right |
|
1215 |
csi.sendData(ColonetServerInterface.MOTOR2_SET + " 0 200", dest); |
|
1216 |
csi.sendData(ColonetServerInterface.MOTOR1_SET + " 1 200", dest); |
|
1217 |
} else if (x < cx && y == cy) { //move left |
|
1218 |
csi.sendData(ColonetServerInterface.MOTOR2_SET + " 1 200", dest); |
|
1219 |
csi.sendData(ColonetServerInterface.MOTOR1_SET + " 0 200", dest); |
|
1220 |
} else if (x == cx && y > cy) { //move forward |
|
1221 |
csi.sendData(ColonetServerInterface.MOTOR2_SET + " 0 225", dest); |
|
1222 |
csi.sendData(ColonetServerInterface.MOTOR1_SET + " 0 225", dest); |
|
1223 |
} else if (x == cx && y < cy) { //move backward |
|
1224 |
csi.sendData(ColonetServerInterface.MOTOR2_SET + " 1 225", dest); |
|
1225 |
csi.sendData(ColonetServerInterface.MOTOR1_SET + " 1 225", dest); |
|
1226 |
} else if (x == cx && y == cy) { //stop! |
|
1227 |
csi.sendData(ColonetServerInterface.MOTOR2_SET + " 1 0", dest); |
|
1228 |
csi.sendData(ColonetServerInterface.MOTOR1_SET + " 1 0", dest); |
|
1229 |
} |
|
1230 |
*/ |
|
1231 |
//Directional commands |
|
1232 |
if (x > cx && y == cy) { //move right |
|
1233 |
csi.sendData(ColonetServerInterface.MOVE_R, dest); |
|
1234 |
} else if (x < cx && y == cy) { //move left |
|
1235 |
csi.sendData(ColonetServerInterface.MOVE_L, dest); |
|
1236 |
} else if (x == cx && y > cy) { //move forward |
|
1237 |
csi.sendData(ColonetServerInterface.MOVE_F, dest); |
|
1238 |
} else if (x == cx && y < cy) { //move backward |
|
1239 |
csi.sendData(ColonetServerInterface.MOVE_B, dest); |
|
1240 |
} else if (x == cx && y == cy) { //stop |
|
1241 |
csi.sendData(ColonetServerInterface.MOTORS_OFF, dest); |
|
1242 |
} |
|
1267 |
if (csi == null) |
|
1268 |
return; |
|
1269 |
|
|
1270 |
String motor1_string; |
|
1271 |
String motor2_string; |
|
1272 |
int motor1 = getMotorL(); |
|
1273 |
int motor2 = getMotorR(); |
|
1274 |
|
|
1275 |
if (motor1 > 0) { |
|
1276 |
motor1_string = " 1 " + motor1; |
|
1277 |
} else { |
|
1278 |
motor1_string = " 0 " + (-motor1); |
|
1243 | 1279 |
} |
1280 |
if (motor2 > 0) { |
|
1281 |
motor2_string = " 1 " + motor2; |
|
1282 |
} else { |
|
1283 |
motor2_string = " 0 " + (-motor2); |
|
1284 |
} |
|
1285 |
|
|
1286 |
csi.sendData(ColonetServerInterface.MOTOR1_SET + motor1_string, dest); |
|
1287 |
csi.sendData(ColonetServerInterface.MOTOR2_SET + motor2_string, dest); |
|
1288 |
|
|
1289 |
/* |
|
1290 |
// Directional commands |
|
1291 |
if (x > cx && y == cy) { //move right |
|
1292 |
csi.sendData(ColonetServerInterface.MOTOR2_SET + " 0 200", dest); |
|
1293 |
csi.sendData(ColonetServerInterface.MOTOR1_SET + " 1 200", dest); |
|
1294 |
} else if (x < cx && y == cy) { //move left |
|
1295 |
csi.sendData(ColonetServerInterface.MOTOR2_SET + " 1 200", dest); |
|
1296 |
csi.sendData(ColonetServerInterface.MOTOR1_SET + " 0 200", dest); |
|
1297 |
} else if (x == cx && y > cy) { //move forward |
|
1298 |
csi.sendData(ColonetServerInterface.MOTOR2_SET + " 0 225", dest); |
|
1299 |
csi.sendData(ColonetServerInterface.MOTOR1_SET + " 0 225", dest); |
|
1300 |
} else if (x == cx && y < cy) { //move backward |
|
1301 |
csi.sendData(ColonetServerInterface.MOTOR2_SET + " 1 225", dest); |
|
1302 |
csi.sendData(ColonetServerInterface.MOTOR1_SET + " 1 225", dest); |
|
1303 |
} else if (x == cx && y == cy) { //stop! |
|
1304 |
csi.sendData(ColonetServerInterface.MOTOR2_SET + " 1 0", dest); |
|
1305 |
csi.sendData(ColonetServerInterface.MOTOR1_SET + " 1 0", dest); |
|
1306 |
} |
|
1307 |
*/ |
|
1308 |
|
|
1309 |
/* |
|
1310 |
// Atomic Directional commands |
|
1311 |
if (x > cx && y == cy) { //move right |
|
1312 |
csi.sendData(ColonetServerInterface.MOVE_R, dest); |
|
1313 |
} else if (x < cx && y == cy) { //move left |
|
1314 |
csi.sendData(ColonetServerInterface.MOVE_L, dest); |
|
1315 |
} else if (x == cx && y > cy) { //move forward |
|
1316 |
csi.sendData(ColonetServerInterface.MOVE_F, dest); |
|
1317 |
} else if (x == cx && y < cy) { //move backward |
|
1318 |
csi.sendData(ColonetServerInterface.MOVE_B, dest); |
|
1319 |
} else if (x == cx && y == cy) { //stop |
|
1320 |
csi.sendData(ColonetServerInterface.MOTORS_OFF, dest); |
|
1321 |
} |
|
1322 |
*/ |
|
1244 | 1323 |
} |
1245 | 1324 |
|
1246 | 1325 |
} |
Also available in: Unified diff