servo and distance at the SAME TIME
haldean
4 years ago
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; |