Commit b4b783bc41c8c11444700f24647a0cf4c9288d3a - d

servo and distance at the SAME TIME haldean 4 years ago

1 changed file(s) with 53 addition(s) and 55 deletion(s). Raw diff Collapse all Expand all

+53

-55

firmware/main.c less more

0		/*
1		* main.c: main libal runner
2		* Copyright (C) 2016, Haldean Brown
3		*
4		* This program is free software; you can redistribute it and/or modify
	0	/* * main.c: main libal runner * Copyright (C) 2016, Haldean Brown * * This program is free software; you can redistribute it and/or modify
5	1	* it under the terms of the GNU General Public License as published by
6	2	* the Free Software Foundation; either version 2 of the License, or
7	3	* (at your option) any later version.

30	26
31	27	static const int addr = 0x62;
32	28
33		int t_lo = 800;
	29	int t_lo = 992;
34	30	int t_hi = 2600;
35		int p_lo = 800;
	31	int p_lo = 992;
36	32	int p_hi = 2600;
37		int period = 60000;
38	33
39	34	static bool
40		wait_while_busy()
	35	sensor_busy()
41	36	{
42	37	int res;
43	38	uint8_t status = 0xFF;
44		do
	39
	40	if ((res = rc_i2c_read_byte(1, STATUS, &status)) != 1)
45	41	{
46		if ((res = rc_i2c_read_byte(1, STATUS, &status)) != 1)
47		{
48		printf("failed to read status from I2C: %d\n", res);
49		return false;
50		}
	42	printf("failed to read status from I2C: %d\n", res);
	43	return true;
51	44	}
52		while (status & STATUS_BUSY);
	45	return status & STATUS_BUSY;
	46	}
53	47
	48	static bool
	49	send_read_cmd()
	50	{
	51	int res;
	52	if ((res = rc_i2c_write_byte(1, CMD, CMD_CAPTURE)) != 0)
	53	{
	54	printf("failed to write command to I2C: %d\n", res);
	55	return false;
	56	}
54	57	return true;
55	58	}
56	59

60	63	int phi = p_lo;
61	64	int theta = t_lo;
62	65	bool inc = true;
	66	uint64_t last_pulse;
63	67
64	68	if (rc_initialize())
65	69	{

67	71	return -1;
68	72	}
69	73
	74	rc_enable_servo_power_rail();
70	75	rc_send_servo_pulse_us(1, theta);
71	76	rc_send_servo_pulse_us(2, phi);
72		rc_enable_servo_power_rail();
	77	last_pulse = rc_nanos_since_boot();
	78
73	79	rc_set_state(RUNNING);
74	80
75	81	if ((res = rc_i2c_init(1, addr)) != 0)

78	84	return -1;
79	85	}
80	86
	87	while (sensor_busy())
	88	usleep(1000);
	89	if (!send_read_cmd())
	90	rc_set_state(EXITING);
	91	rc_set_led(GREEN, ON);
	92
81	93	while (rc_get_state() != EXITING)
82	94	{
83		/*
84		int iters = 1;
85		phi += (inc ? 1 : -1) * 5;
	95	uint64_t now = rc_nanos_since_boot();
	96	if (now - last_pulse > 50000000)
	97	{
	98	rc_send_servo_pulse_us(1, theta);
	99	rc_send_servo_pulse_us(2, phi);
	100	last_pulse = now;
	101	}
	102
	103	if (sensor_busy())
	104	continue;
	105
	106	uint16_t reading;
	107	if ((res = rc_i2c_read_word(1, 0x8F, &reading)) != 0)
	108	{
	109	printf("failed to read measurement from I2C: %d\n", res);
	110	rc_set_state(EXITING);
	111	break;
	112	}
	113	printf("%d\t%d\t%u\n", theta, phi, reading);
	114
	115	phi += (inc ? 1 : -1) * 1;
86	116	if (phi > p_hi \|\| phi < p_lo)
87	117	{
88	118	theta += 50;
89		iters = 100;
90	119	inc = !inc;
91	120	if (theta > t_hi)
92	121	{

94	123	break;
95	124	}
96	125	}
97		for (; iters > 0; iters--)
98		{
99		printf("%d\t%d\n", theta, phi);
100		rc_send_servo_pulse_us(1, theta);
101		rc_send_servo_pulse_us(2, phi);
102		usleep(period);
103		}
104		*/
105
106		wait_while_busy();
107		if ((res = rc_i2c_write_byte(1, CMD, CMD_CAPTURE)) != 0)
108		{
109		printf("failed to write command to I2C: %d\n", res);
110		return false;
111		}
112		wait_while_busy();
113		uint16_t reading;
114		if ((res = rc_i2c_read_word(1, 0x8F, &reading)) != 0)
115		{
116		printf("failed to read measurement from I2C: %d\n", res);
117		return false;
118		}
119		printf("measured: %u\n", reading);
120
121		if (rc_get_state() == RUNNING)
122		{
123		rc_set_led(GREEN, ON);
124		rc_set_led(RED, ON);
125		}
126		else if (rc_get_state() == PAUSED)
127		{
128		rc_set_led(GREEN, OFF);
129		rc_set_led(RED, ON);
130		}
	126	if (!send_read_cmd())
	127	rc_set_state(EXITING);
131	128	}
132	129
	130	rc_set_led(GREEN, OFF);
133	131	rc_disable_servo_power_rail();
134	132	rc_cleanup();
135	133	return 0;