Revision of trackdish.ino

Lucas Teske 已修改 9 years ago. 還原成這個修訂版本

1 file changed, 1 insertion, 5 deletions

trackdish.ino

			@@ -421,8 +421,4 @@ void loop() {
421	421		}
422	422
423	423		machineStateProcess();
424		-	}
425		-
426		-	#define CMD_CALIBRATE 'c'
427		-	#define CMD_STOP_MOTOR 's'
428		-	#define CMD_SET_MOTOR 'w'
	424	+	}

Lucas Teske 已修改 9 years ago. 還原成這個修訂版本

1 file changed, 45 insertions, 10 deletions

trackdish.ino

			@@ -28,7 +28,19 @@
28	28		#define STATE_SET_MOTORS 2
29	29		#define STATE_STOP_MOTORS 3
30	30
31		-	#define CMD_CALIBRATE 'a'
	31	+	#define VER_MTR_PWR_PIN 4
	32	+	#define HOR_MTR_PWR_PIN 5
	33	+	#define VER_MTR_DIR_PIN 6
	34	+	#define HOR_MTR_DIR_PIN 7
	35	+
	36	+	#define MTR_CTRL_VERPWRBIT 1
	37	+	#define MTR_CTRL_HORPWRBIT 2
	38	+	#define MTR_CTRL_VERDIRBIT 4
	39	+	#define MTR_CTRL_HORDIRBIT 8
	40	+
	41	+	#define CMD_CALIBRATE 'c'
	42	+	#define CMD_STOP_MOTOR 's'
	43	+	#define CMD_SET_MOTOR 'w'
