int main(void) {
OSC_init();
TRISA = 0b00000000;
TRISB = 0b00000011;
TRISC = 0b10000000;
ANCON0 = 0b11111111;
ANCON1 = 0b00011111;
INTCON2bits.RBPU = 0; // Pull-up enable
timer0_init(3);
UART_init();
uint8_t txbuf[100];
ringbuf_init(&tx_buf, txbuf, sizeof (txbuf));
INTCONbits.GIE = 1;
while (1) {
static int16_t prev_e;
if (encoder != prev_e) {
prev_e = encoder;
if (encoder < 0) {
tx_send('-');
tx_sendn(-encoder, 5);
tx_send('\n');
} else {
tx_sendn(encoder, 5);
tx_send('\n');
}
}
}
return 0;
}
void main_init(void) {
OSC_init();
TRISA = 0b00010000; // x,x,x,Vcap,x,x,x,x
TRISB = 0b00110001; // x,x,SDA,SCL,x,x,x,x
TRISC = 0b10111010; // RX,TX,D+,D-,Vusb,LED,T1OSI,T1OSO
ANCON0 = 0b11111111; // xxx,xxx,xxx,RA5,RA3,RA2,RA1,RA0
ANCON1 = 0b00011111; // VBG,xxx,xxx,RB0,RC2,RB1,RB3,RB2
INTCON2bits.RBPU = 0; // Pull-up enable
timer1_init(0, T1OSC);
I2C_LCD_init();
}
void main_init(void) {
OSC_init();
TRISA = 0b11100111; // SWB,SWG,SWR,Vcap,x,ADCB,ADCG,ADCR
TRISB = 0b00000000; // PGD,PGC,x,PWMW,x,PWMB,PWMG,PWMR
TRISC = 0b10111010; // RX,TX,D+,D-,Vusb,x,T1OSI,T1OSO
ANCON0 = 0b11111000; // AN2,AN1,AN0 is analog
ANCON1 = 0b00011111; // all digital
INTCON2bits.RBPU = 0; // PORTB Pull-Up Enable
timer0_init(8); // ?
timer1_init(0, T1OSC); // ?
timer3_init(2); // button?
RTCC_init();
PWM_init(PR_VALUE); //250 is 3kHz
USB_CDC_init();
}
int
main (int argc, char ** argv)
{
char * device = DEFAULT_FLOCK_DEVICE;
int number_of_birds = atoi (DEFAULT_NUMBER_OF_BIRDS);
double noise_level = atof (DEFAULT_NOISE_LEVEL);
flock_t flock = NULL;
bird_data_t data_of_birds = NULL;
int flockfd = -1;
int port = 0;
int sockfd = -1;
char * host = NULL;
struct sockaddr_in host_addr;
fd_set input_fd_set;
int maxfd;
OSC_space_t space = NULL;
int c;
int count;
int result = EXIT_SUCCESS;
/* Parse arguments. */
opterr = 0;
while ((c = getopt (argc, argv, "d:b:n:")) != -1)
switch (c)
{
case 'd':
device = optarg;
break;
case 'b':
number_of_birds = atoi (optarg);
break;
case 'n':
noise_level = atof (optarg);
break;
default:
break;
}
if (argc - optind != 2)
{
usage (argv[0]);
exit (EXIT_FAILURE);
}
host = argv[optind++];
port = atoi (argv[optind++]);
get_more_priority ();
signal (SIGINT, handle_signal);
fprintf (stderr, "Preparing flock device: %s, number of birds: %d.\n",
device, number_of_birds);
if ((flock = flock_hl_open (device,
number_of_birds,
flock_bird_record_mode_position_angles,
1, 1)) == NULL)
{
result = EXIT_FAILURE;
goto terminate;
}
flockfd = flock_get_file_descriptor (flock);
/* TODO: use a command-line option for the local port. */
fprintf (stderr, "Opening configuration port: %d.\n", port + 1);
if ((sockfd = open_socket (port + 1)) == -1)
{
result = EXIT_FAILURE;
goto terminate;
}
fprintf (stderr, "Preparing communication with host: %s, port: %d.\n",
host, port);
if ((fill_host_addr (host, port, &host_addr)) == -1)
{
result = EXIT_FAILURE;
goto terminate;
}
FD_ZERO (&input_fd_set);
FD_SET (sockfd, &input_fd_set);
FD_SET (flockfd, &input_fd_set);
maxfd = (sockfd < flockfd) ? flockfd : sockfd;
data_of_birds = (bird_data_t)
malloc (number_of_birds * sizeof (struct bird_data_s));
OSC_init ();
space = OSC_space_make ();
OSC_space_register_method
(space, OSC_method_make ("zthreshold", ",f", set_zthreshold, NULL));
count = 0;
/* First values. */
{
bird_data_t data;
int bird;
if (flock_next_record (flock, 1) == 0)
{
fprintf (stderr, "Can't get response from flock.\n");
result = EXIT_FAILURE;
goto terminate;
}
count++;
for (bird = 0, data = data_of_birds;
bird < number_of_birds;
bird++, data++)
{
memcpy (&data->rec,
flock_get_record (flock, bird + 1),
sizeof (data->rec));
data->zset = 0;
data->zcount = 0;
data->maxdz = 0;
data->lastbump = 0;
data->bumpcount = 0;
}
}
fprintf (stderr, "Running... (Hit Ctrl-C to stop.)\n");
while (!terminate)
{
fd_set read_fd_set;
read_fd_set = input_fd_set;
/* Block until new data is available, either from the flock or
from peer. */
if (select (maxfd + 1, &read_fd_set, NULL, NULL, NULL) == -1)
{
perror ("select");
result = EXIT_FAILURE;
goto terminate;
}
if (FD_ISSET (sockfd, &read_fd_set))
/* Get data from peer. */
receive (space, sockfd);
if (FD_ISSET (flockfd, &read_fd_set))
{
bird_data_t data;
int bird;
/* Get data from flock. */
if (flock_next_record (flock, 1) == 0)
{
result = EXIT_FAILURE;
goto terminate;
}
count++;
for (bird = 0, data = data_of_birds;
bird < number_of_birds;
bird++, data++)
{
double dx, dy, dz;
/* Shift previous record. */
memcpy (&data->prev_rec, &data->rec, sizeof (data->rec));
/* Copy bird's record. */
memcpy (&data->rec,
flock_get_record (flock, bird + 1),
sizeof (data->rec));
dx = data->rec.values.pa.x - data->prev_rec.values.pa.x;
dy = data->rec.values.pa.y - data->prev_rec.values.pa.y;
dz = data->rec.values.pa.z - data->prev_rec.values.pa.z;
{
/* Coordinates. */
double distance = sqrt ((dx * dx) + (dy * dy) + (dz * dz));
if (distance > noise_level)
send_record (sockfd, &host_addr, bird + 1, &data->rec);
}
{
/* Bumps. */
if ((count - data->lastbump) < after_bump_delay)
continue;
if (dz > zthreshold)
{
data->zset = 1;
if (data->maxdz < dz)
data->maxdz = dz;
}
if (!data->zset)
{
data->maxdz = 0;
continue;
}
/* Proposition: delay could depend on maxdz. */
if (dz < 0.5 * zthreshold)
{
send_bump (sockfd, &host_addr, bird + 1, data->maxdz);
/* fprintf (stderr, "bird %d bumps (%g).\n", */
/* bird + 1, data->maxdz); */
data->zset = 0;
data->maxdz = 0;
data->lastbump = count;
data->bumpcount++;
}
}
}
}
}
terminate:
fprintf (stderr, "Closing device and connection.\n");
if (sockfd != -1)
close_socket (sockfd);
if (flock)
flock_hl_close (flock);
if (data_of_birds)
free (data_of_birds);
if (space)
OSC_space_free (space);
return result;
}
