int main(void)
{
lb_init(&lb);
event_init();
motor_init();
uart_init(); // init USART
enc_init();
i2c_init();
adc_init();
kalman_init();
sei(); // enable interrupts
// Wait a second at startup
_delay_ms(1000);
// send initial string
printf_P(PSTR("Hello world!\n"));
imu_init();
for (;/*ever*/;)
{
// ADCSRA |= (1<<ADSC); // Set start conversion bit and wait for conversion to finish
// while(ADCSRA&(1<<ADSC));
// OCR1AL = ADCH; // Set ADC reading to timer 0 compare
if(event_pending())
{
event_action();
}
else // No pending operation, do low priority tasks
{
// dequeue receive buffer if any bytes waiting
while (uart_avail())
{
char c = uart_getc();
if (lb_append(&lb, c) == LB_BUFFER_FULL)
{
lb_init(&lb); // Clear line
printf_P(PSTR("\nMax line length exceeded\n"));
}
// Process command if line buffer is ready ...
if (lb_line_ready(&lb))
{
strcpy(cmd_string,lb_gets(&lb));
do_cmd(cmd_string);
lb_init(&lb);
}
}
}
// Process command if line buffer is terminated by a line feed or carriage return
}
return 0;
}
void on_cell_structure_change()
{
EVENT_TRACE(fprintf(stderr, "%d: on_cell_structure_change\n", this_node));
/* Now give methods a chance to react to the change in cell
structure. Most ES methods need to reinitialize, as they depend
on skin, node grid and so on. Only for a change in box length we
have separate, faster methods, as this might happend frequently
in a NpT simulation. */
#ifdef ELECTROSTATICS
switch (coulomb.method) {
case COULOMB_DH:
break;
#ifdef P3M
case COULOMB_ELC_P3M:
ELC_init();
// fall through
case COULOMB_P3M:
p3m_init();
break;
#endif
case COULOMB_MMM1D:
MMM1D_init();
break;
case COULOMB_MMM2D:
MMM2D_init();
break;
case COULOMB_MAGGS:
maggs_init();
/* Maggs electrostatics needs ghost velocities */
on_ghost_flags_change();
break;
default:
break;
}
#endif /* ifdef ELECTROSTATICS */
#ifdef DIPOLES
switch (coulomb.Dmethod) {
#ifdef DP3M
case DIPOLAR_MDLC_P3M:
// fall through
case DIPOLAR_P3M:
dp3m_init();
break;
#endif
default:
break;
}
#endif /* ifdef DIPOLES */
#ifdef LB
if(lattice_switch & LATTICE_LB) {
lb_init();
}
#endif
}
/* lb_free:
* free members of a line buffer, mark as free
*/
static void
lb_free(LPLB lb)
{
assert(lb != NULL);
free(lb->str);
free(lb->attr);
lb_init(lb);
}
/*----------------------------------------------------------------------------*/
void uart_init()
{
GPIO_InitTypeDef GPIO_InitStructure;
USART_InitTypeDef USART_InitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
lb_init(&uart_buffer.rx, rx, RX_BUFFER_SIZE);
lb_init(&uart_buffer.tx, tx, TX_BUFFER_SIZE);
/* Enable GPIOA, USART1 clock */
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA|RCC_APB2Periph_USART1, ENABLE);
/* Configure USART1 Rx (PA10) as input floating */
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_Init(GPIOA, &GPIO_InitStructure);
/* Configure USART1 Tx (PA9) as alternate function push-pull */
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
GPIO_Init(GPIOA, &GPIO_InitStructure);
USART_InitStructure.USART_BaudRate = BAUD;
USART_InitStructure.USART_WordLength = USART_WordLength_8b;
USART_InitStructure.USART_StopBits = USART_StopBits_1;
USART_InitStructure.USART_Parity = USART_Parity_No ;
USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
USART_Init(USART1, &USART_InitStructure);
USART_Cmd(USART1, ENABLE);
/* Enabling interrupts */
NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn; // канал
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 15; // приоритет
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; // приоритет субгруппы
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; // включаем канал
NVIC_Init(&NVIC_InitStructure); // инициализируем
USART_ITConfig(USART1, USART_IT_TXE, ENABLE); // включаем прерывание на передачу
USART_ITConfig(USART1, USART_IT_RXNE, ENABLE); // включаем прерывание на прием
}
/* Process wave file, write music probability and labels into the specified files. Return 0 on success, print error and return non-zero on error. */
int process(const char* infile,
WAVE* wave,
OpusSM* sm,
FILE* ofp_pmusic,
FILE* ofp_labels,
double sm_segment_min_dur,
double b_segment_min_dur
)
{
double frame_dur = (double)ANALYSIS_FRAME_SIZE/wave->header.SampleRate;
Labeler* lb = lb_init(sm_segment_min_dur/frame_dur, b_segment_min_dur/frame_dur);
float* analysis_pcm = malloc(ANALYSIS_FRAME_SIZE*wave->header.NumChannels*sizeof(float));
int16_t* buffer = malloc(ANALYSIS_FRAME_SIZE*wave->header.NumChannels*sizeof(int16_t));
double total_music_ratio = 0;
int error = 0;
for (int ii = 0; ii <= wave->size - ANALYSIS_FRAME_SIZE; ii = ii + ANALYSIS_FRAME_SIZE) {
int readcount = wread(buffer, ANALYSIS_FRAME_SIZE, wave);
error = (readcount != ANALYSIS_FRAME_SIZE);
if (error) {
fprintf(stderr, "Could not read from wave file \"%s\", read count %d.\n", infile, readcount);
break;
}
int2float(buffer, analysis_pcm, ANALYSIS_FRAME_SIZE, wave->header.NumChannels);
float pmusic = sm_pmusic(sm, analysis_pcm);
total_music_ratio += pmusic;
if (ofp_labels != NULL) {
lb_add_frame(lb, pmusic);
}
fprintf(ofp_pmusic, "%f %f\n", (double)ii / wave->header.SampleRate, pmusic);
}
if (!error) {
if (ofp_labels != NULL) {
lb_finalize(lb);
lb_print_to_file(lb, ofp_labels, frame_dur);
}
total_music_ratio = (total_music_ratio * (double)ANALYSIS_FRAME_SIZE) / (double) wave->size;
fprintf(stderr, "Music ratio: %f\n", total_music_ratio);
}
lb = lb_destroy(lb);
free(analysis_pcm);
free(buffer);
return error;
}
int main(int argc, char *argv[]) {
int i, n_steps, grid[lbmodel.n_dim], vol;
double rho, gamma, kappa;
double start, finish, elapsed, mups;
if (argc!=3) {
fprintf(stderr, "Usage: ./run <kappa> <nsteps>\n");
return -1;
}
lis_initialize(&argc, &argv);
n_steps = atoi(argv[2]);
grid[0] = 100;
grid[1] = 20;
vol = grid[0]*grid[1];
rho = 1.0;
gamma = 0.0;
kappa = atof(argv[1]);
write_eos();
char filename[1024];
sprintf(filename, "profile_k%.03f.dat", kappa);
lb_init(grid,rho,gamma,kappa); lb_mass_mom(0);
fprintf(stdout, "Running %d iterations\n", n_steps); fflush(stdout);
start = (double) clock();
for (i=0; i<n_steps; ++i) {
lb_update(lbf);
lb_mass_mom(i+1);
write_profile(filename, 0);
}
finish = (double) clock();
elapsed = (finish-start)/CLOCKS_PER_SEC;
mups = vol*n_steps/elapsed/1e6;
fprintf(stdout, "Elapsed time: %.3f s (%.3e MUPS)\n", elapsed, mups); fflush(stdout);
write_profile(filename, 0);
lb_finalize();
lis_finalize();
return EXIT_SUCCESS;
}
void on_lb_params_change(int field) {
EVENT_TRACE(fprintf(stderr, "%d: on_lb_params_change\n", this_node));
if (field == LBPAR_AGRID) {
lb_init();
}
if (field == LBPAR_DENSITY) {
lb_reinit_fluid();
}
lb_reinit_parameters();
}
void on_lb_params_change(int field) {
if (field == LBPAR_AGRID) {
lb_init();
}
if (field == LBPAR_DENSITY) {
lb_reinit_fluid();
}
lb_reinit_parameters();
}
void on_boxl_change() {
EVENT_TRACE(fprintf(stderr, "%d: on_boxl_change\n", this_node));
/* Now give methods a chance to react to the change in box length */
#ifdef ELECTROSTATICS
switch(coulomb.method) {
#ifdef P3M
case COULOMB_ELC_P3M:
ELC_init();
// fall through
case COULOMB_P3M_GPU:
case COULOMB_P3M:
p3m_scaleby_box_l();
break;
#endif
case COULOMB_MMM1D:
MMM1D_init();
break;
case COULOMB_MMM2D:
MMM2D_init();
break;
case COULOMB_MAGGS:
maggs_init();
break;
default:
break;
}
#endif
#ifdef DIPOLES
switch(coulomb.Dmethod) {
#ifdef DP3M
case DIPOLAR_MDLC_P3M:
// fall through
case DIPOLAR_P3M:
dp3m_scaleby_box_l();
break;
#endif
default:
break;
}
#endif
#ifdef LB
if(lattice_switch & LATTICE_LB) {
lb_init();
#ifdef LB_BOUNDARIES
lb_init_boundaries();
#endif
}
#endif
}
/* sb_get:
* retrieve (wrapped) line buffer
*/
LPLB
sb_get(LPSB sb, uint index)
{
LPLB line = NULL;
assert(sb != NULL); assert((index < sb->size) || (sb->wrap_at > 0));
assert(sb->lb != NULL);
if (sb->wrap_at == 0) {
if (index < sb_internal_length(sb))
line = &(sb->lb[(sb->head + index) % sb->size]);
} else {
/* count number of wrapped lines */
uint line_count;
uint idx;
uint internal_length = sb_internal_length(sb);
if (sb->last_line <= index) {
/* use cached values */
line_count = sb->last_line;
idx = sb->last_line_index;
} else {
line_count = 0;
idx = 0;
}
for ( ; (idx < internal_length); idx++) {
line_count += sb_lines(sb, sb_internal_get(sb, idx));
if (line_count > index) break;
}
if (idx < internal_length) {
uint wrapped_lines;
uint len;
LPLB lb;
uint start, count;
/* get last line buffer */
lb = sb_internal_get(sb, idx);
len = lb_length(lb);
wrapped_lines = sb_lines(sb, lb);
line_count -= wrapped_lines;
/* cache current index */
sb->last_line_index = idx;
sb->last_line = line_count;
/* index into current line buffer */
start = (index - line_count) * sb->wrap_at;
count = GPMIN(len - start, sb->wrap_at);
/* copy substring from current line buffer */
lb_init(sb->current_line);
if (lb->str) {
sb->current_line->len = count;
sb->current_line->str = lb->str + start;
//lb_insert_str(sb->current_line, 0, lb->str + start, count);
}
/* return temporary buffer */
line = sb->current_line;
}
}
return line;
}
/*----------------------------------------------------------------------------*/
void uart_tx_clear()
{
lb_init(&uart_buffer.tx, tx, TX_BUFFER_SIZE);
}
/*----------------------------------------------------------------------------*/
void uart_rx_clear()
{
lb_init(&uart_buffer.rx, rx, RX_BUFFER_SIZE);
}
int
main(int argc, char* argv[])
{
int i = 0, j = 0, r = 0;
r = lb_init();
if (r < 0) {
fprintf(stderr, "ERROR: lb_init\n");
exit(r);
}
r = lb_get_bl_devices(5);
if (r < 0) {
fprintf(stderr, "ERROR: lb_get_bl_devices\n");
goto cleanup;
}
// search for our specific device named "FIRMATA"
lb_bl_device* firmata = NULL;
r = lb_get_device_by_device_name("FIRMATA", &firmata);
if (r < 0) {
fprintf(stderr, "ERROR: Device FIRMATA not found\n");
goto cleanup;
}
r = lb_connect_device(firmata);
if (r < 0) {
fprintf(stderr, "ERROR: lb_connect_device\n");
goto cleanup;
}
// r = lb_pair_device(firmata);
// if (r < 0) {
// fprintf(stderr, "ERROR: lb_pair_device\n");
// exit(r);
//}
r = lb_get_ble_device_services(firmata);
if (r < 0) {
fprintf(stderr, "ERROR: lb_get_ble_device_services\n");
goto cleanup;
}
printf("Device Found:\nName: %s\nDevice Address: %s\n", firmata->name, firmata->address);
printf("Services found:\n");
for (i = 0; i < firmata->services_size; i++) {
printf("%s\t%s\n", firmata->services[i]->service_path, firmata->services[i]->uuid);
printf("Characteristics Found:\n");
for (j = 0; j < firmata->services[i]->characteristics_size; j++) {
printf("%s\t%s\n", firmata->services[i]->characteristics[j]->char_path,
firmata->services[i]->characteristics[j]->uuid);
}
}
printf("Blinking");
fflush(stdout);
uint8_t led_on[] = { 0x91, 0x20, 0x00 };
uint8_t led_off[] = { 0x91, 0x00, 0x00 };
for (i = 0; i < 10; i++) {
printf(".");
fflush(stdout);
r = lb_write_to_characteristic(firmata, "6e400002-b5a3-f393-e0a9-e50e24dcca9e", 3, led_on);
if (r < 0) {
fprintf(stderr, "ERROR: lb_write_to_characteristic\n");
}
usleep(1000000);
printf(".");
fflush(stdout);
r = lb_write_to_characteristic(firmata, "6e400002-b5a3-f393-e0a9-e50e24dcca9e", 3, led_off);
if (r < 0) {
fprintf(stderr, "ERROR: lb_write_to_characteristic\n");
}
usleep(1000000);
}
printf("\n");
const char* userdata_test = "test";
r =
lb_register_characteristic_read_event(firmata, "6e400003-b5a3-f393-e0a9-e50e24dcca9e",
test_callback, (void*) userdata_test);
if (r < 0) {
fprintf(stderr, "ERROR: lb_register_characteristic_read_event\n");
goto cleanup;
}
printf("get_version\n");
fflush(stdout);
uint8_t get_version[] = { 0xf0, 0x79, 0xf7 };
r = lb_write_to_characteristic(firmata, "6e400002-b5a3-f393-e0a9-e50e24dcca9e", 3, get_version);
if (r < 0) {
fprintf(stderr, "ERROR: lb_write_to_characteristic\n");
}
printf("waiting for callbacks\n");
fflush(stdout);
sleep(2);
cleanup:
// r = lb_unpair_device(firmata);
// if (r < 0) {
// fprintf(stderr, "ERROR: lb_unpair_device\n");
// exit(r);
//}
r = lb_disconnect_device(firmata);
if (r < 0) {
fprintf(stderr, "ERROR: lb_disconnect_device\n");
}
r = lb_destroy();
if (r < 0) {
fprintf(stderr, "ERROR: lb_destroy\n");
}
printf("Done\n");
return 0;
}
void on_parameter_change(int field)
{
/* to prevent two on_coulomb_change */
#if defined(ELECTROSTATICS) || defined(MAGNETOSTATICS)
int cc = 0;
#endif
EVENT_TRACE(fprintf(stderr, "%d: on_parameter_change %s\n", this_node, fields[field].name));
if (field == FIELD_SKIN) {
integrate_vv_recalc_maxrange();
on_parameter_change(FIELD_MAXRANGE);
}
if (field == FIELD_NODEGRID)
grid_changed_n_nodes();
if (field == FIELD_BOXL || field == FIELD_NODEGRID)
grid_changed_box_l();
if (field == FIELD_TIMESTEP || field == FIELD_TEMPERATURE || field == FIELD_LANGEVIN_GAMMA || field == FIELD_DPD_TGAMMA
|| field == FIELD_DPD_GAMMA || field == FIELD_NPTISO_G0 || field == FIELD_NPTISO_GV || field == FIELD_NPTISO_PISTON )
reinit_thermo = 1;
#ifdef NPT
if ((field == FIELD_INTEG_SWITCH) && (integ_switch != INTEG_METHOD_NPT_ISO))
nptiso.invalidate_p_vel = 1;
#endif
#ifdef ADRESS
// if (field == FIELD_BOXL)
// adress_changed_box_l();
#endif
#ifdef ELECTROSTATICS
switch (coulomb.method) {
#ifdef ELP3M
case COULOMB_ELC_P3M:
if (field == FIELD_TEMPERATURE || field == FIELD_BOXL)
cc = 1;
// fall through
case COULOMB_P3M:
if (field == FIELD_TEMPERATURE || field == FIELD_NODEGRID || field == FIELD_SKIN)
cc = 1;
else if (field == FIELD_BOXL) {
P3M_scaleby_box_l_charges();
integrate_vv_recalc_maxrange();
}
break;
#endif
case COULOMB_EWALD:
if (field == FIELD_TEMPERATURE || field == FIELD_SKIN)
cc = 1;
else if (field == FIELD_BOXL) {
EWALD_scaleby_box_l();
integrate_vv_recalc_maxrange();
}
break;
case COULOMB_DH:
if (field == FIELD_TEMPERATURE)
cc = 1;
break;
case COULOMB_RF:
case COULOMB_INTER_RF:
if (field == FIELD_TEMPERATURE)
cc = 1;
break;
case COULOMB_MMM1D:
if (field == FIELD_TEMPERATURE || field == FIELD_BOXL)
cc = 1;
break;
case COULOMB_MMM2D:
if (field == FIELD_TEMPERATURE || field == FIELD_BOXL || field == FIELD_NLAYERS)
cc = 1;
break;
case COULOMB_MAGGS:
/* Maggs electrostatics needs ghost velocities */
on_ghost_flags_change();
cells_re_init(CELL_STRUCTURE_CURRENT);
break;
default: break;
}
#endif /*ifdef ELECTROSTATICS */
#ifdef MAGNETOSTATICS
switch (coulomb.Dmethod) {
#ifdef ELP3M
case DIPOLAR_MDLC_P3M:
if (field == FIELD_TEMPERATURE || field == FIELD_BOXL)
cc = 1;
// fall through
case DIPOLAR_P3M:
if (field == FIELD_TEMPERATURE || field == FIELD_NODEGRID || field == FIELD_SKIN)
cc = 1;
else if (field == FIELD_BOXL) {
P3M_scaleby_box_l_dipoles();
integrate_vv_recalc_maxrange();
}
break;
#endif
default: break;
}
#endif /*ifdef MAGNETOSTATICS */
#if defined(ELECTROSTATICS) || defined(MAGNETOSTATICS)
if (cc)
on_coulomb_change();
#endif
/* DPD needs ghost velocities, other thermostats not */
if (field == FIELD_THERMO_SWITCH) {
on_ghost_flags_change();
cells_re_init(CELL_STRUCTURE_CURRENT);
}
if (field == FIELD_MAXRANGE)
rebuild_verletlist = 1;
switch (cell_structure.type) {
case CELL_STRUCTURE_LAYERED:
if (field == FIELD_NODEGRID) {
if (node_grid[0] != 1 || node_grid[1] != 1) {
char *errtext = runtime_error(128);
ERROR_SPRINTF(errtext, "{091 layered cellsystem requires 1 1 n node grid} ");
}
}
if (field == FIELD_BOXL || field == FIELD_MAXRANGE || field == FIELD_THERMO_SWITCH)
cells_re_init(CELL_STRUCTURE_LAYERED);
break;
case CELL_STRUCTURE_DOMDEC:
if (field == FIELD_BOXL || field == FIELD_NODEGRID || field == FIELD_MAXRANGE ||
field == FIELD_MINNUMCELLS || field == FIELD_MAXNUMCELLS || field == FIELD_THERMO_SWITCH)
cells_re_init(CELL_STRUCTURE_DOMDEC);
break;
}
#ifdef LB
/* LB needs ghost velocities */
if (field == FIELD_LATTICE_SWITCH) {
on_ghost_flags_change();
cells_re_init(CELL_STRUCTURE_CURRENT);
}
if (lattice_switch & LATTICE_LB) {
if (field == FIELD_TEMPERATURE) {
lb_reinit_parameters();
}
if (field == FIELD_BOXL || field == FIELD_CELLGRID || field == FIELD_NNODES || field == FIELD_NODEGRID) {
lb_init();
}
}
#endif
#ifdef LB_GPU
if(this_node == 0){
if (lattice_switch & LATTICE_LB_GPU) {
if (field == FIELD_TEMPERATURE) lb_init_gpu();
}
}
#endif
}
int main(void)
{
// INITIALISATIONS
float x = 0.0;
float y = 0.0;
float theta = 0.0;
float vel = 0.0;
float velref = 0.0;
float Mvel = 0.0;
char* word_array[MAX_CMDS];
int no_of_words = 0;
int error = 1;
DDRC |= 1 << 5; // PortC.5 as output
lb_init(&lb); // init line buffer lb
uart_init(); // init USART
enc_init(); // init Encoder
ctrl_init(); // init Controller
motor_init(); // init Motor
sei(); // enable interrupts
printf_P(PSTR("Sup Bitches\nThis is Command\n"));
for (;/*ever*/;)
{
while (uart_avail())
{
char c = uart_getc(); //gets character from circular buffer
if (lb_append(&lb, c) == LB_BUFFER_FULL) // Add character "c" to line buffer, report status(putc) and handle special characters(append)
{
lb_init(&lb); // Clear line buffer, discards input
printf_P(PSTR("\nMax line length exceeded\n"));
}
}
error = 1;
// Process command if line buffer is terminated by a line feed or carriage return
if (lb_line_ready(&lb)) //if not empty and has null terminator
{
for (int j = 0; j < NUM_CMDS; j++) //re-setting word_array to zero
{
word_array[j] = 0;
}
no_of_words = string_parser( lb_gets(&lb), word_array); // gets serial, puts into word_array
for (int i=0; cmd_table[i].cmd != NULL; ++i) //
{
if( !strcmp(word_array[0], cmd_table[i].cmd))
{
error = 0;
cmd_table[i].func(no_of_words, word_array);
}
}
lb_init(&lb);
// Error checking
if(!no_of_words)
{
printf_P(PSTR("No Command Entered\n"));
}
if(error)
{
printf_P(PSTR("Invalid Command\n"));
}
/* // Note: The following is a terrible way to process strings from the user
// See recommendations section of the lab guide for a better way to
// handle commands with arguments, which scales well to a large
// number of commands.
if (!strncmp_P(lb_gets(&lb), PSTR("help"), 4))
{
printf_P(PSTR(
"MCHA3000 RS232 lab help.\n"
"Replace these lines with your own help instructions.\n"));
}
else if (!strncmp_P(lb_gets(&lb), PSTR("x="), 2)) // takes 'x' co-ordinate
{
x = atof(lb_gets_at(&lb, 2));
}
else if (!strncmp_P(lb_gets(&lb), PSTR("x?"), 2)) // prints 'x' co-ordinate to serial
{
printf_P(PSTR("x is %f\n"), x);
}
else if (!strncmp_P(lb_gets(&lb), PSTR("y="), 2)) // takes 'y' co-ordinate
{
y = atof(lb_gets_at(&lb, 2));
}
else if (!strncmp_P(lb_gets(&lb), PSTR("xy?"), 3)) // prints 'x'*'y' to serial
{
printf_P(PSTR("%f\n"), x*y);
}
else if (!strncmp_P(lb_gets(&lb), PSTR("theta="), 6)) // HIL: takes 'theta'
{
theta = atof(lb_gets_at(&lb, 6));
}
else if (!strncmp_P(lb_gets(&lb), PSTR("vel="), 4)) // HIL: takes 'vel'
{
vel = atof(lb_gets_at(&lb, 4));
}
else if (!strncmp_P(lb_gets(&lb), PSTR("velref="), 7)) // HIL: takes 'velref'
{
velref = atof(lb_gets_at(&lb, 7));
}
else if (!strncmp_P(lb_gets(&lb), PSTR("ctrl?"), 5)) // HIL: initialises feedback loop for cascade controller
{ // and prints control action to serial
float outer_loop = velocity_controller(velref - vel);
float inner_loop = angle_controller(outer_loop - theta);
printf_P(PSTR("%g\n"), inner_loop);
}
else if (!strncmp_P(lb_gets(&lb), PSTR("ecount?"), 7)) // prints enc_count
{
printf_P(PSTR("Encoder1 Count = %d\n"), enc_read1());
printf_P(PSTR("Encoder2 Count = %d\n"), enc_read2());
}
else if (!strncmp_P(lb_gets(&lb), PSTR("ereset"), 6)) // Resets enc_count then prints count
{
enc_reset();
printf_P(PSTR("Encoder1 Count = %d\n"), enc_read1());
printf_P(PSTR("Encoder2 Count = %d\n"), enc_read2());
}
else if (!strncmp_P(lb_gets(&lb), PSTR("mvel="), 5)) // Motor On/Off
{
Mvel=atof(lb_gets_at(&lb, 5));
motor_vel(Mvel);
printf_P(PSTR("Motor Velocity = %f\n"), Mvel);
}
else if (!strncmp_P(lb_gets(&lb), PSTR("I"), 1)) // Motor Current
{
printf_P(PSTR("Motor Current = %f\n"), motor_current());
}
else // WARNING: Unknown command
{
printf_P(PSTR("Unknown command: \"%s\"\n"), lb_gets(&lb));
}
lb_init(&lb); // Reset line buffer
*/
}
}
return 0;
}