git.haldean.org d / b4b783b
servo and distance at the SAME TIME haldean 2 years ago
1 changed file(s) with 53 addition(s) and 55 deletion(s). Raw diff Collapse all Expand all
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
51 * it under the terms of the GNU General Public License as published by
62 * the Free Software Foundation; either version 2 of the License, or
73 * (at your option) any later version.
3026
3127 static const int addr = 0x62;
3228
33 int t_lo = 800;
29 int t_lo = 992;
3430 int t_hi = 2600;
35 int p_lo = 800;
31 int p_lo = 992;
3632 int p_hi = 2600;
37 int period = 60000;
3833
3934 static bool
40 wait_while_busy()
35 sensor_busy()
4136 {
4237 int res;
4338 uint8_t status = 0xFF;
44 do
39
40 if ((res = rc_i2c_read_byte(1, STATUS, &status)) != 1)
4541 {
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;
5144 }
52 while (status & STATUS_BUSY);
45 return status & STATUS_BUSY;
46 }
5347
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 }
5457 return true;
5558 }
5659
6063 int phi = p_lo;
6164 int theta = t_lo;
6265 bool inc = true;
66 uint64_t last_pulse;
6367
6468 if (rc_initialize())
6569 {
6771 return -1;
6872 }
6973
74 rc_enable_servo_power_rail();
7075 rc_send_servo_pulse_us(1, theta);
7176 rc_send_servo_pulse_us(2, phi);
72 rc_enable_servo_power_rail();
77 last_pulse = rc_nanos_since_boot();
78
7379 rc_set_state(RUNNING);
7480
7581 if ((res = rc_i2c_init(1, addr)) != 0)
7884 return -1;
7985 }
8086
87 while (sensor_busy())
88 usleep(1000);
89 if (!send_read_cmd())
90 rc_set_state(EXITING);
91 rc_set_led(GREEN, ON);
92
8193 while (rc_get_state() != EXITING)
8294 {
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;
86116 if (phi > p_hi || phi < p_lo)
87117 {
88118 theta += 50;
89 iters = 100;
90119 inc = !inc;
91120 if (theta > t_hi)
92121 {
94123 break;
95124 }
96125 }
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);
131128 }
132129
130 rc_set_led(GREEN, OFF);
133131 rc_disable_servo_power_rail();
134132 rc_cleanup();
135133 return 0;