1#include <Wire.h>
2#include <Servo.h>
3
4#include <Max3421e.h>
5#include <Usb.h>
6#include <AndroidAccessory.h>
7
8#include <CapSense.h>
9
10#define  LED3_RED       2
11#define  LED3_GREEN     4
12#define  LED3_BLUE      3
13
14#define  LED2_RED       5
15#define  LED2_GREEN     7
16#define  LED2_BLUE      6
17
18#define  LED1_RED       8
19#define  LED1_GREEN     10
20#define  LED1_BLUE      9
21
22#define  SERVO1         11
23#define  SERVO2         12
24#define  SERVO3         13
25
26#define  TOUCH_RECV     14
27#define  TOUCH_SEND     15
28
29#define  RELAY1         A0
30#define  RELAY2         A1
31
32#define  LIGHT_SENSOR   A2
33#define  TEMP_SENSOR    A3
34
35#define  BUTTON1        A6
36#define  BUTTON2        A7
37#define  BUTTON3        A8
38
39#define  JOY_SWITCH     A9      // pulls line down when pressed
40#define  JOY_nINT       A10     // active low interrupt input
41#define  JOY_nRESET     A11     // active low reset output
42
43AndroidAccessory acc("Google, Inc.",
44		     "DemoKit",
45		     "DemoKit Arduino Board",
46		     "1.0",
47		     "http://www.android.com",
48		     "0000000012345678");
49Servo servos[3];
50
51// 10M ohm resistor on demo shield
52CapSense   touch_robot = CapSense(TOUCH_SEND, TOUCH_RECV);
53
54void setup();
55void loop();
56
57void init_buttons()
58{
59	pinMode(BUTTON1, INPUT);
60	pinMode(BUTTON2, INPUT);
61	pinMode(BUTTON3, INPUT);
62	pinMode(JOY_SWITCH, INPUT);
63
64	// enable the internal pullups
65	digitalWrite(BUTTON1, HIGH);
66	digitalWrite(BUTTON2, HIGH);
67	digitalWrite(BUTTON3, HIGH);
68	digitalWrite(JOY_SWITCH, HIGH);
69}
70
71
72void init_relays()
73{
74	pinMode(RELAY1, OUTPUT);
75	pinMode(RELAY2, OUTPUT);
76}
77
78
79void init_leds()
80{
81	digitalWrite(LED1_RED, 1);
82	digitalWrite(LED1_GREEN, 1);
83	digitalWrite(LED1_BLUE, 1);
84
85	pinMode(LED1_RED, OUTPUT);
86	pinMode(LED1_GREEN, OUTPUT);
87	pinMode(LED1_BLUE, OUTPUT);
88
89	digitalWrite(LED2_RED, 1);
90	digitalWrite(LED2_GREEN, 1);
91	digitalWrite(LED2_BLUE, 1);
92
93	pinMode(LED2_RED, OUTPUT);
94	pinMode(LED2_GREEN, OUTPUT);
95	pinMode(LED2_BLUE, OUTPUT);
96
97	digitalWrite(LED3_RED, 1);
98	digitalWrite(LED3_GREEN, 1);
99	digitalWrite(LED3_BLUE, 1);
100
101	pinMode(LED3_RED, OUTPUT);
102	pinMode(LED3_GREEN, OUTPUT);
103	pinMode(LED3_BLUE, OUTPUT);
104}
105
106void init_joystick(int threshold);
107
108byte b1, b2, b3, b4, c;
109void setup()
110{
111	Serial.begin(115200);
112	Serial.print("\r\nStart");
113
114	init_leds();
115	init_relays();
116	init_buttons();
117	init_joystick( 5 );
118
119	// autocalibrate OFF
120	touch_robot.set_CS_AutocaL_Millis(0xFFFFFFFF);
121
122	servos[0].attach(SERVO1);
123	servos[0].write(90);
124	servos[1].attach(SERVO2);
125	servos[1].write(90);
126	servos[2].attach(SERVO3);
127	servos[2].write(90);
128
129
130	b1 = digitalRead(BUTTON1);
131	b2 = digitalRead(BUTTON2);
132	b3 = digitalRead(BUTTON3);
133	b4 = digitalRead(JOY_SWITCH);
134	c = 0;
135
136	acc.powerOn();
137}
138
139void loop()
140{
141	byte err;
142	byte idle;
143	static byte count = 0;
144	byte msg[3];
145	long touchcount;
146
147	if (acc.isConnected()) {
148		int len = acc.read(msg, sizeof(msg), 1);
149		int i;
150		byte b;
151		uint16_t val;
152		int x, y;
153		char c0;
154
155		if (len > 0) {
156			// assumes only one command per packet
157			if (msg[0] == 0x2) {
158				if (msg[1] == 0x0)
159					analogWrite(LED1_RED, 255 - msg[2]);
160				else if (msg[1] == 0x1)
161					analogWrite(LED1_GREEN, 255 - msg[2]);
162				else if (msg[1] == 0x2)
163					analogWrite(LED1_BLUE, 255 - msg[2]);
164				else if (msg[1] == 0x3)
165					analogWrite(LED2_RED, 255 - msg[2]);
166				else if (msg[1] == 0x4)
167					analogWrite(LED2_GREEN, 255 - msg[2]);
168				else if (msg[1] == 0x5)
169					analogWrite(LED2_BLUE, 255 - msg[2]);
170				else if (msg[1] == 0x6)
171					analogWrite(LED3_RED, 255 - msg[2]);
172				else if (msg[1] == 0x7)
173					analogWrite(LED3_GREEN, 255 - msg[2]);
174				else if (msg[1] == 0x8)
175					analogWrite(LED3_BLUE, 255 - msg[2]);
176				else if (msg[1] == 0x10)
177					servos[0].write(map(msg[2], 0, 255, 0, 180));
178				else if (msg[1] == 0x11)
179					servos[1].write(map(msg[2], 0, 255, 0, 180));
180				else if (msg[1] == 0x12)
181					servos[2].write(map(msg[2], 0, 255, 0, 180));
182			} else if (msg[0] == 0x3) {
183				if (msg[1] == 0x0)
184					digitalWrite(RELAY1, msg[2] ? HIGH : LOW);
185				else if (msg[1] == 0x1)
186					digitalWrite(RELAY2, msg[2] ? HIGH : LOW);
187			}
188		}
189
190		msg[0] = 0x1;
191
192		b = digitalRead(BUTTON1);
193		if (b != b1) {
194			msg[1] = 0;
195			msg[2] = b ? 0 : 1;
196			acc.write(msg, 3);
197			b1 = b;
198		}
199
200		b = digitalRead(BUTTON2);
201		if (b != b2) {
202			msg[1] = 1;
203			msg[2] = b ? 0 : 1;
204			acc.write(msg, 3);
205			b2 = b;
206		}
207
208		b = digitalRead(BUTTON3);
209		if (b != b3) {
210			msg[1] = 2;
211			msg[2] = b ? 0 : 1;
212			acc.write(msg, 3);
213			b3 = b;
214		}
215
216		b = digitalRead(JOY_SWITCH);
217		if (b != b4) {
218			msg[1] = 4;
219			msg[2] = b ? 0 : 1;
220			acc.write(msg, 3);
221			b4 = b;
222		}
223
224		switch (count++ % 0x10) {
225		case 0:
226			val = analogRead(TEMP_SENSOR);
227			msg[0] = 0x4;
228			msg[1] = val >> 8;
229			msg[2] = val & 0xff;
230			acc.write(msg, 3);
231			break;
232
233		case 0x4:
234			val = analogRead(LIGHT_SENSOR);
235			msg[0] = 0x5;
236			msg[1] = val >> 8;
237			msg[2] = val & 0xff;
238			acc.write(msg, 3);
239			break;
240
241		case 0x8:
242			read_joystick(&x, &y);
243			msg[0] = 0x6;
244			msg[1] = constrain(x, -128, 127);
245			msg[2] = constrain(y, -128, 127);
246			acc.write(msg, 3);
247			break;
248
249		case 0xc:
250			touchcount = touch_robot.capSense(5);
251
252			c0 = touchcount > 750;
253
254			if (c0 != c) {
255				msg[0] = 0x1;
256				msg[1] = 3;
257				msg[2] = c0;
258				acc.write(msg, 3);
259				c = c0;
260			}
261
262			break;
263		}
264	} else {
265		// reset outputs to default values on disconnect
266		analogWrite(LED1_RED, 255);
267		analogWrite(LED1_GREEN, 255);
268		analogWrite(LED1_BLUE, 255);
269		analogWrite(LED2_RED, 255);
270		analogWrite(LED2_GREEN, 255);
271		analogWrite(LED2_BLUE, 255);
272		analogWrite(LED3_RED, 255);
273		analogWrite(LED3_GREEN, 255);
274		analogWrite(LED3_BLUE, 255);
275		servos[0].write(90);
276		servos[0].write(90);
277		servos[0].write(90);
278		digitalWrite(RELAY1, LOW);
279		digitalWrite(RELAY2, LOW);
280	}
281
282	delay(10);
283}
284
285// ==============================================================================
286// Austria Microsystems i2c Joystick
287void init_joystick(int threshold)
288{
289	byte status = 0;
290
291	pinMode(JOY_SWITCH, INPUT);
292	digitalWrite(JOY_SWITCH, HIGH);
293
294	pinMode(JOY_nINT, INPUT);
295	digitalWrite(JOY_nINT, HIGH);
296
297	pinMode(JOY_nRESET, OUTPUT);
298
299	digitalWrite(JOY_nRESET, 1);
300	delay(1);
301	digitalWrite(JOY_nRESET, 0);
302	delay(1);
303	digitalWrite(JOY_nRESET, 1);
304
305	Wire.begin();
306
307	do {
308		status = read_joy_reg(0x0f);
309	} while ((status & 0xf0) != 0xf0);
310
311	// invert magnet polarity setting, per datasheet
312	write_joy_reg(0x2e, 0x86);
313
314	calibrate_joystick(threshold);
315}
316
317
318int offset_X, offset_Y;
319
320void calibrate_joystick(int dz)
321{
322	char iii;
323	int x_cal = 0;
324	int y_cal = 0;
325
326	// Low Power Mode, 20ms auto wakeup
327	// INTn output enabled
328	// INTn active after each measurement
329	// Normal (non-Reset) mode
330	write_joy_reg(0x0f, 0x00);
331	delay(1);
332
333	// dummy read of Y_reg to reset interrupt
334	read_joy_reg(0x11);
335
336	for(iii = 0; iii != 16; iii++) {
337		while(!joystick_interrupt()) {}
338
339		x_cal += read_joy_reg(0x10);
340		y_cal += read_joy_reg(0x11);
341	}
342
343	// divide by 16 to get average
344	offset_X = -(x_cal>>4);
345	offset_Y = -(y_cal>>4);
346
347	write_joy_reg(0x12, dz - offset_X);  // Xp, LEFT threshold for INTn
348	write_joy_reg(0x13, -dz - offset_X);  // Xn, RIGHT threshold for INTn
349	write_joy_reg(0x14, dz - offset_Y);  // Yp, UP threshold for INTn
350	write_joy_reg(0x15, -dz - offset_Y);  // Yn, DOWN threshold for INTn
351
352	// dead zone threshold detect requested?
353	if (dz)
354		write_joy_reg(0x0f, 0x04);
355}
356
357
358void read_joystick(int *x, int *y)
359{
360	*x = read_joy_reg(0x10) + offset_X;
361	*y = read_joy_reg(0x11) + offset_Y;  // reading Y clears the interrupt
362}
363
364char joystick_interrupt()
365{
366	return digitalRead(JOY_nINT) == 0;
367}
368
369
370#define  JOY_I2C_ADDR    0x40
371
372char read_joy_reg(char reg_addr)
373{
374	char c;
375
376	Wire.beginTransmission(JOY_I2C_ADDR);
377	Wire.write(reg_addr);
378	Wire.endTransmission();
379
380	Wire.requestFrom(JOY_I2C_ADDR, 1);
381
382	while(Wire.available())
383		c = Wire.read();
384
385	return c;
386}
387
388void write_joy_reg(char reg_addr, char val)
389{
390	Wire.beginTransmission(JOY_I2C_ADDR);
391	Wire.write(reg_addr);
392	Wire.write(val);
393	Wire.endTransmission();
394}
395