32	44
33	45		const char magicWord[5] = "DISH";
34	46		const char currVersion[11] = CURRENT_VERSION;
			@@ -319,18 +331,35 @@ void setup() {
319	331		int currentState = STATE_IDLE;
320	332
321	333		void machineStateProcess() {
	334	+	char d;
322	335		switch (currentState) {
323	336		case STATE_IDLE: // Do Nothing
324	337		break;
325	338		case STATE_CALIBRATION:
326		-	Serial.println("\|0\|Entering Calibration State. This can take some time;");
327		-	calibrate();
	339	+	Serial.println("\|0\|Entering Calibration State. This can take some time;");
	340	+	calibrate();
	341	+	currentState = STATE_IDLE;
328	342		break;
329	343		case STATE_SET_MOTORS:
330		-
	344	+	d = Serial.read();
	345	+
	346	+	Serial.print("\|0\|Setting motors to ");
	347	+	Serial.print(d & MTR_CTRL_VERDIRBIT > 0 ? "UP " : "DOWN ");
	348	+	Serial.print(d & MTR_CTRL_VERPWRBIT > 0 ? "ON " : "OFF ");
	349	+	Serial.print(d & MTR_CTRL_HORDIRBIT > 0 ? "LEFT " : "RIGHT ");
	350	+	Serial.println(d & MTR_CTRL_HORPWRBIT > 0 ? "ON;" : "OFF;");
	351	+
	352	+	digitalWrite(VER_MTR_DIR_PIN, d & MTR_CTRL_VERDIRBIT);
	353	+	digitalWrite(HOR_MTR_DIR_PIN, d & MTR_CTRL_HORDIRBIT);
	354	+	digitalWrite(VER_MTR_PWR_PIN, d & MTR_CTRL_VERPWRBIT);
	355	+	digitalWrite(HOR_MTR_PWR_PIN, d & MTR_CTRL_HORPWRBIT);
	356	+	currentState = STATE_IDLE;
331	357		break;
332	358		case STATE_STOP_MOTORS:
333		-
	359	+	Serial.println("\|0\|Stopping Motors.;");
	360	+	digitalWrite(VER_MTR_PWR_PIN, LOW);
	361	+	digitalWrite(HOR_MTR_PWR_PIN, LOW);
	362	+	currentState = STATE_IDLE;
334	363		break;
335	364		}
336	365		}
			@@ -375,12 +404,14 @@ void loop() {
375	404		// Process Serial Data
376	405		if (Serial.available() > 0) {
377	406		char cmd = Serial.read();
378		-	Serial.print("Received cmd ");
379		-	Serial.println(cmd);
380	407		switch (cmd) {
381		-	case CMD_CALIBRATE:
382		-	currentState = STATE_CALIBRATION;
383		-	break;
	408	+	case CMD_CALIBRATE: currentState = STATE_CALIBRATION; break;
	409	+	case CMD_STOP_MOTOR: currentState = STATE_STOP_MOTORS; break;
	410	+	case CMD_SET_MOTOR: currentState = STATE_SET_MOTORS; break;
	411	+	default:
	412	+	Serial.print("\|0\|Unknown command ");
	413	+	Serial.print(cmd);
	414	+	Serial.println(";");
384	415		}
385	416		} else if (lastSerialTime + SERIAL_TIMEOUT < millis() && readingSerial) {
386	417		// Timeout
			@@ -391,3 +422,7 @@ void loop() {
391	422
392	423		machineStateProcess();
393	424		}
	425	+
	426	+	#define CMD_CALIBRATE 'c'
	427	+	#define CMD_STOP_MOTOR 's'
	428	+	#define CMD_SET_MOTOR 'w'

Lucas Teske 已修改 9 years ago. 還原成這個修訂版本

1 file changed, 393 insertions

trackdish.ino(檔案已創建)

		@@ -0,0 +1,393 @@
1	+	#include <EEPROM.h>
2	+	#include "I2Cdev.h"
3	+	#include "MPU6050_6Axis_MotionApps20.h"
4	+	#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
5	+	#include "Wire.h"
6	+	#endif
7	+	#include <avr/wdt.h>
8	+
9	+	// Amount of readings used to average, make it higher to get more precision
10	+	// but sketch will be slower (default:1000)
11	+	#define CALIB_BUFFER_SIZE 1000
12	+	// Acelerometer error allowed, make it lower to get more precision,
13	+	// but sketch may not converge (default:8)
14	+	#define CALIB_ACCEL_DEADZONE 8
15	+	// Giro error allowed, make it lower to get
16	+	// more precision, but sketch may not converge (default:1)
17	+	#define CALIB_GYRO_DEADZONE 1
18	+
19	+	#define INTERRUPT_PIN 2
20	+	#define LED_PIN 13
21	+	#define CURRENT_VERSION "v1.0.0"
22	+	#define CONFIG_VERSION 0
23	+	#define SERIAL_TIMEOUT 100
24	+	#define DMP_WARMUP_TIME 10000
25	+
26	+	#define STATE_IDLE 0
27	+	#define STATE_CALIBRATION 1
28	+	#define STATE_SET_MOTORS 2
29	+	#define STATE_STOP_MOTORS 3
30	+
31	+	#define CMD_CALIBRATE 'a'
32	+
33	+	const char magicWord[5] = "DISH";
34	+	const char currVersion[11] = CURRENT_VERSION;
35	+
36	+	struct Config {
37	+	char magic[4];
38	+	char version[10];
39	+	char configVersion;
40	+	char firstSetup;
41	+	int16_t gyroOffsetX;
42	+	int16_t gyroOffsetY;
43	+	int16_t gyroOffsetZ;
44	+	int16_t accelOffsetX;
45	+	int16_t accelOffsetY;
46	+	int16_t accelOffsetZ;
47	+	};
48	+
49	+
50	+	MPU6050 mpu;
51	+	float ypr[3];
52	+	int mpuStatus;
53	+	int dmpStatus;
54	+	int dmpPacketSize;
55	+	uint16_t fifoCount;
56	+	uint8_t fifoBuffer[64];
57	+	float temperature;
58	+	Config trackConfig;
59	+	Quaternion q;
60	+	VectorFloat gravity;
61	+
62	+	long lastSerialTime;
63	+	bool readingSerial;
64	+
65	+	#define resetDevice() wdt_enable(WDTO_30MS); while(1) {}
66	+
67	+	void measureSensorsMean();
68	+	void saveConfig();
69	+
70	+	/*
71	+	* MPU Interrupt
72	+	*/
73	+
74	+	volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
75	+	void dmpDataReady() {
76	+	mpuInterrupt = true;
77	+	}
78	+
79	+	/*
80	+	* Calibration
81	+	*/
82	+
83	+	int16_t ax, ay, az,gx, gy, gz;
84	+
85	+	int meanAccelX,meanAccelY,meanAccelZ,meanGyroX,meanGyroY,meanGyroZ,state=0;
86	+	int accelOffsetX,accelOffsetY,accelOffsetZ,gyroOffsetX,gyroOffsetY,gyroOffsetZ;
87	+
88	+	void calibrate() {
89	+	Serial.println("\|0\|Reseting offsets.;");
90	+	mpu.setXGyroOffset(0);
91	+	mpu.setYGyroOffset(0);
92	+	mpu.setZGyroOffset(0);
93	+	mpu.setXAccelOffset(0);
94	+	mpu.setYAccelOffset(0);
95	+	mpu.setZAccelOffset(0);
96	+
97	+	Serial.println("\|0\|Reading sensor means.;");
98	+	measureSensorsMean();
99	+
100	+	Serial.println("\|0\|Calculating offsets.;");
101	+	calculateOffsets();
102	+
103	+	Serial.println("\|0\|New offsets calculated.;");
104	+	trackConfig.gyroOffsetX = gyroOffsetX;
105	+	trackConfig.gyroOffsetY = gyroOffsetY;
106	+	trackConfig.gyroOffsetZ = gyroOffsetZ;
107	+
108	+	trackConfig.accelOffsetX = accelOffsetX;
109	+	trackConfig.accelOffsetY = accelOffsetY;
110	+	trackConfig.accelOffsetZ = accelOffsetZ;
111	+
112	+	trackConfig.firstSetup = false;
113	+
114	+	Serial.println("\|0\|Saving configurations.;");
115	+	saveConfig();
116	+	Serial.println("\|0\|Resetting device.;");
117	+	delay(100);
118	+	resetDevice();
119	+	}
120	+
121	+	void calculateOffsets() {
122	+	accelOffsetX=-meanAccelX/8;
123	+	accelOffsetY=-meanAccelY/8;
124	+	accelOffsetZ=(16384-meanAccelZ)/8;
125	+
126	+	gyroOffsetX=-meanGyroX/4;
127	+	gyroOffsetY=-meanGyroY/4;
128	+	gyroOffsetZ=-meanGyroZ/4;
129	+
130	+	while (1) { // TODO: Limit iterations
131	+	int ready=0;
132	+	mpu.setXAccelOffset(accelOffsetX);
133	+	mpu.setYAccelOffset(accelOffsetY);
134	+	mpu.setZAccelOffset(accelOffsetZ);
135	+
136	+	mpu.setXGyroOffset(gyroOffsetX);
137	+	mpu.setYGyroOffset(gyroOffsetY);
138	+	mpu.setZGyroOffset(gyroOffsetZ);
139	+
140	+	measureSensorsMean();
141	+
142	+	if (abs(meanAccelX)<=CALIB_ACCEL_DEADZONE) {
143	+	ready++;
144	+	} else {
145	+	accelOffsetX=accelOffsetX-meanAccelX/CALIB_ACCEL_DEADZONE;
146	+	}
147	+
148	+	if (abs(meanAccelY)<=CALIB_ACCEL_DEADZONE) {
149	+	ready++;
150	+	} else {
151	+	accelOffsetY=accelOffsetY-meanAccelY/CALIB_ACCEL_DEADZONE;
152	+	}
153	+
154	+	if (abs(16384-meanAccelZ)<=CALIB_ACCEL_DEADZONE) {
155	+	ready++;
156	+	} else {
157	+	accelOffsetZ=accelOffsetZ+(16384-meanAccelZ)/CALIB_ACCEL_DEADZONE;
158	+	}
159	+
160	+	if (abs(meanGyroX)<=CALIB_GYRO_DEADZONE) {
161	+	ready++;
162	+	} else {
163	+	gyroOffsetX=gyroOffsetX-meanGyroX/(CALIB_GYRO_DEADZONE+1);
164	+	}
165	+
166	+	if (abs(meanGyroY)<=CALIB_GYRO_DEADZONE) {
167	+	ready++;
168	+	} else {
169	+	gyroOffsetY=gyroOffsetY-meanGyroY/(CALIB_GYRO_DEADZONE+1);
170	+	}
171	+
172	+	if (abs(meanGyroZ)<=CALIB_GYRO_DEADZONE) {
173	+	ready++;
174	+	} else {
175	+	gyroOffsetZ=gyroOffsetZ-meanGyroZ/(CALIB_GYRO_DEADZONE+1);
176	+	}
177	+
178	+	if (ready==6) break;
179	+	}
180	+	}
181	+
182	+	void measureSensorsMean() {
183	+	long i=0,buff_ax=0,buff_ay=0,buff_az=0,buff_gx=0,buff_gy=0,buff_gz=0;
184	+
185	+	while (i<(CALIB_BUFFER_SIZE+101)){
186	+	// read raw accel/gyro measurements from device
187	+	mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
188	+
189	+	if (i>100 && i<=(CALIB_BUFFER_SIZE+100)){ //First 100 measures are discarded
190	+	buff_ax=buff_ax+ax;
191	+	buff_ay=buff_ay+ay;
192	+	buff_az=buff_az+az;
193	+	buff_gx=buff_gx+gx;
194	+	buff_gy=buff_gy+gy;
195	+	buff_gz=buff_gz+gz;
196	+	}
197	+	if (i==(CALIB_BUFFER_SIZE+100)){
198	+	meanAccelX=buff_ax/CALIB_BUFFER_SIZE;
199	+	meanAccelY=buff_ay/CALIB_BUFFER_SIZE;
200	+	meanAccelZ=buff_az/CALIB_BUFFER_SIZE;
201	+	meanGyroX=buff_gx/CALIB_BUFFER_SIZE;
202	+	meanGyroY=buff_gy/CALIB_BUFFER_SIZE;
203	+	meanGyroZ=buff_gz/CALIB_BUFFER_SIZE;
204	+	}
205	+	i++;
206	+	delay(2); //Needed so we don't get repeated measures
207	+	}
208	+	}
209	+
210	+
211	+	/*
212	+	* EEPROM Manipulation
213	+	*/
214	+
215	+	void clearEEPROM() {
216	+	for (int index = 0 ; index < EEPROM.length() ; index++) {
217	+	EEPROM[index] = 0;
218	+	}
219	+	}
220	+
221	+	void saveConfig() {
222	+	EEPROM.put(0, trackConfig);
223	+	}
224	+
225	+	void loadConfig() {
226	+	EEPROM.get(0, trackConfig);
227	+	bool magicOk = true;
228	+	for (int i=0; i<4; i++) {
229	+	if (magicWord[i] != trackConfig.magic[i]) {
230	+	magicOk = false;
231	+	}
232	+	}
233	+
234	+	if (!magicOk) {
235	+	Serial.println("\|0\|First setup. Cleaning EEPROM.;");
236	+	clearEEPROM();
237	+
238	+	for (int i=0; i<4; i++) {
239	+	trackConfig.magic[i] = magicWord[i];
240	+	}
241	+
242	+	for (int i=0; i<10; i++) {
243	+	trackConfig.version[i] = currVersion[i];
244	+	}
245	+
246	+	trackConfig.configVersion = CONFIG_VERSION;
247	+	trackConfig.gyroOffsetX = 0;
248	+	trackConfig.gyroOffsetY = 0;
249	+	trackConfig.gyroOffsetZ = 0;
250	+	trackConfig.accelOffsetX = 0;
251	+	trackConfig.accelOffsetY = 0;
252	+	trackConfig.accelOffsetZ = 0;
253	+	trackConfig.firstSetup = true;
254	+	saveConfig();
255	+	}
256	+	}
257	+
258	+	/*
259	+	* Main Code
260	+	*/
261	+
262	+	void setup() {
263	+	#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
264	+	Wire.begin();
265	+	TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
266	+	#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
267	+	Fastwire::setup(400, true);
268	+	#endif
269	+
270	+	MCUSR = 0;
271	+
272	+	Serial.begin(115200);
273	+	Serial.println("\|0\|Initializing devices.;");
274	+	mpu.initialize();
275	+
276	+	Serial.print("\|1\|MPUStatus\|");
277	+	Serial.print(mpu.testConnection());
278	+	Serial.println(";");
279	+
280	+	Serial.println("\|0\|Initializing DMP.;");
281	+	dmpStatus = mpu.dmpInitialize();
282	+
283	+	Serial.println("\|0\|Loading configuration.;");
284	+	loadConfig();
285	+
286	+	if (dmpStatus == 0) {
287	+	dmpStatus = 1;
288	+	Serial.println("\|0\|DMP Initialized.;");
289	+	} else {
290	+	dmpStatus = 0;
291	+	Serial.println("\|0\|Failed to initialize DMP.;");
292	+	}
293	+
294	+	if (trackConfig.firstSetup) {
295	+	Serial.println("\|1\|CALIBRATION\|Requires Calibration;");
296	+	} else {
297	+	Serial.println("\|0\|Loading Accel / Gyro Calibration;");
298	+	mpu.setXGyroOffset(trackConfig.gyroOffsetX);
299	+	mpu.setYGyroOffset(trackConfig.gyroOffsetY);
300	+	mpu.setZGyroOffset(trackConfig.gyroOffsetZ);
301	+	mpu.setXAccelOffset(trackConfig.accelOffsetX);
302	+	mpu.setYAccelOffset(trackConfig.accelOffsetY);
303	+	mpu.setZAccelOffset(trackConfig.accelOffsetZ);
304	+
305	+	mpu.setDMPEnabled(true);
306	+
307	+	attachInterrupt(0, dmpDataReady, RISING);
308	+	dmpPacketSize = mpu.dmpGetFIFOPacketSize();
309	+	Serial.println("\|0\|Waiting for DMP warmup.;");
310	+	lastSerialTime = millis();
311	+	while (lastSerialTime + DMP_WARMUP_TIME > millis()); // Wait for warmpup
312	+	Serial.println("\|0\|DMP Ready.;");
313	+	}
314	+
315	+	lastSerialTime = 0;
316	+	readingSerial = false;
317	+	}
318	+
319	+	int currentState = STATE_IDLE;
320	+
321	+	void machineStateProcess() {
322	+	switch (currentState) {
323	+	case STATE_IDLE: // Do Nothing
324	+	break;
325	+	case STATE_CALIBRATION:
326	+	Serial.println("\|0\|Entering Calibration State. This can take some time;");
327	+	calibrate();
328	+	break;
329	+	case STATE_SET_MOTORS:
330	+
331	+	break;
332	+	case STATE_STOP_MOTORS:
333	+
334	+	break;
335	+	}
336	+	}
337	+
338	+	void loop() {
339	+	// Process MPU
340	+	if (dmpStatus) {
341	+	if (mpuInterrupt \|\| fifoCount >= dmpPacketSize) {
342	+	mpuInterrupt = false;
343	+	mpuStatus = mpu.getIntStatus();
344	+
345	+	if ((mpuStatus & 0x10) \|\| fifoCount == 1024) {
346	+	mpu.resetFIFO();
347	+	Serial.println("\|0\|MPU FIFO Overflow!;");
348	+	} else if (mpuStatus & 0x02) {
349	+	while (fifoCount < dmpPacketSize) {
350	+	fifoCount = mpu.getFIFOCount();
351	+	}
352	+
353	+	mpu.getFIFOBytes(fifoBuffer, dmpPacketSize);
354	+	fifoCount -= dmpPacketSize;
355	+
356	+	mpu.dmpGetQuaternion(&q, fifoBuffer);
357	+	mpu.dmpGetGravity(&gravity, &q);
358	+	mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
359	+	temperature = mpu.getTemperature() /340.00 + 36.53;
360	+
361	+	Serial.print("\|2\|");
362	+	Serial.print(ypr[0] * 180/M_PI);
363	+	Serial.print("\|");
364	+	Serial.print(ypr[1] * 180/M_PI);
365	+	Serial.print("\|");
366	+	Serial.print(ypr[2] * 180/M_PI);
367	+	Serial.println(";");
368	+
369	+	Serial.print("\|3\|");
370	+	Serial.print(temperature);
371	+	Serial.println(";");
372	+	}
373	+	}
374	+	}
375	+	// Process Serial Data
376	+	if (Serial.available() > 0) {
377	+	char cmd = Serial.read();
378	+	Serial.print("Received cmd ");
379	+	Serial.println(cmd);
380	+	switch (cmd) {
381	+	case CMD_CALIBRATE:
382	+	currentState = STATE_CALIBRATION;
383	+	break;
384	+	}
385	+	} else if (lastSerialTime + SERIAL_TIMEOUT < millis() && readingSerial) {
386	+	// Timeout
387	+	Serial.println("\|0\|Serial Command Timeout;");
388	+
389	+	readingSerial = false;
390	+	}
391	+
392	+	machineStateProcess();
393	+	}