5 #include "nxt_motors.h"
13 #define NXT_AVR_ADDRESS 1
14 #define NXT_AVR_N_OUTPUTS 4
15 #define NXT_AVR_N_INPUTS 4
18 const char avr_brainwash_string[] =
19 "\xCC" "Let's samba nxt arm in arm, (c)LEGO System A/S";
21 static U32 nxt_avr_initialised;
26 S8 output_percent[NXT_AVR_N_OUTPUTS];
33 U16 adc_value[NXT_AVR_N_INPUTS];
37 U8 avr_fw_version_major;
38 U8 avr_fw_version_minor;
41 static U8 data_from_avr[(2 * NXT_AVR_N_INPUTS) + 5];
43 static U8 data_to_avr[5 + NXT_AVR_N_OUTPUTS];
47 /* We're assuming that we get good packing */
50 nxt_avr_start_read(void)
52 memset(data_from_avr, 0, sizeof(data_from_avr));
53 twi_start_read(NXT_AVR_ADDRESS, 0, 0, data_from_avr, sizeof(data_from_avr));
57 nxt_avr_start_send(void)
62 U8 *b = (U8 *) (&io_to_avr);
64 i = sizeof(io_to_avr);
75 twi_start_write(NXT_AVR_ADDRESS, 0, 0, data_to_avr, sizeof(data_to_avr));
80 nxt_avr_power_down(void)
82 io_to_avr.power = 0x5a;
83 io_to_avr.pwm_frequency = 0x00;
88 nxt_avr_firmware_update_mode(void)
90 io_to_avr.power = 0xA5;
91 io_to_avr.pwm_frequency = 0x5A;
95 nxt_avr_link_init(void)
97 twi_start_write(NXT_AVR_ADDRESS, 0, 0, (const U8 *) avr_brainwash_string,
98 strlen(avr_brainwash_string));
103 Unpack16(const U8 *x)
107 retval = (((U16) (x[0])) & 0xff) | ((((U16) (x[1])) << 8) & 0xff00);
131 for (check_sum = i = 0; i < sizeof(data_from_avr); i++) {
136 if (check_sum != 0xff) {
137 nxt_avr_stats.bad_rx++;
141 nxt_avr_stats.good_rx++;
146 for (i = 0; i < NXT_AVR_N_INPUTS; i++) {
147 io_from_avr.adc_value[i] = Unpack16(p);
151 buttonsVal = Unpack16(p);
155 io_from_avr.buttons = 0;
157 if (buttonsVal > 1023) {
158 io_from_avr.buttons |= 1;
162 if (buttonsVal > 720)
163 io_from_avr.buttons |= 0x08;
164 else if (buttonsVal > 270)
165 io_from_avr.buttons |= 0x04;
166 else if (buttonsVal > 60)
167 io_from_avr.buttons |= 0x02;
169 voltageVal = Unpack16(p);
171 io_from_avr.battery_is_AA = (voltageVal & 0x8000) ? 1 : 0;
172 io_from_avr.avr_fw_version_major = (voltageVal >> 13) & 3;
173 io_from_avr.avr_fw_version_minor = (voltageVal >> 10) & 7;
176 // Figure out voltage
177 // The units are 13.848 mV per bit.
178 // To prevent fp, we substitute 13.848 with 14180/1024
180 voltageVal &= 0x3ff; // Toss unwanted bits.
183 io_from_avr.battery_mV = voltageVal;
193 memset(&io_to_avr, 0, sizeof(io_to_avr));
195 io_to_avr.pwm_frequency = 8;
197 nxt_avr_initialised = 1;
200 static U32 update_count;
201 static U32 link_init_wait;
202 static U32 link_running;
205 nxt_avr_1kHz_update(void)
208 if (!nxt_avr_initialised)
211 if (link_init_wait) {
217 nxt_avr_stats.not_ok++;
222 nxt_avr_stats.still_busy++;
227 if (!twi_ok() || twi_busy() || !link_running) {
228 memset(data_from_avr, 0, sizeof(data_from_avr));
233 nxt_avr_stats.resets++;
237 if (update_count & 1) {
238 nxt_avr_start_read();
241 nxt_avr_start_send();
249 return io_from_avr.buttons;
253 battery_voltage(void)
255 return io_from_avr.battery_mV;
262 return io_from_avr.adc_value[n];
269 nxt_avr_set_motor(U32 n, int power_percent, int brake)
271 if (n < NXT_N_MOTORS) {
272 io_to_avr.output_percent[n] = power_percent;
274 io_to_avr.output_mode |= (1 << n);
276 io_to_avr.output_mode &= ~(1 << n);
281 nxt_avr_set_input_power(U32 n, U32 power_type)
283 // This does not correspond to the spec.
284 // It is unclear how to set power always on.
285 // Lego code has a bug in it.
286 if (n < NXT_AVR_N_INPUTS && power_type <= 1) {
287 io_to_avr.input_power &= ~(0x1 << (n));
288 io_to_avr.input_power |= (power_type << (n));