Sailfish 固件的加速度性能分析
StepperAccel 性能分析
StepperAccel 是 Bresenham 算法的一个实现,总的来说是
判断队列耗尽时的判断
这部分代码每个Block只执行一次，就是初始化代码
if (current_block == NULL) {
// Anything in the buffer?
current_block = plan_get_current_block();
if (current_block != NULL) {
SET_IS_WORKING();
pStatus0 &= ~(1<<0);
setup_next_block();
} else {
pStatus0 |= (1<<0);
CLR_IS_WORKING();
TIM_SetAutoreload(TIM2,2000);
//TIM_SetIC1Prescaler(TIM2,(SystemCoreClock/2000000)-1);
TIM2->PSC =(SystemCoreClock/2000000)-1;
stepperAxisSetHardwareEnabledToMatch(axesEnabled);
}
}
这里的setup_next_block()是设置下一个block的位置，这里每个block的结构如下
typedef struct {
// Fields used by the bresenham algorithm for tracing the line
int32_t steps[STEPPER_COUNT]; // Step count along each axis
uint32_t step_event_count; // The number of step events required to complete this block
int32_t starting_position[STEPPER_COUNT];
int32_t accelerate_until; // The index of the step event on which to stop acceleration
int32_t decelerate_after; // The index of the step event on which to start decelerating
int32_t acceleration_rate; // The acceleration rate used for acceleration calculation
unsigned char direction_bits; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h)
unsigned char active_extruder; // Selects the active extruder
uint8_t active_toolhead; // The toolhead currently active. Note this isn't the same as active extruder
#ifdef JKN_ADVANCE
bool use_advance_lead;
int16_t advance_lead_entry;
int16_t advance_lead_exit;
int32_t advance_pressure_relax; //Decel phase only
int16_t advance_lead_prime;
int16_t advance_lead_deprime;
#endif
// Fields used by the motion planner to manage acceleration
FPTYPE nominal_speed; // The nominal speed for this block in mm/min
FPTYPE entry_speed; // Entry speed at previous-current junction in mm/min
FPTYPE max_entry_speed; // Maximum allowable junction entry speed in mm/min
FPTYPE millimeters; // The total travel of this block in mm
FPTYPE acceleration; // acceleration mm/sec^2
unsigned char recalculate_flag; // Planner flag to recalculate trapezoids on entry junction
unsigned char nominal_length_flag; // Planner flag for nominal speed always reached
// Settings for the trapezoid generator
uint32_t nominal_rate; // The nominal step rate for this block in step_events/sec
int64_t nominal_rate_sq; // nominal_rate * nominal_rate
uint32_t initial_rate; // The jerk-adjusted step rate at start of block
uint32_t final_rate; // The minimal rate at exit
uint32_t acceleration_st; // acceleration steps/sec^2
char use_accel; // Use acceleration when true
char speed_changed; // Entry speed has changed
volatile char busy;
#ifdef SIMULATOR
FPTYPE feed_rate; // Original feed rate before being modified for nomimal_speed
int planned; // Count of the number of times the block was passed to caclulate_trapezoid_for_block()
char message[1024];
#endif
#ifdef DEBUG_BLOCK_BY_MOVE_INDEX
uint32_t move_index;
#endif
uint8_t dda_master_axis_index;
uint8_t axesEnabled;
uint8_t isTwoWheelMotor;
int32_t rpm[STEPPER_COUNT]; // Step count along each axis
}
周期性判断的部分
这部分代码是在定时器时钟中断里面的，周期性执行。
if (current_block != NULL) {
// Take multiple steps per interrupt (For high speed moves)
for(int8_t i=0; i < step_loops; i++) {
#ifdef JKN_ADVANCE
if ( current_block->use_accel ) {
for ( uint8_t e = 0; e < EXTRUDERS; e ++ ) {
if ( advance_state == ADVANCE_STATE_ACCEL ) {
stepperAxis_dda_shift_phase16(A_AXIS + e, current_block->advance_lead_entry);
}
if ( advance_state == ADVANCE_STATE_DECEL ) {
stepperAxis_dda_shift_phase16(A_AXIS + e, - current_block->advance_lead_exit);
stepperAxis_dda_shift_phase32(A_AXIS + e, - advance_pressure_relax_accumulator >> 8);
}
}
}
#endif
//Step the dda for each axis
printf("move! %d/%d\n\r",step_events_completed,current_block->step_event_count);
SetPIN(XST_GPIO_PORT,XST_PIN);
for(int i=0;i<5;i++)
_delay_us(1);
asm("nop");
asm("nop");
//stepperAxis_dda_step_mul(0x7F);
ClrPIN(XST_GPIO_PORT,XST_PIN);
#ifdef OVERSAMPLED_DDA
oversampledCount = 0;
#endif
if(!current_block->isTwoWheelMotor){
step_events_completed += 1;
}
if(current_block->isTwoWheelMotor){
if(current_block->step_event_count == 4)
continue;
for(int i = X_AXIS;i<=Y_AXIS;i++){
if(totalWheelCnt[i] >= current_block->steps[i]){
//STEPPER_IOPORT_WRITE(stepperAxisPorts[i].step,false);
STEPPER_IOPORT_WRITE(stepperAxisPorts[i].enable,true);
wheelFinished |= _BV(i);
step_events_completed = ((wheelFinished&1) == 1)+((wheelFinished&2) == 2);
#ifdef DEBUG_ROBOT
//printf("i=%d steps=%d wheelfinished=%d step_events_completed =%d finished\n\r",i,step_events_completed,wheelFinished,step_events_completed);
#endif
}
}
}
if(step_events_completed >= current_block->step_event_count) {
#ifdef DEBUG_ROBOT
printf("event_done = %u lround = %d rroudn = %d\n\r",step_events_completed,totalWheelCnt[0],totalWheelCnt[1]);
#endif
break;
}
}
// Calculate new timer value
uint16_t timer;
//dummy
if (step_events_completed <= (uint32_t)current_block->accelerate_until) { // ACCELERATION PHASE
// Note that we need to convert acceleration_time from units of
// 2 MHz to seconds. That is done by dividing acceleration_time
// by 2000000. But, that will make it 0 when we use integer
// arithmetic. So, we first multiply block->acceleration_rate by
// acceleration_time and then do the divide. However, it's
// convenient to divide by 2^24 ( >> 24 ). So, block->acceleration_rate
// has been prescaled by a factor of 9.388608.
//MultiU24X24toH16(acc_step_rate, acceleration_time, current_block->acceleration_rate);
//acc_step_rate = (acceleration_time* (current_block->acceleration_rate)) >> 24;
acc_step_rate = (uint16_t)((float)acceleration_time*(float)(current_block->acceleration_rate)/2000000.0f);
acc_step_rate += current_block->initial_rate;
#ifdef DEBUG_ST
//printf("acc: acceleration_time = %d step_rate = %d \n\r",acceleration_time,acc_step_rate);
#endif
// upper limit
if (acc_step_rate > current_block->nominal_rate) acc_step_rate = current_block->nominal_rate;
// step_rate to timer interval
timer = calc_timer(acc_step_rate);
if(current_block->isTwoWheelMotor){
timer = 40*1000;
}
uint16_t temp;
#ifdef OVERSAMPLED_DDA
temp = timer >> OVERSAMPLED_DDA;
TIM_SetAutoreload(TIM2,temp);
#else
temp = timer;
//for move
//TIM_SetAutoreload(TIM2,temp);
#endif
acceleration_time += timer;
}
//dummy
else if (step_events_completed > (uint32_t)current_block->decelerate_after) { // DECELERATION PHASE
//else if (step_events_completed > 3663) { // DECELERATION PHASE
#ifdef JKN_ADVANCE
if ( advance_state == ADVANCE_STATE_ACCEL ) {
advance_state = ADVANCE_STATE_PLATEAU;
}
if ( advance_state == ADVANCE_STATE_PLATEAU ) {
advance_state = ADVANCE_STATE_DECEL;
advance_pressure_relax_accumulator = 0;
}
advance_pressure_relax_accumulator += current_block->advance_pressure_relax;
#endif
// Note that we need to convert deceleration_time from units of
// 2 MHz to seconds. That is done by dividing deceleration_time
// by 2000000. But, that will make it 0 when we use integer
// arithmetic. So, we first multiply block->acceleration_rate by
// deceleration_time and then do the divide. However, it's
// convenient to divide by 2^24 ( >> 24 ). So, block->acceleration_rate
// has been prescaled by a factor of 8.388608.
//MultiU24X24toH16(step_rate, deceleration_time, current_block->acceleration_rate);
//step_rate = (deceleration_time * (current_block->acceleration_rate)) >>24;
step_rate = (uint16_t)((float)deceleration_time * (float)current_block->acceleration_rate/2000000.0f);
if(step_rate > acc_step_rate) { // Check step_rate stays positive
step_rate = current_block->final_rate;
} else {
step_rate = acc_step_rate - step_rate; // Decelerate from aceleration end point.
// lower limit
if(step_rate < current_block->final_rate) step_rate = current_block->final_rate;
}
#ifdef DEBUG_FREQUENCY
// printf("current block nominal speed:%f\n", current_block->nominal_speed);
#endif
// step_rate to timer interval
timer = calc_timer(step_rate);
if(current_block->isTwoWheelMotor){
timer = 40*1000;
}
uint16_t temp;
#ifdef OVERSAMPLED_DDA
temp = timer >> OVERSAMPLED_DDA;
TIM_SetAutoreload(TIM2,temp);
#else
temp = timer;
//for move
//TIM_SetAutoreload(TIM2,temp);
#endif
deceleration_time += timer;
} else { //NOMINAL PHASE
#ifdef JKN_ADVANCE
if ( advance_state == ADVANCE_STATE_ACCEL ) {
advance_state = ADVANCE_STATE_PLATEAU;
}
#endif
uint16_t temp;
#ifdef OVERSAMPLED_DDA
temp = OCRnA_nominal >> OVERSAMPLED_DDA;
TIM_SetAutoreload(TIM2,temp);
#else
temp = OCRnA_nominal;
//for move
//TIM_SetAutoreload(TIM2,temp);
#endif
step_loops = step_loops_nominal;
}
// If current block is finished, reset pointer
if (step_events_completed >= current_block->step_event_count) {
#ifdef JKN_ADVANCE_LEAD_DE_PRIME
for ( uint8_t e = 0; e < EXTRUDERS; e ++ ) {
lastAdvanceDeprime[e] = current_block->advance_lead_deprime;
}
#endif
#ifdef DEBUG_ROBOT
printf("block finished\n\r");
#endif
current_block = NULL;
plan_discard_current_block();
block_deleted = true;
//STEPPER_IOPORT_WRITE();
if(current_block->isTwoWheelMotor){
//STEPPER_IOPORT_WRITE(stepperAxisPorts[X_AXIS].step,false);
//STEPPER_IOPORT_WRITE(stepperAxisPorts[Y_AXIS].step,false);
}
#ifdef DEBUG_ACCEL
printf("one block finished\n\r");
#endif
// Preprocess the setup for the next block if have have one
current_block = plan_get_current_block();
if (current_block != NULL) {
SET_IS_WORKING();
pStatus0 &= ~(1<<0);
#ifdef DEBUG_STT
printf("block poped %d %d %d %d %d rate = %dhz use_accel = %d\n\r",
current_block->steps[0],
current_block->steps[1],
current_block->steps[2],
current_block->steps[3],
current_block->steps[4],
current_block->nominal_rate,
current_block->use_accel
);
printf("block acc_until=%d dec_after=%d acc_rate = %d \n\r",
current_block->accelerate_until,
current_block->decelerate_after,
current_block->acceleration_rate);
#endif
setup_next_block();
}else{
CLR_IS_WORKING();
}
}else{
pStatus0 |= (1<<0);
#ifdef DEBUG_ACCEL
// printf("target %u, finished %u\n\r",step_events_completed,current_block->step_event_count);
#endif
}
}
step_loops是做脉冲合并用的 最开始的一段程序，截止到calculate timer value 是用来进行脉冲反转的，后面的部分是根据速度和加速度来做timer中断时间设置的。
在 if (step_events_completed <= (uint32_t)current_block->accelerate_until) { // ACCELERATION PHASE这段代码之后是计算加速阶段该怎么运动的
这里
acc_step_rate = (uint16_t)((float)acceleration_time*(float)(current_block->acceleration_rate)/2000000.0f);
的意思是
V_t = a\times t+V_0
后面的减速部分也一样的 所以总的来看这个st_interrupt除了改变timer的间隔，和形成脉冲宽度没有别的bug了。 脉冲宽度的形成还是用busy_wait形成的。