MATLAB Answers

Servo Library with Simulink and Arduino

14 views (last 30 days)
Stefan Schuhmacher
Stefan Schuhmacher on 25 Feb 2019
Hey guys,
First i started to program the Arduino Uno with Simulink. Everything is working perfect. For our DRS-System we want to use the Herkulex DRS 0201 Servo Motor. It will be controlled by RX TX signals not by PWM. So there is a library which describes the handling of the Servo. But the Library is written in C Code. Now I want to get this code somehow included in my Simulink model. Do you have any Idea how to?
Here's the library:
#ifndef __HERKULEXSERVO_H__
#define __HERKULEXSERVO_H__
#include <Arduino.h>
#include <CircularBuffer.h>
#define HERKULEX_BROADCAST_ID 0xFE
#ifndef HERKULEX_PACKET_RX_TIMEOUT
#define HERKULEX_PACKET_RX_TIMEOUT 100 // microseconds
#endif
#ifndef HERKULEX_PACKET_RETRIES
#define HERKULEX_PACKET_RETRIES 6
#endif
#ifndef HERKULEX_PACKET_RESEND_DELAY
#define HERKULEX_PACKET_RESEND_DELAY 70 // microseconds
#endif
#ifndef HERKULEX_PACKET_RX_MAX_DATA
#define HERKULEX_PACKET_RX_MAX_DATA 10 // bytes
#endif
#ifndef HERKULEX_SERIAL_RX_BUFFER
#define HERKULEX_SERIAL_RX_BUFFER 30 // bytes
#endif
#ifndef HERKULEX_MAX_SCHEDULED_SERVOS
#define HERKULEX_MAX_SCHEDULED_SERVOS 10
#endif
#ifndef HERKULEX_SERIAL_TX_BUFFER
#define HERKULEX_SERIAL_TX_BUFFER (1 + HERKULEX_MAX_SCHEDULED_SERVOS * 5)
#endif
#ifdef HERKULEX_DEBUG
#define HERKULEX_DEBUG_RX_STATS
#endif
enum class HerkulexCommand {
EepWrite = 0x01,
EepRead = 0x02,
RamWrite = 0x03,
RamRead = 0x04,
IJog = 0x05,
SJog = 0x06,
Stat = 0x07,
Rollback = 0x08,
Reboot = 0x09
};
enum class HerkulexEepRegister {
ModelNo1 = 0,
ModelNo2 = 1,
Version1 = 2,
Version2 = 3,
BaudRate = 4,
ID = 6,
AckPolicy = 7,
AlarmLedPolicy = 8,
TorquePolicy = 9,
MaxTemperature = 11,
MinVoltage = 12,
MaxVoltage = 13,
AccelerationRatio = 14,
MaxAccelerationTime = 15,
DeadZone = 16,
SaturatorOffset = 17,
SaturatorSlope = 18,
PwmOffset = 20,
MinPwm = 21,
MaxPwm = 22,
OverloadPwmThreshold = 24,
MinPosition = 26,
MaxPosition = 28,
PositionKp = 30,
PositionKd = 32,
PositionKi = 34,
PositionFF1stGain = 36,
PositionFF2ndGain = 38,
LedBlinkPeriod = 44,
AdcFaultCheckPeriod = 45,
PacketGarbageCheckPeriod = 46,
StopDetectionPeriod = 47,
OverloadProtectionPeriod = 48,
StopThreshold = 49,
InPositionMargin = 50,
CalibrationDifference = 53
};
enum class HerkulexRamRegister {
ID = 0,
AckPolicy = 1,
AlarmLedPolicy = 2,
TorquePolicy = 3,
MaxTemperature = 5,
MinVoltage = 6,
MaxVoltage = 7,
AccelerationRatio = 8,
MaxAccelerationTime = 9,
DeadZone = 10,
SaturatorOffset = 11,
SaturatorSlope = 12,
PwmOffset = 14,
MinPwm = 15,
MaxPwm = 16,
OverloadPwmThreshold = 18,
MinPosition = 20,
MaxPosition = 22,
PositionKp = 24,
PositionKd = 26,
PositionKi = 28,
PositionFF1stGain = 30,
PositionFF2ndGain = 32,
LedBlinkPeriod = 38,
AdcFaultCheckPeriod = 39,
PacketGarbageCheckPeriod = 40,
StopDetectionPeriod = 41,
OverloadProtectionPeriod = 42,
StopThreshold = 43,
InPositionMargin = 44,
CalibrationDifference = 47,
StatusError = 48,
StatusDetail = 49,
TorqueControl = 52,
LedControl = 53,
Voltage = 54,
Temperature = 55,
CurrentControlMode = 56,
Tick = 57,
CalibratedPosition = 58,
AbsolutePosition = 60,
DifferentialPosition = 62,
Pwm = 64,
AbsoluteGoalPosition = 68,
AbsoluteDesiredTrajPos = 70,
DesiredVelocity = 72
};
enum class HerkulexLed : uint8_t {
Off = 0x00,
Red = 0x04,
Green = 0x01,
Blue = 0x02,
Yellow = 0x05,
Cyan = 0x03,
Purple = 0x06,
White = 0x07,
Ignore = 0xFF
};
#define ENUM_FLAG_OPERATOR(T,X) inline T operator X (T lhs, T rhs) { return (T) (static_cast<uint8_t>(lhs) X static_cast<uint8_t>(rhs)); }
#define ENUM_FLAG_ASSIGNMENT_OPERATOR(T,X,Y) inline T& operator X (T& lhs, const T& rhs) { return lhs = (lhs Y rhs); }
enum class HerkulexPacketError : uint8_t {
None = 0,
Timeout = 0b00000001,
Length = 0b00000010,
Command = 0b00000100,
Checksum = 0b00001000
};
ENUM_FLAG_OPERATOR(HerkulexPacketError, |)
ENUM_FLAG_OPERATOR(HerkulexPacketError, &)
ENUM_FLAG_ASSIGNMENT_OPERATOR(HerkulexPacketError, |=, |)
enum class HerkulexStatusError : uint8_t {
None = 0,
InputVoltage = 0b00000001,
PotLimit = 0b00000010,
TemperatureLimit = 0b00000100,
InvalidPacket = 0b00001000,
Overload = 0b00010000,
DriverFault = 0b00100000,
EEPDistorted = 0b01000000,
Reserved = 0b10000000
};
ENUM_FLAG_OPERATOR(HerkulexStatusError, |)
ENUM_FLAG_OPERATOR(HerkulexStatusError, &)
ENUM_FLAG_ASSIGNMENT_OPERATOR(HerkulexStatusError, |=, |)
enum class HerkulexStatusDetail : uint8_t {
None = 0,
Moving = 0b00000001,
InPosition = 0b00000010,
ChecksumError = 0b00000100,
UnknownCommand = 0b00001000,
ExceedRegRange = 0b00010000,
GarbageDetected = 0b00100000,
MotorOn = 0b01000000,
Reserved = 0b10000000
};
ENUM_FLAG_OPERATOR(HerkulexStatusDetail, |)
ENUM_FLAG_OPERATOR(HerkulexStatusDetail, &)
ENUM_FLAG_ASSIGNMENT_OPERATOR(HerkulexStatusDetail, |=, |)
enum class HerkulexScheduleState : uint8_t {
None = 0,
IndividualMove,
SynchronizedMove
};
struct HerkulexPacket {
uint8_t size;
uint8_t id;
HerkulexCommand cmd;
uint8_t checksum1;
uint8_t checksum2;
uint8_t data[HERKULEX_PACKET_RX_MAX_DATA];
uint8_t status_error;
uint8_t status_detail;
HerkulexPacketError error;
};
class HerkulexServo;
class HerkulexServoBus {
public:
HerkulexServoBus(Stream &serial_connection);
void sendPacket(uint8_t id, HerkulexCommand cmd, const uint8_t* data = nullptr, uint8_t data_len = 0);
bool sendPacketAndReadResponse(HerkulexPacket &resp, uint8_t id, HerkulexCommand cmd, const uint8_t* data = nullptr, uint8_t data_len = 0);
void update();
bool getPacket(HerkulexPacket &packet);
#ifdef HERKULEX_DEBUG_RX_STATS
void resetRxStatistics();
void printRxStatistics(Stream &output);
#endif
void prepareIndividualMove();
void prepareSynchronizedMove(uint8_t playtime);
void executeMove();
protected:
Stream* m_serial;
CircularBuffer<uint8_t, HERKULEX_SERIAL_RX_BUFFER> m_rx_buffer;
unsigned long m_last_serial = 0;
HerkulexPacket m_rx_packet = {};
bool m_rx_packet_ready = false;
uint8_t m_tx_buffer[HERKULEX_SERIAL_TX_BUFFER];
uint8_t m_move_tags = 0;
HerkulexScheduleState m_schedule_state = HerkulexScheduleState::None;
#ifdef HERKULEX_DEBUG_RX_STATS
uint16_t m_stats_attempts[HERKULEX_PACKET_RETRIES] = {};
uint16_t m_stats_fails = 0;
uint16_t m_stats_totals = 0;
#endif
void processPacket(bool timeout);
friend class HerkulexServo;
};
class HerkulexServo {
public:
HerkulexServo(HerkulexServoBus &bus, uint8_t id);
void writeRam(HerkulexRamRegister reg, uint8_t val);
void writeRam2(HerkulexRamRegister reg, uint16_t val);
void writeEep(HerkulexEepRegister reg, uint8_t val);
void writeEep2(HerkulexEepRegister reg, uint16_t val);
uint8_t readRam(HerkulexRamRegister reg);
uint16_t readRam2(HerkulexRamRegister reg);
uint8_t readEep(HerkulexEepRegister reg);
uint16_t readEep2(HerkulexEepRegister reg);
void reboot();
void rollbackToFactoryDefaults(bool skipID, bool skipBaud);
uint8_t getID();
void getStatus(HerkulexStatusError &status_error, HerkulexStatusDetail &status_detail);
uint16_t getPosition();
uint16_t getRawPosition();
void setLedColor(HerkulexLed color);
void setTorqueOn();
void setTorqueOff();
void setBrake();
void enablePositionControlMode();
void enableSpeedControlMode();
void setPosition(uint16_t pos, uint8_t playtime = 0, HerkulexLed led = HerkulexLed::Ignore);
void setSpeed(int16_t speed, uint8_t playtime = 0, HerkulexLed led = HerkulexLed::Ignore);
protected:
HerkulexServoBus* m_bus;
uint8_t m_id;
HerkulexLed m_led = HerkulexLed::Off;
bool m_position_control_mode = true;
static HerkulexPacket m_response;
static uint8_t m_tx_buffer[5];
void jog(uint8_t jog_lsb, uint8_t jog_msb, uint8_t set, uint8_t playtime);
};
#endif

  2 Comments

Himanshu Ahuja
Himanshu Ahuja on 27 Oct 2019
Hello. I am working on using this motor with MATLAB as well. Did you make any progress on this? Or any leads? Please let me know. Thank you
Stefan Schuhmacher
Stefan Schuhmacher on 30 Oct 2019
Hi,
sorry no we use it with arduino code. No Simulink programming...

Sign in to comment.

Answers (2)

Madhu Govindarajan
Madhu Govindarajan on 25 Feb 2019
This video has some relevant items - https://www.youtube.com/watch?v=qoj8Pa2wi0A but needs to be adapted for your case

  0 Comments

Sign in to comment.


Pradeep KS
Pradeep KS on 10 Apr 2019
Follow this link https://www.mathworks.com/help/supportpkg/arduino/device-driver-blocks.html to create a custom Simulink Device driver blocks for Arduino support package.

  0 Comments

Sign in to comment.

Sign in to answer this question.

Products


Release

R2018a