mirror of
https://github.com/odriverobotics/ODrive.git
synced 2026-09-21 15:34:33 +08:00
Merge branch 'RazorsFrozenTesting' into RazorsEdge
This commit is contained in:
Unknown
@@ -103,7 +103,7 @@ void HAL_SPI_MspInit(SPI_HandleTypeDef* spiHandle)
|
||||
*/
|
||||
GPIO_InitStruct.Pin = GPIO_PIN_10|GPIO_PIN_11|GPIO_PIN_12;
|
||||
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
|
||||
GPIO_InitStruct.Pull = GPIO_NOPULL;
|
||||
GPIO_InitStruct.Pull = GPIO_PULLUP; // required for disconnect detection on SPI encoders
|
||||
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
|
||||
GPIO_InitStruct.Alternate = GPIO_AF6_SPI3;
|
||||
HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
|
||||
|
||||
@@ -310,6 +310,7 @@ void Encoder::sample_now() {
|
||||
|
||||
case MODE_SPI_ABS_AMS:
|
||||
case MODE_SPI_ABS_CUI:
|
||||
case MODE_SPI_ABS_AEAT:
|
||||
{
|
||||
// Do nothing
|
||||
} break;
|
||||
@@ -351,12 +352,12 @@ bool Encoder::abs_spi_start_transaction(){
|
||||
return false;
|
||||
}
|
||||
HAL_GPIO_WritePin(abs_spi_cs_port_, abs_spi_cs_pin_, GPIO_PIN_RESET);
|
||||
HAL_SPI_TransmitReceive_DMA(hw_config_.spi,(uint8_t*)abs_spi_dma_tx_,(uint8_t*)abs_spi_dma_rx_,1);
|
||||
HAL_SPI_TransmitReceive_DMA(hw_config_.spi, (uint8_t*)abs_spi_dma_tx_, (uint8_t*)abs_spi_dma_rx_, 1);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
uint8_t parity(uint16_t v){
|
||||
uint8_t ams_parity(uint16_t v) {
|
||||
v ^= v >> 8;
|
||||
v ^= v >> 4;
|
||||
v ^= v >> 2;
|
||||
@@ -364,29 +365,48 @@ uint8_t parity(uint16_t v){
|
||||
return v & 1;
|
||||
}
|
||||
|
||||
uint8_t cui_parity(uint16_t v) {
|
||||
v ^= v >> 8;
|
||||
v ^= v >> 4;
|
||||
v ^= v >> 2;
|
||||
return ~v & 3;
|
||||
}
|
||||
|
||||
void Encoder::abs_spi_cb(){
|
||||
HAL_GPIO_WritePin(abs_spi_cs_port_, abs_spi_cs_pin_, GPIO_PIN_SET);
|
||||
|
||||
uint16_t pos;
|
||||
|
||||
switch (mode_) {
|
||||
case MODE_SPI_ABS_AMS: {
|
||||
uint8_t parity_calc, parity_bit;
|
||||
auto rawVal = abs_spi_dma_rx_[0];
|
||||
parity_calc = parity(rawVal & 0x7FFF);
|
||||
parity_bit = rawVal >> 15;
|
||||
|
||||
if (parity_calc == parity_bit) {
|
||||
pos_abs_ = rawVal & 0x3FFF;
|
||||
abs_spi_pos_updated_ = true;
|
||||
uint16_t rawVal = abs_spi_dma_rx_[0];
|
||||
// check if parity is correct (even) and error flag clear
|
||||
if (ams_parity(rawVal) || ((rawVal >> 14) & 1)) {
|
||||
return;
|
||||
}
|
||||
pos = rawVal & 0x3fff;
|
||||
} break;
|
||||
case MODE_SPI_ABS_AEAT: {
|
||||
pos_abs_ = abs_spi_dma_rx_[0];
|
||||
abs_spi_pos_updated_ = true;
|
||||
|
||||
case MODE_SPI_ABS_CUI: {
|
||||
uint16_t rawVal = abs_spi_dma_rx_[0];
|
||||
// check if parity is correct
|
||||
if (cui_parity(rawVal)) {
|
||||
return;
|
||||
}
|
||||
pos = rawVal & 0x3fff;
|
||||
} break;
|
||||
|
||||
default: {
|
||||
set_error(ERROR_UNSUPPORTED_ENCODER_MODE);
|
||||
return;
|
||||
} break;
|
||||
}
|
||||
is_ready_ = true;
|
||||
|
||||
pos_abs_ = pos;
|
||||
abs_spi_pos_updated_ = true;
|
||||
if (config_.pre_calibrated) {
|
||||
is_ready_ = true;
|
||||
}
|
||||
}
|
||||
|
||||
void Encoder::abs_spi_cs_pin_init(){
|
||||
@@ -448,7 +468,7 @@ bool Encoder::update() {
|
||||
case MODE_SPI_ABS_AMS:
|
||||
case MODE_SPI_ABS_CUI:
|
||||
case MODE_SPI_ABS_AEAT: {
|
||||
if (abs_spi_pos_updated_ == false && abs_spi_pos_init_once_) {
|
||||
if (abs_spi_pos_updated_ == false) {
|
||||
// Low pass filter the error
|
||||
spi_error_rate_ += current_meas_period * (1.0f - spi_error_rate_);
|
||||
if (spi_error_rate_ > 0.005f)
|
||||
@@ -464,9 +484,6 @@ bool Encoder::update() {
|
||||
if (delta_enc > config_.cpr/2) {
|
||||
delta_enc -= config_.cpr;
|
||||
}
|
||||
if (!abs_spi_pos_init_once_ && delta_enc != 0) {
|
||||
abs_spi_pos_init_once_ = true;
|
||||
}
|
||||
|
||||
}break;
|
||||
default: {
|
||||
|
||||
@@ -24,9 +24,9 @@ public:
|
||||
MODE_INCREMENTAL,
|
||||
MODE_HALL,
|
||||
MODE_SINCOS,
|
||||
MODE_SPI_ABS_CUI = 0x100,
|
||||
MODE_SPI_ABS_AMS = 0x101,
|
||||
MODE_SPI_ABS_AEAT = 0x102,
|
||||
MODE_SPI_ABS_CUI = 0x100, //!< compatible with CUI AMT23xx
|
||||
MODE_SPI_ABS_AMS = 0x101, //!< compatible with AMS AS5047P, AS5048A/AS5048B (no daisy chain support)
|
||||
MODE_SPI_ABS_AEAT = 0x102, //!< not yet implemented
|
||||
};
|
||||
const uint32_t MODE_FLAG_ABS = 0x100;
|
||||
|
||||
@@ -110,10 +110,9 @@ public:
|
||||
bool abs_spi_start_transaction();
|
||||
void abs_spi_cb();
|
||||
void abs_spi_cs_pin_init();
|
||||
uint16_t abs_spi_dma_tx_[2] = {0xFFFF, 0x0000};
|
||||
uint16_t abs_spi_dma_rx_[2];
|
||||
uint16_t abs_spi_dma_tx_[1] = {0xFFFF};
|
||||
uint16_t abs_spi_dma_rx_[1];
|
||||
bool abs_spi_pos_updated_ = false;
|
||||
bool abs_spi_pos_init_once_ = false;
|
||||
Mode_t mode_ = MODE_INCREMENTAL;
|
||||
GPIO_TypeDef* abs_spi_cs_port_;
|
||||
uint16_t abs_spi_cs_pin_;
|
||||
|
||||
+24
-17
@@ -121,7 +121,7 @@ Connect to the I pin, see if you get a pulse on a complete rotation. Sometimes t
|
||||
If you are using SPI, have a lot at the signal on the CLK, and CS pins. There are many examples on the net for how these should behave.
|
||||
|
||||
## Encoder Noise
|
||||
Noise is found in all circuits, life is just about figuring out if it is preventing your system from working. Lots of users have no problems with noise interfering with their odrive operation, others will tell you "_I've been using the same encoder as you with no problems_". Power to 'em, that may be true, but it doesn't mean it will work for you. If you are concerned about noise, there are several possible sources:
|
||||
Noise is found in all circuits, life is just about figuring out if it is preventing your system from working. Lots of users have no problems with noise interfering with their ODrive operation, others will tell you "_I've been using the same encoder as you with no problems_". Power to 'em, that may be true, but it doesn't mean it will work for you. If you are concerned about noise, there are several possible sources:
|
||||
|
||||
* Importantly, encoder wires may be too close to motor wires, avoid overlap as much as possible
|
||||
* Long wires between encoder and ODrive
|
||||
@@ -135,26 +135,33 @@ If you are using an encoder with an index signal, another problem that has been
|
||||
* when performing an index_search, the motor does not return to the same position each time.
|
||||
One easy step that _might_ fix the noise on the Z input has been to solder a 22nF-47nF capacitor to the Z pin and the GND pin on the underside of the ODrive board.
|
||||
|
||||
## AS5047/AS5048 Encoders
|
||||
The AS5047/AS5048 encoders are Hall Effect/Magnetic sensors that can serve as rotary encoders for the ODrive.
|
||||
|
||||
The AS5047 has 3 independent output interfaces: SPI, ABI, and PWM.
|
||||
The AS5048 has 4 independent output interfaces: SPI, ABI, I2C, and PWM.
|
||||
## SPI Encoders
|
||||
|
||||
Both chips come with evaluation boards that can simplify mounted the chips to your motor. For our purposes if you are using an evaluation board you should select the settings for 3.3v, and tie MOSI high to 3.3v.
|
||||
Apart from (incremental) quadrature encoders, ODrive also supports absolute SPI encoders (since firmware v0.5). These are usually based on are Hall Effect/Magnetic sensors and measure an absolute angle. This means you don't need to repeat the encoder calibration after every ODrive reboot. Currently, the following modes are supported:
|
||||
|
||||
If you are having calibration problems - make sure your magnet is centered on the axis of rotation on the motor, some users report this has a significant impact on calibration. Also make sure your magnet height is within range of the spec sheet.
|
||||
* **CUI protocol**: Compatible with the AMT23xx family (AMT232A, AMT232B, AMT233A, AMT233B).
|
||||
* **AMS protocol**: Compatible with AS5047P and AS5048A/AS5048B.
|
||||
|
||||
#### Using ABI.
|
||||
You can use ABI with the AS5047/AS5048 with the default ODrive firmware. For your wiring, connect A, B, 3.3v, GND to the labeled pins on the odrive
|
||||
The acronym I and Z mean the same thing, connect those as well if you are using an index signal.
|
||||
Some of these chips come with evaluation boards that can simplify mounting the chips to your motor. For our purposes if you are using an evaluation board you should select the settings for 3.3v.
|
||||
|
||||
#### Using SPI.
|
||||
TobinHall has written a [branch](https://github.com/TobinHall/ODrive/tree/Non-Blocking_Absolute_SPI) that supports the SPI option on the AS5047/AS5048. Use his build to flash firmware on your ODrive and connect MISO, SCK, and CS to the labeled pins on the odrive
|
||||
1. Connect the encoder to the ODrive's SPI interface:
|
||||
|
||||
- The encoder's SCK, MISO (aka "DATA" on CUI encoders), GND and 3.3V should connect to the ODrive pins with the same label.
|
||||
- The encoder's MOSI should be tied to 3.3V (AMS encoders only. CUI encoders don't have this pin.)
|
||||
- The encoder's Chip Select (aka nCS/CSn) can be connected to any of the ODrive's GPIOs (caution: GPIOs 1 and 2 are usually used by UART).
|
||||
|
||||
Tie MOSI to 3.3v, connect to the SCK, CLK, MISO, GND and 3.2v pins on the ODrive. (note for SPI users, the acronym SCK and CLK mean the same thing, the acronym CSn and CS mean the same thing.)
|
||||
2. In `odrivetool`, run:
|
||||
|
||||
Add these commands to your calibration / startup script:
|
||||
* `<axis>.encoder.config.abs_spi_cs_gpio_pin = 4` or which ever GPIO pin you choose
|
||||
* `<axis>.encoder.config.mode = 257`
|
||||
* `<odrv>.axis0.encoder.config.cpr = 2**14`
|
||||
<axis>.encoder.config.abs_spi_cs_gpio_pin = 4 # or which ever GPIO pin you choose
|
||||
<axis>.encoder.config.mode = ENCODER_MODE_SPI_ABS_CUI # or ENCODER_MODE_SPI_ABS_AMS
|
||||
<axis>.encoder.config.cpr = 2**14 # or 2**12 for AMT232A and AMT233A
|
||||
<odrv>.save_configuration()
|
||||
<odrv>.reboot()
|
||||
|
||||
3. Run the [offset calibration](#encoder-without-index-signal) and then save the calibration with `<odrv>.save_configuration()`.
|
||||
The next time you reboot, the encoder should be immediately ready.
|
||||
|
||||
Sometimes the encoder takes longer than the ODrive to start, in which case you need to clear the errors after every restart.
|
||||
|
||||
If you are having calibration problems - make sure your magnet is centered on the axis of rotation on the motor, some users report this has a significant impact on calibration. Also make sure your magnet height is within range of the spec sheet.
|
||||
|
||||
@@ -91,6 +91,6 @@ INPUT_MODE_MIRROR = 7
|
||||
ENCODER_MODE_INCREMENTAL = 0x00
|
||||
ENCODER_MODE_HALL = 0x01
|
||||
ENCODER_MODE_SINCOS = 0x02
|
||||
#ENCODER_MODE_SPI_ABS_CUI = 0x100 # currently not functional
|
||||
ENCODER_MODE_SPI_ABS_CUI = 0x100
|
||||
ENCODER_MODE_SPI_ABS_AMS = 0x101
|
||||
ENCODER_MODE_SPI_ABS_AEAT = 0x102
|
||||
|
||||
@@ -10,82 +10,6 @@ from odrive.enums import *
|
||||
from test_runner import *
|
||||
|
||||
|
||||
teensy_code_template = """
|
||||
void setup() {
|
||||
pinMode({enc_a}, OUTPUT);
|
||||
pinMode({enc_b}, OUTPUT);
|
||||
}
|
||||
|
||||
int cpr = 8192;
|
||||
int rpm = 30;
|
||||
|
||||
// the loop routine runs over and over again forever:
|
||||
void loop() {
|
||||
int microseconds_per_count = (1000000 * 60 / cpr / rpm);
|
||||
|
||||
for (;;) {
|
||||
digitalWrite({enc_a}, HIGH);
|
||||
delayMicroseconds(microseconds_per_count);
|
||||
digitalWrite({enc_b}, HIGH);
|
||||
delayMicroseconds(microseconds_per_count);
|
||||
digitalWrite({enc_a}, LOW);
|
||||
delayMicroseconds(microseconds_per_count);
|
||||
digitalWrite({enc_b}, LOW);
|
||||
delayMicroseconds(microseconds_per_count);
|
||||
}
|
||||
}
|
||||
"""
|
||||
|
||||
teensy_code_template2 = """
|
||||
void setup() {
|
||||
analogWriteResolution(10);
|
||||
int freq = 150000000/1024; // ~146.5kHz PWM frequency
|
||||
analogWriteFrequency({enc_sin}, freq);
|
||||
analogWriteFrequency({enc_cos}, freq);
|
||||
}
|
||||
|
||||
int rpm = 60;
|
||||
float pos = 0;
|
||||
|
||||
void loop() {
|
||||
pos += 0.001f * ((float)rpm / 60.0f);
|
||||
if (pos > 1.0f)
|
||||
pos -= 1.0f;
|
||||
analogWrite({enc_sin}, (int)(512.0f + 512.0f * sin(2.0f * M_PI * pos)));
|
||||
analogWrite({enc_cos}, (int)(512.0f + 512.0f * cos(2.0f * M_PI * pos)));
|
||||
delay(1);
|
||||
}
|
||||
"""
|
||||
|
||||
teensy_code_template3 = """
|
||||
void setup() {
|
||||
pinMode({hall_a}, OUTPUT);
|
||||
pinMode({hall_b}, OUTPUT);
|
||||
pinMode({hall_c}, OUTPUT);
|
||||
digitalWrite({hall_a}, HIGH);
|
||||
}
|
||||
|
||||
int cpr = 90; // 15 pole-pairs. Value suggested in hoverboard.md
|
||||
int rpm = 60;
|
||||
int microseconds_per_count = (1000000 * 60 / cpr / rpm);
|
||||
|
||||
void loop() {
|
||||
digitalWrite({hall_b}, HIGH);
|
||||
delayMicroseconds(microseconds_per_count);
|
||||
digitalWrite({hall_a}, LOW);
|
||||
delayMicroseconds(microseconds_per_count);
|
||||
digitalWrite({hall_c}, HIGH);
|
||||
delayMicroseconds(microseconds_per_count);
|
||||
digitalWrite({hall_b}, LOW);
|
||||
delayMicroseconds(microseconds_per_count);
|
||||
digitalWrite({hall_a}, HIGH);
|
||||
delayMicroseconds(microseconds_per_count);
|
||||
digitalWrite({hall_c}, LOW);
|
||||
delayMicroseconds(microseconds_per_count);
|
||||
}
|
||||
"""
|
||||
|
||||
|
||||
class TestEncoderBase():
|
||||
"""
|
||||
Base class for encoder tests.
|
||||
@@ -112,8 +36,6 @@ class TestEncoderBase():
|
||||
encoder.pos_cpr,
|
||||
encoder.vel_estimate,
|
||||
], duration=5.0)
|
||||
|
||||
data = np.array(data)
|
||||
|
||||
short_period = (abs(1 / true_rps) < 5.0)
|
||||
reverse = (true_rps < 0)
|
||||
@@ -130,7 +52,7 @@ class TestEncoderBase():
|
||||
|
||||
# encoder.phase
|
||||
slope, offset, fitted_curve = fit_sawtooth(data[:,(0,3)], pi if reverse else -pi, -pi if reverse else pi, sigma=5)
|
||||
test_assert_eq(slope / 7, 2*pi*true_rps, accuracy=0.01)
|
||||
test_assert_eq(slope / 7, 2*pi*true_rps, accuracy=0.05)
|
||||
test_curve_fit(data[:,(0,3)], fitted_curve, max_mean_err = true_cpr * 0.01, inlier_range = true_cpr * 0.02, max_outliers = len(data[:,0]) * 0.02)
|
||||
|
||||
# encoder.pos_estimate
|
||||
@@ -151,6 +73,31 @@ class TestEncoderBase():
|
||||
|
||||
|
||||
|
||||
teensy_incremental_encoder_emulation_code = """
|
||||
void setup() {
|
||||
pinMode({enc_a}, OUTPUT);
|
||||
pinMode({enc_b}, OUTPUT);
|
||||
}
|
||||
|
||||
int cpr = 8192;
|
||||
int rpm = 30;
|
||||
|
||||
// the loop routine runs over and over again forever:
|
||||
void loop() {
|
||||
int microseconds_per_count = (1000000 * 60 / cpr / rpm);
|
||||
|
||||
for (;;) {
|
||||
digitalWrite({enc_a}, HIGH);
|
||||
delayMicroseconds(microseconds_per_count);
|
||||
digitalWrite({enc_b}, HIGH);
|
||||
delayMicroseconds(microseconds_per_count);
|
||||
digitalWrite({enc_a}, LOW);
|
||||
delayMicroseconds(microseconds_per_count);
|
||||
digitalWrite({enc_b}, LOW);
|
||||
delayMicroseconds(microseconds_per_count);
|
||||
}
|
||||
}
|
||||
"""
|
||||
|
||||
class TestIncrementalEncoder(TestEncoderBase):
|
||||
|
||||
@@ -173,10 +120,10 @@ class TestIncrementalEncoder(TestEncoderBase):
|
||||
yield (encoder, valid_combinations)
|
||||
|
||||
|
||||
def run_test(self, enc: EncoderComponent, teensy: TeensyComponent, teensy_gpio_a: int, teensy_gpio_b: int, logger: Logger):
|
||||
def run_test(self, enc: EncoderComponent, teensy: TeensyComponent, teensy_gpio_a: TeensyGpio, teensy_gpio_b: TeensyGpio, logger: Logger):
|
||||
true_cps = 8192*-0.5 # counts per second generated by the virtual encoder
|
||||
|
||||
code = teensy_code_template.replace("{enc_a}", str(teensy_gpio_a.num)).replace("{enc_b}", str(teensy_gpio_b.num))
|
||||
code = teensy_incremental_encoder_emulation_code.replace("{enc_a}", str(teensy_gpio_a.num)).replace("{enc_b}", str(teensy_gpio_b.num))
|
||||
teensy.compile_and_program(code)
|
||||
|
||||
if enc.handle.config.mode != ENCODER_MODE_INCREMENTAL:
|
||||
@@ -195,6 +142,27 @@ class TestIncrementalEncoder(TestEncoderBase):
|
||||
|
||||
|
||||
|
||||
teensy_sin_cos_encoder_emulation_code = """
|
||||
void setup() {
|
||||
analogWriteResolution(10);
|
||||
int freq = 150000000/1024; // ~146.5kHz PWM frequency
|
||||
analogWriteFrequency({enc_sin}, freq);
|
||||
analogWriteFrequency({enc_cos}, freq);
|
||||
}
|
||||
|
||||
float rps = 1.0f;
|
||||
float pos = 0;
|
||||
|
||||
void loop() {
|
||||
pos += 0.001f * rps;
|
||||
if (pos > 1.0f)
|
||||
pos -= 1.0f;
|
||||
analogWrite({enc_sin}, (int)(512.0f + 512.0f * sin(2.0f * M_PI * pos)));
|
||||
analogWrite({enc_cos}, (int)(512.0f + 512.0f * cos(2.0f * M_PI * pos)));
|
||||
delay(1);
|
||||
}
|
||||
"""
|
||||
|
||||
class TestSinCosEncoder(TestEncoderBase):
|
||||
def get_test_cases(self, testrig: TestRig):
|
||||
for odrive in testrig.get_components(ODriveComponent):
|
||||
@@ -213,7 +181,7 @@ class TestSinCosEncoder(TestEncoderBase):
|
||||
|
||||
|
||||
def run_test(self, enc: EncoderComponent, teensy: TeensyComponent, teensy_gpio_sin: TeensyGpio, teensy_gpio_cos: TeensyGpio, logger: Logger):
|
||||
code = teensy_code_template2.replace("{enc_sin}", str(teensy_gpio_sin.num)).replace("{enc_cos}", str(teensy_gpio_cos.num))
|
||||
code = teensy_sin_cos_encoder_emulation_code.replace("{enc_sin}", str(teensy_gpio_sin.num)).replace("{enc_cos}", str(teensy_gpio_cos.num))
|
||||
teensy.compile_and_program(code)
|
||||
|
||||
if enc.handle.config.mode != ENCODER_MODE_SINCOS:
|
||||
@@ -229,6 +197,34 @@ class TestSinCosEncoder(TestEncoderBase):
|
||||
|
||||
|
||||
|
||||
teensy_hall_effect_encoder_emulation_code = """
|
||||
void setup() {
|
||||
pinMode({hall_a}, OUTPUT);
|
||||
pinMode({hall_b}, OUTPUT);
|
||||
pinMode({hall_c}, OUTPUT);
|
||||
digitalWrite({hall_a}, HIGH);
|
||||
}
|
||||
|
||||
int cpr = 90; // 15 pole-pairs. Value suggested in hoverboard.md
|
||||
float rps = 1.0f;
|
||||
int us_per_count = (1000000.0f / cpr / rps);
|
||||
|
||||
void loop() {
|
||||
digitalWrite({hall_b}, HIGH);
|
||||
delayMicroseconds(us_per_count);
|
||||
digitalWrite({hall_a}, LOW);
|
||||
delayMicroseconds(us_per_count);
|
||||
digitalWrite({hall_c}, HIGH);
|
||||
delayMicroseconds(us_per_count);
|
||||
digitalWrite({hall_b}, LOW);
|
||||
delayMicroseconds(us_per_count);
|
||||
digitalWrite({hall_a}, HIGH);
|
||||
delayMicroseconds(us_per_count);
|
||||
digitalWrite({hall_c}, LOW);
|
||||
delayMicroseconds(us_per_count);
|
||||
}
|
||||
"""
|
||||
|
||||
class TestHallEffectEncoder(TestEncoderBase):
|
||||
|
||||
def get_test_cases(self, testrig: TestRig):
|
||||
@@ -251,11 +247,11 @@ class TestHallEffectEncoder(TestEncoderBase):
|
||||
yield (encoder, valid_combinations)
|
||||
|
||||
|
||||
def run_test(self, enc: EncoderComponent, teensy: TeensyComponent, teensy_gpio_a: int, teensy_gpio_b: int, teensy_gpio_c: int, logger: Logger):
|
||||
def run_test(self, enc: EncoderComponent, teensy: TeensyComponent, teensy_gpio_a: TeensyGpio, teensy_gpio_b: TeensyGpio, teensy_gpio_c: TeensyGpio, logger: Logger):
|
||||
true_cpr = 90
|
||||
true_rps = -1.0
|
||||
|
||||
code = teensy_code_template3.replace("{hall_a}", str(teensy_gpio_a.num)).replace("{hall_b}", str(teensy_gpio_b.num)).replace("{hall_c}", str(teensy_gpio_c.num))
|
||||
code = teensy_hall_effect_encoder_emulation_code.replace("{hall_a}", str(teensy_gpio_a.num)).replace("{hall_b}", str(teensy_gpio_b.num)).replace("{hall_c}", str(teensy_gpio_c.num))
|
||||
teensy.compile_and_program(code)
|
||||
|
||||
if enc.handle.config.mode != ENCODER_MODE_HALL:
|
||||
@@ -270,9 +266,242 @@ class TestHallEffectEncoder(TestEncoderBase):
|
||||
enc.handle.config.cpr = 8192
|
||||
|
||||
|
||||
|
||||
# This encoder emulation mimics the specification given in the following datasheets:
|
||||
#
|
||||
# With {mode} == ENCODER_MODE_SPI_ABS_CUI:
|
||||
# AMT23xx: https://www.cuidevices.com/product/resource/amt23.pdf
|
||||
#
|
||||
# With {mode} == ENCODER_MODE_SPI_ABS_AMS:
|
||||
# AS5047P: https://ams.com/documents/20143/36005/AS5047P_DS000324_2-00.pdf/a7d44138-51f1-2f6e-c8b6-2577b369ace8
|
||||
# AS5048A/AS5048B: https://ams.com/documents/20143/36005/AS5048_DS000298_4-00.pdf/910aef1f-6cd3-cbda-9d09-41f152104832
|
||||
# => Only the read command on address 0x3fff is currently implemented.
|
||||
|
||||
teensy_spi_encoder_emulation_code = """
|
||||
#define ENCODER_MODE_SPI_ABS_CUI 0x100
|
||||
#define ENCODER_MODE_SPI_ABS_AMS 0x101
|
||||
#define ENCODER_MODE_SPI_ABS_AEAT 0x102
|
||||
|
||||
static float rps = 1.0f;
|
||||
static uint32_t cpr = 16384;
|
||||
static uint32_t us_per_revolution = (uint32_t)(1000000.0f / rps);
|
||||
static uint16_t spi_txd = 0; // first output word: NOP
|
||||
static uint32_t zerotime = 0;
|
||||
|
||||
void setup() {
|
||||
pinMode({ncs}, INPUT_PULLUP);
|
||||
}
|
||||
|
||||
uint16_t get_pos_now() {
|
||||
uint32_t time = micros();
|
||||
return ((uint64_t)((time - zerotime) % us_per_revolution)) * cpr / us_per_revolution;
|
||||
}
|
||||
|
||||
|
||||
#if {mode} == ENCODER_MODE_SPI_ABS_AMS
|
||||
|
||||
uint8_t ams_parity(uint16_t v) {
|
||||
v ^= v >> 8;
|
||||
v ^= v >> 4;
|
||||
v ^= v >> 2;
|
||||
v ^= v >> 1;
|
||||
return v & 1;
|
||||
}
|
||||
|
||||
uint16_t handle_command(uint16_t cmd) {
|
||||
const uint16_t ERROR_RESPONSE = 0xc000; // error flag and parity bit set
|
||||
|
||||
if (ams_parity(cmd)) {
|
||||
return ERROR_RESPONSE;
|
||||
}
|
||||
|
||||
if (!(cmd & 14)) { // write not supported
|
||||
return ERROR_RESPONSE;
|
||||
}
|
||||
|
||||
uint16_t addr = cmd & 0x3fff;
|
||||
uint16_t data;
|
||||
|
||||
switch (addr) {
|
||||
case 0x3fff: data = get_pos_now(); break;
|
||||
default: return ERROR_RESPONSE;
|
||||
}
|
||||
|
||||
return data | (ams_parity(data) << 15);
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
#if {mode} == ENCODER_MODE_SPI_ABS_CUI
|
||||
|
||||
uint8_t cui_parity(uint16_t v) {
|
||||
v ^= v >> 8;
|
||||
v ^= v >> 4;
|
||||
v ^= v >> 2;
|
||||
return ~v & 3;
|
||||
}
|
||||
|
||||
uint16_t handle_command(uint16_t cmd) {
|
||||
(void) cmd; // input not used on CUI
|
||||
|
||||
// Test the cui_parity function itself with the example given in the datasheet
|
||||
if ((0x21AB | (cui_parity(0x21AB) << 14)) != 0x61AB) {
|
||||
return 0x0000;
|
||||
}
|
||||
|
||||
uint16_t data = get_pos_now();
|
||||
return data | (cui_parity(data) << 14);
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
void loop() {
|
||||
while (digitalReadFast({reset})) {
|
||||
zerotime = micros();
|
||||
}
|
||||
|
||||
if (!digitalReadFast({ncs})) {
|
||||
static uint16_t spi_rxd = 0;
|
||||
|
||||
pinMode({miso}, OUTPUT);
|
||||
|
||||
for (;;) {
|
||||
while (!digitalReadFast({sck}))
|
||||
if (digitalReadFast({ncs}))
|
||||
goto cs_deasserted;
|
||||
|
||||
// Rising edge: Push output bit
|
||||
|
||||
bool output_bit = spi_txd & 0x8000;
|
||||
digitalWriteFast({miso}, output_bit);
|
||||
spi_txd <<= 1;
|
||||
|
||||
while (digitalReadFast({sck}))
|
||||
if (digitalReadFast({ncs}))
|
||||
goto cs_deasserted;
|
||||
|
||||
// Falling edge: Sample input bit (only in AMS mode)
|
||||
|
||||
#if {mode} == ENCODER_MODE_SPI_ABS_AMS
|
||||
bool input_bit = digitalReadFast({mosi});
|
||||
spi_rxd <<= 1;
|
||||
if (input_bit) {
|
||||
spi_rxd |= 1;
|
||||
} else {
|
||||
spi_rxd &= ~1;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
cs_deasserted:
|
||||
// chip deselected: Process command
|
||||
pinMode({miso}, INPUT);
|
||||
|
||||
spi_txd = handle_command(spi_rxd);
|
||||
}
|
||||
}
|
||||
"""
|
||||
|
||||
class TestSpiEncoder(TestEncoderBase):
|
||||
def __init__(self, mode: int):
|
||||
self.mode = mode
|
||||
|
||||
def get_test_cases(self, testrig: TestRig):
|
||||
for odrive in testrig.get_components(ODriveComponent):
|
||||
for encoder in odrive.encoders:
|
||||
odrive_ncs_gpio = odrive.gpio7 # this GPIO choice is completely arbitrary
|
||||
gpio_conns = [
|
||||
testrig.get_connected_components(odrive.sck, TeensyGpio),
|
||||
testrig.get_connected_components(odrive.miso, TeensyGpio),
|
||||
testrig.get_connected_components(odrive.mosi, TeensyGpio),
|
||||
testrig.get_connected_components(odrive_ncs_gpio, TeensyGpio),
|
||||
]
|
||||
|
||||
valid_combinations = []
|
||||
for combination in itertools.product(*gpio_conns):
|
||||
if (len(set(c.parent for c in combination)) != 1):
|
||||
continue
|
||||
teensy = combination[0].parent
|
||||
reset_pin_options = []
|
||||
for gpio in teensy.gpios:
|
||||
for local_gpio in testrig.get_connected_components(gpio, LinuxGpioComponent):
|
||||
reset_pin_options.append((gpio, local_gpio))
|
||||
valid_combinations.append((teensy, *combination, reset_pin_options))
|
||||
|
||||
yield (encoder, 7, valid_combinations)
|
||||
|
||||
|
||||
def run_test(self, enc: EncoderComponent, odrive_ncs_gpio: int, teensy: TeensyComponent, teensy_gpio_sck: TeensyGpio, teensy_gpio_miso: TeensyGpio, teensy_gpio_mosi: TeensyGpio, teensy_gpio_ncs: TeensyGpio, teensy_gpio_reset: TeensyGpio, reset_gpio: LinuxGpioComponent, logger: Logger):
|
||||
true_cpr = 16384
|
||||
true_rps = 1.0
|
||||
|
||||
reset_gpio.config(output=True) # hold encoder and disable its SPI
|
||||
reset_gpio.write(True)
|
||||
|
||||
code = (teensy_spi_encoder_emulation_code
|
||||
.replace("{sck}", str(teensy_gpio_sck.num))
|
||||
.replace("{miso}", str(teensy_gpio_miso.num))
|
||||
.replace("{mosi}", str(teensy_gpio_mosi.num))
|
||||
.replace("{ncs}", str(teensy_gpio_ncs.num))
|
||||
.replace("{reset}", str(teensy_gpio_reset.num))
|
||||
.replace("{mode}", str(self.mode)))
|
||||
teensy.compile_and_program(code)
|
||||
|
||||
logger.debug(f'Configuring absolute encoder in mode 0x{self.mode:x}...')
|
||||
enc.handle.config.mode = self.mode
|
||||
enc.handle.config.abs_spi_cs_gpio_pin = odrive_ncs_gpio
|
||||
enc.handle.config.cpr = true_cpr
|
||||
enc.parent.save_config_and_reboot()
|
||||
|
||||
time.sleep(1.0)
|
||||
|
||||
logger.debug('Testing absolute readings and SPI errors...')
|
||||
|
||||
# Encoder is still disabled - expect recurring error
|
||||
enc.handle.error = 0
|
||||
time.sleep(0.002)
|
||||
# This fails from time to time because the pull-up on the ODrive only manages
|
||||
# to pull MISO to 1.8V, leaving it in the undefined range.
|
||||
test_assert_eq(enc.handle.error, errors.encoder.ERROR_ABS_SPI_COM_FAIL)
|
||||
|
||||
# Enable encoder and expect error to go away
|
||||
reset_gpio.write(False)
|
||||
release_time = time.monotonic()
|
||||
enc.handle.error = 0
|
||||
time.sleep(0.002)
|
||||
test_assert_eq(enc.handle.error, 0)
|
||||
|
||||
# Check absolute position after 1.5s
|
||||
time.sleep(1.5)
|
||||
true_delta_t = time.monotonic() - release_time
|
||||
test_assert_eq(enc.handle.pos_abs, (true_delta_t * true_rps * true_cpr) % true_cpr, range = true_cpr*0.001)
|
||||
|
||||
test_assert_eq(enc.handle.error, 0)
|
||||
reset_gpio.write(True)
|
||||
time.sleep(0.002)
|
||||
test_assert_eq(enc.handle.error, errors.encoder.ERROR_ABS_SPI_COM_FAIL)
|
||||
reset_gpio.write(False)
|
||||
release_time = time.monotonic()
|
||||
enc.handle.error = 0
|
||||
time.sleep(0.002)
|
||||
test_assert_eq(enc.handle.error, 0)
|
||||
|
||||
# Check absolute position after 1.5s
|
||||
time.sleep(1.5)
|
||||
true_delta_t = time.monotonic() - release_time
|
||||
test_assert_eq(enc.handle.pos_abs, (true_delta_t * true_rps * true_cpr) % true_cpr, range = true_cpr*0.001)
|
||||
|
||||
self.run_generic_encoder_test(enc.handle, true_cpr, true_rps)
|
||||
enc.handle.config.cpr = 8192
|
||||
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
test_runner.run([
|
||||
TestIncrementalEncoder(),
|
||||
TestSinCosEncoder(),
|
||||
#TestIncrementalEncoder(),
|
||||
#TestSinCosEncoder(),
|
||||
TestHallEffectEncoder(),
|
||||
TestSpiEncoder(ENCODER_MODE_SPI_ABS_AMS),
|
||||
TestSpiEncoder(ENCODER_MODE_SPI_ABS_CUI),
|
||||
])
|
||||
|
||||
@@ -194,6 +194,9 @@ class ODriveComponent(Component):
|
||||
for i in range(1,9):
|
||||
self.__setattr__('gpio' + str(i), Component(self))
|
||||
self.can = Component(self)
|
||||
self.sck = Component(self)
|
||||
self.miso = Component(self)
|
||||
self.mosi = Component(self)
|
||||
|
||||
def get_subcomponents(self):
|
||||
for enc_ctx in self.encoders:
|
||||
@@ -203,6 +206,9 @@ class ODriveComponent(Component):
|
||||
for i in range(1,9):
|
||||
yield ('gpio' + str(i)), getattr(self, 'gpio' + str(i))
|
||||
yield 'can', self.can
|
||||
yield 'spi.sck', self.sck
|
||||
yield 'spi.miso', self.miso
|
||||
yield 'spi.mosi', self.mosi
|
||||
|
||||
def prepare(self, logger: Logger):
|
||||
"""
|
||||
|
||||
+11
-8
@@ -65,18 +65,18 @@ components:
|
||||
connections:
|
||||
- ['odrive.can', 'rpi.can0']
|
||||
- ['teensy.program', 'rpi.gpio26']
|
||||
- ['teensy.gpio11', 'rpi.uart0.tx']
|
||||
- ['teensy.gpio12', 'rpi.uart0.rx']
|
||||
- ['teensy.gpio10', 'odrive.gpio1']
|
||||
- ['teensy.gpio9', 'odrive.gpio2']
|
||||
- ['teensy.gpio8', 'odrive.gpio3']
|
||||
- ['teensy.gpio7', 'odrive.gpio4']
|
||||
- ['teensy.gpio12', 'rpi.uart0.tx']
|
||||
- ['teensy.gpio13', 'rpi.uart0.rx']
|
||||
- ['teensy.gpio11', 'odrive.gpio1']
|
||||
- ['teensy.gpio10', 'odrive.gpio2']
|
||||
- ['teensy.gpio9', 'odrive.gpio3']
|
||||
- ['teensy.gpio8', 'odrive.gpio4']
|
||||
- ['teensy.gpio14', 'odrive.gpio5']
|
||||
- ['teensy.gpio15', 'odrive.gpio6']
|
||||
- ['teensy.gpio16', 'odrive.gpio7']
|
||||
- ['teensy.gpio17', 'odrive.gpio8']
|
||||
- ['teensy.gpio4', 'rpi.gpio20']
|
||||
- ['teensy.gpio5', 'rpi.gpio19']
|
||||
- ['teensy.gpio6', 'rpi.gpio20']
|
||||
- ['teensy.gpio7', 'rpi.gpio19']
|
||||
- ['teensy.gpio23', 'odrive.encoder0.z']
|
||||
- ['teensy.gpio22', 'odrive.encoder0.a']
|
||||
- ['teensy.gpio21', 'odrive.encoder0.b']
|
||||
@@ -86,6 +86,9 @@ connections:
|
||||
- ['teensy.gpio0', 'real_encoder.z']
|
||||
- ['teensy.gpio1', 'real_encoder.a']
|
||||
- ['teensy.gpio2', 'real_encoder.b']
|
||||
- ['teensy.gpio3', 'odrive.spi.mosi']
|
||||
- ['teensy.gpio4', 'odrive.spi.miso']
|
||||
- ['teensy.gpio5', 'odrive.spi.sck']
|
||||
- ['odrive.axis0', 'D5065-270KV_0']
|
||||
- ['D5065-270KV_0', 'real_encoder']
|
||||
- ['odrive.gpio3', 'lpf0']
|
||||
|
||||
Reference in New Issue
Block a user