/*
* Update the animation state for the given model.
*/
void world_tick_model(struct md3_model_t* m) {
double now, elapsed, frame_duration;
if (!m->anim_state.animated)
/* if we are not in a state of animation t should not change */
return;
now = get_time_in_ms();
elapsed = (now - m->anim_state.last_time);
frame_duration = (1000.0 / g_world->anims[m->anim_state.id].fps);
#ifdef USE_INTERPOLATION
if (WORLD_IS_SET(ENGINE_INTERPOLATE))
m->anim_state.t = (elapsed / frame_duration);
#endif
if (elapsed >= frame_duration) {
/* tick the frame to the next key frame */
m->anim_state.frame++;
m->anim_state.frame = m->anim_state.next_frame;
m->anim_state.next_frame = get_next_frame(&m->anim_state);
m->anim_state.last_time = now;
m->anim_state.t = 0;
}
}
// Check to see if the future time has elapsed.
int64_t expired_ms(int64_t timeout_ms)
{
if (timeout_ms < get_time_in_ms())
{
return true;
}
return false;
}
// Pass in the elapsed time before it times out, returns the future time
int64_t set_ms_timeout(int64_t timeout_ms)
{
return get_time_in_ms() + timeout_ms;
}
// Pass in the ms_ticks when the timer started, get back the number of ms since then
// Pass in a future value to get time until timeout occurs, a neg value
int64_t elapsed_ms(int64_t start_ms)
{
return get_time_in_ms() - start_ms;
}
void delay_ms(uint64_t milliseconds)
{
uint64_t trigger_time = get_time_in_ms() + milliseconds;
while (get_time_in_ms() < trigger_time)
;
}
int main( void ){
uint8_t error;
Packet packet;
Queue* packets;
packets = Packets_getQueue();
// initialize pwm
pwm_init();
pwm_set_scalar(1);
pwm_set(PWM_CHAN_A, 127);
// initialize Time
time_init();
TimeResult tr, rpm_tr;
uint32_t previous = 0;
rpm_tr.previous = 0;
// enable external interrupt for INT0
//setbit(MCUCR, BIT(ISC01) | BIT(ISC00));
setbit(MCUCR,BIT(ISC00));
setbit(GICR, BIT(INT0));
// initialize rpm counter
InterruptCounterResult icr;
icr.previous = 0;
float rpm = 0, prev_rpm = 0, time_ms = 0, rpm_delta = 0;
uint16_t* rpm_target_p;
float pwm_power = 0;
float pwm_acc = 0;
float rpm_target = 0;
rpm_target_p = get_rpm_handle();
*rpm_target_p = 1000;
// initialize usart
usart_init();
// enable interrupts
sei();
// set initial value
pwm_set_scalar(3);
pwm_set(1,256);
// loop forever
while(1){
error = Packets_getError();
if (error){
Packets_sendError(ERROR_PACKETS, error);
continue;
}
if (packets->count > 0){
error = 0x00;
packet = Packets_getNext();
error = Queue_getError(packets);
if (error){
Packets_sendError(ERROR_QUEUE, error);
continue;
}
handle_packet(packet);
}
tr = time_get_time_delta(previous);
// every 20ms, calculate rpm
if (get_time_in_ms(tr.delta) > 20){
previous = tr.previous;
// calculate RPM
rpm_tr = time_get_time_delta(rpm_tr.previous);
icr = interrupt_counter_delta(icr.previous);
time_ms = get_time_in_ms(rpm_tr.delta);
rpm = (double) icr.delta /(double) 32 / (double) time_ms * (double) 1000 * (double) 60;
rpm_target_p = get_rpm_handle();
rpm_target = (float) *rpm_target_p;
rpm_delta = rpm_delta * 0.65 + (rpm - prev_rpm) * 0.35;
//pwm_acc = (rpm_target - rpm) * 0.01 - rpm_delta * 0.05;
pwm_acc = (rpm_target - rpm) * 0.015 - rpm_delta * 0.15;
if (pwm_acc < - pwm_power) pwm_power = 0;
else if (pwm_acc + pwm_power > 1023) pwm_power = 1023;
else{
pwm_power = pwm_power + pwm_acc;
}
if (rpm_target == 0) pwm_power = 0;
pwm_set(1,floor(pwm_power));
prev_rpm = rpm;
cmd_send_debug16( 'r', (uint16_t) rpm);
}
}
}
int main( void ){
DDRC = 0xDB;
//PORTC = 0x7E;;
PORTC = 0xE7;
uint8_t error;
Packet packet;
Queue* packets;
packets = Packets_getQueue();
// initialize globals
globals_init();
// initialize Time
time_init();
TimeResult tr;
uint32_t previous = 0;
uint32_t watchdog = 0;
// initialize usart
usart_init();
// initialize motors
motors_init();
motors_set(MOTORS_LEFT, 0);
motors_set(MOTORS_RIGHT, 0);
// initialize analog input
analoginput_init();
// enable interrupts
sei();
// loop forever
while(1){
error = Packets_getError();
if (error){
Packets_sendError(ERROR_PACKETS, error);
continue;
}
if (packets->count > 0){
error = 0x00;
packet = Packets_getNext();
error = Queue_getError(packets);
if (error){
Packets_sendError(ERROR_QUEUE, error);
continue;
}
handle_packet(packet);
watchdog = time_get_time();
}
tr = time_get_time_delta(previous);
// every 20ms, calculate rpm
if (get_time_in_ms(tr.delta) > 20){
previous = tr.previous;
motors_tick();
send_status();
}
tr = time_get_time_delta(watchdog);
if (get_time_in_ms(tr.delta) > 500){
motors_set(MOTORS_LEFT, 0);
motors_set(MOTORS_RIGHT, 0);
}
}
}
void write_statefile(void)
{
#ifdef DEBUG
ir_uint64 ms1;
ir_uint64 ms2;
#endif /* DEBUG */
char *statefile_tmp, *statefile_bkup;
int fd;
int callval;
statefile_hdr_t *hdr;
size_t length;
ir_moutput_t bout;
updatecontext();
if (gdata.statefile == NULL)
{
return;
}
#ifdef DEBUG
ms1 = get_time_in_ms();
#endif /* DEBUG */
statefile_tmp = mystrsuffix(gdata.statefile, ".tmp");
statefile_bkup = mystrsuffix(gdata.statefile, "~");
if (gdata.debug > 0)
{
ioutput(OUT_S|OUT_L|OUT_D, COLOR_NO_COLOR, "Saving State File... ");
}
fd = open(statefile_tmp,
O_WRONLY | O_CREAT | O_TRUNC | ADDED_OPEN_FLAGS,
CREAT_PERMISSIONS);
if (fd < 0)
{
outerror(OUTERROR_TYPE_WARN_LOUD, "Cant Create State File '%s': %s",
statefile_tmp, strerror(errno));
goto error_out;
}
ir_moutput_init(&bout, fd);
/*** write ***/
write_statefile_direct(&bout, STATEFILE_MAGIC);
write_statefile_direct(&bout, gdata.old_statefile ? STATEFILE_OLD_VERSION : STATEFILE_VERSION);
updatecontext();
{
unsigned char *data;
unsigned char *next;
length = sizeof(statefile_hdr_t) + maxtextlength;
data = mycalloc(length);
/* header */
hdr = (statefile_hdr_t*)data;
next = (unsigned char*)(&hdr[1]);
length = add_snprintf((char *)next, maxtextlength,
"iroffer-dinoex " VERSIONLONG ", %s", gdata.osstring);
create_statefile_hdr(hdr, STATEFILE_TAG_IROFFER_VERSION, sizeof(statefile_hdr_t) + ceiling(length+1, 4));
write_statefile_item(&bout, data);
mydelete(data);
}
write_statefile_dinoex(&bout);
updatecontext();
{
MD5Digest digest;
bzero(digest, sizeof(digest));
ir_moutput_get_md5sum(&bout, digest);
write_statefile_raw(&bout, &digest, sizeof(digest));
}
/*** end write ***/
updatecontext();
close(fd);
/* abort if statefile was not written in full */
if (bout.error > 0)
{
goto error_out;
}
/* remove old bkup */
callval = unlink(statefile_bkup);
if ((callval < 0) && (errno != ENOENT))
{
outerror(OUTERROR_TYPE_WARN_LOUD, "Cant Remove Old State File '%s': %s",
statefile_bkup, strerror(errno));
/* ignore, continue */
}
/* backup old -> bkup */
callval = link(gdata.statefile, statefile_bkup);
if ((callval < 0) && (errno != ENOENT))
{
outerror(OUTERROR_TYPE_WARN_LOUD, "Cant Backup Old State File '%s' -> '%s': %s",
gdata.statefile, statefile_bkup, strerror(errno));
/* ignore, continue */
}
/* rename new -> current */
callval = rename(statefile_tmp, gdata.statefile);
if (callval < 0)
{
outerror(OUTERROR_TYPE_WARN_LOUD, "Cant Save New State File '%s': %s",
gdata.statefile, strerror(errno));
/* ignore, continue */
}
if (gdata.debug > 0)
{
#ifdef DEBUG
ms2 = get_time_in_ms();
ioutput(OUT_S|OUT_L|OUT_D, COLOR_NO_COLOR, "Done"
" running: %ld ms", (long)(ms2 - ms1));
#else
ioutput(OUT_S|OUT_L|OUT_D, COLOR_NO_COLOR, "Done");
#endif /* DEBUG */
}
error_out:
mydelete(statefile_tmp);
mydelete(statefile_bkup);
return;
}