-
Notifications
You must be signed in to change notification settings - Fork 0
/
homing.c
102 lines (89 loc) · 3.38 KB
/
homing.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
#include "hardware.h"
#include "control_isr.h"
#include "homing.h"
#include "stepper.h"
#include "parameters.h"
#include "config.h"
#include <util.h>
#define MICROS_PER_MINUTE 60000000L
#define PULSE_LENGTH 2 // General pulse length / propagation delay for all stepper signals
#define MICROS_PER_TICK (1000000L / ACCELERATION_TICKS_PER_SECOND)
static int32_t untrigger_limit(uint32_t mask, uint32_t invert_mask, uint32_t speed){
int32_t steps = 0;
while((CONTROL_PORT(DIR) & mask) ^ invert_mask){
STEPPER_PORT(SOR) = STEP_BIT;
delay_microseconds(PULSE_LENGTH);
STEPPER_PORT(COR) = STEP_BIT;
steps++;
delay_microseconds(speed);
}
return steps;
}
static int32_t trigger_limit(uint32_t mask, uint32_t invert_mask, uint32_t speed){
int32_t steps = 0;
while(!((CONTROL_PORT(DIR) & mask) ^ invert_mask)){
STEPPER_PORT(SOR) = STEP_BIT;
delay_microseconds(PULSE_LENGTH);
STEPPER_PORT(COR) = STEP_BIT;
steps++;
delay_microseconds(speed);
}
return steps;
}
static void take_steps(uint32_t steps, uint32_t speed){
while(steps-- > 0){
STEPPER_PORT(SOR) = STEP_BIT;
delay_microseconds(PULSE_LENGTH);
STEPPER_PORT(COR) = STEP_BIT;
delay_microseconds(speed);
}
}
static void set_direction(uint32_t dir){
STEPPER_PORT(DOR) = (STEPPER_PORT(DOR) & ~DIR_BIT) | (dir ? DIR_BIT : 0);
delay_microseconds(PULSE_LENGTH);
}
void enter_homing_routine(void){
uint32_t homing, direction_bit;
uint32_t inv_homing_feed, backoff;
uint32_t mask, invert_mask;
int32_t steps;
// If we're not in idle mode, hard error
if(st.state != STATE_IDLE){
st.state = STATE_ERROR; // Probably should set a real error code
return;
}
CONTROL_PORT(DDR) = SYNC_BIT;
homing = parameters.homing;
// Disable the appropriate hard limit function - choose the side we're homing to, don't change the pull-up
// state, and pass a homing bit mask that results in either direction disabled.
configure_limit_gpio(homing & HOME_DIR, PRESERVE_PULLUP, ~ (ENABLE_MIN | ENABLE_MAX));
// Make sure our motor is enabled
enable_stepper();
// If the flip bit is asserted, we should home to the opposite of the home direction bit
direction_bit = !((homing & FLIP_AXIS) ^ ((homing & HOME_DIR) >> 1));
set_direction(direction_bit);
inv_homing_feed = MICROS_PER_MINUTE / parameters.homing_feedrate;
backoff = HOMING_BACKOFF * 1000 / inv_homing_feed;
// Compute the masks to use in determining our end condition
if(homing & HOME_DIR){
mask = MAX_LIMIT_BIT;
invert_mask = (homing & INVERT_MAX) ? mask : 0;
}else{
mask = MIN_LIMIT_BIT;
invert_mask = (homing & INVERT_MIN) ? mask : 0;
}
// Execute the first homing pass at the nominal homing speed
steps = trigger_limit(mask, invert_mask, inv_homing_feed);
// Back off at minimum velocity until the switch is deasserted, taking into account the negative distance
set_direction(!direction_bit);
steps -= untrigger_limit(mask, invert_mask, MICROS_PER_MINUTE / MINIMUM_STEPS_PER_MINUTE);
// This is the most accurate guess of the pre-homing position we get
parameters.last_home = homing & HOME_DIR ? parameters.home_pos - steps : parameters.home_pos + steps;
take_steps(backoff, inv_homing_feed);
// We're now at the home position!
st.position = parameters.home_pos + backoff;
// restore hard limits
delay(10);
configure_limit_gpio(homing & HOME_DIR, PRESERVE_PULLUP, homing);
float_sync_line();
}