ManuelW/grbl-LPC-CoreXY
1
0
Fork 0
Code Issues Pull Requests Releases 1 Wiki Activity

the trapezoid generator seems to be working

Browse Source
This commit is contained in:
Simen Svale Skogsrud 2011-01-23 21:14:38 +01:00
parent 4103e6ca00
commit 8325bfb96e
2 changed files with 24 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
stepper.c
View File

@ -92,11 +92,12 @@ inline void trapezoid_generator_tick() {
trapezoid_rate += current_block->rate_delta; trapezoid_rate += current_block->rate_delta;
set_step_events_per_minute(trapezoid_rate); set_step_events_per_minute(trapezoid_rate);
} else if (step_event_count > current_block->decelerate_after) { } else if (step_event_count > current_block->decelerate_after) {
trapezoid_rate -= current_block->rate_delta; // NOTE: We will only reduce speed if the result will be > 0. This catches small
// rounding errors that might leave steps hanging after the last trapezoid tick.
if(current_block->rate_delta < trapezoid_rate) {
trapezoid_rate -= current_block->rate_delta;
}
set_step_events_per_minute(trapezoid_rate); set_step_events_per_minute(trapezoid_rate);
} else {
printInteger(trapezoid_rate);
while(1){};
} }
} }
PORTD ^= (1<<2); PORTD ^= (1<<2);

39
stepper_plan.c
View File

