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