int main(void)
{
sched_init(); /* initialize the scheduler */
led_init(); /* initialize led */
button_init(); /* initialize button */
adc_init(); /* initialize ADC (battery voltage measurement) */
serial_init(); /* initialize serial communication */
wheel_init(); /* initialize encoders, PWM output, PID etc. */
pid_interval = 50; /* default PID update interval 50ms */
pid_rate = 1000/pid_interval; /* [Hz] always remember after setting pid_interval */
pfbst_interval = 20; /* send $PFBST at 20 ms interval */
nmea_wd_timeout = 1; /* set PFBCT watchdog timeout to 100ms */
nmea_wd = NMEA_WD_TOUT+1; /* make sure we begin in watchdog timeout state */
voltage_min = VOLTAGE_MIN_DEFAULT;
battery_low_warning = false;
state_update();
sei(); /* enable interrupts */
nmea_init(); /* initialize nmea protocol handler */
for (;;) /* go into an endless loop */
{
/* motor_update(); */
if (t1ms != 0) /* if the interrupt has timed out after 10ms */
{
t1ms --;
sched_update(); /* run the scheduler */
}
else
{
nmea_rx_update();
}
}
return 0; /* just for the principle as we never get here */
}
/* The real main loop of the Eruta engine. Most of the work happens in state.c,
* and the main.rb script, though. */
int real_main(void) {
Image * border = NULL;
Image * sheet = NULL;
Tileset * tileset = NULL;
Tile * tile = NULL;
State * state = NULL;
Camera * camera = NULL;
Tilepane * tilepane = NULL;
Tilemap * map = NULL;
Thing * actor = NULL;
Tracker * tracker = NULL;
Tracker * maptracker = NULL;
Sprite * sprite = NULL;
SpriteState * spritestate = NULL;
AlpsShower shower;
int actor_id = -1;
int sprite_id = -1;
int npc1_id = -1;
int npc2_id = -1;
React react;
ALLEGRO_COLOR myblack = {0.0, 0.0, 0.0, 1.0};
state = state_alloc();
state_set(state);
if((!(state)) || (!state_init(state, FALSE))) {
perror(state_errmsg(state));
return 1;
}
alpsshower_init(&shower, state_camera(state), 100, 1.0, bevec(10.0, 10000.0));
/* Initializes the reactor, the game state is it's data. */
react_initempty(&react, state);
react.keyboard_key_up = main_react_key_up;
react.keyboard_key_down = main_react_key_down;
puts_standard_path(ALLEGRO_EXENAME_PATH, "ALLEGRO_EXENAME_PATH:");
camera = state_camera(state);
/* Finally initialize ruby and call the ruby startup function. */
rh_load_main();
callrb_on_start();
/* Main game loop, controlled by the State object. */
while(state_busy(state)) {
Point spritenow = bevec(100, 120);
react_poll(&react, state);
alpsshower_update(&shower, state_frametime(state));
state_update(state);
state_draw(state);
/* alpsshower_draw(&shower, camera); */
state_flip_display(state);
}
state_done(state);
state_free(state);
return 0;
}
int streamdecode_process(struct stream_state *s, size_t count, double samples[count])
{
unsigned window_size = WINDOW_SIZE(&s->as);
for (unsigned i = 0; i < count; i++) {
s->gbltick++;
s->tick++;
filter_put(s->chan, samples[i]);
double bandpassed = filter_get(s->chan);
for (int b = 0; b < 2; b++) {
filter_put(s->bit[b], bandpassed);
double *energy = &s->energy[b];
double *trailing = &s->ehist[b][(i + window_size - 1) % window_size];
*energy -= *trailing;
double bitval = filter_get(s->bit[b]);
double term = bitval * bitval;
*trailing = term;
*energy += term;
}
// drop the first WINDOW_SIZE samples to make energy readings meaningful
if (s->gbltick > window_size)
if (state_update(s))
return -1;
}
return 0;
}
/**
* Subroutine that simulates the processor.
* The processor should fetch instructions as appropriate, until all instructions have executed
* XXX: You're responsible for completing this routine
*
* @p_stats Pointer to the statistics structure
*/
void run_proc(proc_stats_t* p_stats) {
while (ifetched>icompleated || was_cache_stalled()){
state_update();
execute();
schedule();
dispatch(p_stats);
fetch(p_stats);
state_update2();
p_stats->total_dqueue_size += fetched.size();
cycle++;
}
}
void sched_update (void)
{
t1ms_cnt++;
if (t1ms_cnt == 10000)
t1ms_cnt = 0;
if (t1ms_cnt % 20 == 0) /* each 20 ms */
{
wheel_update_ticks_buffers();
}
if (t1ms_cnt % pid_interval == 0) /* motor controller update */
{
if (pid_enable)
wheel_update_pid(); /* update PID motor controller */
else
wheel_update_open_loop(); /* update open loop motor controller */
}
if (t1ms_cnt % pfbst_interval == 0) /* send $PFBST */
{
nmea_tx_status();
}
/* each 10 ms */
if (t1ms_cnt % 10 == 0) /* each 10 ms */
{
if (t1ms_cnt % 20 == 0) /* each 20 ms */
{
}
if (t1ms_cnt % 50 == 0) /* each 50 ms */
{
}
if (t1ms_cnt % 100 == 0) /* each 100 ms */
{
if (nmea_wd_timeout)
nmea_wd++; /* increase watchdog timeout */
else
nmea_wd = 0;
voltage = adc_data[0]; /* request voltage measurement */
state_update();
}
if (t1ms_cnt % 200 == 0) /* each 200 ms */
{
button_update();
led_update();
voltage_update();
}
}
}
void MainWindow::update_data(){
ROS_INFO_STREAM("test");
state_update();
ui.label_6->setText(QString::number(100*qnode.open_s, 'd',0) + " %");
ui.label_7->setText(QString::number(100*qnode.close_s,'d',0) + " %");
if(model_n == 2 && qnode.model_number == 1){
ui.label_cur_model->setPixmap(QPixmap(":images/model1.bmp"));
model_n = 1;
}
if(model_n == 1 && qnode.model_number == 2){
ui.label_cur_model->setPixmap(QPixmap(":images/model2.bmp"));
model_n = 2;
}
ui.progressBar1->setValue(qnode.open_s*100);
ui.progressBar2->setValue(qnode.close_s*100);
if(first_run){
ui.line1->setGeometry(ui.progressBar1->x() + qnode.open_t*(ui.progressBar1->width() - ui.line1->width()), ui.line1->y(), ui.line1->width(), ui.line1->height());
ui.line2->setGeometry(ui.progressBar2->x() + qnode.close_t*(ui.progressBar2->width() - ui.line2->width()), ui.line2->y(), ui.line2->width(), ui.line2->height());
first_run = false;
}
}
/*========================================================================
** The Main Procedure
** Initialization and FIFO command dispatch routine.
========================================================================*/
static int real_main(int argc, char *argv[])
{
const char function[] = "real_main";
int option_foreground = FALSE;
int FIFO; /* First-in-first-out which feeds us requests */
sigset_t lock_set;
struct timeval next_tick;
/* Initialize internation messages library. */
#ifdef INTERNATIONAL
setlocale(LC_ALL, "");
bindtextdomain(PACKAGE_PPRD, LOCALEDIR);
textdomain(PACKAGE_PPRD);
#endif
/*
** Set some environment variables, (PATH, IFS, and
** SHELL) for safety and for the convenience of the
** programs we launch (HOME, and PPR_VERSION).
** Remove unnecessary and potentially misleading
** variables.
*/
set_ppr_env();
prune_env();
parse_command_line(argc, argv, &option_foreground);
/* Switch all UIDs to USER_PPR, all GIDS to GROUP_PPR. */
adjust_ids();
/* If the --forground switch wasn't used, then dropt into background. */
if(! option_foreground)
gu_daemon(PPR_PPRD_UMASK);
else
umask(PPR_PPRD_UMASK);
/* Change the home directory to the PPR home directory: */
chdir(HOMEDIR);
/* Create /var/spool/ppr/pprd.pid. */
create_lock_file();
/* Signal handlers for silly stuff. */
signal_restarting(SIGPIPE, signal_ignore);
signal_restarting(SIGHUP, signal_ignore);
/* Signal handler for shutdown request. */
signal_interupting(SIGTERM, sigterm_handler);
/* Arrange for child termination to be noted. */
signal_restarting(SIGCHLD, sigchld_handler);
/* Move /var/spool/ppr/logs/pprd to pprd.old before we call debug()
for the first time (below). */
rename_old_log_file();
/*
** This code must come after adjust_ids() and gu_daemon().
** It makes the first log entry and tells queue-display
** programs that we are starting up.
*/
debug("PPRD startup, pid=%ld", (long)getpid());
state_update("STARTUP");
/* Make sure the local node gets the node id of 0. */
if(! nodeid_is_local_node(nodeid_assign(ppr_get_nodename())))
fatal(1, "%s(): line %d: assertion failed", function, __LINE__);
/* Initialize other subsystems. */
question_init();
/* Load the printers database. */
DODEBUG_STARTUP(("loading printers database"));
load_printers();
/* Load the groups database. */
DODEBUG_STARTUP(("loading groups database"));
load_groups();
/* Set up the FIFO. */
DODEBUG_STARTUP(("opening FIFO"));
FIFO = open_fifo();
/* Initialize the queue. This is likely to start printers. */
DODEBUG_STARTUP(("initializing the queue"));
initialize_queue();
/* Schedule the first timer tick. */
gettimeofday(&next_tick, NULL);
next_tick.tv_sec += TICK_INTERVAL;
/*
** Create a signal block set which will be used to block SIGCHLD except
** when we are calling select().
*/
sigemptyset(&lock_set);
sigaddset(&lock_set, SIGCHLD);
sigprocmask(SIG_BLOCK, &lock_set, (sigset_t*)NULL);
/*
** This is the Main Loop. It runs until the sigterm_handler
** sets sigterm_received.
*/
while(!sigterm_received)
{
int readyfds; /* return value from select() */
fd_set rfds; /* list of file descriptors for select() to watch */
struct timeval time_now; /* the current time */
DODEBUG_MAINLOOP(("top of main loop"));
gettimeofday(&time_now, NULL);
/* If it is time for or past time for the next tick, */
if(gu_timeval_cmp(&time_now, &next_tick) >= 0)
{
readyfds = 0;
}
/* If it is not time for the next tick yet, */
else
{
/* Set the select() timeout so that it will return in time for the
next tick(). */
gu_timeval_cpy(&select_tv, &next_tick);
gu_timeval_sub(&select_tv, &time_now);
/* Create a file descriptor list which contains only the descriptor
of the FIFO. */
FD_ZERO(&rfds);
FD_SET(FIFO, &rfds);
/* Call select() with SIGCHLD unblocked. */
sigprocmask(SIG_UNBLOCK, &lock_set, (sigset_t*)NULL);
readyfds = select(FIFO + 1, &rfds, NULL, NULL, &select_tv);
sigprocmask(SIG_BLOCK, &lock_set, (sigset_t*)NULL);
}
/* If there is something to read, */
if(readyfds > 0)
{
if(!FD_ISSET(FIFO, &rfds))
fatal(0, "%s(): assertion failed: select() returned but FIFO not ready", function);
do_command(FIFO);
continue;
}
/* If the SIGCHLD handler set the flag, handle child termination. Once
we have done that, we must go back to the top of the loop because
we don't really know if it is time for a tick() call yet. */
if(sigchld_caught)
{
sigchld_caught = FALSE;
reapchild();
continue;
}
/* If there was no error and no file descriptors are ready, then the
timeout must have expired. Call tick(). */
if(readyfds == 0)
{
tick();
next_tick.tv_sec += TICK_INTERVAL;
continue;
}
/* If interupted by a system call, restart it. */
if(errno == EINTR)
continue;
/* If we get this far, there was an error. */
fatal(0, "%s(): select() failed, errno=%d (%s)", function, errno, gu_strerror(errno));
} /* end of endless while() loop */
state_update("SHUTDOWN");
fatal(0, "Received SIGTERM, exiting");
} /* end of real_main() */
/*****************************************************************************
* Balance -
*****************************************************************************/
void balance(void)
{
unsigned long TimerMsWork;
long int g_bias = 0;
double x_offset = 532; //offset value 2.56V * 1024 / 4.93V = 4254
double q_m = 0.0;
double int_angle = 0.0;
double x = 0.0;
double tilt = 0.0;
int pwm;
InitADC();
init_pwm();
// initialize the UART (serial port)
uartInit();
// set the baud rate of the UART for our debug/reporting output
uartSetBaudRate(115200);
// initialize rprintf system
rprintfInit(uartSendByte);
// initialize vt100 library
vt100Init();
// clear the terminal screen
vt100ClearScreen();
TimerMsWork = TimerMsCur();
DDRB |= (1 << PB0); // Make B0 an output for LED
/* as a 1st step, a reference measurement of the angular rate sensor is
* done. This value is used as offset compensation */
for (int i=1 ; i<=200; i++) // determine initial value for bias of gyro
{
g_bias = g_bias + GetADC(gyro_sensor);
}
g_bias = g_bias / 200;
while (!(getkey() == 1))
{
/* insure loop runs at specified Hz */
while (!TimerCheck(TimerMsWork, (dt_PARAM * 1000) -1))
;
TimerMsWork = TimerMsCur();
// toggle pin B0 for oscilloscope timings.
PORTB = PINB ^ (1 << PB0);
// get rate gyro reading and convert to deg/sec
// q_m = (GetADC(gyro_sensor) - g_bias) / -3.072; // -3.07bits/deg/sec (neg. because forward is CCW)
q_m = (GetADC(gyro_sensor) - g_bias) * -0.3255; // each bit = 0.3255 /deg/sec (neg. because forward is CCW)
state_update(q_m);
// get Accelerometer reading and convert to units of gravity.
// x = (GetADC(accel_sensor) - x_offset) / 204.9; // (205 bits/G)
x = (GetADC(accel_sensor) - x_offset) * 0.00488; // each bit = 0.00488/G
// x is measured in multiples of earth gravitation g
// therefore x = sin (tilt) or tilt = arcsin(x)
// for small angles in rad (not deg): arcsin(x)=x
// Calculation of deg from rad: 1 deg = 180/pi = 57.29577951
tilt = 57.29577951 * (x);
kalman_update(tilt);
int_angle += angle * dt_PARAM;
rprintf(" x:");
rprintfFloat(8, x);
rprintf(" angle:");
rprintfFloat(8, angle);
rprintf(" rate:");
rprintfFloat(8, rate);
// Balance. The most important line in the entire program.
// balance_torque = Kp * (current_angle - neutral) + Kd * current_rate;
// rprintf("bal_torq: ");
// rprintfFloat(8, balance_torque);
// rprintfCRLF();
//steer_knob = 0;
// change from current angle to something proportional to speed
// should this be the abs val of the cur speed or just curr speed?
//double steer_cmd = (1.0 / (1.0 + Ksteer2 * fabs(current_angle))) * (Ksteer * steer_knob);
//double steer_cmd = 0.0;
// Get current rate of turn
//double current_turn = left_speed - right_speed; //<-- is this correct
//double turn_accel = current_turn - prev_turn;
//prev_turn = current_turn;
// Closed-loop turn rate PID
//double steer_cmd = KpTurn * (current_turn - steer_desired)
// + KdTurn * turn_accel;
// //+ KiTurn * turn_integrated;
// Possibly optional
//turn_integrated += current_turn - steer_cmd;
// Differential steering
//left_motor_torque = balance_torque + steer_cmd; //+ cur_speed + steer_cmd;
//right_motor_torque = balance_torque - steer_cmd; //+ cur_speed - steer_cmd;
// Limit extents of torque demand
//left_motor_torque = flim(left_motor_torque, -MAX_TORQUE, MAX_TORQUE);
// if (left_motor_torque < -MAX_TORQUE) left_motor_torque = -MAX_TORQUE;
// if (left_motor_torque > MAX_TORQUE) left_motor_torque = MAX_TORQUE;
//right_motor_torque = flim(right_motor_torque, -MAX_TORQUE, MAX_TORQUE);
// if (right_motor_torque < -MAX_TORQUE) right_motor_torque = -MAX_TORQUE;
// if (right_motor_torque > MAX_TORQUE) right_motor_torque = MAX_TORQUE;
pwm = (int) ((angle -3.5) * Kp) + (rate * Kd); // + (int_angle * Ki);
rprintf(" pwm:%d\r\n", pwm);
// Set PWM values for both motors
SetLeftMotorPWM(pwm);
SetRightMotorPWM(pwm);
}
SetLeftMotorPWM(0);
SetRightMotorPWM(0);
}
static void applier(void *state, raft_update_t update, raft_bool_t snapshot)
{
Assert(state);
state_update(state, (RaftableUpdate *)update.data, snapshot);
}