@ -44,18 +44,17 @@ inline double estimate_acceleration_distance(double initial_rate, double target_
// you started at speed initial_rate and accelerated until this point and want to end at the final_rate after // you started at speed initial_rate and accelerated until this point and want to end at the final_rate after
// a total travel of distance. This can be used to compute the intersection point between acceleration and // a total travel of distance. This can be used to compute the intersection point between acceleration and
// deceleration in the cases where the trapezoid has no plateau (i.e. never reaches maximum speed) // deceleration in the cases where the trapezoid has no plateau (i.e. never reaches maximum speed)
/*
+ <- some rate that must be < maximum allowable rate /* + <- some rate that the client must be certain will not exceed the maximum allowable
/|\ /|\
/ | \ / | \
/ | + <- final_rate / | + <- final_rate
/ | | / | |
initial_rate -> +----+--+ initial_rate -> +----+--+
0 ^ ^ ^ ^
| | | |
result distance intersection_distance distance */
*/
inline double intersection_distance(double initial_rate, double final_rate, double acceleration, double distance) { inline double intersection_distance(double initial_rate, double final_rate, double acceleration, double distance) {
return((2*acceleration*distance-initial_rate*initial_rate+final_rate*final_rate)/(4*acceleration)); return((2*acceleration*distance-initial_rate*initial_rate+final_rate*final_rate)/(4*acceleration));
} }
@ -66,17 +65,17 @@ inline double intersection_distance(double initial_rate, double final_rate, doub
// Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors. // Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors.
// In practice both factors must be in the range 0 ... 1.0 // In practice both factors must be in the range 0 ... 1.0
void calculate_trapezoid_for_block(struct Block *block, double entry_factor, double exit_factor) { void calculate_trapezoid_for_block(struct Block *block, double entry_factor, double exit_factor) {
block->initial_rate = round(block->nominal_rate*entry_factor); block->initial_rate = ceil(block->nominal_rate*entry_factor);
int32_t final_rate = round(block->nominal_rate*entry_factor); int32_t final_rate = ceil(block->nominal_rate*entry_factor);
int32_t acceleration_per_second = block->rate_delta*ACCELERATION_TICKS_PER_SECOND; int32_t acceleration_per_minute = block->rate_delta*ACCELERATION_TICKS_PER_SECOND*60.0;
int32_t accelerate_steps = int32_t accelerate_steps =
round(estimate_acceleration_distance(block->initial_rate, block->nominal_rate, acceleration_per_second)); ceil(estimate_acceleration_distance(block->initial_rate, block->nominal_rate, acceleration_per_minute));
int32_t decelerate_steps = int32_t decelerate_steps =
estimate_acceleration_distance(block->nominal_rate, final_rate, -acceleration_per_second); estimate_acceleration_distance(block->nominal_rate, final_rate, -acceleration_per_minute);
printString("ir="); printInteger(block->initial_rate); printString("\n\r"); printString("ir="); printInteger(block->initial_rate); printString("\n\r");
printString("nr="); printInteger(block->nominal_rate); printString("\n\r"); printString("nr="); printInteger(block->nominal_rate); printString("\n\r");
printString("rd="); printInteger(block->rate_delta); printString("\n\r"); printString("rd="); printInteger(block->rate_delta); printString("\n\r");
printString("aps="); printInteger(acceleration_per_second); printString("\n\r"); printString("aps="); printInteger(acceleration_per_minute); printString("\n\r");
printString("acs="); printInteger(accelerate_steps); printString("\n\r"); printString("acs="); printInteger(accelerate_steps); printString("\n\r");
printString("dcs="); printInteger(decelerate_steps); printString("\n\r"); printString("dcs="); printInteger(decelerate_steps); printString("\n\r");
printString("ts="); printInteger(block->step_event_count); printString("\n\r"); printString("ts="); printInteger(block->step_event_count); printString("\n\r");
@ -85,8 +84,8 @@ void calculate_trapezoid_for_block(struct Block *block, double entry_factor, dou
// fit within the allotted step events. // fit within the allotted step events.
int32_t plateau_steps = block->step_event_count-accelerate_steps-decelerate_steps; int32_t plateau_steps = block->step_event_count-accelerate_steps-decelerate_steps;
if (plateau_steps < 0) { if (plateau_steps < 0) {
accelerate_steps = round( accelerate_steps = ceil(
intersection_distance(block->initial_rate, final_rate, acceleration_per_second, block->step_event_count)); intersection_distance(block->initial_rate, final_rate, acceleration_per_minute, block->step_event_count));
plateau_steps = 0; plateau_steps = 0;
printString("No plateau, so: acs="); printInteger(accelerate_steps); printString("\n\r"); printString("No plateau, so: acs="); printInteger(accelerate_steps); printString("\n\r");
} }
@ -200,7 +199,7 @@ void plan_buffer_line(int32_t steps_x, int32_t steps_y, int32_t steps_z, uint32_
block->speed_y = block->steps_y*multiplier/settings.steps_per_mm[1]; block->speed_y = block->steps_y*multiplier/settings.steps_per_mm[1];
block->speed_z = block->steps_z*multiplier/settings.steps_per_mm[2]; block->speed_z = block->steps_z*multiplier/settings.steps_per_mm[2];
block->nominal_speed = millimeters*multiplier; block->nominal_speed = millimeters*multiplier;
block->nominal_rate = round(block->step_event_count*multiplier); block->nominal_rate = ceil(block->step_event_count*multiplier);
// Compute the acceleration rate for the trapezoid generator. Depending on the slope of the line // Compute the acceleration rate for the trapezoid generator. Depending on the slope of the line
// average travel per step event changes. For a line along one axis the travel per step event // average travel per step event changes. For a line along one axis the travel per step event
@ -211,7 +210,7 @@ void plan_buffer_line(int32_t steps_x, int32_t steps_y, int32_t steps_z, uint32_
double travel_per_step = millimeters/block->step_event_count; double travel_per_step = millimeters/block->step_event_count;
printString("travel_per_step*10000="); printString("travel_per_step*10000=");
printInteger(travel_per_step*10000);printString("\n\r"); printInteger(travel_per_step*10000);printString("\n\r");
block->rate_delta = round( block->rate_delta = ceil(
((settings.acceleration*60.0)/(ACCELERATION_TICKS_PER_SECOND))/ // acceleration mm/sec/sec per acceleration_tick ((settings.acceleration*60.0)/(ACCELERATION_TICKS_PER_SECOND))/ // acceleration mm/sec/sec per acceleration_tick
travel_per_step); // convert to: acceleration steps/min/acceleration_tick travel_per_step); // convert to: acceleration steps/min/acceleration_tick
if (acceleration_management) { if (acceleration_management) {