public void run() {
MOVE_PREV = MOVE_DOWN;
System.out.println("INIT!");
map = new int[mapX][mapY];
for (int i = 0; i < map.length; i++) {
for (int j = 0; j < map[i].length; j++) {
map[i][j] = 0;
}
}
map[blockP.x][blockP.y] = 1;
// map[0][20] = 1;
StdDraw.setXscale(-1.0, 1.0);
StdDraw.setYscale(-1.0, 1.0);
// initial values
// double vx = 0.015, vy = 0.023; // velocity
// main animation loop
while (true) {
drawGame();
try {
Thread.sleep(100);
} catch (InterruptedException e) {
// TODO Auto-generated catch block
e.printStackTrace();
}
movePrev();
}
}
public void handleImage() {
while (true) {
Image src = null;
try {
src = new Image(visionImage.take(), width, height);
} catch (InterruptedException e) {
e.printStackTrace();
continue;
}
// Apply Blob Track to Image
Image dest = new Image(src);
blobTrack.apply(src, dest);
OdometryMsg msg = new OdometryMsg();
if (LOCALIZE && System.currentTimeMillis() - last_localization > LOCALIZATION_INTERVAL) {
ResetMsg stop_msg = new ResetMsg();
stop_msg.reset = false;
stopPub.publish(stop_msg);
Point2D.Double curr_point;
synchronized (localization) {
curr_point = localization.localize(odo_x, odo_y, blobTrack.fiducials);
}
msg.x = curr_point.x;
msg.y = curr_point.y;
last_localization = System.currentTimeMillis();
} else {
msg.x = odo_x;
msg.y = odo_y;
}
msg.theta = odo_theta;
localPub.publish(msg);
// Update newly formed vision message
gui.setVisionImage(dest.toArray(), width, height);
try {
Thread.sleep(1000);
} catch (Exception exc) {
exc.printStackTrace();
}
ResetMsg stop_msg = new ResetMsg();
stop_msg.reset = true;
stopPub.publish(stop_msg);
}
}
public void run() {
try {
while (running) {
try {
Thread.sleep(framerate);
} catch (InterruptedException e) {
e.printStackTrace();
}
repaint();
updateObjects();
}
} catch (Exception e) {
e.printStackTrace();
}
}
private static int[] makeGradientPallet() {
BufferedImage image = new BufferedImage(100, 1, BufferedImage.TYPE_INT_RGB);
Graphics2D g2 = image.createGraphics();
Point2D start = new Point2D.Float(0f, 0f);
Point2D end = new Point2D.Float(99f, 0f);
float[] dist = {0f, .5f, 1f};
Color[] colors = {Color.RED, Color.YELLOW, Color.GREEN};
g2.setPaint(new LinearGradientPaint(start, end, dist, colors));
g2.fillRect(0, 0, 100, 1);
g2.dispose();
int width = image.getWidth(null);
int[] pallet = new int[width];
PixelGrabber pg = new PixelGrabber(image, 0, 0, width, 1, pallet, 0, width);
try {
pg.grabPixels();
} catch (InterruptedException ex) {
ex.printStackTrace();
}
return pallet;
}
