Revision 635
Moved some code. Added documentation. Enabled multiple timings for requests
| Colonet.java | ||
|---|---|---|
| 191 | 191 |
panelRobotDirectionButtons.add(btnR); |
| 192 | 192 |
|
| 193 | 193 |
imageVectorControl = gc.createCompatibleImage(VECTOR_CONTROLLER_WIDTH, VECTOR_CONTROLLER_HEIGHT); |
| 194 |
vectorController = new VectorController(imageVectorControl); |
|
| 194 |
vectorController = new VectorController(imageVectorControl, self, csi);
|
|
| 195 | 195 |
panelRobotDirection.setLayout(new BorderLayout()); |
| 196 | 196 |
panelRobotDirection.add(vectorController, BorderLayout.CENTER); |
| 197 | 197 |
panelRobotDirection.add(panelRobotDirectionButtons, BorderLayout.SOUTH); |
| ... | ... | |
| 810 | 810 |
|
| 811 | 811 |
/* |
| 812 | 812 |
* DataUpdater thread. |
| 813 |
* The purpose of this thread is to request data from the server at regular intervals. |
|
| 813 |
* The purpose of this thread is to request data from the server at regular intervals. |
|
| 814 |
* Types of data requested: XBee IDs, robot positions, battery levels. |
|
| 814 | 815 |
* |
| 815 | 816 |
*/ |
| 816 | 817 |
class DataUpdater extends Thread {
|
| 817 | 818 |
final int DATAUPDATER_DELAY = 200; |
| 819 |
int count; |
|
| 818 | 820 |
public DataUpdater () {
|
| 819 | 821 |
super("Colonet DataUpdater");
|
| 822 |
count = 0; |
|
| 820 | 823 |
} |
| 821 | 824 |
|
| 822 | 825 |
public synchronized void run () {
|
| 823 | 826 |
while (true) {
|
| 824 | 827 |
try {
|
| 825 |
//request more data |
|
| 826 | 828 |
if (csi != null && csi.isReady()) {
|
| 827 |
csi.sendPositionRequest(); |
|
| 828 |
csi.sendXBeeIDRequest(); |
|
| 829 |
for (Map.Entry<Integer,RobotIcon> entry : robotIcons.entrySet()) {
|
|
| 830 |
RobotIcon r = entry.getValue(); |
|
| 831 |
int id = r.id; |
|
| 832 |
if (id >= 0) {
|
|
| 833 |
csi.sendBatteryRequest(id); |
|
| 834 |
System.out.println("Sent battery request (" + id + ")");
|
|
| 835 |
} |
|
| 829 |
// XBee ID Request |
|
| 830 |
if (count % 5 == 0) {
|
|
| 831 |
csi.sendXBeeIDRequest(); |
|
| 836 | 832 |
} |
| 833 |
// Robot Position Request |
|
| 834 |
if (count % 1 == 0) {
|
|
| 835 |
csi.sendPositionRequest(); |
|
| 836 |
} |
|
| 837 |
// Battery Request |
|
| 838 |
if (count % 30 == 0) {
|
|
| 839 |
for (Map.Entry<Integer,RobotIcon> entry : robotIcons.entrySet()) {
|
|
| 840 |
RobotIcon r = entry.getValue(); |
|
| 841 |
int id = r.id; |
|
| 842 |
if (id >= 0) {
|
|
| 843 |
csi.sendBatteryRequest(id); |
|
| 844 |
System.out.println("Sent battery request (" + id + ")");
|
|
| 845 |
} |
|
| 846 |
} |
|
| 847 |
} |
|
| 837 | 848 |
} |
| 849 |
count++; |
|
| 850 |
if (count > 1000) |
|
| 851 |
count = 0; |
|
| 838 | 852 |
Thread.sleep(DATAUPDATER_DELAY); |
| 839 | 853 |
} catch (InterruptedException e) {
|
| 840 | 854 |
return; |
| ... | ... | |
| 846 | 860 |
} |
| 847 | 861 |
} |
| 848 | 862 |
|
| 849 |
/* |
|
| 850 |
* GraphicsPanel class |
|
| 851 |
* An extension of JPanel, designed for holding an image that will be repainted regularly. |
|
| 852 |
*/ |
|
| 853 |
class GraphicsPanel extends JPanel {
|
|
| 854 |
protected Image img; |
|
| 855 | 863 |
|
| 856 |
public GraphicsPanel (Image img) {
|
|
| 857 |
this(img, true); |
|
| 858 |
} |
|
| 859 |
|
|
| 860 |
public GraphicsPanel (Image img, boolean isDoubleBuffered) {
|
|
| 861 |
super(isDoubleBuffered); |
|
| 862 |
this.img = img; |
|
| 863 |
} |
|
| 864 |
|
|
| 865 |
public void paint (Graphics g) {
|
|
| 866 |
// Place the buffered image on the screen, inside the panel |
|
| 867 |
g.drawImage(img, 0, 0, Color.WHITE, this); |
|
| 868 |
} |
|
| 869 |
} |
|
| 870 |
|
|
| 871 | 864 |
/* |
| 872 | 865 |
* WebcamPanel class |
| 873 | 866 |
* Enables more efficient image handling in a component-controlled environment |
| ... | ... | |
| 974 | 967 |
g.drawImage(buffer, 0, 0, this); |
| 975 | 968 |
} |
| 976 | 969 |
|
| 977 |
/* |
|
| 970 |
/**
|
|
| 978 | 971 |
* Convert a click on the webcam panel to a coordinate that is consistent with the |
| 979 | 972 |
* original size of the image that the panel contains. |
| 980 | 973 |
*/ |
| ... | ... | |
| 1071 | 1064 |
} |
| 1072 | 1065 |
} |
| 1073 | 1066 |
|
| 1074 |
/* |
|
| 1075 |
* VectorController class |
|
| 1076 |
* Manages robot motion control graphically |
|
| 1077 |
*/ |
|
| 1078 |
class VectorController extends GraphicsPanel implements MouseListener, MouseMotionListener {
|
|
| 1079 |
int x, y, cx, cy; |
|
| 1080 |
final int WIDTH, HEIGHT; |
|
| 1081 |
final int SIDE; |
|
| 1082 |
final int BOT_SIZE = 70; |
|
| 1083 |
final int WHEEL_SIZE = 15; |
|
| 1084 |
|
|
| 1085 |
public VectorController (Image img) {
|
|
| 1086 |
super (img); |
|
| 1087 |
WIDTH = img.getWidth(null); |
|
| 1088 |
HEIGHT = img.getHeight(null); |
|
| 1089 |
cx = WIDTH/2; |
|
| 1090 |
cy = HEIGHT/2; |
|
| 1091 |
x = cx; |
|
| 1092 |
y = cy; |
|
| 1093 |
if (WIDTH < HEIGHT) {
|
|
| 1094 |
SIDE = WIDTH; |
|
| 1095 |
} else {
|
|
| 1096 |
SIDE = HEIGHT; |
|
| 1097 |
} |
|
| 1098 |
this.addMouseListener(this); |
|
| 1099 |
this.addMouseMotionListener(this); |
|
| 1100 |
} |
|
| 1101 |
|
|
| 1102 |
public void setPoint (int x, int y) {
|
|
| 1103 |
if (isValidPoint(x, y)) {
|
|
| 1104 |
this.x = x; |
|
| 1105 |
this.y = y; |
|
| 1106 |
} |
|
| 1107 |
} |
|
| 1108 |
|
|
| 1109 |
public boolean isValidPoint (int x, int y) {
|
|
| 1110 |
double xsq = Math.pow(1.0*(x - cx)/(SIDE/2), 2); |
|
| 1111 |
double ysq = Math.pow(1.0*(y - cy)/(SIDE/2), 2); |
|
| 1112 |
return (xsq + ysq <= 1); |
|
| 1113 |
} |
|
| 1114 |
|
|
| 1115 |
public void notifyMouseEvent (MouseEvent e, boolean send) {
|
|
| 1116 |
if (!isValidPoint(e.getX(), e.getY())) {
|
|
| 1117 |
return; |
|
| 1118 |
} |
|
| 1119 |
setPoint(e.getX(), e.getY()); |
|
| 1120 |
repaint(); |
|
| 1121 |
if (send) {
|
|
| 1122 |
Runnable r = new Runnable () {
|
|
| 1123 |
public void run () {
|
|
| 1124 |
sendToServer(); |
|
| 1125 |
} |
|
| 1126 |
}; |
|
| 1127 |
(new Thread(r)).start(); |
|
| 1128 |
} |
|
| 1129 |
} |
|
| 1130 |
|
|
| 1131 |
public void mouseExited(MouseEvent e) {
|
|
| 1132 |
} |
|
| 1133 |
public void mouseEntered(MouseEvent e) {
|
|
| 1134 |
} |
|
| 1135 |
public void mouseReleased(MouseEvent e) {
|
|
| 1136 |
this.notifyMouseEvent(e, true); |
|
| 1137 |
} |
|
| 1138 |
public void mouseClicked(MouseEvent e) {
|
|
| 1139 |
this.notifyMouseEvent(e, false); |
|
| 1140 |
} |
|
| 1141 |
public void mousePressed(MouseEvent e) {
|
|
| 1142 |
} |
|
| 1143 |
public void mouseDragged(MouseEvent e) {
|
|
| 1144 |
vectorController.notifyMouseEvent(e, false); |
|
| 1145 |
} |
|
| 1146 |
public void mouseMoved(MouseEvent e) {
|
|
| 1147 |
} |
|
| 1148 |
|
|
| 1149 |
public int getSpeed () {
|
|
| 1150 |
int dx = x - cx; |
|
| 1151 |
int dy = y - cy; |
|
| 1152 |
int s = (int) Math.sqrt( Math.pow(dx, 2) + Math.pow(dy, 2) ); |
|
| 1153 |
int maxspeed = SIDE/2; |
|
| 1154 |
return s * 512 / SIDE; |
|
| 1155 |
} |
|
| 1156 |
|
|
| 1157 |
/** |
|
| 1158 |
* Returns the angle of the control vector in positive degrees west of north, |
|
| 1159 |
* or negative degrees east of north, whichever is less than or equal to |
|
| 1160 |
* 180 degrees total. |
|
| 1161 |
*/ |
|
| 1162 |
public int getAngle () {
|
|
| 1163 |
int dx = x - cx; |
|
| 1164 |
int dy = cy - y; |
|
| 1165 |
// find reference angle in radians |
|
| 1166 |
double theta = Math.atan2(Math.abs(dx), Math.abs(dy)); |
|
| 1167 |
// transform to degrees |
|
| 1168 |
theta = theta * 180 / Math.PI; |
|
| 1169 |
// adjust for quadrant |
|
| 1170 |
if (dx < 0 && dy < 0) |
|
| 1171 |
theta = 90 + theta; |
|
| 1172 |
else if (dx < 0 && dy >= 0) |
|
| 1173 |
theta = 90 - theta; |
|
| 1174 |
else if (dx >= 0 && dy < 0) |
|
| 1175 |
theta = -90 - theta; |
|
| 1176 |
else |
|
| 1177 |
theta = -90 + theta; |
|
| 1178 |
return (int) theta; |
|
| 1179 |
} |
|
| 1180 |
|
|
| 1181 |
public int getMotorL () {
|
|
| 1182 |
if (getSpeed() == 0) |
|
| 1183 |
return 0; |
|
| 1184 |
int dx = x - cx; |
|
| 1185 |
int dy = (cy - y) * 255 / getSpeed(); |
|
| 1186 |
int val = 0; |
|
| 1187 |
// Dependent on quadrant |
|
| 1188 |
if (dx < 0 && dy < 0) |
|
| 1189 |
val = -255; |
|
| 1190 |
else if (dx < 0 && dy >= 0) |
|
| 1191 |
val = dy * 1024 / SIDE - 255; |
|
| 1192 |
else if (dx >= 0 && dy < 0) |
|
| 1193 |
val = dy * 1024 / SIDE + 255; |
|
| 1194 |
else |
|
| 1195 |
val = 255; |
|
| 1196 |
// Normalize to 0-255 |
|
| 1197 |
return val * getSpeed() / 255; |
|
| 1198 |
} |
|
| 1199 |
|
|
| 1200 |
public int getMotorR () {
|
|
| 1201 |
if (getSpeed() == 0) |
|
| 1202 |
return 0; |
|
| 1203 |
int dx = x - cx; |
|
| 1204 |
int dy = (cy - y) * 255 / getSpeed(); |
|
| 1205 |
int val = 0; |
|
| 1206 |
// Dependent on quadrant |
|
| 1207 |
if (dx < 0 && dy < 0) |
|
| 1208 |
val = dy * 1024 / SIDE + 255; |
|
| 1209 |
else if (dx < 0 && dy >= 0) |
|
| 1210 |
val = 255; |
|
| 1211 |
else if (dx >= 0 && dy < 0) |
|
| 1212 |
val = -255; |
|
| 1213 |
else |
|
| 1214 |
val = dy * 1024 / SIDE - 255; |
|
| 1215 |
// Normalize to 0-255 |
|
| 1216 |
return val * getSpeed() / 255; |
|
| 1217 |
} |
|
| 1218 |
|
|
| 1219 |
private int eliminateDeadZone (int x) {
|
|
| 1220 |
final int START = 150; |
|
| 1221 |
int val; |
|
| 1222 |
if (x == 0) |
|
| 1223 |
return 0; |
|
| 1224 |
if (x > 0) |
|
| 1225 |
val = (int) ((1 - 1.0 * START / 255) * x + START); |
|
| 1226 |
else |
|
| 1227 |
val = (int) ((1 - 1.0 * START / 255) * x - START); |
|
| 1228 |
return val; |
|
| 1229 |
|
|
| 1230 |
} |
|
| 1231 |
|
|
| 1232 |
public void paint (Graphics g) {
|
|
| 1233 |
// Clear image |
|
| 1234 |
g.setColor(Color.BLACK); |
|
| 1235 |
g.fillRect(0, 0, WIDTH, HEIGHT); |
|
| 1236 |
((Graphics2D)g).setStroke(new BasicStroke(1)); |
|
| 1237 |
|
|
| 1238 |
// Motor indicators |
|
| 1239 |
int motor1 = getMotorL() * BOT_SIZE / 512; |
|
| 1240 |
int motor2 = getMotorR() * BOT_SIZE / 512; |
|
| 1241 |
g.setColor(Color.YELLOW); |
|
| 1242 |
if (motor1 < 0) |
|
| 1243 |
g.fillRect(cx-BOT_SIZE/2 - WHEEL_SIZE, cy, WHEEL_SIZE, -motor1); |
|
| 1244 |
else |
|
| 1245 |
g.fillRect(cx-BOT_SIZE/2 - WHEEL_SIZE, cy-motor1, WHEEL_SIZE, motor1); |
|
| 1246 |
if (motor2 < 0) |
|
| 1247 |
g.fillRect(cx+BOT_SIZE/2, cy, WHEEL_SIZE, -motor2); |
|
| 1248 |
else |
|
| 1249 |
g.fillRect(cx+BOT_SIZE/2, cy-motor2, WHEEL_SIZE, motor2); |
|
| 1250 |
|
|
| 1251 |
// Watermark |
|
| 1252 |
g.setColor(Color.GRAY); |
|
| 1253 |
g.drawOval(cx-BOT_SIZE/2, cy-BOT_SIZE/2, BOT_SIZE, BOT_SIZE); |
|
| 1254 |
g.drawRect(cx-BOT_SIZE/2 - WHEEL_SIZE, cy-BOT_SIZE/2, WHEEL_SIZE, BOT_SIZE); |
|
| 1255 |
g.drawRect(cx+BOT_SIZE/2, cy-BOT_SIZE/2, WHEEL_SIZE, BOT_SIZE); |
|
| 1256 |
|
|
| 1257 |
// Targeting circle |
|
| 1258 |
g.setColor(Color.RED); |
|
| 1259 |
g.drawOval(cx-SIDE/2, cy-SIDE/2, SIDE, SIDE); |
|
| 1260 |
((Graphics2D)g).setStroke(new BasicStroke(2)); |
|
| 1261 |
|
|
| 1262 |
// Vector Line |
|
| 1263 |
g.setColor(Color.GREEN); |
|
| 1264 |
g.drawLine(cx, cy, x, y); |
|
| 1265 |
g.fillOval(x-3, y-3, 6, 6); |
|
| 1266 |
} |
|
| 1267 |
|
|
| 1268 |
public void setMaxForward () {
|
|
| 1269 |
setPoint(cx, cy - (SIDE/2) + 1); |
|
| 1270 |
} |
|
| 1271 |
|
|
| 1272 |
public void setMaxReverse () {
|
|
| 1273 |
setPoint(cx, cy + (SIDE/2) - 1); |
|
| 1274 |
} |
|
| 1275 |
|
|
| 1276 |
public void setMaxLeft () {
|
|
| 1277 |
setPoint(cx - (SIDE/2) + 1, cy); |
|
| 1278 |
} |
|
| 1279 |
|
|
| 1280 |
public void setMaxRight () {
|
|
| 1281 |
setPoint(cx + (SIDE/2) - 1, cy); |
|
| 1282 |
} |
|
| 1283 |
|
|
| 1284 |
public void setZero () {
|
|
| 1285 |
setPoint(cx, cy); |
|
| 1286 |
} |
|
| 1287 |
|
|
| 1288 |
public void sendToServer () {
|
|
| 1289 |
// Determine destination ID |
|
| 1290 |
String dest = ColonetServerInterface.GLOBAL_DEST; |
|
| 1291 |
if (cmbRobotNum != null && cmbRobotNum.getSelectedIndex() > 0) {
|
|
| 1292 |
dest = (String)cmbRobotNum.getSelectedItem(); |
|
| 1293 |
} |
|
| 1294 |
|
|
| 1295 |
if (csi == null) |
|
| 1296 |
return; |
|
| 1297 |
|
|
| 1298 |
String motor1_string; |
|
| 1299 |
String motor2_string; |
|
| 1300 |
int motor1 = eliminateDeadZone(getMotorL()); |
|
| 1301 |
int motor2 = eliminateDeadZone(getMotorR()); |
|
| 1302 |
|
|
| 1303 |
if (motor1 > 0) {
|
|
| 1304 |
motor1_string = " 1 " + motor1; |
|
| 1305 |
} else {
|
|
| 1306 |
motor1_string = " 0 " + (-motor1); |
|
| 1307 |
} |
|
| 1308 |
if (motor2 > 0) {
|
|
| 1309 |
motor2_string = " 1 " + motor2; |
|
| 1310 |
} else {
|
|
| 1311 |
motor2_string = " 0 " + (-motor2); |
|
| 1312 |
} |
|
| 1313 |
|
|
| 1314 |
csi.sendData(ColonetServerInterface.MOTOR1_SET + motor1_string, dest); |
|
| 1315 |
csi.sendData(ColonetServerInterface.MOTOR2_SET + motor2_string, dest); |
|
| 1316 |
|
|
| 1317 |
/* |
|
| 1318 |
// Directional commands |
|
| 1319 |
if (x > cx && y == cy) { //move right
|
|
| 1320 |
csi.sendData(ColonetServerInterface.MOTOR2_SET + " 0 200", dest); |
|
| 1321 |
csi.sendData(ColonetServerInterface.MOTOR1_SET + " 1 200", dest); |
|
| 1322 |
} else if (x < cx && y == cy) { //move left
|
|
| 1323 |
csi.sendData(ColonetServerInterface.MOTOR2_SET + " 1 200", dest); |
|
| 1324 |
csi.sendData(ColonetServerInterface.MOTOR1_SET + " 0 200", dest); |
|
| 1325 |
} else if (x == cx && y > cy) { //move forward
|
|
| 1326 |
csi.sendData(ColonetServerInterface.MOTOR2_SET + " 0 225", dest); |
|
| 1327 |
csi.sendData(ColonetServerInterface.MOTOR1_SET + " 0 225", dest); |
|
| 1328 |
} else if (x == cx && y < cy) { //move backward
|
|
| 1329 |
csi.sendData(ColonetServerInterface.MOTOR2_SET + " 1 225", dest); |
|
| 1330 |
csi.sendData(ColonetServerInterface.MOTOR1_SET + " 1 225", dest); |
|
| 1331 |
} else if (x == cx && y == cy) { //stop!
|
|
| 1332 |
csi.sendData(ColonetServerInterface.MOTOR2_SET + " 1 0", dest); |
|
| 1333 |
csi.sendData(ColonetServerInterface.MOTOR1_SET + " 1 0", dest); |
|
| 1334 |
} |
|
| 1335 |
*/ |
|
| 1336 |
|
|
| 1337 |
/* |
|
| 1338 |
// Atomic Directional commands |
|
| 1339 |
if (x > cx && y == cy) { //move right
|
|
| 1340 |
csi.sendData(ColonetServerInterface.MOVE_R, dest); |
|
| 1341 |
} else if (x < cx && y == cy) { //move left
|
|
| 1342 |
csi.sendData(ColonetServerInterface.MOVE_L, dest); |
|
| 1343 |
} else if (x == cx && y > cy) { //move forward
|
|
| 1344 |
csi.sendData(ColonetServerInterface.MOVE_F, dest); |
|
| 1345 |
} else if (x == cx && y < cy) { //move backward
|
|
| 1346 |
csi.sendData(ColonetServerInterface.MOVE_B, dest); |
|
| 1347 |
} else if (x == cx && y == cy) { //stop
|
|
| 1348 |
csi.sendData(ColonetServerInterface.MOTORS_OFF, dest); |
|
| 1349 |
} |
|
| 1350 |
*/ |
|
| 1351 |
} |
|
| 1352 |
|
|
| 1353 |
} |
|
| 1354 |
|
|
| 1355 |
|
|
| 1356 |
|
|
| 1357 |
} |
|
| 1067 |
} |
|
Also available in: Unified diff