kopia lustrzana https://gitlab.com/markol/Teathimble_Firmware
Fixed speed planner slow down procedure.
rodzic
6149d396f7
commit
055ca528a8
25
config.h
25
config.h
|
@ -88,7 +88,7 @@
|
||||||
Units: mm/s^2
|
Units: mm/s^2
|
||||||
Useful range: 1 to 10'000
|
Useful range: 1 to 10'000
|
||||||
*/
|
*/
|
||||||
#define ACCELERATION 2000
|
#define ACCELERATION 2500
|
||||||
|
|
||||||
/** \def ENDSTOP_CLEARANCE
|
/** \def ENDSTOP_CLEARANCE
|
||||||
|
|
||||||
|
@ -132,6 +132,7 @@
|
||||||
|
|
||||||
/** \def TRIGGERED_MOVEMENT
|
/** \def TRIGGERED_MOVEMENT
|
||||||
Define this to start new G1 movement only by external interrupt from additional sensor switch.
|
Define this to start new G1 movement only by external interrupt from additional sensor switch.
|
||||||
|
Always used in case of embroidery machines.
|
||||||
*/
|
*/
|
||||||
#define TRIGGERED_MOVEMENT
|
#define TRIGGERED_MOVEMENT
|
||||||
|
|
||||||
|
@ -140,6 +141,7 @@
|
||||||
Define this to enable look-ahead during *ramping* acceleration to smoothly
|
Define this to enable look-ahead during *ramping* acceleration to smoothly
|
||||||
transition between moves instead of performing a dead stop every move.
|
transition between moves instead of performing a dead stop every move.
|
||||||
Enabling look-ahead requires about 3600 bytes of flash memory.
|
Enabling look-ahead requires about 3600 bytes of flash memory.
|
||||||
|
Keep it disabled for embroidery.
|
||||||
*/
|
*/
|
||||||
//#define LOOKAHEAD
|
//#define LOOKAHEAD
|
||||||
|
|
||||||
|
@ -248,6 +250,13 @@
|
||||||
#define E_INVERT_ENABLE
|
#define E_INVERT_ENABLE
|
||||||
*/
|
*/
|
||||||
|
|
||||||
|
#define PWR_OUT1_PIN PD4
|
||||||
|
#define PWR_OUT2_PIN PD5
|
||||||
|
#define ANALOG_IN_PIN 33
|
||||||
|
#define SWITCH1_PIN 34
|
||||||
|
#define SWITCH2_PIN 35
|
||||||
|
#define SWITCH3_PIN PD2
|
||||||
|
|
||||||
/** \def ENCODER_PIN
|
/** \def ENCODER_PIN
|
||||||
* ENCODER_PIN_A must be connected to INT1 pin for external interrupt to work!
|
* ENCODER_PIN_A must be connected to INT1 pin for external interrupt to work!
|
||||||
* Motor encoder needs INT0 pin.
|
* Motor encoder needs INT0 pin.
|
||||||
|
@ -256,6 +265,8 @@
|
||||||
#define ENCODER_PIN_B PA4
|
#define ENCODER_PIN_B PA4
|
||||||
#define INVERT_DIRECTION_ENCODER
|
#define INVERT_DIRECTION_ENCODER
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
||||||
/** \def PULSES_PER_TURN
|
/** \def PULSES_PER_TURN
|
||||||
* Motor encoder pulses per one machine shaft revolution.
|
* Motor encoder pulses per one machine shaft revolution.
|
||||||
* If encoder wheel is mounted on motor pulley for better resolution,
|
* If encoder wheel is mounted on motor pulley for better resolution,
|
||||||
|
@ -273,6 +284,7 @@
|
||||||
**/
|
**/
|
||||||
#define MIN_MOTOR_SPEED 80
|
#define MIN_MOTOR_SPEED 80
|
||||||
#define MAX_MOTOR_SPEED 800
|
#define MAX_MOTOR_SPEED 800
|
||||||
|
#define DEFAULT_MOTOR_SPEED 500
|
||||||
|
|
||||||
/** \def KX_FACTOR
|
/** \def KX_FACTOR
|
||||||
* Proportional and integral factor for speed controller.
|
* Proportional and integral factor for speed controller.
|
||||||
|
@ -303,13 +315,6 @@
|
||||||
**/
|
**/
|
||||||
#define POINT_SHIFT 100000
|
#define POINT_SHIFT 100000
|
||||||
|
|
||||||
#define PWR_OUT1_PIN PD4
|
|
||||||
#define PWR_OUT2_PIN PD5
|
|
||||||
#define ANALOG_IN_PIN 33
|
|
||||||
#define SWITCH1_PIN 34
|
|
||||||
#define SWITCH2_PIN 35
|
|
||||||
#define SWITCH3_PIN PD2
|
|
||||||
|
|
||||||
/** \def MAX_JUMP_LENGTH
|
/** \def MAX_JUMP_LENGTH
|
||||||
* This parameter tells motor speed planner to use lowest defined speed
|
* This parameter tells motor speed planner to use lowest defined speed
|
||||||
* for stitches of length close to this value.
|
* for stitches of length close to this value.
|
||||||
|
@ -328,9 +333,9 @@
|
||||||
Sane values: 50 to 400
|
Sane values: 50 to 400
|
||||||
Valid range: 1 to 2000
|
Valid range: 1 to 2000
|
||||||
**/
|
**/
|
||||||
#define JUMP_MOTOR_SPEED_DIFF_MAX 100
|
#define JUMP_MOTOR_SPEED_DIFF_MAX 80
|
||||||
|
#define JUMP_MOTOR_SPEED_DIFF_MAX_SLOWDOWN 20
|
||||||
|
|
||||||
#endif
|
|
||||||
|
|
||||||
/** \def MOVEBUFFER_SIZE
|
/** \def MOVEBUFFER_SIZE
|
||||||
Move buffer size, in number of moves.
|
Move buffer size, in number of moves.
|
||||||
|
|
7
motor.c
7
motor.c
|
@ -416,8 +416,8 @@ void dda_create(DDA *dda, const TARGET *target) {
|
||||||
else if(dc_motor_speed - prev_dda->dc_motor_speed > JUMP_MOTOR_SPEED_DIFF_MAX )
|
else if(dc_motor_speed - prev_dda->dc_motor_speed > JUMP_MOTOR_SPEED_DIFF_MAX )
|
||||||
dc_motor_speed = prev_dda->dc_motor_speed + JUMP_MOTOR_SPEED_DIFF_MAX;
|
dc_motor_speed = prev_dda->dc_motor_speed + JUMP_MOTOR_SPEED_DIFF_MAX;
|
||||||
// slow down, decrease previous dda speed if needed
|
// slow down, decrease previous dda speed if needed
|
||||||
else if(prev_dda->dc_motor_speed - dc_motor_speed > JUMP_MOTOR_SPEED_DIFF_MAX )
|
else if(prev_dda->dc_motor_speed - dc_motor_speed > JUMP_MOTOR_SPEED_DIFF_MAX_SLOWDOWN )
|
||||||
prev_dda->dc_motor_speed = dc_motor_speed + JUMP_MOTOR_SPEED_DIFF_MAX;
|
queue_set_prev_dc_motor(dc_motor_speed);
|
||||||
}
|
}
|
||||||
else // no previous movement, start slowly as well
|
else // no previous movement, start slowly as well
|
||||||
dc_motor_speed = MIN_MOTOR_SPEED;
|
dc_motor_speed = MIN_MOTOR_SPEED;
|
||||||
|
@ -428,6 +428,7 @@ void dda_create(DDA *dda, const TARGET *target) {
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
dda->dc_motor_speed = 0;
|
dda->dc_motor_speed = 0;
|
||||||
|
queue_set_prev_dc_motor(MIN_MOTOR_SPEED);
|
||||||
}
|
}
|
||||||
// end of motor speed planner
|
// end of motor speed planner
|
||||||
|
|
||||||
|
@ -444,7 +445,7 @@ void dda_start(DDA *dda) {
|
||||||
uint8_t queue_elements = queue_current_size();
|
uint8_t queue_elements = queue_current_size();
|
||||||
if(dda->dc_motor_speed == 0 )
|
if(dda->dc_motor_speed == 0 )
|
||||||
{
|
{
|
||||||
// turn off motor for jump move
|
// turn off motor now for jump move
|
||||||
if (dda->waitfor == 0)
|
if (dda->waitfor == 0)
|
||||||
desired_speed = 0;
|
desired_speed = 0;
|
||||||
else
|
else
|
||||||
|
|
16
queue.c
16
queue.c
|
@ -179,5 +179,21 @@ void queue_wait() {
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// sets dc motor speed in all previous blocks if it is too high
|
||||||
|
#define MB_PREV(x) ((x) == 0 ? MOVEBUFFER_SIZE -1 : (x) - 1)
|
||||||
|
void queue_set_prev_dc_motor(int16_t speed)
|
||||||
|
{
|
||||||
|
int8_t i = (mb_head);
|
||||||
|
int8_t end = MB_PREV(mb_tail);
|
||||||
|
while (i != end){
|
||||||
|
speed += JUMP_MOTOR_SPEED_DIFF_MAX_SLOWDOWN;
|
||||||
|
if (movebuffer[i].dc_motor_speed <= speed)
|
||||||
|
break;
|
||||||
|
//sersendf_P(PSTR("Old/new: %d/%d\n"), movebuffer[i].dc_motor_speed, speed);
|
||||||
|
movebuffer[i].dc_motor_speed = speed;
|
||||||
|
i = MB_PREV(i);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
1
queue.h
1
queue.h
|
@ -42,5 +42,6 @@ inline void enqueue(const TARGET *t) {
|
||||||
}
|
}
|
||||||
|
|
||||||
void queue_wait();
|
void queue_wait();
|
||||||
|
void queue_set_prev_dc_motor(int16_t);
|
||||||
|
|
||||||
#endif /* _QUEUE_H */
|
#endif /* _QUEUE_H */
|
||||||
|
|
|
@ -8,9 +8,9 @@ uint16_t motor_pulses = 0;
|
||||||
int32_t ki, kp;
|
int32_t ki, kp;
|
||||||
int16_t error_speed_sum;
|
int16_t error_speed_sum;
|
||||||
int16_t desired_speed, error_speed;
|
int16_t desired_speed, error_speed;
|
||||||
int16_t margin_max_speed = MAX_MOTOR_SPEED;
|
int16_t margin_max_speed = DEFAULT_MOTOR_SPEED;
|
||||||
|
|
||||||
// Init INT0 and INT1 interrupts for optical sensors, init pwm and timers for dc motor speed controller
|
/// Init INT0 and INT1 interrupts for optical sensors, init pwm and timers for dc motor speed controller
|
||||||
void sensing_init()
|
void sensing_init()
|
||||||
{
|
{
|
||||||
PULL_OFF(PD2);
|
PULL_OFF(PD2);
|
||||||
|
@ -86,6 +86,7 @@ void set_dc_motor_speed(int16_t value)
|
||||||
desired_speed = value;
|
desired_speed = value;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// sets machine maximal work speed
|
||||||
void set_dc_motor_speed_margin(int16_t value)
|
void set_dc_motor_speed_margin(int16_t value)
|
||||||
{
|
{
|
||||||
if (value <= MAX_MOTOR_SPEED)
|
if (value <= MAX_MOTOR_SPEED)
|
||||||
|
@ -99,7 +100,7 @@ void set_dc_motor_speed_margin(int16_t value)
|
||||||
margin_max_speed = MAX_MOTOR_SPEED;
|
margin_max_speed = MAX_MOTOR_SPEED;
|
||||||
}
|
}
|
||||||
|
|
||||||
// activates stop of the the motor on needle interrupt
|
/// activates stop of the the motor on needle interrupt
|
||||||
void stop_dc_motor()
|
void stop_dc_motor()
|
||||||
{
|
{
|
||||||
if(desired_speed == 0) return;
|
if(desired_speed == 0) return;
|
||||||
|
@ -107,7 +108,7 @@ void stop_dc_motor()
|
||||||
stop_motor_flag = 1;
|
stop_motor_flag = 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
//PI control update procedure
|
/// PI control update procedure
|
||||||
void update_dc_motor()
|
void update_dc_motor()
|
||||||
{
|
{
|
||||||
int16_t pwm_pulse;
|
int16_t pwm_pulse;
|
||||||
|
@ -153,7 +154,7 @@ void update_dc_motor()
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// needle interrupt
|
/// needle interrupt
|
||||||
ISR(INT1_vect)
|
ISR(INT1_vect)
|
||||||
{
|
{
|
||||||
// check if second sensor is trigged to estimate needle direction
|
// check if second sensor is trigged to estimate needle direction
|
||||||
|
@ -186,13 +187,13 @@ ISR(INT1_vect)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// motor rotary encoder interrupt for speed measurement
|
/// motor rotary encoder interrupt for speed measurement
|
||||||
ISR(INT0_vect)
|
ISR(INT0_vect)
|
||||||
{
|
{
|
||||||
++motor_pulses;
|
++motor_pulses;
|
||||||
}
|
}
|
||||||
|
|
||||||
// timer tick interrupt
|
/// timer tick interrupt
|
||||||
ISR(TIMER2_OVF_vect)
|
ISR(TIMER2_OVF_vect)
|
||||||
{
|
{
|
||||||
++interrupt_ticks;
|
++interrupt_ticks;
|
||||||
|
|
Ładowanie…
Reference in New Issue