bool line_kernel(void) {
register state_t *s asm("r28") = state_ptr;
uint8_t flag;
LOG_STRING("Stepper: LINE\n");
// first part: step the necessary axes
if (s->axis_mask & X_AXIS_MASK) {
if (s->count_direction[X_AXIS] == 1) {
SET_X_INC;
} else {
SET_X_DEC;
}
ADVANCE_STEP_COUNTER(s->step_ctr[X_AXIS], s->steps[X_AXIS], s->steps_total, flag);
if (flag) {
SET_X_STEP;
if (s->count_direction[X_AXIS] == 1) {LOG_STEP("X+");} else LOG_STEP("X-");
ADVANCE_POSITION(s->position[X_AXIS], s->count_direction[X_AXIS]);
CLR_X_STEP;
}
}
if (s->axis_mask & Y_AXIS_MASK) {
if (s->count_direction[Y_AXIS] == 1) {
SET_Y_INC;
} else {
SET_Y_DEC;
}
ADVANCE_STEP_COUNTER(s->step_ctr[Y_AXIS], s->steps[Y_AXIS], s->steps_total, flag);
if (flag) {
SET_Y_STEP;
if (s->count_direction[Y_AXIS] == 1) {LOG_STEP("Y+");} else LOG_STEP("Y-");
ADVANCE_POSITION(s->position[Y_AXIS], s->count_direction[Y_AXIS]);
CLR_Y_STEP;
}
}
if (s->axis_mask & Z_AXIS_MASK) {
if (s->count_direction[Z_AXIS] == 1) {
SET_Z_INC;
} else {
SET_Z_DEC;
}
ADVANCE_STEP_COUNTER(s->step_ctr[Z_AXIS], s->steps[Z_AXIS], s->steps_total, flag);
if (flag) {
SET_Z_STEP;
if (s->count_direction[Z_AXIS] == 1) {LOG_STEP("Z+");} else LOG_STEP("Z-");
ADVANCE_POSITION(s->position[Z_AXIS], s->count_direction[Z_AXIS]);
CLR_Z_STEP;
}
};
LOG_POS;
OCR1A = 65535;
// fail-safe:
if (!(s->axis_mask & 7))
return FALSE; // axis mask empty -> finished
return (--s->steps_to_go != 0);
}
void SysReset( void )
{
LOG_ENTER();
// rearmed hack: EmuReset() runs some code when real BIOS is used,
// but we usually do reset from menu while GPU is not open yet,
// so we need to prevent updateLace() call..
void* real_lace = GPUupdateLace;
GPU_updateLace = dummy_lace;
LOG_STEP();
EmuReset();
LOG_STEP();
// hmh core forgets this
CDR_stop();
GPU_updateLace = real_lace;
LOG_LEAVE();
}
/* Start Sound Core */
void SetupSound(void)
{
LOG_STEP();
#ifndef NOSOUND
if( enable_audio == 0 ) return;
#if 0
last_ticks = 0;
last_render_ticks = 0;
#endif
#if 0
sound_buffer_bytes = 0;
sound_new_buffer = 0;
sound_thread_exit = 0;
pthread_create( &sound_thread, NULL, sound_thread_play, NULL);
#else
memset(sound_buffer_silence, 0, 882 * 2);
#endif
#endif
}
float anneal(Annealing *search) {
unsigned int wmcount = search->pssm_count;
unsigned int i, k, wm;
float temperature, max_score;;
float score = 0.0;
init_temp(search, STARTEMP_ITERATIONS);
temperature = search->start_temp;
acc = 0.5;
PSSM *best = malloc(wmcount * sizeof(PSSM));
i = wmcount;
while (i--) {
best[i] = clone_pssm(WM(i));
search->best_cut[i] = search->cut[i];
score = anneal_search(search, i);
/* struct HitTable *hits = HITS(i); */
}
max_score = score;
search->best_step = 0;
search->best_pos = search->pos_score;
search->best_neg = search->neg_score;
i = wmcount;
while (i--)
save_wm(search, i);
/* Annealing */
if (search->loglevel >= NORMAL) {
printf("\nAnnealing\n=============================================\n");
}
wm = 0;
i = search->maxit;
LOG_STEP("---", NORMAL);
while (i--) {
if (wmcount == 2) wm = !wm;
score = anneal_step_wm(search, temperature, score, wm);
/* New best score => save results */
if (score > max_score) {
max_score = score;
search->best_step = search->maxit - i;
search->best_pos = search->pos_score;
search->best_neg = search->neg_score;
k = wmcount;
while (k--) {
copy_pssm(WM(k), best[k]);
search->best_cut[k] = search->cut[k];
}
LOG_STEP("***", NORMAL);
}
if (!(i % 10000)){
LOG_STEP("---", NORMAL);
} else {
LOG_STEP("---", TRACE);
}
/* Dynamic ratio */
if (search->schedule == EXPONENTIAL) {
temperature *= EXP_FACTOR;
} else {
temperature -= LIN_FACTOR;
}
}
k = wmcount;
while (k--) {
copy_pssm(best[k], WM(k));
search->cut[k] = search->best_cut[k];
}
search->best_score = max_score;
/* Clean up */
i = wmcount;
while (i--) {
anneal_search(search, i);
free(best[i]);
}
free(best);
score = (*search->scorefunc)(search);
return score;
}
// ALWAYS supported. may use accel/deccel for faster moves. do only use with OFF tool (or none)
bool move_kernel(void) {
register state_t *s asm("r28") = state_ptr;
uint8_t flag;
uint16_t tmp;
LOG_STRING("Stepper: MOVE\n");
// first part: step the necessary axes
if (s->axis_mask & X_AXIS_MASK) {
if (s->count_direction[X_AXIS] == 1) {
SET_X_INC;
} else {
SET_X_DEC;
}
ADVANCE_STEP_COUNTER(s->step_ctr[X_AXIS], s->steps[X_AXIS], s->steps_total, flag);
if (flag) {
SET_X_STEP;
if (s->count_direction[X_AXIS] == 1) {LOG_STEP("X+");} else LOG_STEP("X-");
ADVANCE_POSITION(s->position[X_AXIS], s->count_direction[X_AXIS]);
CLR_X_STEP;
}
}
if (s->axis_mask & Y_AXIS_MASK) {
if (s->count_direction[Y_AXIS] == 1) {
SET_Y_INC;
} else {
SET_Y_DEC;
}
ADVANCE_STEP_COUNTER(s->step_ctr[Y_AXIS], s->steps[Y_AXIS], s->steps_total, flag);
if (flag) {
SET_Y_STEP;
if (s->count_direction[Y_AXIS] == 1) {LOG_STEP("Y+");} else LOG_STEP("Y-");
ADVANCE_POSITION(s->position[Y_AXIS], s->count_direction[Y_AXIS]);
CLR_Y_STEP;
}
}
if (s->axis_mask & Z_AXIS_MASK) {
if (s->count_direction[Z_AXIS] == 1) {
SET_Z_INC;
} else {
SET_Z_DEC;
}
ADVANCE_STEP_COUNTER(s->step_ctr[Z_AXIS], s->steps[Z_AXIS], s->steps_total, flag);
if (flag) {
SET_Z_STEP;
if (s->count_direction[Z_AXIS] == 1) {LOG_STEP("Z+");} else LOG_STEP("Z-");
ADVANCE_POSITION(s->position[Z_AXIS], s->count_direction[Z_AXIS]);
CLR_Z_STEP;
}
};
LOG_POS;
// fail-safe:
if (!(s->axis_mask & 7))
return FALSE; // axis mask empty -> finished
// stupid ramp, but faster than staying at 244Hz. clamp to at most 8Khz.
// 'mantissa' bits. the more, the slower is the accel/deccel. 6 is good
#define MAGIC 6
#define MAGIC2 (16-MAGIC+1)*(1<<MAGIC)
tmp = min(s->steps_to_go, 1 + s->steps_total - s->steps_to_go);
if (tmp <= MAGIC2)
tmp = MAGIC2 - tmp;
else
tmp = 0;
LOG_STRING("MOVE_OCR1A_CALC");LOG_X16(tmp);
// extract exponent (as flag)
flag = tmp >> MAGIC;LOG_U8(flag);
if (flag) {
// if no underflow, force highest bit set
tmp |= 1 << MAGIC;
flag--;
}
// zero exponents bits
tmp &= (2 << MAGIC) -1;
// solve
for(;flag;flag--) {
if (tmp < 32768) {
tmp <<= 1;
} else {
// overflow
tmp = 65535;
break;
}
}
LOG_X16(tmp);
OCR1A = max(2000U, tmp);
LOG_X16(OCR1A);LOG_STRING("ST: new OCR1A Value\n");
return (--s->steps_to_go != 0);
}
// ONLY to be called from ISR !!!
bool arc_kernel(void) {
register state_t *s asm("r28") = state_ptr;
LOG_STRING("Stepper: ARC ");LOG_U8(s->job);LOG_NEWLINE;
switch (s->job) {
// CCW octants
case STEPPER_ARC_CCW_OCT0:
// octant 1: x > 0, y >= 0, x > y
// always y+, sometimes x-
if (s->arc_err <= s->arc_dy -s->arc_x)
ARC_KERNEL_STEP_X_NEG;
ARC_KERNEL_STEP_Y_POS;
if (s->arc_y >= s->arc_x)
s->job = STEPPER_ARC_CCW_OCT1;
break;
case STEPPER_ARC_CCW_OCT1:
// octant 2: x > 0, y > 0, x <= y
// always x-, sometimes y+
if (s->arc_err > s->arc_y - s->arc_dx)
ARC_KERNEL_STEP_Y_POS;
ARC_KERNEL_STEP_X_NEG;
if (s->arc_x <= 0)
s->job = STEPPER_ARC_CCW_OCT2;
break;
case STEPPER_ARC_CCW_OCT2:
// octant 3: x <= 0, y > 0, -x <= y
// always x-, sometimes y-
if (s->arc_err <= -(s->arc_dx + s->arc_y))
ARC_KERNEL_STEP_Y_NEG;
ARC_KERNEL_STEP_X_NEG;
if (s->arc_x <= -s->arc_y)
s->job = STEPPER_ARC_CCW_OCT3;
break;
case STEPPER_ARC_CCW_OCT3:
// octant 4: x < 0, y > 0, -x > y
// always y-, sometimes x-
if (s->arc_err > -(s->arc_dy + s->arc_x))
ARC_KERNEL_STEP_X_NEG;
ARC_KERNEL_STEP_Y_NEG;
if (s->arc_y <= 0)
s->job = STEPPER_ARC_CCW_OCT4;
break;
case STEPPER_ARC_CCW_OCT4:
// octant 5: x < 0, y <= 0, -x > -y
// always y-, sometimes x+
if (s->arc_err <= s->arc_x - s->arc_dy)
ARC_KERNEL_STEP_X_POS;
ARC_KERNEL_STEP_Y_NEG;
if (s->arc_x >= s->arc_y)
s->job = STEPPER_ARC_CCW_OCT5;
break;
case STEPPER_ARC_CCW_OCT5:
// octant 6: x < 0, y < 0, -x <= -y
// always x+, sometimes y-
if (s->arc_err > s->arc_dx - s->arc_y)
ARC_KERNEL_STEP_Y_NEG;
ARC_KERNEL_STEP_X_POS;
if (s->arc_x >= 0)
s->job = STEPPER_ARC_CCW_OCT6;
break;
case STEPPER_ARC_CCW_OCT6:
// octant 7: x >= 0, y < 0, x <= -y
// always x+, sometimes y+
if (s->arc_err <= s->arc_dx + s->arc_y)
ARC_KERNEL_STEP_Y_POS;
ARC_KERNEL_STEP_X_POS;
if (s->arc_x >= -s->arc_y)
s->job = STEPPER_ARC_CCW_OCT7;
break;
case STEPPER_ARC_CCW_OCT7:
// octant 8: x > 0, y < 0, x > -y
// always y+, sometimes x+
if (s->arc_err > s->arc_x + s->arc_dy)
ARC_KERNEL_STEP_X_POS;
ARC_KERNEL_STEP_Y_POS;
if (s->arc_y >= 0)
s->job = STEPPER_ARC_CCW_OCT0;
break;
// CW octants
case STEPPER_ARC_CW_OCT0:
// octant 1: x > 0, y >= 0, x > y
// always y-, sometimes x+
if (s->arc_err > -s->arc_dy + s->arc_x)
ARC_KERNEL_STEP_X_POS;
ARC_KERNEL_STEP_Y_NEG;
if (s->arc_y <= 0)
s->job = STEPPER_ARC_CW_OCT7;
break;
case STEPPER_ARC_CW_OCT1:
// octant 2: x > 0, y > 0, x <= y
// always x+, sometimes y-
if (s->arc_err <= -s->arc_y + s->arc_dx)
ARC_KERNEL_STEP_Y_NEG;
ARC_KERNEL_STEP_X_POS;
if (s->arc_y < s->arc_x)
s->job = STEPPER_ARC_CW_OCT0;
break;
case STEPPER_ARC_CW_OCT2:
// octant 3: x <= 0, y > 0, -x <= y
// always x+, sometimes y+
if (s->arc_err > s->arc_dx + s->arc_y)
ARC_KERNEL_STEP_Y_POS;
ARC_KERNEL_STEP_X_POS;
if (s->arc_x >= 0)
s->job = STEPPER_ARC_CW_OCT1;
break;
case STEPPER_ARC_CW_OCT3:
// octant 4: x < 0, y > 0, -x > y
// always y+, sometimes x+
if (s->arc_err <= s->arc_dy + s->arc_x)
ARC_KERNEL_STEP_X_POS;
ARC_KERNEL_STEP_Y_POS;
if (s->arc_x > -s->arc_y)
s->job = STEPPER_ARC_CW_OCT2;
break;
case STEPPER_ARC_CW_OCT4:
// octant 5: x < 0, y <= 0, -x > -y
// always y+, sometimes x-
if (s->arc_err > -s->arc_x + s->arc_dy)
ARC_KERNEL_STEP_X_NEG;
ARC_KERNEL_STEP_Y_POS;
if (s->arc_y >= 0)
s->job = STEPPER_ARC_CW_OCT3;
break;
case STEPPER_ARC_CW_OCT5:
// octant 6: x < 0, y < 0, -x <= -y
// always x-, sometimes y+
if (s->arc_err <= -s->arc_dx + s->arc_y)
ARC_KERNEL_STEP_Y_POS;
ARC_KERNEL_STEP_X_NEG;
if (s->arc_x < s->arc_y)
s->job = STEPPER_ARC_CW_OCT4;
break;
case STEPPER_ARC_CW_OCT6:
// octant 7: x >= 0, y < 0, x <= -y
// always x-, sometimes y-
if (s->arc_err > -(s->arc_dx + s->arc_y))
ARC_KERNEL_STEP_Y_NEG;
ARC_KERNEL_STEP_X_NEG;
if (s->arc_x <= 0)
s->job = STEPPER_ARC_CW_OCT5;
break;
case STEPPER_ARC_CW_OCT7:
// octant 8: x > 0, y < 0, x > -y
// always y-, sometimes x-
if (s->arc_err <= -(s->arc_x + s->arc_dy))
ARC_KERNEL_STEP_X_NEG;
ARC_KERNEL_STEP_Y_NEG;
if (s->arc_x < -s->arc_y)
s->job = STEPPER_ARC_CW_OCT6;
break;
};
// handle Z movement, if any
if (s->axis_mask & Z_AXIS_MASK) {
uint8_t flag;
if (s->count_direction[Z_AXIS] == 1) {
SET_Z_INC;
} else {
SET_Z_DEC;
}
ADVANCE_STEP_COUNTER(s->step_ctr[Z_AXIS], s->steps[Z_AXIS], s->steps_total, flag);
if (flag) {
SET_Z_STEP;
if (s->count_direction[Z_AXIS] == 1) {LOG_STEP("Z+");} else LOG_STEP("Z-");
ADVANCE_POSITION(s->position[Z_AXIS], s->count_direction[Z_AXIS]);
CLR_Z_STEP;
}
}
LOG_POS;
//~ avg_speed in x: |y|/R * mm_per_step * tickrate
//~ total avg_speed is R/max(|x|,|y|) * mm_per_step * steprate
//~ ->
//~ F_CPU/OCR1A = steprate = avg_speed * max(|x|,|y|) / (R*mm_per_step)
//~ ->
//~ OCR1A = F_CPU * R * mm_per_step / (avg_speed * max(|x|,|y|)
//~ at hor/vert tangents:
//~ OCR1A_0 = F_CPU * mm_per_step / avg_speed = s-> base_ticks
//~ ->
//~ OCR1A = s->base_ticks * R / max(|x|,|y|) = s->base_ticks ... sqrt(2)*s->base_ticks
//~ -> s->base_ticks <= 40404 !!!!
//s->speedfactor = round(256.0*sqrt(square(s->arc_x) + square(s->arc_y)) / max(abs(s->arc_x), abs(s->arc_y)));
OCR1A = (((uint32_t) s->base_ticks) * (s->arc_radius)) / max(abs(s->arc_x),abs(s->arc_y));
//~ OCR1A = s->base_ticks; // XXX: speedfactor correction!
LOG_STRING("new OCR1A:");LOG_X16(OCR1A);LOG_X16(s->base_ticks);LOG_NEWLINE;
s->steps_to_go--;
return (s->steps_to_go != 0);
}
