├── media └── device.jpg ├── .gitignore ├── .vscode └── extensions.json ├── platformioConf └── genericSTM32G030C8.json ├── include └── README ├── platformio.ini ├── lib └── simplefocDxlCore │ ├── dxlUtils.h │ ├── simplefocDxlCore.h │ ├── dxlCom.h │ ├── dxlCom.cpp │ ├── dxlMemory.cpp │ ├── dxlMemory.h │ └── simplefocDxlCore.cpp ├── src └── main.cpp └── README.md /media/device.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotsmith/SimpleFocDXLCore/HEAD/media/device.jpg -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | .pio 2 | .vscode/.browse.c_cpp.db* 3 | .vscode/c_cpp_properties.json 4 | .vscode/launch.json 5 | .vscode/ipch 6 | -------------------------------------------------------------------------------- /.vscode/extensions.json: -------------------------------------------------------------------------------- 1 | { 2 | // See http://go.microsoft.com/fwlink/?LinkId=827846 3 | // for the documentation about the extensions.json format 4 | "recommendations": [ 5 | "platformio.platformio-ide" 6 | ], 7 | "unwantedRecommendations": [ 8 | "ms-vscode.cpptools-extension-pack" 9 | ] 10 | } 11 | -------------------------------------------------------------------------------- /platformioConf/genericSTM32G030C8.json: -------------------------------------------------------------------------------- 1 | { 2 | "build": { 3 | "core": "stm32", 4 | "cpu": "cortex-m0plus", 5 | "extra_flags": "-DSTM32G0 -DSTM32G030xx", 6 | "f_cpu": "64000000L", 7 | "mcu": "stm32g030c8", 8 | "product_line": "STM32G030xx", 9 | "variant": "STM32G0xx/G030C(6-8)T" 10 | }, 11 | "debug": { 12 | "default_tools": [ 13 | "stlink" 14 | ], 15 | "jlink_device": "STM32G030C8", 16 | "onboard_tools": [ 17 | "stlink" 18 | ], 19 | "openocd_target": "stm32g0x", 20 | "svd_path": "STM32G030.svd" 21 | }, 22 | "frameworks": [ 23 | "arduino", 24 | "cmsis", 25 | "stm32cube" 26 | ], 27 | "name": "stm32G030C8Tx (8k RAM. 64k Flash)", 28 | "upload": { 29 | "maximum_ram_size": 8192, 30 | "maximum_size": 65536, 31 | "protocol": "stlink", 32 | "protocols": [ 33 | "stlink" 34 | ] 35 | }, 36 | "url": "https://www.st.com/en/microcontrollers-microprocessors/stm32g030c8.html", 37 | "vendor": "Generic" 38 | } 39 | -------------------------------------------------------------------------------- /include/README: -------------------------------------------------------------------------------- 1 | 2 | This directory is intended for project header files. 3 | 4 | A header file is a file containing C declarations and macro definitions 5 | to be shared between several project source files. You request the use of a 6 | header file in your project source file (C, C++, etc) located in `src` folder 7 | by including it, with the C preprocessing directive `#include'. 8 | 9 | ```src/main.c 10 | 11 | #include "header.h" 12 | 13 | int main (void) 14 | { 15 | ... 16 | } 17 | ``` 18 | 19 | Including a header file produces the same results as copying the header file 20 | into each source file that needs it. Such copying would be time-consuming 21 | and error-prone. With a header file, the related declarations appear 22 | in only one place. If they need to be changed, they can be changed in one 23 | place, and programs that include the header file will automatically use the 24 | new version when next recompiled. The header file eliminates the labor of 25 | finding and changing all the copies as well as the risk that a failure to 26 | find one copy will result in inconsistencies within a program. 27 | 28 | In C, the usual convention is to give header files names that end with `.h'. 29 | It is most portable to use only letters, digits, dashes, and underscores in 30 | header file names, and at most one dot. 31 | 32 | Read more about using header files in official GCC documentation: 33 | 34 | * Include Syntax 35 | * Include Operation 36 | * Once-Only Headers 37 | * Computed Includes 38 | 39 | https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html 40 | -------------------------------------------------------------------------------- /platformio.ini: -------------------------------------------------------------------------------- 1 | ; PlatformIO Project Configuration File 2 | ; 3 | ; Build options: build flags, source filter 4 | ; Upload options: custom upload port, speed and extra flags 5 | ; Library options: dependencies, extra library storages 6 | ; Advanced options: extra scripting 7 | ; 8 | ; Please visit documentation for the other options and examples 9 | ; https://docs.platformio.org/page/projectconf.html 10 | 11 | [env:genericSTM32G030C8] 12 | platform = ststm32 13 | board = genericSTM32G030C8 14 | framework = arduino 15 | lib_deps = askuric/Simple FOC@^2.2.1 16 | lib_archive = false 17 | ;build_type = debug 18 | ; WORKING 19 | ;build_flags = -Os ; SIMPLE 20 | build_flags = -Os -DI2C_TIMING -DHAL_RTC_MODULE_DISABLED -DHAL_DAC_MODULE_DISABLED -DHAL_EXTI_MODULE_DISABLED -DHAL_ETH_MODULE_DISABLED -DHAL_SD_MODULE_DISABLED -DHAL_QSPI_MODULE_DISABLED -DHAL_I2S_MODULE_DISABLED -DHAL_SAI_MODULE_DISABLED -faggressive-loop-optimizations 21 | 22 | ; NOT WORKING 23 | ;build_flags = -flto -DI2C_TIMING -DHAL_RTC_MODULE_DISABLED -DHAL_DAC_MODULE_DISABLED -DHAL_EXTI_MODULE_DISABLED -DHAL_ETH_MODULE_DISABLED -DHAL_SD_MODULE_DISABLED -DHAL_QSPI_MODULE_DISABLED -DHAL_I2S_MODULE_DISABLED -DHAL_SAI_MODULE_DISABLED -mcpu=cortex-m0plus -DUSE_FULL_LL_DRIVER -c -Os -w -std=gnu++14 -ffunction-sections -fdata-sections -nostdlib -fno-threadsafe-statics --param max-inline-insns-single=500 -fno-rtti -fno-exceptions -fno-use-cxa-atexit -faggressive-loop-optimizations ; OK 24 | ;build_flags = -c -Os -flto -std=gnu11 -ffunction-sections -fdata-sections -nostdlib --param max-inline-insns-single=500 ;-fexceptions -flto -D PIO_FRAMEWORK_ARDUINO_NANOLIB_FLOAT_PRINTF 25 | ;build_unflags = -fno-exceptions -------------------------------------------------------------------------------- /lib/simplefocDxlCore/dxlUtils.h: -------------------------------------------------------------------------------- 1 | #ifndef DXLUTILS_H 2 | #define DXLUTILS_H 3 | #pragma once 4 | /** @breaf CRC check from robotis 5 | CRC CHECK 6 | Note: http://support.robotis.com/en/product/actuator/dynamixel_pro/communication/crc.htm 7 | **/ 8 | inline unsigned short crc_conversion(unsigned short crc_accum, unsigned char *data_blk_ptr, unsigned short data_blk_size) // http://support.robotis.com/en/product/actuator/dynamixel_pro/communication/crc.htm 9 | { 10 | unsigned short i, j; 11 | unsigned short crc_table[256] = { 12 | 0x0000, 0x8005, 0x800F, 0x000A, 0x801B, 0x001E, 0x0014, 0x8011, 13 | 0x8033, 0x0036, 0x003C, 0x8039, 0x0028, 0x802D, 0x8027, 0x0022, 14 | 0x8063, 0x0066, 0x006C, 0x8069, 0x0078, 0x807D, 0x8077, 0x0072, 15 | 0x0050, 0x8055, 0x805F, 0x005A, 0x804B, 0x004E, 0x0044, 0x8041, 16 | 0x80C3, 0x00C6, 0x00CC, 0x80C9, 0x00D8, 0x80DD, 0x80D7, 0x00D2, 17 | 0x00F0, 0x80F5, 0x80FF, 0x00FA, 0x80EB, 0x00EE, 0x00E4, 0x80E1, 18 | 0x00A0, 0x80A5, 0x80AF, 0x00AA, 0x80BB, 0x00BE, 0x00B4, 0x80B1, 19 | 0x8093, 0x0096, 0x009C, 0x8099, 0x0088, 0x808D, 0x8087, 0x0082, 20 | 0x8183, 0x0186, 0x018C, 0x8189, 0x0198, 0x819D, 0x8197, 0x0192, 21 | 0x01B0, 0x81B5, 0x81BF, 0x01BA, 0x81AB, 0x01AE, 0x01A4, 0x81A1, 22 | 0x01E0, 0x81E5, 0x81EF, 0x01EA, 0x81FB, 0x01FE, 0x01F4, 0x81F1, 23 | 0x81D3, 0x01D6, 0x01DC, 0x81D9, 0x01C8, 0x81CD, 0x81C7, 0x01C2, 24 | 0x0140, 0x8145, 0x814F, 0x014A, 0x815B, 0x015E, 0x0154, 0x8151, 25 | 0x8173, 0x0176, 0x017C, 0x8179, 0x0168, 0x816D, 0x8167, 0x0162, 26 | 0x8123, 0x0126, 0x012C, 0x8129, 0x0138, 0x813D, 0x8137, 0x0132, 27 | 0x0110, 0x8115, 0x811F, 0x011A, 0x810B, 0x010E, 0x0104, 0x8101, 28 | 0x8303, 0x0306, 0x030C, 0x8309, 0x0318, 0x831D, 0x8317, 0x0312, 29 | 0x0330, 0x8335, 0x833F, 0x033A, 0x832B, 0x032E, 0x0324, 0x8321, 30 | 0x0360, 0x8365, 0x836F, 0x036A, 0x837B, 0x037E, 0x0374, 0x8371, 31 | 0x8353, 0x0356, 0x035C, 0x8359, 0x0348, 0x834D, 0x8347, 0x0342, 32 | 0x03C0, 0x83C5, 0x83CF, 0x03CA, 0x83DB, 0x03DE, 0x03D4, 0x83D1, 33 | 0x83F3, 0x03F6, 0x03FC, 0x83F9, 0x03E8, 0x83ED, 0x83E7, 0x03E2, 34 | 0x83A3, 0x03A6, 0x03AC, 0x83A9, 0x03B8, 0x83BD, 0x83B7, 0x03B2, 35 | 0x0390, 0x8395, 0x839F, 0x039A, 0x838B, 0x038E, 0x0384, 0x8381, 36 | 0x0280, 0x8285, 0x828F, 0x028A, 0x829B, 0x029E, 0x0294, 0x8291, 37 | 0x82B3, 0x02B6, 0x02BC, 0x82B9, 0x02A8, 0x82AD, 0x82A7, 0x02A2, 38 | 0x82E3, 0x02E6, 0x02EC, 0x82E9, 0x02F8, 0x82FD, 0x82F7, 0x02F2, 39 | 0x02D0, 0x82D5, 0x82DF, 0x02DA, 0x82CB, 0x02CE, 0x02C4, 0x82C1, 40 | 0x8243, 0x0246, 0x024C, 0x8249, 0x0258, 0x825D, 0x8257, 0x0252, 41 | 0x0270, 0x8275, 0x827F, 0x027A, 0x826B, 0x026E, 0x0264, 0x8261, 42 | 0x0220, 0x8225, 0x822F, 0x022A, 0x823B, 0x023E, 0x0234, 0x8231, 43 | 0x8213, 0x0216, 0x021C, 0x8219, 0x0208, 0x820D, 0x8207, 0x0202}; 44 | 45 | for (j = 0; j < data_blk_size; j++) 46 | { 47 | i = ((unsigned short)(crc_accum >> 8) ^ data_blk_ptr[j]) & 0xFF; 48 | crc_accum = (crc_accum << 8) ^ crc_table[i]; 49 | } 50 | 51 | return crc_accum; 52 | } 53 | 54 | 55 | #endif -------------------------------------------------------------------------------- /lib/simplefocDxlCore/simplefocDxlCore.h: -------------------------------------------------------------------------------- 1 | #ifndef SERVO_SIMPLEFOCDXLCORE_H 2 | #define SERVO_SIMPLEFOCDXLCORE_H 3 | 4 | #include "Arduino.h" 5 | #include 6 | #include 7 | #include 8 | #define EEPROM_ENABLED 9 | 10 | // Add time record of functionnal loops. Optionnal for debug 11 | #define DEBUG_RECORD_TIME 12 | // LED BLINKING TIMEOUT IN FAULT MODE 13 | #define ERROR_BLINKING_TIMEOUT 200 14 | 15 | 16 | // LED BLINKING TIMEOUT IN FAULT MODE 17 | #define ERROR_BLINKING_TIMEOUT 200 18 | 19 | class simplefocDxlCore 20 | { 21 | public: 22 | // *** Functions 23 | 24 | // Constructor 25 | simplefocDxlCore(BLDCMotor *_motor); 26 | // INIT 27 | void init(); 28 | /* 29 | Update the dxl core 30 | */ 31 | void update(); 32 | /* 33 | attach the serial port to the dxlCore 34 | @param &serial Serial port 35 | */ 36 | void attachSerial(HardwareSerial &serial); 37 | /* 38 | attach other Hardware 39 | @param1 LED_PIN 40 | @param2 TEMPERATURE_PIN 41 | @param3 INPUT_VOLTAGE_PIN 42 | */ 43 | 44 | void attachHarware(byte nrst_drv_pin, 45 | byte nslp_drv_pin, 46 | byte fault_drv_pin, 47 | byte led_pin, 48 | byte temperature_pin, 49 | byte input_voltage_pin); 50 | 51 | // Factory reset memory 52 | void factoryResetMem(); 53 | 54 | // load default parameters in memory 55 | void loadDefaultMem(); 56 | 57 | // Execute the command in the DXL packet 58 | void executePacketCommand(); 59 | 60 | // Blink status led 61 | void blinkStatus(uint8_t nb, uint16_t delay_); 62 | 63 | // SET FAULT MODE 64 | // @param mode : 1=fault 0=normal 65 | void setFaultMode(bool mode); 66 | 67 | // *** Variables 68 | private: 69 | // *** Functions 70 | 71 | // Refresh data from motor and update DXL memory in RAM 72 | void refreshRAMData(); 73 | 74 | // Refresh Current data from Device 75 | void refreshPresentData(); 76 | // Update parameters from com to memory 77 | void update_parameters(); 78 | 79 | // *** Variables 80 | 81 | // Simplefoc motor 82 | BLDCMotor *motor; 83 | 84 | // DXL memory 85 | dxlMemory dxlmem; 86 | // DXL com handler 87 | dxlCom dxlcom; 88 | 89 | // Parameter not yet inserted in memory 90 | bool pending_parameter = false; 91 | 92 | // *** PIN Storage 93 | 94 | // LED PIN 95 | byte _led_pin; 96 | // TEMPERATURE PIN 97 | byte _temp_pin; 98 | // INPUT VOLTAGE 99 | byte _in_voltage; 100 | 101 | 102 | // DRIVER SPECIAL PINS 103 | 104 | // NOT RESET PIN 105 | byte _nrst_drv_pin; 106 | // NOT SLEEP PIN 107 | byte _nslp_drv_pin; 108 | // FAULT PIN 109 | byte _fault_drv_pin; 110 | 111 | #ifdef DEBUG_RECORD_TIME 112 | // Time record 113 | long time_record; 114 | #endif 115 | 116 | // Fault_mode_time 117 | long fault_mode_time; 118 | 119 | // Refresh Counter 120 | uint8_t rcount = 0; 121 | 122 | // Hardware Error flag 123 | byte hardware_error; 124 | 125 | // Fault Mode 126 | bool fault_mode = false; 127 | 128 | }; 129 | 130 | #endif -------------------------------------------------------------------------------- /src/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include "simplefocDxlCore.h" 7 | 8 | 9 | // SERIAL 10 | #define SERIAL_BAUDRATE 1000000 11 | // I2C 12 | 13 | #define I2C_SPEED 400000 14 | 15 | 16 | 17 | #define DRV_IN1 PA6 18 | #define DRV_IN2 PA7 19 | #define DRV_IN3 PB0 20 | #define DRV_EN PB1 21 | #define DRV_NSLP PB2 22 | #define DRV_FLT PB15 23 | #define DRV_NRST PB13 24 | #define STS_LED PB14 25 | #define INVOLTAGE_PIN PA0 26 | #define TEMPERATURE_PIN PA1 27 | 28 | MagneticSensorI2CConfig_s MySensorConfig = { 29 | .chip_address = 0x40, 30 | .bit_resolution = 12, 31 | .angle_register = 0x0C, 32 | .data_start_bit = 11}; 33 | MagneticSensorI2C sensor = MagneticSensorI2C(MySensorConfig); 34 | // BLDC motor & driver instance 35 | BLDCMotor motor = BLDCMotor(7); 36 | BLDCDriver3PWM driver = BLDCDriver3PWM(DRV_IN1, DRV_IN2, DRV_IN3, DRV_EN); 37 | // COM 38 | #ifdef HALF_DUPLEX_MODE 39 | HardwareSerial Serial1(PA9); 40 | #else 41 | HardwareSerial Serial1(PA10, PA9); 42 | #endif 43 | 44 | // DYNAMIXEL DEVICE 45 | simplefocDxlCore mydxl(&motor); 46 | 47 | // I2C 48 | // SDA SCL 49 | TwoWire Wire2(PB11, PB10); 50 | // SETUP 51 | void setup() 52 | { 53 | 54 | 55 | // Attach Hardware 56 | mydxl.attachHarware(DRV_NRST, DRV_NSLP, DRV_FLT, STS_LED, TEMPERATURE_PIN, INVOLTAGE_PIN); 57 | 58 | // initialise magnetic sensor hardware 59 | sensor.init(&Wire2); 60 | Wire2.setClock(I2C_SPEED); 61 | // link the motor to the sensor 62 | motor.linkSensor(&sensor); 63 | // OK 64 | 65 | // driver config 66 | // power supply voltage [V] 67 | driver.voltage_power_supply = 12; 68 | driver.init(); 69 | // link driver 70 | motor.linkDriver(&driver); 71 | 72 | // control loop type and torque mode 73 | // motor.torque_controller = TorqueControlType::foc_current; 74 | motor.controller = MotionControlType::angle; 75 | // motor.motion_downsample = 0.0; 76 | 77 | // velocity loop PID 78 | motor.PID_velocity.P = 0.2; 79 | motor.PID_velocity.I = 1.0; 80 | motor.PID_velocity.D = 0.0; 81 | motor.PID_velocity.output_ramp = 0.0; 82 | motor.PID_velocity.limit = 300.0; 83 | 84 | // Low pass filtering time constant 85 | motor.LPF_velocity.Tf = 0.02; 86 | // angle loop PID 87 | motor.P_angle.P = 50.0; 88 | motor.P_angle.I = 500.0; 89 | motor.P_angle.D = 0.001; 90 | motor.P_angle.output_ramp = 0.0; 91 | motor.P_angle.limit = 50.0; 92 | // Low pass filtering time constant 93 | motor.LPF_angle.Tf = 0.02; 94 | 95 | motor.P_angle.limit = 100.0; 96 | 97 | // motor phase resistance 98 | // motor.phase_resistance = 4.823; 99 | // pwm modulation settings 100 | motor.foc_modulation = FOCModulationType::SinePWM; 101 | // motor.modulation_centered = 1.0; 102 | 103 | // Limits 104 | motor.velocity_limit = 1000.0 / 9.5493; // Rad/s -> RPM : 1000RPM 105 | motor.voltage_limit = 4.0; 106 | motor.current_limit = 0.0; 107 | 108 | // use monitoring with serial for motor init 109 | // monitoring port 110 | #ifdef HALF_DUPLEX_MODE 111 | Serial1.setHalfDuplex(); 112 | #endif 113 | Serial1.begin(SERIAL_BAUDRATE); 114 | mydxl.attachSerial(Serial1); 115 | 116 | 117 | // INIT DXL DEVICE 118 | mydxl.init(); 119 | _delay(200); 120 | } 121 | 122 | void loop() 123 | { 124 | 125 | 126 | 127 | motor.loopFOC(); 128 | motor.move(); 129 | 130 | // Update DXL Device 131 | mydxl.update(); 132 | 133 | } 134 | -------------------------------------------------------------------------------- /lib/simplefocDxlCore/dxlCom.h: -------------------------------------------------------------------------------- 1 | #ifndef DXLCOM_H 2 | #define DXLCOM_H 3 | 4 | // @todo: faire une classe packet 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | // COM 11 | #define HALF_DUPLEX_MODE 12 | 13 | // COM BUFFERS 14 | #define PACKET_BUFFER_SIZE 180 15 | 16 | //— Instruction — 17 | #define INST_PING 0x01 18 | #define INST_READ 0x02 19 | #define INST_WRITE 0x03 20 | #define INST_REG_WRITE 0x04 21 | #define INST_ACTION 0x05 22 | #define INST_STATUS 0x55 23 | #define INST_FACTORY_RESET 0x06 24 | #define INST_REBOOT 0x08 25 | #define INST_SYSTEM_READ 0x0C 26 | #define INST_SYSTEM_WRITE 0x0D 27 | #define INST_SYNC_WRITE 0x83 28 | #define INST_SYNC_REG_WRITE 0x84 29 | 30 | #define P_HEADER_1 0xFF 31 | #define P_HEADER_2 0xFF 32 | #define P_HEADER_3 0xFD 33 | #define P_RESERVED 0x00 34 | 35 | // PACKET MANAGEMENT 36 | #define PARAM_GAP 8 37 | /* 38 | This class is a packet description with everything to decode or recode 39 | */ 40 | class dxlPacket 41 | { 42 | public: 43 | // Constructor 44 | dxlPacket(); 45 | // Clear packet 46 | void clear(); 47 | 48 | /* Return true if the packet is complete 49 | @return true if the packet is complete 50 | */ 51 | bool isComplete() { return _complete; } 52 | /* 53 | Store a byte in the packet. The goal here is to decode an incoming message 54 | @param b byte to decode 55 | */ 56 | uint8_t storeByte(byte b); 57 | /* 58 | To check what kind of packet it is 59 | @return type 0 : command packet 1 : status packet 60 | */ 61 | uint8_t getType() { return _type; } 62 | /* 63 | Define the type of packet 64 | @param type 0 : command packet 1 : status packet 65 | */ 66 | void setType(uint8_t type) { _type = type; } 67 | 68 | // Data raw buffer 69 | uint8_t buffer[PACKET_BUFFER_SIZE]; 70 | 71 | /* Push data into last element of the buffer 72 | @param element data to push 73 | @return error true if data does not fit 74 | */ 75 | bool pushback(char element); 76 | /* Pop back info 77 | @return the first element of the packet 78 | */ 79 | char pop_front(); 80 | /* Check if the packet is empty or not 81 | @return true if the packet is empty 82 | */ 83 | bool isEmpty(); 84 | /* Get the size of the buffer 85 | @return size : length of the buffer 86 | */ 87 | uint16_t size() { return currentSize; } 88 | /* Set ID of the DXL device 89 | @param id ID of the packet's DXL device 90 | */ 91 | void setId(uint8_t id) { buffer[4] = id; } 92 | /* Get current packet's device ID of the packet 93 | @return id of the packet's device ID 94 | */ 95 | uint8_t getId() { return buffer[4]; } 96 | /* Close packet and fill with informations 97 | */ 98 | void close(); 99 | /* Get instruction of the packet 100 | @return instruction 101 | */ 102 | uint8_t instruction() { return buffer[7]; } 103 | // size of the buffer 104 | uint16_t currentSize; 105 | // crc error for input packet 106 | byte protocol_error; 107 | 108 | private: 109 | // Packet type: type 0 = command packet / 1 = status packet 110 | uint8_t _type; 111 | // is complete flag 112 | bool _complete; 113 | 114 | // Current reading index 115 | uint16_t idx; 116 | 117 | // Expected size 118 | uint16_t ExpectedSize; 119 | // crc 120 | uint16_t crc; 121 | }; 122 | 123 | // inline unsigned char param[PARAMSIZE]; 124 | class dxlCom 125 | { 126 | public: 127 | // FUNCTIONS 128 | 129 | // Constructor 130 | dxlCom(); 131 | 132 | /* Attach serial port to the communication handler 133 | */ 134 | void attach(HardwareSerial &serial); 135 | /* Packet availability 136 | @return true if available 137 | */ 138 | bool packetAvailable(); 139 | /* Send the status packet will all informations 140 | */ 141 | void sendOutPacket(); 142 | /* Close Status packet, fill with all informations needed to be correct 143 | */ 144 | void closeStatusPacket(); 145 | /* Set ID of the attached device 146 | @param id ID of the packet 147 | */ 148 | void setId(unsigned int _id); 149 | 150 | // *** VARIABLES 151 | 152 | // Hardware Serial stream port 153 | HardwareSerial *dxlserial = nullptr; 154 | 155 | // check serial data 156 | void checkSerial(); 157 | 158 | // Incomming packet 159 | dxlPacket inPacket; 160 | // Output packet 161 | dxlPacket outPacket; 162 | 163 | private: 164 | // An input packet is available 165 | bool packetIsAvailable = false; 166 | }; 167 | 168 | #endif 169 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # SimpleFocDXLCore : SimpleFoc running Dynamixel Protocol 2 | 3 | ## Purpose 4 | 5 | The goal of this library is to use SimpleFoc as a Dynamixel device.  6 | This is a Work In Progress and quickly coded, not a final version. However I believe, it can be a good start in case you need to explore this topic. 7 | 8 | 9 | Here is a dirty video to show the devices: 10 | 11 | [](https://www.youtube.com/watch?v=e_fn1X6Afq0) 12 | 13 | 14 | ## Actual device 15 | Today the device is a STM32G030C8T6 + DRV8313 but I tried to keep the code quite usable under other device. Just be carefull with the EEPROM parameter in library. 16 | 17 | ## How to use it? 18 | This was coded and use the platformio generics structure. So use the /lib folder and add to you main code: 19 | 20 | - include library 21 | 22 | 23 | In setup: 24 | 25 | - your device linked to simpleFoc motor: simplefocDxlCore mydxl(&motor); 26 | - link some extra hardware if used: mydxl.attachHarware(DRV_NRST, DRV_NSLP, DRV_FLT, STS_LED, TEMPERATURE_PIN, INVOLTAGE_PIN); 27 | I think you can add dummy pin for STS_LED and Temperature pin 28 | - link the serial port : mydxl.attachSerial(Serial1); 29 | 30 | 31 | In Loop: 32 | 33 | - update : mydxl.update(); 34 | Note: do not hesitate to check the main.cpp file as an example. 35 | 36 | ## Functionnal Dynamixel Features 37 | ### COMMANDS 38 | 39 | See \[https://emanual.robotis.com/docs/en/dxl/protocol2/#instruction-packet\](Dynamixel Protocol 2.0) 40 | 41 | - [x] 0x01 Ping Instruction 42 | - [x] 0x02 Read Instruction to read data from the Device 43 | - [x] 0x03 Write Instruction to write data on the Device 44 | - [ ] 0x04 Reg Write 45 | - [ ] 0x05 Action 46 | - [x] 0x06 Factory Reset 47 | - [x] 0x08 Reboot Instruction to reboot the Device 48 | - [ ] 0x10 Clear Instruction to reset certain information 49 | - [ ] 0x20 Control Table Backup 50 | - [x] 0x55 Status 51 | - [ ] 0x82 Sync Read 52 | - [ ] 0x83 Sync Write 53 | - [ ] 0x8A Fast Sync Read 54 | - [ ] 0x92 Bulk Read 55 | - [ ] 0x93 Bulk Write 56 | - [ ] 0x9A Fast Bulk Read 57 | 58 | ### ERRORS 59 | 60 | #### Protocol Errors 61 | 62 | - [ ] 0x01 Result Fail Failed to process the sent Instruction Packet 63 | - [x] 0x02 Instruction Error Undefined Instruction has been used 64 | - [x] 0x03 CRC Error CRC of the sent Packet does not match 65 | - [ ] 0x04 Data Range Error Data 66 | - [ ] 0x05 Data Length Error Attempt to write Data that is shorter than the data length 67 | - [ ] 0x06 Data Limit Error Data 68 | - [ ] 0x07 Access Error Attempt 69 | 70 | #### Hardware errors 71 | 72 | - [x] Bit 7 - Unused, Always ‘0’ 73 | - [x] Bit 6 - Unused, Always ‘0’ 74 | - [ ] Bit 5 Overload Error 75 | - [x] Bit 4 Electrical Shock Error 76 | - [ ] Bit 3 Motor Encoder Error Detects malfunction of the motor encoder 77 | - [x] Bit 2 Overheating Error 78 | - [x] Bit 1 - Unused, Always ‘0’ 79 | - [x] Bit 0 Input Voltage Error 80 | 81 | ### MEMORY XM430 82 | 83 | See \[https://emanual.robotis.com/docs/en/dxl/x/xm430-w210/#hardware-error-status\](XM430 Specific instructions) 84 | 85 | #### EEPROM 86 | 87 | Name / Address 88 | 89 | - [x] MODEL_NUMBER 0 90 | - [x] MODEL_INFORMATION 2 91 | - [x] VERSION\_OF\_FIRMWARE 6 92 | - [x] ID 7 93 | - [ ] BAUDRATE 8 94 | - [ ] RETURN\_DELAY\_TIME 9 95 | - [ ] DRIVE_MODE 10 96 | - [ ] OPERATING_MODE 11 97 | - [ ] SECONDARY_ID 12 98 | - [ ] PROTOCOL_VERSION 13 99 | - [ ] HOMING_OFFSET 20 100 | - [ ] MOVING_THRESHOLD 24 101 | - [ ] TEMPERATURE_LIMIT 31 102 | - [ ] MAX\_VOLTAGE\_LIMIT 32 103 | - [ ] MIN\_VOLTAGE\_LIMIT 34 104 | - [ ] PWM_LIMIT 36 105 | - [ ] CURRENT_LIMIT 38 106 | - [ ] ACCELERATION_LIMIT 40 107 | - [x] VELOCITY_LIMIT 44 108 | - [ ] MAX\_POSITION\_LIMIT 48 109 | - [ ] MIN\_POSITION\_LIMIT 52 110 | - [x] STARTUP CONFIGURATION 60 \[Only torque on startup) 111 | - [ ] SHUTDOWN 63 112 | 113 | #### RAM 114 | 115 | - [x] TORQUE_ENABLE 64 116 | - [x] #define ADD_LED 65 (PEUT ETRE AMELIORER 117 | - [ ] STATUS\_RETURN\_LEVEL 68 118 | - [ ] REGISTERED_INSTRUCTION 69 119 | - [x] HARDWARE\_ERROR\_STATUS 70 120 | - [x] VELOCITY\_I\_GAIN 76 121 | - [x] VELOCITY\_P\_GAIN 78 122 | - [x] POSITION\_D\_GAIN 80 123 | - [x] POSITION\_I\_GAIN 82 124 | - [x] POSITION\_P\_GAIN 84 125 | - [ ] FEEDFORWARD\_ACCELERATION\_GAIN 88 126 | - [ ] FEEDFORWARD\_VELOCITY\_GAIN 90 127 | - [ ] BUS_WATCHDOG 98 128 | - [ ] GOAL_PWM 100 129 | - [ ] GOAL_CURRENT 102 130 | - [ ] GOAL_VELOCITY 104 131 | - [ ] PROFILE_ACCELERATION 108 132 | - [ ] PROFILE_VELOCITY 112 133 | - [x] GOAL_POSITION 116 134 | - [ ] REALTIME_TICK 120 135 | - [ ] MOVING 122 136 | - [ ] MOVING_STATUS 123 137 | - [ ] PRESENT_PWM 124 138 | - [ ] PRESENT_CURRENT 126 139 | - [x] PRESENT_VELOCITY 128 140 | - [x] PRESENT_POSITION 132 141 | - [ ] VELOCITY_TRAJECTORY 136 142 | - [ ] POSITION_TRAJECTORY 140 143 | - [x] PRESENT\_INPUT\_VOLTAGE 144 144 | - [x] PRESENT_TEMPERATURE 146 145 | 146 | ## Licence 147 | 148 | SimpleFocDXLCore library is released under the GNU GPL v3, which you can find at http://www.gnu.org/licenses/gpl-3.0.en.html 149 | -------------------------------------------------------------------------------- /lib/simplefocDxlCore/dxlCom.cpp: -------------------------------------------------------------------------------- 1 | #include "dxlCom.h" 2 | 3 | // 4 | 5 | dxlCom::dxlCom() 6 | { 7 | // Set packet type 8 | inPacket.setType(0); // Command packet 9 | outPacket.setType(1); // Status packet 10 | } 11 | 12 | void dxlCom::attach(HardwareSerial &serial) 13 | { 14 | dxlserial = &serial; 15 | } 16 | 17 | bool dxlCom::packetAvailable() 18 | { 19 | return inPacket.isComplete(); 20 | } 21 | 22 | 23 | void dxlCom::checkSerial() 24 | { 25 | #ifdef HALF_DUPLEX_MODE 26 | dxlserial->enableHalfDuplexRx(); 27 | #endif 28 | while (dxlserial->available()) 29 | { 30 | // get the new byte: 31 | char inChar = (char)dxlserial->read(); 32 | uint8_t current_error = inPacket.storeByte(inChar); 33 | } 34 | } 35 | void dxlCom::sendOutPacket() 36 | { 37 | // while (!outPacket.isEmpty()) 38 | dxlserial->write(outPacket.buffer, outPacket.currentSize); 39 | // dxlserial->print((char)outPacket.pop_front()); 40 | 41 | 42 | dxlserial->write(outPacket.buffer, outPacket.currentSize); 43 | 44 | 45 | } 46 | /* function setId() 47 | @param unsigned int _id : Dynamixel device indentifier (id) 48 | */ 49 | void dxlCom::setId(unsigned int _id) 50 | { 51 | inPacket.setId(_id); 52 | outPacket.setId(_id); 53 | } 54 | void dxlCom::closeStatusPacket() 55 | { 56 | outPacket.close(); 57 | } 58 | dxlPacket::dxlPacket() 59 | { 60 | // Initialize 61 | clear(); 62 | } 63 | void dxlPacket::close() 64 | { 65 | 66 | 67 | // Instruction 68 | buffer[7] = 0x55; 69 | 70 | // Protocol error 71 | buffer[8] = protocol_error; 72 | 73 | // Length 74 | uint16_t packetSize = currentSize - 5; // 4bytes headers + 1 ID + 2 length + 1 Error - 2 CRC + 1 because of size+1 75 | buffer[5] = packetSize & 0xFF; // Length L 76 | buffer[6] = packetSize >> 8; // Length H 77 | 78 | // CRC 79 | unsigned short crc = crc_conversion(0, buffer, currentSize); 80 | buffer[currentSize] = crc & 0xFF; 81 | currentSize++; 82 | buffer[currentSize] = crc >> 8; 83 | currentSize++; 84 | } 85 | // Clear packet 86 | void dxlPacket::clear() 87 | { 88 | // Initialize the exportation index 89 | idx = 0; 90 | // Reset complete flag 91 | _complete = false; 92 | 93 | if (_type == 1) // Is that a status packet? 94 | { 95 | // Initialize the parameter index 96 | // Initialize 97 | buffer[0] = (P_HEADER_1); 98 | buffer[1] = (P_HEADER_2); 99 | buffer[2] = (P_HEADER_3); 100 | buffer[3] = (P_RESERVED); 101 | buffer[4] = 0; // ID 102 | // buffer[5] = (0); // length 103 | // buffer[6] = (0); // length 104 | // buffer[7] = 0 push_back(0); 105 | 106 | currentSize = 9; // Command packet does includes an ERROR byte 107 | } 108 | else 109 | currentSize = 0; 110 | // Clear error 111 | protocol_error = 0x00; 112 | 113 | } 114 | bool dxlPacket::pushback(char element) 115 | { 116 | if (idx < PACKET_BUFFER_SIZE) 117 | { 118 | buffer[idx] = element; 119 | idx++; 120 | return false; 121 | } 122 | 123 | // Issue 124 | return true; 125 | } 126 | /* Pop back info 127 | @return the first element of the packet 128 | */ 129 | char dxlPacket::pop_front() 130 | { 131 | if (idx < PACKET_BUFFER_SIZE) 132 | { 133 | // Increment le reading buffer 134 | idx++; 135 | } 136 | else 137 | return 0xFF; 138 | 139 | return buffer[idx - 1]; 140 | 141 | } 142 | /* Check if the packet is empty or not 143 | @return true if the packet is empty 144 | */ 145 | bool dxlPacket::isEmpty() 146 | { 147 | // If the reading buffer is > to the buffer size then the packet is empty 148 | if (idx >= currentSize) 149 | return true; 150 | // Not empty 151 | return false; 152 | } 153 | uint8_t dxlPacket::storeByte(byte b) 154 | { 155 | 156 | // Add the byte to the buffer 157 | buffer[currentSize] = b; 158 | 159 | bool abortPacket = false; 160 | if (((currentSize == 0) && (b != P_HEADER_1)) || 161 | ((currentSize == 1) && (b != P_HEADER_2)) || 162 | ((currentSize == 2) && (b != P_HEADER_3)) || 163 | ((currentSize == 3) && (b != P_RESERVED))) 164 | { 165 | abortPacket = true; 166 | } 167 | else if (currentSize == 5) 168 | { 169 | ExpectedSize = b; 170 | } 171 | else if (currentSize == 6) 172 | { 173 | ExpectedSize = ExpectedSize + (unsigned int)(b << 8); 174 | } 175 | else if (currentSize > 7) 176 | { 177 | //[HEADERS ID 178 | if (currentSize - 7 < ExpectedSize - 2) 179 | { // this is a parameter 180 | } 181 | else 182 | { // we should be in CRC here 183 | if (currentSize - 8 - (ExpectedSize - 3) == 0) 184 | { 185 | crc = b; 186 | } 187 | else if (currentSize - 8 - (ExpectedSize - 3) == 1) 188 | { 189 | crc += b << 8; 190 | 191 | unsigned short crcc = crc_conversion(0, buffer, currentSize - 1); 192 | 193 | if (crc == crcc) 194 | { 195 | // Packet is complete and crc is OK 196 | _complete = true; 197 | return 0; 198 | 199 | } 200 | else 201 | { 202 | // abortPacket = true; 203 | // Error is CRC error 204 | protocol_error |= 0x03; 205 | } 206 | } 207 | else 208 | abortPacket = true; 209 | } 210 | } 211 | if (abortPacket) 212 | { // IGNORE PACKET 213 | clear(); 214 | return 1; 215 | } 216 | else 217 | { 218 | currentSize++; 219 | if (currentSize > PACKET_BUFFER_SIZE) 220 | clear(); 221 | } 222 | return 0; 223 | } 224 | -------------------------------------------------------------------------------- /lib/simplefocDxlCore/dxlMemory.cpp: -------------------------------------------------------------------------------- 1 | #include "dxlMemory.h" 2 | 3 | dxlMemory::dxlMemory() { clear(); } 4 | 5 | bool dxlMemory::load() 6 | { 7 | return loadEEPROM(); 8 | } 9 | 10 | /* 11 | bool dxlMemory::CheckEEPROM_CRC() 12 | { 13 | eeprom_buffer_fill(); 14 | // GET CRC 15 | unsigned short crc = crc_conversion(0, DXL_data, EEPROM_ADDRESSES - 1); // CRC WITHOUT CRC VALUE IN EEPROM (beginning after EEPROM_FIRST_ADDRESS ) 16 | char crcl = eeprom_buffered_read_byte(EEPROM_CRCL); 17 | char crch = eeprom_buffered_read_byte(EEPROM_CRCH); 18 | unsigned short crc_in_eeprom = crcl + (crch << 8); 19 | // EEPROM GOT THE RIGHT CRC ?? 20 | if ((crc == crc_in_eeprom) && (crc != 0)) 21 | return false; 22 | 23 | // ELSE 24 | return true; 25 | } 26 | */ 27 | // EEPROM MANAGEMENT 28 | bool dxlMemory::loadEEPROM() 29 | { 30 | 31 | eeprom_buffer_fill(); 32 | for (uint16_t i = 0; i < EEPROM_ADDRESSES; i++) 33 | { 34 | if (i + EEPROM_FIRST_ADDRESS > EEPROM_LENGTH) 35 | return true; 36 | //*(data + i) = EEPROM.read(i + EEPROM_FIRST_ADDRESS); 37 | *(DXL_data + i) = eeprom_buffered_read_byte(i + EEPROM_FIRST_ADDRESS); 38 | } 39 | // Check CRC 40 | unsigned short crc = crc_conversion(0, DXL_data, EEPROM_ADDRESSES - 1); // CRC WITHOUT CRC VALUE IN EEPROM (beginning after EEPROM_FIRST_ADDRESS ) 41 | char crcl = eeprom_buffered_read_byte(EEPROM_CRCL); 42 | char crch = eeprom_buffered_read_byte(EEPROM_CRCH); 43 | unsigned short crc_in_eeprom = crcl + (crch << 8); 44 | // EEPROM GOT THE RIGHT CRC ?? 45 | if ((crc != crc_in_eeprom) || (crc == 0)) 46 | return true; // ISSUE 47 | 48 | // OK 49 | return false; 50 | } 51 | 52 | void dxlMemory::data2EEPROM() 53 | { 54 | for (uint16_t i = 0; i < EEPROM_ADDRESSES; i++) 55 | { 56 | // EEPROM.update(i + EEPROM_FIRST_ADDRESS, *(data + i)); 57 | eeprom_buffered_write_byte(i + EEPROM_FIRST_ADDRESS, *(DXL_data + i)); 58 | } 59 | 60 | // Compute and store CRC 61 | uint16_t crc = DataDynCRC(); 62 | eeprom_buffered_write_byte(EEPROM_CRCL, crc & 0xFF); 63 | eeprom_buffered_write_byte(EEPROM_CRCH, crc >> 8); 64 | 65 | // Store into EEPROM 66 | eeprom_buffer_flush(); 67 | } 68 | 69 | void dxlMemory::clear() 70 | { 71 | for (uint16_t i = 0; i < DXL_DATA_SIZE; i++) 72 | *(DXL_data + i) = 0; 73 | } 74 | 75 | uint8_t dxlMemory::getValueFromDxlData(uint16_t address) 76 | { 77 | return *(DXL_data + address); 78 | } 79 | uint32_t dxlMemory::getValueFromDxlData(uint16_t address, uint8_t len) 80 | { 81 | uint32_t tmp = 0; 82 | if (address + len > DXL_DATA_SIZE) 83 | return 0; 84 | for (uint8_t i = 0; i < len; i++) 85 | { 86 | tmp = tmp | (*(DXL_data + address + i) << (8 * i)); 87 | } 88 | 89 | return tmp; 90 | } 91 | void dxlMemory::store(uint16_t address, uint8_t value) 92 | { 93 | if (address + 1 < DXL_DATA_SIZE) 94 | { 95 | *(DXL_data + address) = value & 0xff; 96 | 97 | // IF ROM data, save into EEPROM 98 | #ifdef EEPROM_ENABLED 99 | if (address < EEPROM_ADDRESSES) 100 | { 101 | storeToEEPROM = true; 102 | } 103 | #endif 104 | } 105 | } 106 | void dxlMemory::store(uint16_t address, int value) 107 | { 108 | store(address, (uint8_t)value); 109 | } 110 | void dxlMemory::store(uint16_t address, uint16_t value) 111 | { 112 | if (address + 2 < DXL_DATA_SIZE) 113 | { 114 | for (uint8_t i = 0; i < 2; i++) 115 | { 116 | store(address + i, (uint8_t)((value >> (8 * i)) & 0xff)); 117 | } 118 | } 119 | } 120 | void dxlMemory::store(uint16_t address, uint32_t value) 121 | { 122 | if (address + 4 < DXL_DATA_SIZE) 123 | { 124 | for (uint8_t i = 0; i < 4; i++) 125 | { 126 | store(address + i, (uint8_t)((value >> (8 * i)) & 0xff)); 127 | } 128 | } 129 | } 130 | // memory write with data segment 131 | void dxlMemory::store(uint16_t address, uint16_t wSize, uint8_t *value) 132 | { 133 | if (address + (wSize) < DXL_DATA_SIZE) 134 | { 135 | for (uint8_t i = 0; i < (wSize); i++) 136 | { 137 | store(address + i, (uint8_t) * (value + i)); 138 | } 139 | } 140 | } 141 | // READ Data from data_dyn 142 | void dxlMemory::memRead(uint16_t address, uint16_t *readSize, uint8_t *outData, uint16_t *outDataSize) 143 | { 144 | // Resize max lenght 145 | uint16_t sizetoread = *readSize; 146 | if (address + *readSize >= DXL_DATA_SIZE) 147 | { 148 | sizetoread = DXL_DATA_SIZE - address; 149 | } 150 | for (uint16_t i = 0; i < sizetoread; i++) 151 | { 152 | *(outData + *outDataSize) = *(DXL_data + i + address); 153 | *(outDataSize) += 1; 154 | } 155 | } 156 | void dxlMemory::memRead(uint16_t address, uint16_t readSize, uint8_t *outData, uint16_t *outDataSize) 157 | { 158 | // Resize max lenght 159 | uint16_t sizetoread = readSize; 160 | if (address + readSize >= DXL_DATA_SIZE) 161 | { 162 | sizetoread = DXL_DATA_SIZE - address; 163 | } 164 | for (uint16_t i = 0; i < sizetoread; i++) 165 | { 166 | *(outData + *outDataSize) = *(DXL_data + i + address); 167 | *(outDataSize) += 1; 168 | } 169 | } 170 | 171 | uint8_t dxlMemory::writeEEPROM(uint16_t address, uint8_t value) 172 | { 173 | if (address > EEPROM_LENGTH) 174 | return 0x07; 175 | // EEPROM.write(address + EEPROM_FIRST_ADDRESS, value); 176 | EEPROM[address + EEPROM_FIRST_ADDRESS] = value; 177 | /*eeprom[_buffered_write_byte(address + EEPROM_FIRST_ADDRESS, value); 178 | eeprom_buffer_flush();*/ 179 | // Compute CRC 180 | uint16_t crc = DataDynCRC(); 181 | // WRITE CRC 182 | EEPROM[EEPROM_CRCL] = crc & 0xFF; 183 | EEPROM[EEPROM_CRCH] = crc >> 8; 184 | 185 | return 0; 186 | } 187 | 188 | uint16_t dxlMemory::DataDynCRC() 189 | { 190 | 191 | unsigned short crc = crc_conversion(0, DXL_data, EEPROM_ADDRESSES - 1); 192 | 193 | return crc; 194 | } 195 | -------------------------------------------------------------------------------- /lib/simplefocDxlCore/dxlMemory.h: -------------------------------------------------------------------------------- 1 | #ifndef DXLMEMORY_H 2 | #define DXLMEMORY_H 3 | #include 4 | #include 5 | #include 6 | 7 | //#define LIGHT_VERSION 8 | #define EEPROM_ENABLED 9 | 10 | //#define FLASH_END 0x0801FFFU 11 | #define EEPROM_LENGTH E2END 12 | #define EEPROM_FIRST_ADDRESS 0 13 | #define EEPROM_CRCL EEPROM_FIRST_ADDRESS + 150 14 | #define EEPROM_CRCH EEPROM_FIRST_ADDRESS + 151 15 | #define EEPROM_ADDRESSES 64 16 | 17 | // RAM Storage 18 | #define DXL_DATA_SIZE 150 19 | 20 | // ADDRESSES 21 | // DATA ADDRESSES 22 | #define ADD_MODEL_NUMBER 0 23 | #define ADD_MODEL_INFORMATION 2 24 | #define ADD_VERSION_OF_FIRMWARE 6 25 | #define ADD_ID 7 26 | #define ADD_BAUDRATE 8 27 | #define ADD_RETURN_DELAY_TIME 9 28 | #define ADD_DRIVE_MODE 10 29 | #define ADD_OPERATING_MODE 11 30 | #define ADD_SECONDARY_ID 12 31 | #define ADD_PROTOCOL_VERSION 13 32 | #define ADD_HOMING_OFFSET 20 33 | #define ADD_MOVING_THRESHOLD 24 34 | #define ADD_TEMPERATURE_LIMIT 31 35 | #define ADD_MAX_VOLTAGE_LIMIT 32 36 | #define ADD_MIN_VOLTAGE_LIMIT 34 37 | #define ADD_PWM_LIMIT 36 38 | #define ADD_CURRENT_LIMIT 38 39 | #define ADD_ACCELERATION_LIMIT 40 40 | #define ADD_VELOCITY_LIMIT 44 41 | #define ADD_MAX_POSITION_LIMIT 48 42 | #define ADD_MIN_POSITION_LIMIT 52 43 | #define ADD_STARTUP_CONFIGURATION 60 44 | #define ADD_SHUTDOWN 63 45 | // RAM 46 | #define ADD_TORQUE_ENABLE 64 47 | #define ADD_LED 65 48 | #define ADD_STATUS_RETURN_LEVEL 68 49 | #define ADD_REGISTERED_INSTRUCTION 69 50 | #define ADD_HARDWARE_ERROR_STATUS 70 51 | #define ADD_VELOCITY_I_GAIN 76 52 | #define ADD_VELOCITY_P_GAIN 78 53 | #define ADD_POSITION_D_GAIN 80 54 | #define ADD_POSITION_I_GAIN 82 55 | #define ADD_POSITION_P_GAIN 84 56 | #define ADD_FEEDFORWARD_ACCELERATION_GAIN 88 57 | #define ADD_FEEDFORWARD_VELOCITY_GAIN 90 58 | #define ADD_BUS_WATCHDOG 98 59 | #define ADD_GOAL_PWM 100 60 | #define ADD_GOAL_CURRENT 102 61 | #define ADD_GOAL_VELOCITY 104 62 | #define ADD_PROFILE_ACCELERATION 108 63 | #define ADD_PROFILE_VELOCITY 112 64 | #define ADD_GOAL_POSITION 116 65 | #define ADD_REALTIME_TICK 120 66 | #define ADD_MOVING 122 67 | #define ADD_MOVING_STATUS 123 68 | #define ADD_PRESENT_PWM 124 69 | #define ADD_PRESENT_CURRENT 126 70 | #define ADD_PRESENT_VELOCITY 128 71 | #define ADD_PRESENT_POSITION 132 72 | #define ADD_VELOCITY_TRAJECTORY 136 73 | #define ADD_POSITION_TRAJECTORY 140 74 | #define ADD_PRESENT_INPUT_VOLTAGE 144 75 | #define ADD_PRESENT_TEMPERATURE 146 76 | 77 | /* 78 | #define ADD_EXTERNAL_PORT_DATA_1 152 79 | 80 | #define ADD_EXTERNAL_PORT_DATA_2 154 81 | #define ADD_EXTERNAL_PORT_DATA_3 156 82 | #define ADD_INDIRECT_ADDR_1 168 83 | #define ADD_INDIRECT_DATA_1 224 84 | */ 85 | //#define ADD_INDIRECT_ADDR_29 578 86 | 87 | /* 88 | Change the ram and flash of the Dynamixel device 89 | */ 90 | class dxlMemory 91 | { 92 | public: 93 | // *** Functions 94 | // Contructor of dxl memory 95 | dxlMemory(); 96 | 97 | /* Load parameters from EEPROM or default 98 | @return 1 if memory is not correct (CRC not correct) 99 | */ 100 | bool load(); 101 | /* Check EEPROM integrity with CRC method 102 | @return true if there is an error on CRC read 103 | */ 104 | bool CheckEEPROM_CRC(); 105 | /* Load EEPROMto memory 106 | @return true if there is an error on loading 107 | */ 108 | bool loadEEPROM(); 109 | 110 | // Clear Memory 111 | void clear(); 112 | 113 | /* @brief get value from DXL device memory 114 | @param Dxl device memory address 115 | return value @ address 116 | */ 117 | uint8_t getValueFromDxlData(uint16_t address); 118 | /* @brief get value from DXL device memory 119 | @param1 Dxl device memory address 120 | @param2 data length (maximum 4 Bytes) 121 | return value @ address 122 | */ 123 | uint32_t getValueFromDxlData(uint16_t address, uint8_t len); 124 | /* @brief read device memory and copy to table 125 | @param1 Dxl device memory address 126 | @param2 Size of reading 127 | @param3 Output data table pointer 128 | @param4 Actual size of the table pointer 129 | */ 130 | void memRead(uint16_t address, uint16_t readSize, uint8_t *outData, uint16_t *outDataSize); 131 | /* @brief read device memory and copy to table 132 | @param1 Dxl device memory address 133 | @param2 pointer of size of reading 134 | @param3 Output data table pointer 135 | @param4 Actual size of the table pointer 136 | */ 137 | void memRead(uint16_t address, uint16_t *readSize, uint8_t *outData, uint16_t *outDataSize); 138 | /* @brief Store value at address 139 | @param1 memory address 140 | @param2 value 141 | */ 142 | void store(uint16_t address, uint8_t value); 143 | /* @brief Store value at address [int8_t] 144 | @param1 memory address 145 | @param2 value 146 | */ 147 | void store(uint16_t address, int value); 148 | /* @brief Store value at address [int] 149 | @param1 memory address 150 | @param2 value 151 | */ 152 | void store(uint16_t address, uint16_t value); 153 | /* @brief Store value at address [int16_t] 154 | @param1 memory address 155 | @param2 value 156 | */ 157 | void store(uint16_t address, uint32_t value); 158 | /* @brief Store value at address [data segment] 159 | @param1 memory address 160 | @param2 size of reading 161 | @param3 output table pointer 162 | */ 163 | void store(uint16_t address, uint16_t wSize, uint8_t *value); 164 | 165 | // EEPROM MANAGEMENT 166 | 167 | /* @brief write value into EEPROM 168 | @param1 EEPROM memory address 169 | @param2 value 170 | @return error 171 | */ 172 | uint8_t writeEEPROM(uint16_t address, uint8_t value); 173 | /* @brief Copy DXL data into EEPROM 174 | */ 175 | void data2EEPROM(); 176 | /* @brief get DXL data CRC 177 | @return crc 178 | */ 179 | uint16_t DataDynCRC(); 180 | 181 | // *** Vars 182 | // Stored DXL MEMORY 183 | uint8_t DXL_data[DXL_DATA_SIZE]; 184 | // Flag to store into EEPROM 185 | bool storeToEEPROM = false; 186 | }; 187 | #endif -------------------------------------------------------------------------------- /lib/simplefocDxlCore/simplefocDxlCore.cpp: -------------------------------------------------------------------------------- 1 | #include "simplefocDxlCore.h" 2 | 3 | simplefocDxlCore::simplefocDxlCore(BLDCMotor *_motor) 4 | { 5 | // Associate simplefoc motor 6 | motor = _motor; 7 | 8 | hardware_error = 0; 9 | fault_mode = false; 10 | } 11 | void simplefocDxlCore::attachHarware(byte nrst_drv_pin, 12 | byte nslp_drv_pin, 13 | byte fault_drv_pin, 14 | byte led_pin, 15 | byte temperature_pin, 16 | byte input_voltage_pin) 17 | { 18 | _nrst_drv_pin = nrst_drv_pin; 19 | _nslp_drv_pin = nslp_drv_pin; 20 | _fault_drv_pin = fault_drv_pin; 21 | _led_pin = led_pin; 22 | _temp_pin = temperature_pin; 23 | _in_voltage = input_voltage_pin; 24 | 25 | // Init outputs 26 | pinMode(_fault_drv_pin, INPUT); 27 | pinMode(_nslp_drv_pin, OUTPUT); 28 | pinMode(_nrst_drv_pin, OUTPUT); 29 | pinMode(_led_pin, OUTPUT); 30 | pinMode(_temp_pin, INPUT); 31 | pinMode(_in_voltage, INPUT); 32 | 33 | digitalWrite(_led_pin, LOW); 34 | digitalWrite(_nslp_drv_pin, HIGH); 35 | 36 | // Reset driver sequence 37 | digitalWrite(_nrst_drv_pin, LOW); 38 | delay(10); 39 | digitalWrite(_nrst_drv_pin, HIGH); 40 | } 41 | void simplefocDxlCore::setFaultMode(bool mode) 42 | { 43 | fault_mode = mode; 44 | if (mode) 45 | { 46 | fault_mode_time = millis(); 47 | // Stop motor 48 | motor->disable(); 49 | update_parameters(); 50 | } 51 | 52 | } 53 | void simplefocDxlCore::factoryResetMem() 54 | { 55 | // Load default 56 | loadDefaultMem(); 57 | // Store into EEPROM 58 | #ifdef EEPROM_ENABLED 59 | dxlmem.data2EEPROM(); 60 | #endif 61 | } 62 | void simplefocDxlCore::init() 63 | { 64 | 65 | // Init memory, if memory is not correct make a factory reset 66 | #ifndef EEPROM_ENABLED 67 | blinkStatus(10, 100); 68 | #else 69 | if (dxlmem.load()) 70 | { 71 | 72 | factoryResetMem(); 73 | blinkStatus(5, 100); 74 | // Reboot for safety 75 | NVIC_SystemReset(); 76 | } 77 | else 78 | blinkStatus(2, 500); 79 | #endif 80 | 81 | // blinkStatus(10, 500); 82 | 83 | // Init motor and FOC 84 | // initialise motor 85 | motor->init(); 86 | // align encoder and start FOC 87 | motor->initFOC(); 88 | 89 | if (dxlmem.getValueFromDxlData(ADD_STARTUP_CONFIGURATION, 1) & 0x01) 90 | { 91 | dxlmem.store(ADD_TORQUE_ENABLE, (uint8_t)1); 92 | } 93 | 94 | 95 | // Refresh parameter from simplefoc 96 | refreshRAMData(); 97 | update_parameters(); 98 | } 99 | void simplefocDxlCore::loadDefaultMem() 100 | { 101 | 102 | // Clear memory data 103 | dxlmem.clear(); 104 | 105 | // MODEL NUMBER 106 | dxlmem.store(ADD_MODEL_NUMBER, (uint16_t)0x0406); // Default model : XM430 107 | // MODEL FIRMWARE 108 | 109 | dxlmem.store(ADD_VERSION_OF_FIRMWARE, (uint8_t)0x2D); // Default FW = V45 110 | 111 | // ID 112 | dxlmem.store(ADD_ID, (uint8_t)0x01); // Default ID = 1 113 | // BAUDRATE 114 | dxlmem.store(ADD_BAUDRATE, (uint8_t)0x03); // 1M 115 | // Drive mode 116 | dxlmem.store(ADD_DRIVE_MODE, (uint8_t)0x08); // TORQUE ON GOAL UPDATE 117 | // control mode 118 | dxlmem.store(ADD_OPERATING_MODE, (uint8_t)0x03); // POSITION CONTROL 119 | // protocol mode 120 | dxlmem.store(ADD_PROTOCOL_VERSION, (uint8_t)0x02); // PROTOCOL 2 121 | 122 | // MAX temperature 123 | dxlmem.store(ADD_TEMPERATURE_LIMIT, (uint8_t)0x50); // 80°C 124 | // Max voltage 125 | dxlmem.store(ADD_MAX_VOLTAGE_LIMIT, (uint16_t)0x00C8); // 20V 126 | // Min voltage 127 | dxlmem.store(ADD_MIN_VOLTAGE_LIMIT, (uint16_t)0x0050); // 8V 128 | 129 | // PWM limit 130 | 131 | // CURRENT LIMIT 132 | 133 | // ACCELERATION LIMIT 134 | 135 | // VELOCITY LIMIT 136 | dxlmem.store(ADD_VELOCITY_LIMIT, (uint32_t)(1000 / 0.02398)); 137 | // POSITION LIMIT 138 | dxlmem.store(ADD_MAX_POSITION_LIMIT, (uint16_t)0xFFFFF); 139 | // dxlmem.store(ADD_MIN_POSITION_LIMIT, (uint16_t)0x0); 140 | // TORQUE 141 | // dxlmem.store(ADD_TORQUE_ENABLE, (uint8_t)0x00); // Default Torque is 0 142 | } 143 | 144 | void simplefocDxlCore::attachSerial(HardwareSerial &serial) 145 | { 146 | dxlcom.attach(serial); 147 | } 148 | void simplefocDxlCore::update() 149 | { 150 | #ifdef DEBUG_RECORD_TIME 151 | long temps_DXLC = 0; 152 | if (rcount == 0) 153 | temps_DXLC = micros(); 154 | 155 | #endif 156 | 157 | if (fault_mode) 158 | { 159 | if ((millis() - fault_mode_time) > ERROR_BLINKING_TIMEOUT) 160 | { 161 | // Toggle led 162 | digitalWrite(_led_pin, !digitalRead(_led_pin)); 163 | fault_mode_time = millis(); 164 | } 165 | } 166 | 167 | 168 | // Check incoming serial 169 | dxlcom.checkSerial(); 170 | 171 | // Check if a incoming packet is available 172 | if (dxlcom.packetAvailable()) 173 | { 174 | 175 | // Is this packet for this device? 176 | if (dxlcom.inPacket.getId() == dxlmem.getValueFromDxlData(ADD_ID) || 177 | dxlcom.inPacket.getId() == 0xFE) 178 | { 179 | 180 | // InPacket CRC OK ? 181 | if (dxlcom.inPacket.protocol_error == 0) 182 | { 183 | 184 | // Clear outpacket 185 | dxlcom.outPacket.clear(); 186 | // Execute the packet 187 | executePacketCommand(); 188 | // Update parameter if one is available 189 | if (pending_parameter) 190 | { 191 | update_parameters(); 192 | pending_parameter = false; 193 | } 194 | 195 | // Store in EEPROM if needed 196 | #ifdef EEPROM_ENABLED 197 | if (dxlmem.storeToEEPROM) 198 | { 199 | // remove flag 200 | dxlmem.storeToEEPROM = false; 201 | dxlmem.data2EEPROM(); 202 | // blinkStatus(2, 100); 203 | } 204 | #endif 205 | } 206 | else 207 | { 208 | // Transfer the input packet error decoding to the outpacket 209 | dxlcom.outPacket.protocol_error |= dxlcom.inPacket.protocol_error; 210 | } 211 | // Set ID before sending 212 | dxlcom.outPacket.setId(dxlmem.getValueFromDxlData(ADD_ID)); 213 | // Finish packet 214 | dxlcom.closeStatusPacket(); 215 | 216 | dxlcom.sendOutPacket(); 217 | 218 | } 219 | // Clear packet 220 | dxlcom.inPacket.clear(); 221 | } 222 | 223 | // Update data from motor 224 | if (rcount == 0) 225 | { 226 | refreshPresentData(); 227 | 228 | #ifdef DEBUG_RECORD_TIME 229 | 230 | uint16_t rec_dxl = micros() - temps_DXLC; 231 | uint32_t rec_foc = micros() - time_record - rec_dxl; 232 | 233 | dxlmem.store(ADD_GOAL_VELOCITY, rec_foc); 234 | dxlmem.store(ADD_GOAL_CURRENT, rec_dxl); 235 | #endif 236 | } 237 | 238 | rcount++; 239 | if (rcount >= 200) 240 | { 241 | #ifdef DEBUG_RECORD_TIME 242 | time_record = micros(); 243 | #endif 244 | rcount = 0; 245 | } 246 | } 247 | void simplefocDxlCore::executePacketCommand() 248 | { 249 | 250 | // Execute instructions 251 | if (dxlcom.inPacket.instruction() == INST_PING) 252 | { 253 | // Add model number to the packet 254 | dxlmem.memRead(ADD_MODEL_NUMBER, 2, dxlcom.outPacket.buffer, &(dxlcom.outPacket.currentSize)); 255 | // Add firmware version to the packet 256 | dxlmem.memRead(ADD_VERSION_OF_FIRMWARE, 1, dxlcom.outPacket.buffer, &(dxlcom.outPacket.currentSize)); 257 | } 258 | else if (dxlcom.inPacket.instruction() == INST_READ) 259 | { 260 | // ADDRESS READ 261 | uint16_t address = *(dxlcom.inPacket.buffer + PARAM_GAP) + (*(dxlcom.inPacket.buffer + 1 + PARAM_GAP) << 8); 262 | uint16_t readsize = *(dxlcom.inPacket.buffer + 2 + PARAM_GAP) + (*(dxlcom.inPacket.buffer + 3 + PARAM_GAP) << 8); 263 | // FILL READ DATA 264 | if (readsize + address + dxlcom.outPacket.currentSize + 2 > PACKET_BUFFER_SIZE) 265 | readsize = PACKET_BUFFER_SIZE - address - dxlcom.outPacket.currentSize - 2; 266 | dxlmem.memRead(address, readsize, dxlcom.outPacket.buffer, &(dxlcom.outPacket.currentSize)); 267 | } 268 | else if (dxlcom.inPacket.instruction() == INST_WRITE) 269 | { 270 | // A new parameter is available 271 | pending_parameter = true; 272 | 273 | // Writ address 274 | uint16_t wAddress = *(dxlcom.inPacket.buffer + PARAM_GAP) + (*(dxlcom.inPacket.buffer + 1 + PARAM_GAP) << 8); 275 | // Remove address 4 (CRC + size position) bytes from size and select the right parameters (+2 bytes) 276 | // |L=8:HEADERS,ID,... (PARAM_GAP)|L=2:ADD|L=2:size to read|L=2:CRC| 277 | uint16_t size = dxlcom.inPacket.currentSize - PARAM_GAP - 3; 278 | dxlmem.store(wAddress, (size), (dxlcom.inPacket.buffer + PARAM_GAP + 2)); 279 | 280 | } 281 | else if (dxlcom.inPacket.instruction() == INST_REBOOT) 282 | { 283 | NVIC_SystemReset(); 284 | } 285 | else if (dxlcom.inPacket.instruction() == INST_FACTORY_RESET) 286 | { 287 | factoryResetMem(); 288 | } 289 | else // Undifined instruction 290 | { 291 | dxlcom.outPacket.protocol_error |= 0x02; 292 | } 293 | 294 | } 295 | void simplefocDxlCore::refreshRAMData() 296 | { 297 | dxlmem.store(ADD_VELOCITY_LIMIT, (uint32_t)(motor->PID_velocity.limit)); // / 0.02398 298 | // ADD_VELOCITY_I_GAIN 299 | dxlmem.store(ADD_VELOCITY_I_GAIN, (uint16_t)(motor->PID_velocity.I * 128)); 300 | // ADD_VELOCITY_P_GAIN 301 | dxlmem.store(ADD_VELOCITY_P_GAIN, (uint16_t)(motor->PID_velocity.P * 128)); 302 | // ADD_POSITION_D_GAIN 303 | dxlmem.store(ADD_POSITION_D_GAIN, (uint16_t)(motor->P_angle.D * 128)); 304 | // ADD_POSITION_I_GAIN 305 | dxlmem.store(ADD_POSITION_I_GAIN, (uint16_t)(motor->P_angle.I * 128)); 306 | // ADD_POSITION_P_GAIN 307 | dxlmem.store(ADD_POSITION_P_GAIN, (uint16_t)(motor->P_angle.P * 16)); 308 | refreshPresentData(); 309 | } 310 | void simplefocDxlCore::refreshPresentData() 311 | { 312 | 313 | // ADD_VELOCITY_LIMIT 314 | // 315 | 316 | // ADD_MOVING 317 | // ADD_MOVING_STATUS 318 | // ADD_PRESENT_PWM 319 | // ADD_PRESENT_CURRENT 320 | // dxlmem.store(ADD_PRESENT_CURRENT, (uint32_t)(abs(motor->current_sp*2.69))); 321 | // ADD_PRESENT_VELOCITY 322 | dxlmem.store(ADD_PRESENT_VELOCITY, (uint32_t)(abs(motor->shaft_velocity / 0.02398))); 323 | // ADD_PRESENT_POSITION 324 | dxlmem.store(ADD_PRESENT_POSITION, (uint32_t)(motor->shaft_angle / (2 * 3.1415 / 4095))); 325 | // ADD_VELOCITY_TRAJECTORY 326 | // ADD_POSITION_TRAJECTORY 327 | 328 | #ifndef LIGHT_VERSION 329 | // ADD_PRESENT_INPUT_VOLTAGE 330 | // R2 = 2200 / R1 = 10000 / MCU voltage 3.3V / DXL multiplier 10 331 | uint16_t involtage = (double)(analogRead(_in_voltage) * 0.18); 332 | dxlmem.store(ADD_PRESENT_INPUT_VOLTAGE, (uint16_t)(involtage)); 333 | 334 | 335 | // Involtage error handling 336 | if ((involtage < dxlmem.getValueFromDxlData(ADD_MIN_VOLTAGE_LIMIT, 2)) || 337 | (involtage > dxlmem.getValueFromDxlData(ADD_MAX_VOLTAGE_LIMIT, 2))) 338 | hardware_error |= 0x01; 339 | 340 | 341 | // ADD_PRESENT_TEMPERATURE MCP9700T-E/TT 342 | float voltage = float(analogRead(_temp_pin)) * 3300.0 / 1024.0; 343 | float temperature = (voltage - 500.0) / 10; 344 | // uint8_t temperature = (double)(analogRead(_temp_pin) * 0.0806); 345 | dxlmem.store(ADD_PRESENT_TEMPERATURE, (uint8_t)temperature); 346 | 347 | // Overheating error handling 348 | if (temperature > dxlmem.getValueFromDxlData(ADD_TEMPERATURE_LIMIT)) 349 | hardware_error |= 0x04; 350 | 351 | // Hardware error on the driver 352 | if (!digitalRead(_fault_drv_pin)) 353 | hardware_error |= 0x10; 354 | 355 | // Collect error and store it 356 | if (hardware_error != dxlmem.getValueFromDxlData(ADD_HARDWARE_ERROR_STATUS)) 357 | { 358 | dxlmem.store(ADD_HARDWARE_ERROR_STATUS, (uint8_t)hardware_error); 359 | } 360 | 361 | #endif 362 | 363 | if ((dxlmem.getValueFromDxlData(ADD_HARDWARE_ERROR_STATUS) > 0) && !fault_mode) 364 | setFaultMode(true); 365 | 366 | } 367 | 368 | void simplefocDxlCore::update_parameters() 369 | { 370 | 371 | // ADD_MAX_VOLTAGE_LIMIT 372 | // ADD_MIN_VOLTAGE_LIMIT 373 | // ADD_PWM_LIMIT 374 | // ADD_CURRENT_LIMIT 375 | // ADD_ACCELERATION_LIMIT 376 | // ADD_VELOCITY_LIMIT 377 | volatile uint32_t stored_vel = dxlmem.getValueFromDxlData(ADD_VELOCITY_LIMIT, 4); 378 | double velocity = stored_vel; // * 0.02398; 379 | motor->PID_velocity.limit = (double)velocity; 380 | // ADD_MAX_POSITION_LIMIT 381 | // ADD_MIN_POSITION_LIMIT 382 | // ADD_SHUTDOWN 383 | 384 | //***RAM 385 | if (!fault_mode) 386 | { 387 | // ADD_TORQUE_ENABLE 388 | if (dxlmem.getValueFromDxlData(ADD_TORQUE_ENABLE, 1)) 389 | { 390 | motor->enable(); 391 | } 392 | else 393 | motor->disable(); 394 | 395 | // ADD_LED 396 | if (dxlmem.getValueFromDxlData(ADD_LED, 1) == 1) 397 | { 398 | digitalWrite(_led_pin, HIGH); 399 | } 400 | else 401 | digitalWrite(_led_pin, LOW); 402 | } 403 | else // IN FAULT MODE 404 | { 405 | if (dxlmem.getValueFromDxlData(ADD_TORQUE_ENABLE, 1)) 406 | dxlmem.store(ADD_TORQUE_ENABLE, (uint8_t)0); 407 | if (dxlmem.getValueFromDxlData(ADD_LED, 1)) 408 | dxlmem.store(ADD_LED, (uint8_t)0); 409 | } 410 | 411 | // ADD_HARDWARE_ERROR_STATUS 412 | // ADD_VELOCITY_I_GAIN 413 | motor->PID_velocity.I = (double)dxlmem.getValueFromDxlData(ADD_VELOCITY_I_GAIN, 2) / 128; 414 | // ADD_VELOCITY_P_GAIN 415 | motor->PID_velocity.P = (double)dxlmem.getValueFromDxlData(ADD_VELOCITY_P_GAIN, 2) / 128; 416 | // ADD_POSITION_D_GAIN 417 | motor->P_angle.D = (double)dxlmem.getValueFromDxlData(ADD_POSITION_D_GAIN, 2) / 128; 418 | // ADD_POSITION_I_GAIN 419 | motor->P_angle.I = (double)dxlmem.getValueFromDxlData(ADD_POSITION_I_GAIN, 2) / 128; 420 | // ADD_POSITION_P_GAIN 421 | motor->P_angle.P = (double)dxlmem.getValueFromDxlData(ADD_POSITION_P_GAIN, 2) / 16; 422 | // ADD_FEEDFORWARD_ACCELERATION_GAIN 423 | // ADD_FEEDFORWARD_VELOCITY_GAIN 424 | // ADD_BUS_WATCHDOG 425 | // ADD_GOAL_PWM 426 | // ADD_GOAL_CURRENT 427 | // ADD_GOAL_VELOCITY 428 | // ADD_PROFILE_ACCELERATION 429 | // ADD_PROFILE_VELOCITY 430 | // ADD_GOAL_POSITION 431 | motor->target = (double)((uint32_t)dxlmem.getValueFromDxlData(ADD_GOAL_POSITION, 4)) * 2 * 3.1415 / 4095; 432 | 433 | // ADD_REALTIME_TICK 434 | 435 | //*** FEEDBACK 436 | // ADD_MOVING 437 | // ADD_MOVING_STATUS 438 | // ADD_PRESENT_PWM 439 | // ADD_PRESENT_CURRENT 440 | // ADD_PRESENT_VELOCITY 441 | 442 | // ADD_PRESENT_POSITION 443 | // ADD_VELOCITY_TRAJECTORY 444 | // ADD_POSITION_TRAJECTORY 445 | // ADD_PRESENT_INPUT_VOLTAGE 446 | 447 | // ADD_PRESENT_TEMPERATURE 448 | } 449 | 450 | void simplefocDxlCore::blinkStatus(uint8_t nb, uint16_t delay_) 451 | { 452 | for (uint8_t i = 0; i < nb; i++) 453 | { 454 | digitalWrite(_led_pin, HIGH); 455 | delay(delay_ / 2); 456 | digitalWrite(_led_pin, LOW); 457 | delay(delay_ / 2); 458 | } 459 | } --------------------------------------------------------------------------------