- Initial commit
This commit is contained in:
abausano
GitHub
768
src/lcec_epocat.c
Normal file
768
src/lcec_epocat.c
Normal file
@@ -0,0 +1,768 @@
|
||||
//
|
||||
// Copyright (C) 2014 Sascha Ittner <sascha.ittner@modusoft.de>
|
||||
//
|
||||
// This program is free software; you can redistribute it and/or modify
|
||||
// it under the terms of the GNU General Public License as published by
|
||||
// the Free Software Foundation; either version 2 of the License, or
|
||||
// (at your option) any later version.
|
||||
//
|
||||
// This program is distributed in the hope that it will be useful,
|
||||
// but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
// GNU General Public License for more details.
|
||||
//
|
||||
// You should have received a copy of the GNU General Public License
|
||||
// along with this program; if not, write to the Free Software
|
||||
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
//
|
||||
|
||||
#include "lcec.h"
|
||||
#include "lcec_epocat.h"
|
||||
|
||||
|
||||
|
||||
typedef struct {
|
||||
hal_bit_t *DROK_0;
|
||||
hal_bit_t *DROK_1;
|
||||
hal_bit_t *DROK_2;
|
||||
hal_bit_t *DROK_3;
|
||||
hal_bit_t *DROK_4;
|
||||
hal_bit_t *DROK_INVERTER;
|
||||
hal_bit_t *ENABLE_24V_0;
|
||||
hal_bit_t *ENABLE_24V_1;
|
||||
hal_bit_t *ENABLE_24V_2;
|
||||
hal_bit_t *ENABLE_24V_3;
|
||||
hal_bit_t *ENABLE_24V_4;
|
||||
hal_bit_t *ENABLE_24V_INVERTER;
|
||||
hal_bit_t *ENABLE_5V_0;
|
||||
hal_bit_t *ENABLE_5V_1;
|
||||
hal_bit_t *ENABLE_5V_2;
|
||||
hal_bit_t *ENABLE_5V_3;
|
||||
hal_bit_t *ENABLE_5V_4;
|
||||
hal_bit_t *HOME_0;
|
||||
hal_bit_t *HOME_1;
|
||||
hal_bit_t *HOME_2;
|
||||
hal_bit_t *HOME_3;
|
||||
hal_bit_t *HOME_4;
|
||||
hal_bit_t *IN_0;
|
||||
hal_bit_t *IN_1;
|
||||
hal_bit_t *IN_2;
|
||||
hal_bit_t *IN_3;
|
||||
hal_bit_t *IN_4;
|
||||
hal_bit_t *OUT_0;
|
||||
hal_bit_t *OUT_1;
|
||||
|
||||
hal_s32_t *HZ_0;
|
||||
hal_s32_t *HZ_1;
|
||||
hal_s32_t *HZ_2;
|
||||
hal_s32_t *HZ_3;
|
||||
hal_s32_t *HZ_4;
|
||||
|
||||
hal_float_t *enc_00_position;
|
||||
hal_float_t *enc_01_position;
|
||||
hal_float_t *enc_02_position;
|
||||
hal_float_t *enc_03_position;
|
||||
hal_float_t *enc_04_position;
|
||||
|
||||
hal_s32_t *enc_00_count;
|
||||
hal_s32_t *enc_01_count;
|
||||
hal_s32_t *enc_02_count;
|
||||
hal_s32_t *enc_03_count;
|
||||
hal_s32_t *enc_04_count;
|
||||
|
||||
hal_float_t enc_00_scale;
|
||||
hal_float_t enc_01_scale;
|
||||
hal_float_t enc_02_scale;
|
||||
hal_float_t enc_03_scale;
|
||||
hal_float_t enc_04_scale;
|
||||
|
||||
hal_bit_t *enc_00_index_enable;
|
||||
hal_bit_t *enc_01_index_enable;
|
||||
hal_bit_t *enc_02_index_enable;
|
||||
hal_bit_t *enc_03_index_enable;
|
||||
hal_bit_t *enc_04_index_enable;
|
||||
|
||||
hal_float_t *dac_00_value;
|
||||
hal_float_t *dac_01_value;
|
||||
hal_float_t *dac_02_value;
|
||||
hal_float_t *dac_03_value;
|
||||
hal_float_t *dac_04_value;
|
||||
|
||||
hal_float_t *dac_inverter_value;
|
||||
hal_bit_t *inverter_forward;
|
||||
hal_bit_t *inverter_reverse;
|
||||
|
||||
hal_float_t dac_00_scale;
|
||||
hal_float_t dac_01_scale;
|
||||
hal_float_t dac_02_scale;
|
||||
hal_float_t dac_03_scale;
|
||||
hal_float_t dac_04_scale;
|
||||
|
||||
hal_float_t *adc1;
|
||||
hal_float_t *adc2;
|
||||
|
||||
unsigned int off_dig_out;
|
||||
unsigned int off_dig_inp;
|
||||
unsigned int off_PWM;
|
||||
unsigned int off_DIR;
|
||||
unsigned int off_ENC;
|
||||
unsigned int off_ENABLE_24V;
|
||||
unsigned int off_ENABLE_5V;
|
||||
unsigned int off_DRVOK;
|
||||
unsigned int off_HOME;
|
||||
unsigned int off_ADC1;
|
||||
unsigned int off_ADC2;
|
||||
unsigned int off_INVERTER;
|
||||
unsigned int off_LATCH;
|
||||
unsigned int off_FLAG;
|
||||
unsigned int off_RESET_FLAG;
|
||||
unsigned int off_POLARITY;
|
||||
} lcec_epocat_data_t;
|
||||
|
||||
static const lcec_pindesc_t slave_pins[] = {
|
||||
{ HAL_BIT, HAL_OUT, offsetof(lcec_epocat_data_t, DROK_0), "%s.%s.%s.axis0-drvok" },
|
||||
{ HAL_BIT, HAL_OUT, offsetof(lcec_epocat_data_t, DROK_1), "%s.%s.%s.axis1-drvok" },
|
||||
{ HAL_BIT, HAL_OUT, offsetof(lcec_epocat_data_t, DROK_2), "%s.%s.%s.axis2-drvok" },
|
||||
{ HAL_BIT, HAL_OUT, offsetof(lcec_epocat_data_t, DROK_3), "%s.%s.%s.axis3-drvok" },
|
||||
{ HAL_BIT, HAL_OUT, offsetof(lcec_epocat_data_t, DROK_4), "%s.%s.%s.axis4-drvok" },
|
||||
{ HAL_BIT, HAL_OUT, offsetof(lcec_epocat_data_t, DROK_INVERTER), "%s.%s.%s.din-inverter-drvok" },
|
||||
{ HAL_BIT, HAL_IN, offsetof(lcec_epocat_data_t, ENABLE_24V_0), "%s.%s.%s.axis0-enable24V" },
|
||||
{ HAL_BIT, HAL_IN, offsetof(lcec_epocat_data_t, ENABLE_24V_1), "%s.%s.%s.axis1-enable24V" },
|
||||
{ HAL_BIT, HAL_IN, offsetof(lcec_epocat_data_t, ENABLE_24V_2), "%s.%s.%s.axis2-enable24V" },
|
||||
{ HAL_BIT, HAL_IN, offsetof(lcec_epocat_data_t, ENABLE_24V_3), "%s.%s.%s.axis3-enable24V" },
|
||||
{ HAL_BIT, HAL_IN, offsetof(lcec_epocat_data_t, ENABLE_24V_4), "%s.%s.%s.axis4-enable24V" },
|
||||
{ HAL_BIT, HAL_IN, offsetof(lcec_epocat_data_t, ENABLE_24V_INVERTER), "%s.%s.%s.dout-inverter-enable" },
|
||||
{ HAL_BIT, HAL_IN, offsetof(lcec_epocat_data_t, ENABLE_5V_0), "%s.%s.%s.axis0-enable5V" },
|
||||
{ HAL_BIT, HAL_IN, offsetof(lcec_epocat_data_t, ENABLE_5V_1), "%s.%s.%s.axis1-enable5V" },
|
||||
{ HAL_BIT, HAL_IN, offsetof(lcec_epocat_data_t, ENABLE_5V_2), "%s.%s.%s.axis2-enable5V" },
|
||||
{ HAL_BIT, HAL_IN, offsetof(lcec_epocat_data_t, ENABLE_5V_3), "%s.%s.%s.axis3-enable5V" },
|
||||
{ HAL_BIT, HAL_IN, offsetof(lcec_epocat_data_t, ENABLE_5V_4), "%s.%s.%s.axis4-enable5V" },
|
||||
{ HAL_BIT, HAL_OUT, offsetof(lcec_epocat_data_t, HOME_0), "%s.%s.%s.axis0-home" },
|
||||
{ HAL_BIT, HAL_OUT, offsetof(lcec_epocat_data_t, HOME_1), "%s.%s.%s.axis1-home" },
|
||||
{ HAL_BIT, HAL_OUT, offsetof(lcec_epocat_data_t, HOME_2), "%s.%s.%s.axis2-home" },
|
||||
{ HAL_BIT, HAL_OUT, offsetof(lcec_epocat_data_t, HOME_3), "%s.%s.%s.axis3-home" },
|
||||
{ HAL_BIT, HAL_OUT, offsetof(lcec_epocat_data_t, HOME_4), "%s.%s.%s.axis4-home" },
|
||||
{ HAL_BIT, HAL_OUT, offsetof(lcec_epocat_data_t, IN_0), "%s.%s.%s.din-0" },
|
||||
{ HAL_BIT, HAL_OUT, offsetof(lcec_epocat_data_t, IN_1), "%s.%s.%s.din-1" },
|
||||
{ HAL_BIT, HAL_OUT, offsetof(lcec_epocat_data_t, IN_2), "%s.%s.%s.din-2" },
|
||||
{ HAL_BIT, HAL_OUT, offsetof(lcec_epocat_data_t, IN_3), "%s.%s.%s.din-3" },
|
||||
{ HAL_BIT, HAL_OUT, offsetof(lcec_epocat_data_t, IN_4), "%s.%s.%s.probe" },
|
||||
{ HAL_BIT, HAL_IN, offsetof(lcec_epocat_data_t, OUT_0), "%s.%s.%s.dout-0" },
|
||||
{ HAL_BIT, HAL_IN, offsetof(lcec_epocat_data_t, OUT_1), "%s.%s.%s.dout-1" },
|
||||
|
||||
{ HAL_S32, HAL_OUT, offsetof(lcec_epocat_data_t, HZ_0), "%s.%s.%s.axis0-hz" },
|
||||
{ HAL_S32, HAL_OUT, offsetof(lcec_epocat_data_t, HZ_1), "%s.%s.%s.axis1-hz" },
|
||||
{ HAL_S32, HAL_OUT, offsetof(lcec_epocat_data_t, HZ_2), "%s.%s.%s.axis2-hz" },
|
||||
{ HAL_S32, HAL_OUT, offsetof(lcec_epocat_data_t, HZ_3), "%s.%s.%s.axis3-hz" },
|
||||
{ HAL_S32, HAL_OUT, offsetof(lcec_epocat_data_t, HZ_4), "%s.%s.%s.axis4-hz" },
|
||||
|
||||
{ HAL_FLOAT, HAL_OUT, offsetof(lcec_epocat_data_t, enc_00_position), "%s.%s.%s.axis0-enc-pos" },
|
||||
{ HAL_FLOAT, HAL_OUT, offsetof(lcec_epocat_data_t, enc_01_position), "%s.%s.%s.axis1-enc-pos" },
|
||||
{ HAL_FLOAT, HAL_OUT, offsetof(lcec_epocat_data_t, enc_02_position), "%s.%s.%s.axis2-enc-pos" },
|
||||
{ HAL_FLOAT, HAL_OUT, offsetof(lcec_epocat_data_t, enc_03_position), "%s.%s.%s.axis3-enc-pos" },
|
||||
{ HAL_FLOAT, HAL_OUT, offsetof(lcec_epocat_data_t, enc_04_position), "%s.%s.%s.axis4-enc-pos" },
|
||||
|
||||
{ HAL_S32, HAL_OUT, offsetof(lcec_epocat_data_t, enc_00_count), "%s.%s.%s.axis0-raw-count" },
|
||||
{ HAL_S32, HAL_OUT, offsetof(lcec_epocat_data_t, enc_01_count), "%s.%s.%s.axis1-raw-count" },
|
||||
{ HAL_S32, HAL_OUT, offsetof(lcec_epocat_data_t, enc_02_count), "%s.%s.%s.axis2-raw-count" },
|
||||
{ HAL_S32, HAL_OUT, offsetof(lcec_epocat_data_t, enc_03_count), "%s.%s.%s.axis3-raw-count" },
|
||||
{ HAL_S32, HAL_OUT, offsetof(lcec_epocat_data_t, enc_04_count), "%s.%s.%s.axis4-raw-count" },
|
||||
|
||||
{ HAL_BIT, HAL_IO, offsetof(lcec_epocat_data_t, enc_00_index_enable), "%s.%s.%s.axis0-index-enable" },
|
||||
{ HAL_BIT, HAL_IO, offsetof(lcec_epocat_data_t, enc_01_index_enable), "%s.%s.%s.axis1-index-enable" },
|
||||
{ HAL_BIT, HAL_IO, offsetof(lcec_epocat_data_t, enc_02_index_enable), "%s.%s.%s.axis2-index-enable" },
|
||||
{ HAL_BIT, HAL_IO, offsetof(lcec_epocat_data_t, enc_03_index_enable), "%s.%s.%s.axis3-index-enable" },
|
||||
{ HAL_BIT, HAL_IO, offsetof(lcec_epocat_data_t, enc_04_index_enable), "%s.%s.%s.axis4-index-enable" },
|
||||
|
||||
{ HAL_FLOAT, HAL_IN, offsetof(lcec_epocat_data_t, dac_00_value), "%s.%s.%s.axis0-dac-value" },
|
||||
{ HAL_FLOAT, HAL_IN, offsetof(lcec_epocat_data_t, dac_01_value), "%s.%s.%s.axis1-dac-value" },
|
||||
{ HAL_FLOAT, HAL_IN, offsetof(lcec_epocat_data_t, dac_02_value), "%s.%s.%s.axis2-dac-value" },
|
||||
{ HAL_FLOAT, HAL_IN, offsetof(lcec_epocat_data_t, dac_03_value), "%s.%s.%s.axis3-dac-value" },
|
||||
{ HAL_FLOAT, HAL_IN, offsetof(lcec_epocat_data_t, dac_04_value), "%s.%s.%s.axis4-dac-value" },
|
||||
|
||||
{ HAL_FLOAT, HAL_IN, offsetof(lcec_epocat_data_t, dac_inverter_value), "%s.%s.%s.inverter-dac-value" },
|
||||
{ HAL_BIT, HAL_IN, offsetof(lcec_epocat_data_t, inverter_forward), "%s.%s.%s.inverter-forward" },
|
||||
{ HAL_BIT, HAL_IN, offsetof(lcec_epocat_data_t, inverter_reverse), "%s.%s.%s.inverter-reverse" },
|
||||
|
||||
{ HAL_FLOAT, HAL_OUT, offsetof(lcec_epocat_data_t, adc1), "%s.%s.%s.adc0-value" },
|
||||
{ HAL_FLOAT, HAL_OUT, offsetof(lcec_epocat_data_t, adc2), "%s.%s.%s.adc1-value" },
|
||||
|
||||
{ HAL_TYPE_UNSPECIFIED, HAL_DIR_UNSPECIFIED, -1, NULL }
|
||||
};
|
||||
|
||||
static const lcec_pindesc_t slave_params[] = {
|
||||
{ HAL_FLOAT, HAL_RW, offsetof(lcec_epocat_data_t, enc_00_scale), "%s.%s.%s.axis0-enc-scale" },
|
||||
{ HAL_FLOAT, HAL_RW, offsetof(lcec_epocat_data_t, enc_01_scale), "%s.%s.%s.axis1-enc-scale" },
|
||||
{ HAL_FLOAT, HAL_RW, offsetof(lcec_epocat_data_t, enc_02_scale), "%s.%s.%s.axis2-enc-scale" },
|
||||
{ HAL_FLOAT, HAL_RW, offsetof(lcec_epocat_data_t, enc_03_scale), "%s.%s.%s.axis3-enc-scale" },
|
||||
{ HAL_FLOAT, HAL_RW, offsetof(lcec_epocat_data_t, enc_04_scale), "%s.%s.%s.axis4-enc-scale" },
|
||||
|
||||
{ HAL_FLOAT, HAL_RW, offsetof(lcec_epocat_data_t, dac_00_scale), "%s.%s.%s.axis0-dac-scale" },
|
||||
{ HAL_FLOAT, HAL_RW, offsetof(lcec_epocat_data_t, dac_01_scale), "%s.%s.%s.axis1-dac-scale" },
|
||||
{ HAL_FLOAT, HAL_RW, offsetof(lcec_epocat_data_t, dac_02_scale), "%s.%s.%s.axis2-dac-scale" },
|
||||
{ HAL_FLOAT, HAL_RW, offsetof(lcec_epocat_data_t, dac_03_scale), "%s.%s.%s.axis3-dac-scale" },
|
||||
{ HAL_FLOAT, HAL_RW, offsetof(lcec_epocat_data_t, dac_04_scale), "%s.%s.%s.axis4-dac-scale" },
|
||||
|
||||
{ HAL_TYPE_UNSPECIFIED, HAL_DIR_UNSPECIFIED, -1, NULL }
|
||||
};
|
||||
|
||||
static ec_pdo_entry_info_t lcec_epocat_pdo_entries[] = {
|
||||
{ 0x7000, 0x01, 16 }, /* Spindle vel */
|
||||
{ 0x7001, 0x01, 16 }, /* Drive 1 freq */
|
||||
{ 0x7001, 0x02, 16 }, /* Drive 2 freq */
|
||||
{ 0x7001, 0x03, 16 }, /* Drive 3 freq */
|
||||
{ 0x7001, 0x04, 16 }, /* Drive 4 freq */
|
||||
{ 0x7001, 0x05, 16 }, /* Drive 5 freq */
|
||||
{ 0x7002, 0x01, 1 }, /* Drive 1 en */
|
||||
{ 0x7002, 0x02, 1 }, /* Drive 2 en */
|
||||
{ 0x7002, 0x03, 1 }, /* Drive 3 en */
|
||||
{ 0x7002, 0x04, 1 }, /* Drive 4 en */
|
||||
{ 0x7002, 0x05, 1 }, /* Drive 5 en */
|
||||
{ 0x0000, 0x06, 1 }, /* Pad */
|
||||
{ 0x0000, 0x07, 1 }, /* Pad */
|
||||
{ 0x0000, 0x08, 1 }, /* Pad */
|
||||
{ 0x7003, 0x01, 1 }, /* Drive 1 dir */
|
||||
{ 0x7003, 0x02, 1 }, /* Drive 2 dir */
|
||||
{ 0x7003, 0x03, 1 }, /* Drive 3 dir */
|
||||
{ 0x7003, 0x04, 1 }, /* Drive 4 dir */
|
||||
{ 0x7003, 0x05, 1 }, /* Drive 5 dir */
|
||||
{ 0x0000, 0x06, 1 }, /* Pad */
|
||||
{ 0x0000, 0x07, 1 }, /* Pad */
|
||||
{ 0x0000, 0x07, 1 }, /* Pad */
|
||||
{ 0x7004, 0x01, 1 }, /* Drive 1 res flag */
|
||||
{ 0x7004, 0x02, 1 }, /* Drive 2 res flag */
|
||||
{ 0x7004, 0x03, 1 }, /* Drive 3 res flag */
|
||||
{ 0x7004, 0x04, 1 }, /* Drive 4 res flag */
|
||||
{ 0x7004, 0x05, 1 }, /* Drive 5 res flag */
|
||||
{ 0x7004, 0x06, 1 }, /* Selezione idx/probe */
|
||||
{ 0x7004, 0x07, 1 }, /* Tristate single ended */
|
||||
{ 0x0000, 0x08, 1 }, /* Pad */
|
||||
{ 0x7005, 0x01, 1 }, /* Drive 1 polarity */
|
||||
{ 0x7005, 0x02, 1 }, /* Drive 2 polarity */
|
||||
{ 0x7005, 0x03, 1 }, /* Drive 3 polarity */
|
||||
{ 0x7005, 0x04, 1 }, /* Drive 4 polarity */
|
||||
{ 0x7005, 0x05, 1 }, /* Drive 5 polarity */
|
||||
{ 0x0000, 0x06, 1 }, /* Pad */
|
||||
{ 0x0000, 0x07, 1 }, /* Pad */
|
||||
{ 0x0000, 0x08, 1 }, /* Pad */
|
||||
{ 0x7006, 0x01, 1 }, /* Drive 1 en 24V */
|
||||
{ 0x7006, 0x02, 1 }, /* Drive 2 en 24V */
|
||||
{ 0x7006, 0x03, 1 }, /* Drive 3 en 24V */
|
||||
{ 0x7006, 0x04, 1 }, /* Drive 4 en 24V */
|
||||
{ 0x7006, 0x05, 1 }, /* Drive 5 en 24V */
|
||||
{ 0x7006, 0x06, 1 }, /* Spindle en */
|
||||
{ 0x7006, 0x01, 1 }, /* Out 1 */
|
||||
{ 0x7006, 0x01, 1 }, /* Out 2 */
|
||||
{ 0x6000, 0x01, 16 }, /* Ingresso 1 */
|
||||
{ 0x6000, 0x02, 16 }, /* Ingresso 2 */
|
||||
{ 0x6001, 0x01, 16 }, /* Drive 1 count */
|
||||
{ 0x6001, 0x02, 16 }, /* Drive 2 count */
|
||||
{ 0x6001, 0x03, 16 }, /* Drive 3 count */
|
||||
{ 0x6001, 0x04, 16 }, /* Drive 4 count */
|
||||
{ 0x6001, 0x05, 16 }, /* Drive 5 count */
|
||||
{ 0x6002, 0x01, 16 }, /* Drive 1 latch */
|
||||
{ 0x6002, 0x02, 16 }, /* Drive 2 latch */
|
||||
{ 0x6002, 0x03, 16 }, /* Drive 3 latch */
|
||||
{ 0x6002, 0x04, 16 }, /* Drive 4 latch */
|
||||
{ 0x6002, 0x05, 16 }, /* Drive 5 latch */
|
||||
{ 0x6006, 0x01, 1 }, /* Drive 1 flag */
|
||||
{ 0x6006, 0x02, 1 }, /* Drive 2 flag */
|
||||
{ 0x6006, 0x03, 1 }, /* Drive 3 flag */
|
||||
{ 0x6006, 0x04, 1 }, /* Drive 4 flag */
|
||||
{ 0x6006, 0x05, 1 }, /* Drive 5 flag */
|
||||
{ 0x0000, 0x06, 1 }, /* Pad */
|
||||
{ 0x0000, 0x07, 1 }, /* Pad */
|
||||
{ 0x0000, 0x08, 1 }, /* Pad */
|
||||
{ 0x6004, 0x01, 1 }, /* Drive 1 home */
|
||||
{ 0x6004, 0x02, 1 }, /* Drive 2 home */
|
||||
{ 0x6004, 0x03, 1 }, /* Drive 3 home */
|
||||
{ 0x6004, 0x04, 1 }, /* Drive 4 home */
|
||||
{ 0x6004, 0x05, 1 }, /* Drive 5 home */
|
||||
{ 0x0000, 0x06, 1 }, /* Pad */
|
||||
{ 0x0000, 0x07, 1 }, /* Pad */
|
||||
{ 0x0000, 0x08, 1 }, /* Pad */
|
||||
{ 0x6003, 0x01, 1 }, /* Drive 1 ok */
|
||||
{ 0x6003, 0x02, 1 }, /* Drive 2 ok */
|
||||
{ 0x6003, 0x03, 1 }, /* Drive 3 ok */
|
||||
{ 0x6003, 0x04, 1 }, /* Drive 4 ok */
|
||||
{ 0x6003, 0x05, 1 }, /* Drive 5 ok */
|
||||
{ 0x6003, 0x06, 1 }, /* Spindle ok */
|
||||
{ 0x0000, 0x07, 1 }, /* Pad */
|
||||
{ 0x0000, 0x08, 1 }, /* Pad */
|
||||
{ 0x6005, 0x01, 1 }, /* Drive 1 idx */
|
||||
{ 0x6005, 0x02, 1 }, /* Drive 2 idx */
|
||||
{ 0x6005, 0x03, 1 }, /* Drive 3 idx */
|
||||
{ 0x6005, 0x04, 1 }, /* Drive 4 idx */
|
||||
{ 0x6005, 0x05, 1 }, /* Drive 5 idx */
|
||||
{ 0x0000, 0x06, 1 }, /* Pad */
|
||||
{ 0x0000, 0x07, 1 }, /* Pad */
|
||||
{ 0x0000, 0x08, 1 }, /* Pad */
|
||||
{ 0x6007, 0x01, 1 }, /* In 1 */
|
||||
{ 0x6007, 0x02, 1 }, /* In 2 */
|
||||
{ 0x6007, 0x03, 1 }, /* In 3 */
|
||||
{ 0x6007, 0x04, 1 }, /* In 4 */
|
||||
{ 0x6007, 0x05, 1 }, /* Probe */
|
||||
{ 0x0000, 0x06, 1 }, /* Pad */
|
||||
{ 0x0000, 0x07, 1 }, /* Pad */
|
||||
{ 0x0000, 0x08, 1 }, /* Pad */
|
||||
};
|
||||
|
||||
static ec_pdo_info_t lcec_epocat_pdos[] = {
|
||||
{ 0x1600, 1, lcec_epocat_pdo_entries + 0 }, /* Velocita' mandrino */
|
||||
{ 0x1601, 5, lcec_epocat_pdo_entries + 1 }, /* Frequenza drive */
|
||||
{ 0x1602, 8, lcec_epocat_pdo_entries + 6 }, /* Abilitazione drive */
|
||||
{ 0x1603, 8, lcec_epocat_pdo_entries + 14 }, /* Direzione drive */
|
||||
{ 0x1604, 8, lcec_epocat_pdo_entries + 22 }, /* Comandi drive */
|
||||
{ 0x1605, 8, lcec_epocat_pdo_entries + 30 }, /* Impulso singolo */
|
||||
{ 0x1606, 8, lcec_epocat_pdo_entries + 38 }, /* Uscite digitali */
|
||||
{ 0x1a00, 2, lcec_epocat_pdo_entries + 46 }, /* Ingressi analogici */
|
||||
{ 0x1a01, 5, lcec_epocat_pdo_entries + 48 }, /* Contatore impulsi */
|
||||
{ 0x1a02, 5, lcec_epocat_pdo_entries + 53 }, /* Latch impulsi */
|
||||
{ 0x1a06, 8, lcec_epocat_pdo_entries + 58 }, /* Flag latch idx/probe */
|
||||
{ 0x1a04, 8, lcec_epocat_pdo_entries + 66 }, /* Home */
|
||||
{ 0x1a03, 8, lcec_epocat_pdo_entries + 74 }, /* Stato drive */
|
||||
{ 0x1a05, 8, lcec_epocat_pdo_entries + 82 }, /* Index */
|
||||
{ 0x1a07, 8, lcec_epocat_pdo_entries + 90 }, /* Ingressi digitali */
|
||||
};
|
||||
|
||||
static ec_sync_info_t lcec_epocat_syncs[] = {
|
||||
{ 0, EC_DIR_OUTPUT, 7, lcec_epocat_pdos + 0, EC_WD_DISABLE },
|
||||
{ 1, EC_DIR_INPUT, 8, lcec_epocat_pdos + 7, EC_WD_DISABLE },
|
||||
{ 0xff }
|
||||
};
|
||||
|
||||
static uint16_t raw_counts_old[5];
|
||||
static int32_t counts[5];
|
||||
static int32_t counts_latch[5];
|
||||
|
||||
void lcec_epocat_read(struct lcec_slave *slave, long period);
|
||||
void lcec_epocat_write(struct lcec_slave *slave, long period);
|
||||
|
||||
int lcec_epocat_init(int comp_id, struct lcec_slave *slave, ec_pdo_entry_reg_t *pdo_entry_regs) {
|
||||
lcec_master_t *master = slave->master;
|
||||
lcec_epocat_data_t *hal_data;
|
||||
int err;
|
||||
|
||||
// initialize callbacks
|
||||
slave->proc_read = lcec_epocat_read;
|
||||
slave->proc_write = lcec_epocat_write;
|
||||
|
||||
// alloc hal memory
|
||||
if ((hal_data = hal_malloc(sizeof(lcec_epocat_data_t))) == NULL) {
|
||||
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "hal_malloc() for slave %s.%s failed\n", master->name, slave->name);
|
||||
return -EIO;
|
||||
}
|
||||
memset(hal_data, 0, sizeof(lcec_epocat_data_t));
|
||||
slave->hal_data = hal_data;
|
||||
|
||||
// initializer sync info
|
||||
slave->sync_info = lcec_epocat_syncs;
|
||||
|
||||
// initialize POD entries
|
||||
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x7006, 1, &hal_data->off_dig_out, NULL);
|
||||
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6007, 1, &hal_data->off_dig_inp, NULL);
|
||||
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x7001, 1, &hal_data->off_PWM, NULL);
|
||||
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x7003, 1, &hal_data->off_DIR, NULL);
|
||||
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6001, 1, &hal_data->off_ENC, NULL);
|
||||
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x7006, 1, &hal_data->off_ENABLE_24V, NULL);
|
||||
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x7002, 1, &hal_data->off_ENABLE_5V, NULL);
|
||||
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6003, 1, &hal_data->off_DRVOK, NULL);
|
||||
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6004, 1, &hal_data->off_HOME, NULL);
|
||||
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x7000, 1, &hal_data->off_INVERTER, NULL);
|
||||
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6000, 1, &hal_data->off_ADC1, NULL);
|
||||
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6000, 2, &hal_data->off_ADC2, NULL);
|
||||
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6002, 1, &hal_data->off_LATCH, NULL);
|
||||
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6006, 1, &hal_data->off_FLAG, NULL);
|
||||
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x7004, 1, &hal_data->off_RESET_FLAG, NULL);
|
||||
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x7005, 1, &hal_data->off_POLARITY, NULL);
|
||||
|
||||
// export pins
|
||||
if ((err = lcec_pin_newf_list(hal_data, slave_pins, LCEC_MODULE_NAME, master->name, slave->name)) != 0) {
|
||||
return err;
|
||||
}
|
||||
|
||||
// export parameters
|
||||
if ((err = lcec_param_newf_list(hal_data, slave_params, LCEC_MODULE_NAME, master->name, slave->name)) != 0) {
|
||||
return err;
|
||||
}
|
||||
|
||||
// initialize variables
|
||||
raw_counts_old[0] = 0;
|
||||
raw_counts_old[1] = 0;
|
||||
raw_counts_old[2] = 0;
|
||||
raw_counts_old[3] = 0;
|
||||
raw_counts_old[4] = 0;
|
||||
|
||||
counts[0] = 0;
|
||||
counts[1] = 0;
|
||||
counts[2] = 0;
|
||||
counts[3] = 0;
|
||||
counts[4] = 0;
|
||||
|
||||
counts_latch[0] = 0;
|
||||
counts_latch[1] = 0;
|
||||
counts_latch[2] = 0;
|
||||
counts_latch[3] = 0;
|
||||
counts_latch[4] = 0;
|
||||
|
||||
hal_data->enc_00_scale = 1;
|
||||
hal_data->enc_01_scale = 1;
|
||||
hal_data->enc_02_scale = 1;
|
||||
hal_data->enc_03_scale = 1;
|
||||
hal_data->enc_04_scale = 1;
|
||||
|
||||
hal_data->dac_00_scale = 1;
|
||||
hal_data->dac_01_scale = 1;
|
||||
hal_data->dac_02_scale = 1;
|
||||
hal_data->dac_03_scale = 1;
|
||||
hal_data->dac_04_scale = 1;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
void lcec_epocat_read(struct lcec_slave *slave, long period) {
|
||||
lcec_master_t *master = slave->master;
|
||||
lcec_epocat_data_t *hal_data = (lcec_epocat_data_t *) slave->hal_data;
|
||||
uint8_t *pd = master->process_data;
|
||||
|
||||
uint16_t raw_counts[5];
|
||||
uint16_t raw_latch_counts[5];
|
||||
|
||||
*(hal_data->IN_0) = EC_READ_BIT(&pd[hal_data->off_dig_inp], 0);
|
||||
*(hal_data->IN_1) = EC_READ_BIT(&pd[hal_data->off_dig_inp], 1);
|
||||
*(hal_data->IN_2) = EC_READ_BIT(&pd[hal_data->off_dig_inp], 2);
|
||||
*(hal_data->IN_3) = EC_READ_BIT(&pd[hal_data->off_dig_inp], 3);
|
||||
*(hal_data->IN_4) = EC_READ_BIT(&pd[hal_data->off_dig_inp], 4);
|
||||
|
||||
*(hal_data->DROK_0) = EC_READ_BIT(&pd[hal_data->off_DRVOK], 0);
|
||||
*(hal_data->DROK_1) = EC_READ_BIT(&pd[hal_data->off_DRVOK], 1);
|
||||
*(hal_data->DROK_2) = EC_READ_BIT(&pd[hal_data->off_DRVOK], 2);
|
||||
*(hal_data->DROK_3) = EC_READ_BIT(&pd[hal_data->off_DRVOK], 3);
|
||||
*(hal_data->DROK_4) = EC_READ_BIT(&pd[hal_data->off_DRVOK], 4);
|
||||
*(hal_data->DROK_INVERTER) = EC_READ_BIT(&pd[hal_data->off_DRVOK], 5);
|
||||
|
||||
*(hal_data->HOME_0) = EC_READ_BIT(&pd[hal_data->off_HOME], 0);
|
||||
*(hal_data->HOME_1) = EC_READ_BIT(&pd[hal_data->off_HOME], 1);
|
||||
*(hal_data->HOME_2) = EC_READ_BIT(&pd[hal_data->off_HOME], 2);
|
||||
*(hal_data->HOME_3) = EC_READ_BIT(&pd[hal_data->off_HOME], 3);
|
||||
*(hal_data->HOME_4) = EC_READ_BIT(&pd[hal_data->off_HOME], 4);
|
||||
|
||||
*(hal_data->adc1) = EC_READ_U16(&pd[hal_data->off_ADC1]);
|
||||
*(hal_data->adc2) = EC_READ_U16(&pd[hal_data->off_ADC2]);
|
||||
|
||||
raw_counts[0] = EC_READ_U16(&pd[hal_data->off_ENC]);
|
||||
raw_counts[1] = EC_READ_U16(&pd[hal_data->off_ENC + 2]);
|
||||
raw_counts[2] = EC_READ_U16(&pd[hal_data->off_ENC + 4]);
|
||||
raw_counts[3] = EC_READ_U16(&pd[hal_data->off_ENC + 6]);
|
||||
raw_counts[4] = EC_READ_U16(&pd[hal_data->off_ENC + 8]);
|
||||
|
||||
// check for index pulse detected
|
||||
if (EC_READ_BIT(&pd[hal_data->off_FLAG], 0) == 1)
|
||||
{
|
||||
// cancel index-enable
|
||||
*(hal_data->enc_00_index_enable) = 0;
|
||||
|
||||
raw_latch_counts[0] = EC_READ_U16(&pd[hal_data->off_LATCH]);
|
||||
counts_latch[0] = counts[0];
|
||||
counts_latch[0] += (int16_t)(raw_latch_counts[0] - raw_counts_old[0]);
|
||||
}
|
||||
if (EC_READ_BIT(&pd[hal_data->off_FLAG], 1) == 1)
|
||||
{
|
||||
// cancel index-enable
|
||||
*(hal_data->enc_01_index_enable) = 0;
|
||||
|
||||
raw_latch_counts[1] = EC_READ_U16(&pd[hal_data->off_LATCH + 2]);
|
||||
counts_latch[1] = counts[1];
|
||||
counts_latch[1] += (int16_t)(raw_latch_counts[1] - raw_counts_old[1]);
|
||||
}
|
||||
if (EC_READ_BIT(&pd[hal_data->off_FLAG], 2) == 1)
|
||||
{
|
||||
// cancel index-enable
|
||||
*(hal_data->enc_02_index_enable) = 0;
|
||||
|
||||
raw_latch_counts[2] = EC_READ_U16(&pd[hal_data->off_LATCH + 4]);
|
||||
counts_latch[2] = counts[2];
|
||||
counts_latch[2] += (int16_t)(raw_latch_counts[2] - raw_counts_old[2]);
|
||||
}
|
||||
if (EC_READ_BIT(&pd[hal_data->off_FLAG], 3) == 1)
|
||||
{
|
||||
// cancel index-enable
|
||||
*(hal_data->enc_03_index_enable) = 0;
|
||||
|
||||
raw_latch_counts[3] = EC_READ_U16(&pd[hal_data->off_LATCH + 6]);
|
||||
counts_latch[3] = counts[3];
|
||||
counts_latch[3] += (int16_t)(raw_latch_counts[3] - raw_counts_old[3]);
|
||||
}
|
||||
if (EC_READ_BIT(&pd[hal_data->off_FLAG], 4) == 1)
|
||||
{
|
||||
// cancel index-enable
|
||||
*(hal_data->enc_04_index_enable) = 0;
|
||||
|
||||
raw_latch_counts[4] = EC_READ_U16(&pd[hal_data->off_LATCH + 8]);
|
||||
counts_latch[4] = counts[4];
|
||||
counts_latch[4] += (int16_t)(raw_latch_counts[4] - raw_counts_old[4]);
|
||||
}
|
||||
|
||||
counts[0] += (int16_t)(raw_counts[0] - raw_counts_old[0]);
|
||||
raw_counts_old[0] = raw_counts[0];
|
||||
counts[0] -= counts_latch[0];
|
||||
|
||||
counts[1] += (int16_t)(raw_counts[1] - raw_counts_old[1]);
|
||||
raw_counts_old[1] = raw_counts[1];
|
||||
counts[1] -= counts_latch[1];
|
||||
|
||||
counts[2] += (int16_t)(raw_counts[2] - raw_counts_old[2]);
|
||||
raw_counts_old[2] = raw_counts[2];
|
||||
counts[2] -= counts_latch[2];
|
||||
|
||||
counts[3] += (int16_t)(raw_counts[3] - raw_counts_old[3]);
|
||||
raw_counts_old[3] = raw_counts[3];
|
||||
counts[3] -= counts_latch[3];
|
||||
|
||||
counts[4] += (int16_t)(raw_counts[4] - raw_counts_old[4]);
|
||||
raw_counts_old[4] = raw_counts[4];
|
||||
counts[4] -= counts_latch[4];
|
||||
|
||||
*(hal_data->enc_00_position) = counts[0] * hal_data->enc_00_scale;
|
||||
*(hal_data->enc_01_position) = counts[1] * hal_data->enc_01_scale;
|
||||
*(hal_data->enc_02_position) = counts[2] * hal_data->enc_02_scale;
|
||||
*(hal_data->enc_03_position) = counts[3] * hal_data->enc_03_scale;
|
||||
*(hal_data->enc_04_position) = counts[4] * hal_data->enc_04_scale;
|
||||
|
||||
*(hal_data->enc_00_count) = counts[0];
|
||||
*(hal_data->enc_01_count) = counts[1];
|
||||
*(hal_data->enc_02_count) = counts[2];
|
||||
*(hal_data->enc_03_count) = counts[3];
|
||||
*(hal_data->enc_04_count) = counts[4];
|
||||
}
|
||||
|
||||
void lcec_epocat_write(struct lcec_slave *slave, long period) {
|
||||
lcec_master_t *master = slave->master;
|
||||
lcec_epocat_data_t *hal_data = (lcec_epocat_data_t *) slave->hal_data;
|
||||
uint8_t *pd = master->process_data;
|
||||
|
||||
float fValue;
|
||||
|
||||
float fTmp;
|
||||
int iDir1;
|
||||
int iDir2;
|
||||
int iDir3;
|
||||
int iDir4;
|
||||
int iDir5;
|
||||
|
||||
// Selezione idx/probe (0=Probe, 1 = index)
|
||||
EC_WRITE_BIT(&pd[hal_data->off_RESET_FLAG], 5, 1);
|
||||
|
||||
// Tristate single ended
|
||||
EC_WRITE_BIT(&pd[hal_data->off_RESET_FLAG], 6, 0);
|
||||
|
||||
// Polarity
|
||||
EC_WRITE_BIT(&pd[hal_data->off_POLARITY], 0, 0);
|
||||
EC_WRITE_BIT(&pd[hal_data->off_POLARITY], 1, 0);
|
||||
EC_WRITE_BIT(&pd[hal_data->off_POLARITY], 2, 0);
|
||||
EC_WRITE_BIT(&pd[hal_data->off_POLARITY], 3, 0);
|
||||
EC_WRITE_BIT(&pd[hal_data->off_POLARITY], 4, 0);
|
||||
|
||||
// Check for index enable request from HAL
|
||||
if (*(hal_data->enc_00_index_enable))
|
||||
{
|
||||
// tell hardware to latch on index
|
||||
EC_WRITE_BIT(&pd[hal_data->off_RESET_FLAG], 0, 0);
|
||||
}
|
||||
else
|
||||
{
|
||||
// cancel hardware latch on index
|
||||
EC_WRITE_BIT(&pd[hal_data->off_RESET_FLAG], 0, 1);
|
||||
}
|
||||
if (*(hal_data->enc_01_index_enable))
|
||||
{
|
||||
// tell hardware to latch on index
|
||||
EC_WRITE_BIT(&pd[hal_data->off_RESET_FLAG], 1, 0);
|
||||
}
|
||||
else
|
||||
{
|
||||
// cancel hardware latch on index
|
||||
EC_WRITE_BIT(&pd[hal_data->off_RESET_FLAG], 1, 1);
|
||||
}
|
||||
if (*(hal_data->enc_02_index_enable))
|
||||
{
|
||||
// tell hardware to latch on index
|
||||
EC_WRITE_BIT(&pd[hal_data->off_RESET_FLAG], 2, 0);
|
||||
}
|
||||
else
|
||||
{
|
||||
// cancel hardware latch on index
|
||||
EC_WRITE_BIT(&pd[hal_data->off_RESET_FLAG], 2, 1);
|
||||
}
|
||||
if (*(hal_data->enc_03_index_enable))
|
||||
{
|
||||
// tell hardware to latch on index
|
||||
EC_WRITE_BIT(&pd[hal_data->off_RESET_FLAG], 3, 0);
|
||||
}
|
||||
else
|
||||
{
|
||||
// cancel hardware latch on index
|
||||
EC_WRITE_BIT(&pd[hal_data->off_RESET_FLAG], 3, 1);
|
||||
}
|
||||
if (*(hal_data->enc_04_index_enable))
|
||||
{
|
||||
// tell hardware to latch on index
|
||||
EC_WRITE_BIT(&pd[hal_data->off_RESET_FLAG], 4, 0);
|
||||
}
|
||||
else
|
||||
{
|
||||
// cancel hardware latch on index
|
||||
EC_WRITE_BIT(&pd[hal_data->off_RESET_FLAG], 4, 1);
|
||||
}
|
||||
|
||||
EC_WRITE_BIT(&pd[hal_data->off_dig_out], 6, *(hal_data->OUT_0));
|
||||
EC_WRITE_BIT(&pd[hal_data->off_dig_out], 7, *(hal_data->OUT_1));
|
||||
|
||||
EC_WRITE_BIT(&pd[hal_data->off_ENABLE_24V], 0, *(hal_data->ENABLE_24V_0));
|
||||
EC_WRITE_BIT(&pd[hal_data->off_ENABLE_24V], 1, *(hal_data->ENABLE_24V_1));
|
||||
EC_WRITE_BIT(&pd[hal_data->off_ENABLE_24V], 2, *(hal_data->ENABLE_24V_2));
|
||||
EC_WRITE_BIT(&pd[hal_data->off_ENABLE_24V], 3, *(hal_data->ENABLE_24V_3));
|
||||
EC_WRITE_BIT(&pd[hal_data->off_ENABLE_24V], 4, *(hal_data->ENABLE_24V_4));
|
||||
EC_WRITE_BIT(&pd[hal_data->off_ENABLE_24V], 5, *(hal_data->ENABLE_24V_INVERTER));
|
||||
|
||||
EC_WRITE_BIT(&pd[hal_data->off_ENABLE_5V], 0, *(hal_data->ENABLE_5V_0));
|
||||
EC_WRITE_BIT(&pd[hal_data->off_ENABLE_5V], 1, *(hal_data->ENABLE_5V_1));
|
||||
EC_WRITE_BIT(&pd[hal_data->off_ENABLE_5V], 2, *(hal_data->ENABLE_5V_2));
|
||||
EC_WRITE_BIT(&pd[hal_data->off_ENABLE_5V], 3, *(hal_data->ENABLE_5V_3));
|
||||
EC_WRITE_BIT(&pd[hal_data->off_ENABLE_5V], 4, *(hal_data->ENABLE_5V_4));
|
||||
|
||||
fValue = (((*(hal_data->dac_00_value) * 1000) / hal_data->enc_00_scale) / 16) * hal_data->dac_00_scale;
|
||||
if (fValue > 65534)
|
||||
{
|
||||
fValue = 65534;
|
||||
}
|
||||
if (fValue < -65534)
|
||||
{
|
||||
fValue = -65534;
|
||||
}
|
||||
|
||||
if (fValue >= 0)
|
||||
{
|
||||
iDir1 = 0;
|
||||
fTmp = fValue;
|
||||
}
|
||||
else
|
||||
{
|
||||
iDir1 = 1;
|
||||
fTmp = -fValue;
|
||||
}
|
||||
|
||||
*(hal_data->HZ_0) = fTmp;
|
||||
|
||||
fValue = (((*(hal_data->dac_01_value) * 1000) / hal_data->enc_01_scale) / 16) * hal_data->dac_01_scale;
|
||||
if (fValue > 65534)
|
||||
{
|
||||
fValue = 65534;
|
||||
}
|
||||
if (fValue < -65534)
|
||||
{
|
||||
fValue = -65534;
|
||||
}
|
||||
|
||||
if (fValue >= 0)
|
||||
{
|
||||
iDir2 = 0;
|
||||
fTmp = fValue;
|
||||
}
|
||||
else
|
||||
{
|
||||
iDir2 = 1;
|
||||
fTmp = -fValue;
|
||||
}
|
||||
|
||||
*(hal_data->HZ_1) = fTmp;
|
||||
|
||||
fValue = (((*(hal_data->dac_02_value) * 1000) / hal_data->enc_02_scale) / 16) * hal_data->dac_02_scale;
|
||||
if (fValue > 65534)
|
||||
{
|
||||
fValue = 65534;
|
||||
}
|
||||
if (fValue < -65534)
|
||||
{
|
||||
fValue = -65534;
|
||||
}
|
||||
|
||||
if (fValue >= 0)
|
||||
{
|
||||
iDir3 = 0;
|
||||
fTmp = fValue;
|
||||
}
|
||||
else
|
||||
{
|
||||
iDir3 = 1;
|
||||
fTmp = -fValue;
|
||||
}
|
||||
|
||||
*(hal_data->HZ_2) = fTmp;
|
||||
|
||||
fValue = (((*(hal_data->dac_03_value) * 1000) / hal_data->enc_03_scale) / 16) * hal_data->dac_03_scale;
|
||||
if (fValue > 65534)
|
||||
{
|
||||
fValue = 65534;
|
||||
}
|
||||
if (fValue < -65534)
|
||||
{
|
||||
fValue = -65534;
|
||||
}
|
||||
|
||||
if (fValue >= 0)
|
||||
{
|
||||
iDir4 = 0;
|
||||
fTmp = fValue;
|
||||
}
|
||||
else
|
||||
{
|
||||
iDir4 = 1;
|
||||
fTmp = -fValue;
|
||||
}
|
||||
|
||||
*(hal_data->HZ_3) = fTmp;
|
||||
|
||||
fValue = (((*(hal_data->dac_04_value) * 1000) / hal_data->enc_04_scale) / 16) * hal_data->dac_04_scale;
|
||||
if (fValue > 65534)
|
||||
{
|
||||
fValue = 65534;
|
||||
}
|
||||
if (fValue < -65534)
|
||||
{
|
||||
fValue = -65534;
|
||||
}
|
||||
|
||||
if (fValue >= 0)
|
||||
{
|
||||
iDir5 = 0;
|
||||
fTmp = fValue;
|
||||
}
|
||||
else
|
||||
{
|
||||
iDir5 = 1;
|
||||
fTmp = -fValue;
|
||||
}
|
||||
|
||||
*(hal_data->HZ_4) = fTmp;
|
||||
|
||||
EC_WRITE_BIT(&pd[hal_data->off_DIR], 0, iDir1);
|
||||
EC_WRITE_U16(&pd[hal_data->off_PWM], *(hal_data->HZ_0));
|
||||
|
||||
EC_WRITE_BIT(&pd[hal_data->off_DIR], 1, iDir2);
|
||||
EC_WRITE_U16(&pd[hal_data->off_PWM + 2], *(hal_data->HZ_1));
|
||||
|
||||
EC_WRITE_BIT(&pd[hal_data->off_DIR], 2, iDir3);
|
||||
EC_WRITE_U16(&pd[hal_data->off_PWM + 4], *(hal_data->HZ_2));
|
||||
|
||||
EC_WRITE_BIT(&pd[hal_data->off_DIR], 3, iDir4);
|
||||
EC_WRITE_U16(&pd[hal_data->off_PWM + 6], *(hal_data->HZ_3));
|
||||
|
||||
EC_WRITE_BIT(&pd[hal_data->off_DIR], 4, iDir5);
|
||||
EC_WRITE_U16(&pd[hal_data->off_PWM + 8], *(hal_data->HZ_4));
|
||||
|
||||
fValue = *(hal_data->dac_inverter_value) / 10 * 0x7fff;
|
||||
|
||||
EC_WRITE_U16(&pd[hal_data->off_INVERTER], (uint16_t)fValue);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user