Project

General

Profile

Revision 637

Added supplemental files

View differences:

trunk/code/projects/colonet/client/GraphicsPanel.java
1
import java.awt.*;
2
import javax.swing.*;
3

  
4
/*
5
* GraphicsPanel class
6
* An extension of JPanel, designed for holding an image that will be repainted regularly.
7
*/
8
public class GraphicsPanel extends JPanel {
9
	protected Image img;
10

  
11
	public GraphicsPanel (Image img) {
12
		this(img, true);
13
	}
14

  
15
	public GraphicsPanel (Image img, boolean isDoubleBuffered) {
16
		super(isDoubleBuffered);
17
		this.img = img;
18
	}
19

  
20
	public void paint (Graphics g) {
21
		// Place the buffered image on the screen, inside the panel
22
		g.drawImage(img, 0, 0, Color.WHITE, this);
23
	}
24
}
trunk/code/projects/colonet/client/VectorController.java
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

  

Also available in: Unified diff