/*!
* print interval
*
* @param output output stream to print to
* @param i interval to print
*/
void print_interval(const char *output, struct pmm_interval *i) {
SWITCHPRINTF(output, "type: %s\n", interval_type_to_string(i->type));
switch (i->type) {
case IT_GBBP_EMPTY :
case IT_GBBP_CLIMB :
SWITCHPRINTF(output, "climb_step: %d\n", i->climb_step);
case IT_GBBP_BISECT :
case IT_GBBP_INFLECT :
SWITCHPRINTF(output, "plane: %d\n", i->plane);
SWITCHPRINTF(output, "n_p: %d\n", i->n_p);
SWITCHPRINTF(output, "start:\n");
print_params(output, i->start, i->n_p);
SWITCHPRINTF(output, "end:\n");
print_params(output, i->end, i->n_p);
break;
case IT_BOUNDARY_COMPLETE:
break;
case IT_COMPLETE:
break;
case IT_POINT:
SWITCHPRINTF(output, "n_p: %d\n", i->n_p);
print_params(output, i->start, i->n_p);
break;
default:
break;
}
}
void simple_cokriging_markI(
const sugarbox_grid_t & grid,
const cont_property_array_t & input_prop,
const cont_property_array_t & secondary_data,
mean_t primary_mean,
mean_t secondary_mean,
double secondary_variance,
double correlation_coef,
const neighbourhood_param_t & neighbourhood_params,
const covariance_param_t & primary_cov_params,
cont_property_array_t & output_prop)
{
if (input_prop.size() != output_prop.size())
throw hpgl_exception("simple_cokriging", boost::format("Input data size: %s. Output data size: %s. Must be equal.") % input_prop.size() % output_prop.size());
print_algo_name("Simple Colocated Cokriging Markov Model I");
print_params(neighbourhood_params);
print_params(primary_cov_params);
print_param("Primary mean", primary_mean);
print_param("Secondary mean", secondary_mean);
print_param("Secondary variance", secondary_variance);
print_param("Correllation coef", correlation_coef);
cov_model_t cov(primary_cov_params);
cross_cov_model_mark_i_t<cov_model_t> cross_cov(correlation_coef, secondary_variance, &cov);
int data_size = input_prop.size();
neighbour_lookup_t<sugarbox_grid_t, cov_model_t> n_lookup(&grid, &cov, neighbourhood_params);
progress_reporter_t report(data_size);
report.start(data_size);
// for each node
for (node_index_t i = 0; i < data_size; ++i)
{
// calc value
cont_value_t result = -500;
if (input_prop.is_informed(i))
{
result = input_prop[i];
}
else
{
cont_value_t secondary_value = secondary_data.is_informed(i) ? secondary_data[i] : secondary_mean;
if (!calc_value(i, input_prop, secondary_value, primary_mean, secondary_mean,
secondary_variance, cov, cross_cov, n_lookup, result))
{
result = primary_mean + secondary_value - secondary_mean;
}
}
// set value at node
output_prop.set_at(i, result);
report.next_lap();
}
}
static void print_method(compile_t* c, printbuf_t* buf, reach_type_t* t,
reach_method_t* m)
{
if(!emit_fun(m->r_fun))
return;
AST_GET_CHILDREN(m->r_fun, cap, id, typeparams, params, rtype, can_error,
body, docstring);
// Print the docstring if we have one.
if(ast_id(docstring) == TK_STRING)
{
printbuf(buf,
"/*\n"
"%s"
"*/\n",
ast_name(docstring)
);
}
// Print the function signature.
if((ast_id(cap) != TK_AT) || !is_none(rtype))
print_type_name(c, buf, rtype);
else
printbuf(buf, "void");
printbuf(buf, " %s", m->full_name);
switch(ast_id(m->r_fun))
{
case TK_NEW:
case TK_BE:
{
ast_t* def = (ast_t*)ast_data(t->ast);
if(ast_id(def) == TK_ACTOR)
printbuf(buf, "__send");
break;
}
default: {}
}
printbuf(buf, "(");
if(ast_id(cap) != TK_AT)
{
print_type_name(c, buf, t->ast);
printbuf(buf, " self");
print_params(c, buf, params, true);
} else {
print_params(c, buf, params, false);
}
printbuf(buf, ");\n\n");
}
main()
{
init_params();
set_int("SCALE", "7");
set_flg("VERBOSE");
set_str("DISTRIBUTIONS", "'some file name'");
print_params();
set_int("s" , "8");
clr_flg("VERBOSE");
printf("DIST is %s\n", get_str("DISTRIBUTIONS"));
print_params();
usage(NULL, NULL);
}
static void
parse_content_disposition (unsigned char **in, unsigned char *inend, disposition_t *disposition, GError **err)
{
unsigned char *inptr = *in;
/* a) NIL
* b) ("INLINE" NIL)
* c) ("ATTACHMENT" ("FILENAME", "myfile.ext"))
* d) "ALTERNATIVE" ("BOUNDARY", "---") */
while (inptr < inend && *inptr == ' ')
inptr++;
if (*inptr == '(') {
inptr++; /* My '(' */
/* cases c & b */
disposition->type = decode_qstring (&inptr, inend, err);
debug_printf ("disposition.type: %s\n", PRINT_NULL (disposition->type));
disposition->params = decode_params (&inptr, inend, err);
print_params (disposition->params);
while (inptr < inend && *inptr == ' ')
inptr++;
if (*inptr != ')') {
g_free (disposition->type);
disposition->type = NULL;
if (disposition->params)
mimeparam_destroy (disposition->params);
*in = inptr;
set_error (err, in);
return;
}
inptr++; /* My ')' */
} else {
if (strncmp ((const char *) inptr, "NIL", 3) != 0) {
/* case d */
disposition->type = decode_qstring (&inptr, inend, err);
debug_printf ("disposition.type: %s\n", PRINT_NULL (disposition->type));
disposition->params = decode_params (&inptr, inend, err);
print_params (disposition->params);
} else /* case a */
inptr += 3;
}
*in = inptr;
return;
}
// Function called by GLUT to display the scene
void display()
{
// Clear the screen
glClear(GL_COLOR_BUFFER_BIT);
// Load the identity matrix
glLoadIdentity();
// Set the view angle
glRotated(ph, 1, 0, 0);
glRotated(th, 0, 1, 0);
// Draw the Lorenz Attractor
glColor3f(0, 0, 1);
draw_attractor();
// Draw the axes
draw_axes();
glColor3f(0, 0, 1);
if(animate == 1) {
// Refresh the Attractor
draw_attractor();
// Draw the animation
animate_particle();
}
// Print the Lorenz Parameters
print_params();
// Make the scene visible
glFlush();
// Remove Clipping
glutSwapBuffers();
}
void sequential_indicator_simulation_lvm(
indicator_property_array_t & property,
const sugarbox_grid_t & grid,
const ik_params_t & params,
int seed,
const mean_t ** mean_data,
progress_reporter_t & report,
bool use_corellogram,
const unsigned char * mask)
{
print_algo_name("Sequential Indicator Simulation");
print_params(params);
print_param("LVM", "on");
if(use_corellogram)
{
print_param("Corellogram", "on");
if (mask == NULL)
do_sis(property, grid, params, seed, mean_data, report, corellogram_weight_calculator_t(), no_mask_t());
else
do_sis(property, grid, params, seed, mean_data, report, corellogram_weight_calculator_t(), mask);
} else {
print_param("Corellogram", "off");
if (mask == NULL)
do_sis(property, grid, params, seed, mean_data, report, sk_weight_calculator_t(), no_mask_t());
else
do_sis(property, grid, params, seed, mean_data, report, sk_weight_calculator_t(), mask);
}
}
int main(int argc, char *argv[])
{
Parameters *params; // user defined parameters
double ***phi; // flux array
FILE *fp = NULL; // output file
// Get inputs
params = set_default_params();
parse_params("parameters", params);
read_CLI(argc, argv, params);
print_params(params);
// Initial guess of flux
phi = init_flux(params);
solve(phi, params);
printf("keff = %f\n", params->k);
// Write solution
if(params->write_flux == TRUE) {
write_flux(phi, params, fp);
}
// Free memory
free_flux(phi);
free(params);
return 0;
}
main(int argc, char** argv)
{
if (argc != 2) {
fprintf(stderr, "usage: params filename\n");
exit(1);
}
if (read_param_file(argv[1])) {
print_params(stdout);
for (;;) {
char type;
char name[100], line[100];
printf("> ");
gets(line);
if (sscanf(line, "%c %s", &type, name) != 2) return;
switch (type) {
case 's': printf("%s\n", string_param(name)); break;
case 'b': printf("%d\n", bool_param(name)); break;
case 'i': printf("%d\n", int_param(name)); break;
case 'd': printf("%lf\n", double_param(name)); break;
default: printf("unknown type %c\n", type);
}
}
}
}
int UavcanNode::list_params(int remote_node_id)
{
int rv = 0;
int index = 0;
uavcan::protocol::param::GetSet::Response resp;
set_setget_response(&resp);
while (true) {
uavcan::protocol::param::GetSet::Request req;
req.index = index++;
_callback_success = false;
int call_res = get_set_param(remote_node_id, nullptr, req);
if (call_res < 0 || !_callback_success) {
std::printf("Failed to get param: %d\n", call_res);
rv = -1;
break;
}
if (resp.name.empty()) { // Empty name means no such param, which means we're finished
break;
}
print_params(resp);
}
free_setget_response();
return rv;
}
int main ()
{ int x = 1;
while (x == 1)
{
int selection;
printf("\nPerformance Assessment: \n");
printf("-----------------------\n");
printf("1) Enter parameters\n");
printf("2) Print table of parameters\n");
printf("3) Print table of performance\n");
printf("4) Quit\n\n");
printf("Enter Selection: ");
scanf("%d", &selection);
if (selection == 1)
enter_params();
else if (selection == 2)
print_params();
else if (selection == 3)
print_performance();
else if (selection == 4)
{
deallocate_memory();
x = 2;
}
}
return 0;
}
void
Continuous_calibration::reconfigure_callback(Config &config, uint32_t level){
bool init_subscriber = false;
bool init_publisher = false;
if( this->config.subscribe_pcl_topic != config.subscribe_pcl_topic
|| this->config.subscribe_image_topic != config.subscribe_image_topic
|| this->config.subscribe_image_info_topic != config.subscribe_image_info_topic)
{
init_subscriber = true;
}
if( this->config.publish_pointcloud_topic != config.publish_pointcloud_topic
|| this->config.publish_pointcloud_color_topic != config.publish_pointcloud_color_topic)
{
init_publisher = true;
}
this->config = config;
init_params();
print_params();
if(init_publisher){
init_pub();
}
if(init_subscriber){
init_sub();
}
}
void SetupParams(int argc, char **argv) {
char fname[STRLEN];
init_params(argc, argv);
// PARAMETER DEFINITIONS GO HERE
STRING_PARAM(FileBase);
INT_PARAM(ElecNo);
INT_PARAM(MinClusters);
INT_PARAM(MaxClusters);
INT_PARAM(MaxPossibleClusters);
INT_PARAM(nStarts);
INT_PARAM(RandomSeed);
BOOLEAN_PARAM(Debug);
INT_PARAM(Verbose);
STRING_PARAM(UseFeatures);
INT_PARAM(DistDump);
FLOAT_PARAM(DistThresh);
INT_PARAM(FullStepEvery);
FLOAT_PARAM(ChangedThresh);
BOOLEAN_PARAM(Log);
BOOLEAN_PARAM(Screen);
INT_PARAM(MaxIter);
STRING_PARAM(StartCluFile);
INT_PARAM(SplitEvery);
FLOAT_PARAM(PenaltyMix);
INT_PARAM(Subset);
if (argc<3) {
fprintf(stderr, "Usage: KlustaKwik FileBase ElecNo [Arguments]\n\n");
fprintf(stderr, "Default Parameters: \n");
print_params(stderr);
exit(1);
}
strcpy(FileBase, argv[1]);
ElecNo = atoi(argv[2]);
if (Screen) print_params(stdout);
// open log file, if required
if (Log) {
sprintf(fname, "%s.klg.%d", FileBase, ElecNo);
logfp = fopen_safe(fname, "w");
print_params(logfp);
}
}
static void print_method(compile_t* c, printbuf_t* buf, reachable_type_t* t,
const char* name, ast_t* typeargs)
{
const char* funname = genname_fun(t->name, name, typeargs);
LLVMValueRef func = LLVMGetNamedFunction(c->module, funname);
if(func == NULL)
return;
// Get a reified function.
ast_t* fun = get_fun(t->ast, name, typeargs);
if(fun == NULL)
return;
AST_GET_CHILDREN(fun, cap, id, typeparams, params, rtype, can_error, body,
docstring);
// Print the docstring if we have one.
if(ast_id(docstring) == TK_STRING)
{
printbuf(buf,
"/*\n"
"%s"
"*/\n",
ast_name(docstring)
);
}
// Print the function signature.
print_type_name(c, buf, rtype);
printbuf(buf, " %s", funname);
switch(ast_id(fun))
{
case TK_NEW:
case TK_BE:
{
ast_t* def = (ast_t*)ast_data(t->ast);
if(ast_id(def) == TK_ACTOR)
printbuf(buf, "__send");
break;
}
default: {}
}
printbuf(buf, "(");
print_type_name(c, buf, t->ast);
printbuf(buf, " self");
print_params(c, buf, params);
printbuf(buf, ");\n\n");
ast_free_unattached(fun);
}
static void _print_params(char *name, char *comment, struct floppy_struct *ft)
{
printf("\n\"%s\":", name);
if(comment)
printf(" #%s", comment);
else
printf("\n");
col = 0;
print_params(0, ft, level, cpm, print_token);
}
/*! \brief
* Print structure, for debugging only
*/
void print_event(event_t* e)
{
fprintf(stderr, "===Event===\n");
fprintf(stderr, "name : \'%.*s\'\n", STR_FMT(&e->name));
fprintf(stderr, "type: %d\n", e->type);
if (e->params.list) {
print_params(stderr, e->params.list);
}
fprintf(stderr, "===/Event===\n");
}
Aqwin *aqwin_open(const char *cfg) {
Aqwin *win;
aqwin_param *p;
if ((p = parse_cfg(cfg)) == NULL) {
complain("aqwin_open: invalid config \"%s\"\n", cfg);
return NULL;
}
print_params(p);
win = aqwin_create(p);
aqwin_param_free(p);
return win;
}
static int report_blocks(FILE* fw, Block** bb, int num)
{
print_params(fw);
sort_block_list(bb, num);
int i, j,k;
int n = MIN(num, po->RPT_BLOCK);
bool flag;
Block **output;
AllocArray(output, n);
Block **bb_ptr = output;
Block *b_ptr;
double cur_rows, cur_cols;
double inter_rows, inter_cols;
/*double proportion;*/
/* the major post-processing here, filter overlapping blocks*/
i = 0; j = 0;
while (i < num && j < n)
{
b_ptr = bb[i];
cur_rows = b_ptr->block_rows;
cur_cols = b_ptr->block_cols;
flag = TRUE;
k = 0;
while (k < j)
{
inter_rows = dsIntersect(output[k]->genes, b_ptr->genes);
inter_cols = dsIntersect(output[k]->conds, b_ptr->conds);
if (inter_rows*inter_cols > po->FILTER*cur_rows*cur_cols)
{
flag = FALSE; break;
}
/*proportion=(inter_rows*inter_cols)/(cur_rows*cur_cols);
printf ("%d\t%d\t%.3f\n",j,k,proportion);*/
k++;
}
i++;
if (flag)
{
print_bc(fw, b_ptr, j++);
*bb_ptr++ = b_ptr;
}
}
return j;
}
int main(int argc, char* argv[])
{
if (argc > 1)
{
if (strcmp(argv[1], "est")==0)
{
read_params(argv[6]);
print_params();
em(argv[2], atoi(argv[3]), argv[4], argv[5]);
return(0);
}
if (strcmp(argv[1], "inf")==0)
{
read_params(argv[5]);
print_params();
inference(argv[2], argv[3], argv[4]);
return(0);
}
}
printf("usage : ctm est <dataset> <# topics> <rand/seed/model> <dir> <settings>\n");
printf(" ctm inf <dataset> <model-prefix> <results-prefix> <settings>\n");
return(0);
}
void get_syscall_infos(pid_t pid, struct user *usr, char *mode)
{
long code;
t_sysinfo syscall;
if ((code = ptrace(PTRACE_PEEKTEXT, pid, usr->regs.rip, NULL)) == -1
|| (code & 0xFFFF) != 0x50f)
return ;
else if (usr->regs.rax > NB_SYSCALL)
return ;
*mode = 1;
syscall = g_syscalls[usr->regs.rax];
fprintf(stderr, "%s(", syscall.call);
print_params(pid, &syscall, &(usr->regs));
fprintf(stderr, ")");
}
/*! \brief
* Print list of RRs, just for debugging
*/
void print_rr(FILE* _o, rr_t* _r)
{
rr_t* ptr;
ptr = _r;
while(ptr) {
fprintf(_o, "---RR---\n");
print_nameaddr(_o, &ptr->nameaddr);
fprintf(_o, "r2 : %p\n", ptr->r2);
if (ptr->params) {
print_params(_o, ptr->params);
}
fprintf(_o, "len: %d\n", ptr->len);
fprintf(_o, "---/RR---\n");
ptr = ptr->next;
}
}
int UavcanNode::get_param(int remote_node_id, const char *name)
{
uavcan::protocol::param::GetSet::Request req;
uavcan::protocol::param::GetSet::Response resp;
set_setget_response(&resp);
int rv = get_set_param(remote_node_id, name, req);
if (rv < 0 || resp.name.empty()) {
std::printf("Failed to get param: %s\n", name);
rv = -1;
} else {
print_params(resp);
rv = 0;
}
free_setget_response();
return rv;
}
/*
Print proc prologue, body, and epilogue
*/
void print_proc(Proc proc) {
// Print proc label
printf("proc_%s:", proc->header->id);
int stack_count = getStackSize(proc->header->id);
printf("\n");
// Print prologue comment in output
printf("#prologue\n");
if (proc->decls || proc->header->params) {
// Get the number of declarations.
printf("push_stack_frame %d", stack_count);
printf("\n");
// Print proc parameters
if(proc->header->params)
print_params(proc->header->params,proc->header->id);
// Declare int and real constants
printf("int_const r0, 0\n");
printf("real_const r1, 0.0\n");
printf("\n");
// Print proc declarations
if(proc->decls)
print_decls(proc->decls, proc->header->id);
}
printf("\n");
// Print proc body
if (proc->body) {
print_stmts(proc->body, proc->header->id);
}
// Print epilogue
printf("#epilogue\n");
if (stack_count != 0)
printf("pop_stack_frame %d\n", stack_count);
printf("return\n");
}
void lvm_kriging(
const cont_property_array_t & input,
const mean_t * mean_data,
const sugarbox_grid_t & grid,
const ok_params_t & params,
cont_property_array_t & output)
{
print_algo_name("LVM Kriging");
print_params(params);
progress_reporter_t reporter(grid.size());
kriging_stats_t stats;
typedef precalculated_covariances_t covariances_t;
covariances_t pcov(cov_model_t(params), params.m_radiuses);
hpgl::cont_kriging(input, grid, params, mean_data, pcov,
sk_weight_calculator_t(),
output, reporter, stats, mean_on_failure);
write(boost::format("%1%") % stats);
}
void sequential_indicator_simulation(
indicator_property_array_t & property,
const sugarbox_grid_t & grid,
const ik_params_t & params,
int seed,
progress_reporter_t & report,
bool use_corellogram,
const unsigned char * mask)
{
print_algo_name("Sequential Indicator Simulation");
print_params(params);
if (property.size() != grid.size())
throw hpgl_exception("sequential_indicator_simulation", boost::format("Property size '%s' is not equal to grid size '%s'") % property.size() % grid.size());
std::vector<single_mean_t> single_means;
create_means(params.m_marginal_probs, single_means);
if (mask == NULL)
do_sis(property, grid, params, seed, single_means, report, sk_weight_calculator_t(), no_mask_t());
else
do_sis(property, grid, params, seed, single_means, report, sk_weight_calculator_t(), mask);
}
/*
* Print list of contacts, just for debugging
*/
void print_contacts(FILE* _o, contact_t* _c)
{
contact_t* ptr;
ptr = _c;
while(ptr) {
fprintf(_o, "---Contact---\n");
fprintf(_o, "name : '%.*s'\n", ptr->name.len, ptr->name.s);
fprintf(_o, "URI : '%.*s'\n", ptr->uri.len, ptr->uri.s);
fprintf(_o, "q : %p\n", ptr->q);
fprintf(_o, "expires : %p\n", ptr->expires);
fprintf(_o, "received: %p\n", ptr->received);
fprintf(_o, "method : %p\n", ptr->methods);
fprintf(_o, "len : %d\n", ptr->len);
if (ptr->params) {
print_params(_o, ptr->params);
}
fprintf(_o, "---/Contact---\n");
ptr = ptr->next;
}
}
/*
* Routine: process_options(int count, char **vector)
* Purpose: process a set of command line options
* Algorithm:
* Data Structures:
*
* Params:
* Returns:
* Called By:
* Calls:
* Assumptions:
* Side Effects:
* TODO: 20000309 need to return integer to allow processing of left-over args
*/
int
process_options (int count, char **vector)
{
int option_num = 1,
res = 1;
init_params();
while (option_num < count)
{
if (*vector[option_num] == OPTION_START)
{
if (option_num == (count - 1))
res = set_option(vector[option_num] + 1, NULL);
else
res = set_option(vector[option_num] + 1,
vector[option_num + 1]);
}
if (res < 0)
{
printf ("ERROR: option '%s' or its argument unknown.\n",
(vector[option_num] + 1));
usage (NULL, NULL);
exit (1);
}
else
option_num += res;
}
#ifdef JMS
if (is_set("VERBOSE"))
print_params();
#endif
return(option_num);
}
int main(int argc, char** argv)
{
mc_param_t param;
mc_result_t result;
double t1, t2;
long seed;
int nthreads;
srandom(clock());
process_args(argc, argv, ¶m);
mc_result_init(&result);
t1 = omp_get_wtime();
#pragma offload target(mic)
#pragma omp parallel shared(result, param, nthreads) private(seed)
{
#pragma omp single
nthreads = omp_get_num_threads();
#pragma omp critical
seed = random();
thread_main(&result, ¶m, seed);
}
t2 = omp_get_wtime();
/* Print results */
if (param.verbose) {
print_params(¶m);
print_results(&result);
printf("%d threads (OpenMP): %e s\n", nthreads, t2-t1);
} else {
printf("%e\n", t2-t1);
}
return 0;
}
/*******************************************************************
* MAIN()
*******************************************************************/
int
main(void)
{
long lEEPROMRetStatus;
uint16_t i=0;
uint8_t halted_latch = 0;
// Set the clocking to run at 80 MHz from the PLL.
// (Well we were at 80MHz with SYSCTL_SYSDIV_2_5 but according to the errata you can't
// write to FLASH at frequencies greater than 50MHz so I slowed it down. I supposed we
// could slow the clock down when writing to FLASH but then we need to find out how long
// it takes for the clock to stabilize. This is on at the bottom of my list of things to do
// for now)
SysCtlClockSet(SYSCTL_SYSDIV_4_5 | SYSCTL_USE_PLL | SYSCTL_XTAL_16MHZ | SYSCTL_OSC_MAIN);
// Initialize the device pinout.
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOA);
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOB);
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOC);
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOD);
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOE);
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOF);
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOG);
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOH);
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOJ);
// Enable processor interrupts.
IntMasterEnable();
// Setup the UART's
my_uart_0_init(115200, (UART_CONFIG_WLEN_8 | UART_CONFIG_STOP_ONE | UART_CONFIG_PAR_NONE));
// command_handler_init overwrites the baud rate. We still need to configure the pins though
my_uart_1_init(38400, (UART_CONFIG_WLEN_8 | UART_CONFIG_STOP_ONE | UART_CONFIG_PAR_NONE));
// Enable the command handler
command_handler_init(); // We set the baud in here
// Start the timers
my_timer0_init();
my_timer1_init();
i2c_init();
motor_init();
qei_init();
gyro_init();
accel_init();
led_init();
//rc_radio_init();
//setupBluetooth();
// Initialize the EEPROM emulation region.
lEEPROMRetStatus = SoftEEPROMInit(EEPROM_START_ADDR, EEPROM_END_ADDR, EEPROM_PAGE_SIZE);
if(lEEPROMRetStatus != 0) UART0Send("EEprom ERROR!\n", 14);
#if 0
// If ever we wanted to write some parameters to FLASH without the HMI
// we could do it here.
SoftEEPROMWriteDouble(kP_ID, 10.00);
SoftEEPROMWriteDouble(kI_ID, 10.00);
SoftEEPROMWriteDouble(kD_ID, 10.00);
SoftEEPROMWriteDouble(ANG_ID, 0.0);
SoftEEPROMWriteDouble(COMPC_ID, 0.99);
#endif
kP = SoftEEPROMReadDouble(kP_ID);
kI = SoftEEPROMReadDouble(kI_ID);
kD = SoftEEPROMReadDouble(kD_ID);
commanded_ang = zero_ang = SoftEEPROMReadDouble(ANG_ID);
COMP_C = SoftEEPROMReadDouble(COMPC_ID);
pid_init(kP, kI, kD, &pid_ang);
motor_controller_init(20, 100, 10, &mot_left);
motor_controller_init(20, 100, 10, &mot_right);
//pid_init(0.0, 0.0, 0.0, &pid_pos_left);
//pid_init(0.0, 0.0, 0.0, &pid_pos_right);
//UART0Send("Hello World!\n", 13);
// Tell the HMI what the initial parameters are.
print_params(1);
while(1)
{
delta_t = myTimerValueGet();
myTimerZero();
sum_delta_t += delta_t;
// Read our sensors
accel_get_xyz_cal(&accel_x, &accel_y, &accel_z, true);
gyro_get_y_cal(&gyro_y, false);
// Calculate the pitch angle with the accelerometer only
R = sqrt(pow(accel_x, 2) + pow(accel_z, 2));
accel_pitch_ang = (acos(accel_z / R)*(RAD_TO_DEG)) - 90.0 - zero_ang;
//accel_pitch_ang = (double)((atan2(accel_x, -accel_z))*RAD_TO_DEG - 90.0);
gyro_pitch_ang += (double)gyro_y*g_gyroScale*CONV_TO_SEC(delta_t);
// Kalman filter
//filtered_ang = kalman((double)accel_pitch_ang, ((double)gyro_y)*g_gyroScale, CONV_TO_SEC(delta_t));
filtered_ang = (COMP_C*(filtered_ang+((double)gyro_y*g_gyroScale*CONV_TO_SEC(delta_t)))) + ((1.0-COMP_C)*(double)accel_pitch_ang);
// Skip the rest of the process until the angle stabilizes
if(i < 250) { i++; continue; }
// Tell the HMI what's going on every 100ms
if(sum_delta_t >= 1000)
{
print_update(1);
print_debug(0);
//print_control_surfaces(0);
led_toggle();
//print_angle();
sum_delta_t = 0;
}
// See if the HMI has anything to say
command_handler();
//continue;
// If we are leaning more than +/- FALL_ANG deg off center it's hopeless.
// Turn off the motors in hopes of some damage control
if( abs(filtered_ang) > FALL_ANG )
{
if(halted_latch) continue;
stop_motors();
halted_latch = 1;
continue;
}
halted_latch = 0;
motor_val = pid_controller(calc_commanded_angle(0), filtered_ang, delta_t, &pid_ang);
motor_left = motor_right = motor_val;
drive_motors(motor_left*left_mot_gain, motor_right*right_mot_gain);
}
}
int main( int argc, char *argv[] )
{
unsigned iter;
FILE *infile, *resfile;
char *resfilename;
// algorithmic parameters
algoparam_t param;
int np;
double runtime, flop;
double residual=0.0;
// check arguments
if( argc < 2 )
{
usage( argv[0] );
return 1;
}
// check input file
if( !(infile=fopen(argv[1], "r")) )
{
fprintf(stderr,
"\nError: Cannot open \"%s\" for reading.\n\n", argv[1]);
usage(argv[0]);
return 1;
}
// check result file
resfilename= (argc>=3) ? argv[2]:"heat.ppm";
if( !(resfile=fopen(resfilename, "w")) )
{
fprintf(stderr,
"\nError: Cannot open \"%s\" for writing.\n\n",
resfilename);
usage(argv[0]);
return 1;
}
// check input
if( !read_input(infile, ¶m) )
{
fprintf(stderr, "\nError: Error parsing input file.\n\n");
usage(argv[0]);
return 1;
}
print_params(¶m);
if( !initialize(¶m) )
{
fprintf(stderr, "Error in Solver initialization.\n\n");
usage(argv[0]);
return 1;
}
// full size (param.resolution are only the inner points)
np = param.resolution + 2;
#if _EXTRAE_
Extrae_init();
#endif
// starting time
runtime = wtime();
iter = 0;
while(1) {
switch( param.algorithm ) {
case 0: // JACOBI
residual = relax_jacobi(param.u, param.uhelp, np, np);
// Copy uhelp into u
copy_mat(param.uhelp, param.u, np, np);
break;
case 1: // GAUSS
residual = relax_gauss(param.u, np, np);
break;
}
iter++;
// solution good enough ?
if (residual < 0.00005) break;
// max. iteration reached ? (no limit with maxiter=0)
if (param.maxiter>0 && iter>=param.maxiter) break;
}
// Flop count after iter iterations
flop = iter * 11.0 * param.resolution * param.resolution;
// stopping time
runtime = wtime() - runtime;
#if _EXTRAE_
Extrae_fini();
#endif
fprintf(stdout, "Time: %04.3f \n", runtime);
fprintf(stdout, "Flops and Flops per second: (%3.3f GFlop => %6.2f MFlop/s)\n",
flop/1000000000.0,
flop/runtime/1000000);
fprintf(stdout, "Convergence to residual=%f: %d iterations\n", residual, iter);
// for plot...
coarsen( param.u, np, np,
param.uvis, param.visres+2, param.visres+2 );
write_image( resfile, param.uvis,
param.visres+2,
param.visres+2 );
finalize( ¶m );
return 0;
}
