Skip to content

Commit

Permalink
Refactor and clean up
Browse files Browse the repository at this point in the history
  • Loading branch information
@nicolas71640
nicolas71640 committed Dec 9, 2024
1 parent cec6049 commit e298ff4
Show file tree
Hide file tree
Showing 11 changed files with 244 additions and 323 deletions.
6 changes: 1 addition & 5 deletions examples/master/easycat/easycat_example.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -97,7 +97,6 @@ int main(int argc, char* argv[])

printf("Slave DL status: %s", toString(bus.slaves().at(0).dl_status).c_str());
});
printf("HEREEEEEE\n");

bus.requestState(State::SAFE_OP);
bus.waitForState(State::SAFE_OP, 1s);
Expand All @@ -119,11 +118,8 @@ int main(int argc, char* argv[])
printInfo(slave);
printESC(slave);
}
printf("HERE2\n");

auto callback_error = [](DatagramState const&){ THROW_ERROR("something bad happened"); };
auto callback_error2 = [](DatagramState const&){ THROW_ERROR("something bad happened2"); };
printf("HERE3\n");

// Set valid output to exit safe op.
auto& easycat = bus.slaves().at(0);
Expand All @@ -142,7 +138,7 @@ int main(int argc, char* argv[])
//TODO: need a way to check expected working counter depending on state
// -> in safe op write is not working
}
bus.processDataWrite(callback_error2);
bus.processDataWrite(callback_error);

try
{
Expand Down
26 changes: 5 additions & 21 deletions lib/slave/include/kickcat/AbstractESC.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,35 +20,19 @@ namespace kickcat
SyncManagerType type;
};

#define SYNC_MANAGER_PI_IN(index, address, length) \
SyncManagerConfig \
{ \
index, address, length, 0x20, SyncManagerType::Input \
}
#define SYNC_MANAGER_PI_OUT(index, address, length) \
SyncManagerConfig \
{ \
index, address, length, 0x64, SyncManagerType::Output \
}
#define SYNC_MANAGER_PI_IN(index, address, length) SyncManagerConfig{index, address, length, 0x20, SyncManagerType::Input}
#define SYNC_MANAGER_PI_OUT(index, address, length) SyncManagerConfig{index, address, length, 0x64, SyncManagerType::Output}

#define SYNC_MANAGER_MBX_IN(index, address, length) \
SyncManagerConfig \
{ \
index, address, length, 0x02, SyncManagerType::MailboxIn \
}
#define SYNC_MANAGER_MBX_OUT(index, address, length) \
SyncManagerConfig \
{ \
index, address, length, 0x06, SyncManagerType::MailboxOut \
}
#define SYNC_MANAGER_MBX_IN(index, address, length) SyncManagerConfig{index, address, length, 0x02, SyncManagerType::MailboxIn}
#define SYNC_MANAGER_MBX_OUT(index, address, length) SyncManagerConfig{index, address, length, 0x06, SyncManagerType::MailboxOut}


namespace mailbox::response
{
class Mailbox;
}


// Regarding the state machine, see ETG1000.6 6.4.1 AL state machine
class AbstractESC
{
public:
Expand Down
30 changes: 15 additions & 15 deletions lib/slave/include/kickcat/ESC/Lan9252.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,8 +3,8 @@

#include "kickcat/AbstractESC.h"
#include "kickcat/AbstractSPI.h"
#include "kickcat/OS/Time.h"
#include "kickcat/protocol.h"
#include "kickcat/OS/Time.h"

#include <memory>

Expand All @@ -13,23 +13,23 @@ namespace kickcat
// Host to Network byte order helper (Reminder: EtherCAT is LE, network is BE)

// SPI INSTRUCTIONS
constexpr uint8_t READ = 0x03;
constexpr uint8_t WRITE = 0x02;
constexpr uint8_t READ = 0x03;
constexpr uint8_t WRITE = 0x02;
constexpr uint8_t BYTE_TEST = 0x64;

// Registers
constexpr uint16_t HW_CFG = 0x0074; // Is device ready
constexpr uint16_t RESET_CTL = 0x01F8; // reset register
constexpr uint16_t ECAT_CSR_DATA = 0x0300; // EtherCAT CSR Interface Data Register
constexpr uint16_t ECAT_CSR_CMD = 0x0304; // EtherCAT CSR Interface Command Register
constexpr uint16_t HW_CFG = 0x0074; // Is device ready
constexpr uint16_t RESET_CTL = 0x01F8; // reset register
constexpr uint16_t ECAT_CSR_DATA = 0x0300; // EtherCAT CSR Interface Data Register
constexpr uint16_t ECAT_CSR_CMD = 0x0304; // EtherCAT CSR Interface Command Register

constexpr uint16_t ECAT_PRAM_RD_ADDR_LEN = 0X0308;
constexpr uint16_t ECAT_PRAM_RD_CMD = 0X030C;
constexpr uint16_t ECAT_PRAM_WR_ADDR_LEN = 0X0310;
constexpr uint16_t ECAT_PRAM_WR_CMD = 0X0314;

constexpr uint16_t ECAT_PRAM_RD_DATA = 0x000; // until 0x01C
constexpr uint16_t ECAT_PRAM_WR_DATA = 0x020; // until 0x03C
constexpr uint16_t ECAT_PRAM_RD_DATA = 0x000; // until 0x01C
constexpr uint16_t ECAT_PRAM_WR_DATA = 0x020; // until 0x03C

constexpr uint16_t NUM_BYTE_INPUT = 32;
constexpr uint16_t NUM_BYTE_OUTPUT = 32;
Expand All @@ -41,7 +41,7 @@ namespace kickcat
constexpr uint32_t PRAM_BUSY = 0x1 << 31;
constexpr uint32_t PRAM_AVAIL = 0x01;

constexpr uint32_t DIGITAL_RST = 0x01;
constexpr uint32_t DIGITAL_RST = 0x01;
constexpr uint32_t BYTE_TEST_DEFAULT = 0x87654321;

constexpr milliseconds TIMEOUT{10};
Expand All @@ -50,9 +50,9 @@ namespace kickcat
constexpr uint8_t CSR_CMD_HEADER_SIZE = 3;
struct InternalRegisterControl
{
uint8_t instruction; // Read / write // TODO enum full.
uint8_t instruction; // Read / write // TODO enum full.
uint16_t LAN9252_register_address; // address of SYSTEM CONTROL AND STATUS REGISTERS
uint8_t payload[64]; // Max payload size is 64 bytes (fifo).
uint8_t payload[64]; // Max payload size is 64 bytes (fifo).
} __attribute__((__packed__));


Expand All @@ -62,8 +62,8 @@ namespace kickcat
static constexpr uint8_t ESC_READ = 0xC0;

uint16_t ethercat_register_address;
uint8_t ethercat_register_size; // only size 1,2,4 allowed (ECAT_CSR_CMD specification)
uint8_t ethercat_register_operation; // read / write
uint8_t ethercat_register_size; // only size 1,2,4 allowed (ECAT_CSR_CMD specification)
uint8_t ethercat_register_operation; // read / write
} __attribute__((__packed__));


Expand Down Expand Up @@ -101,7 +101,7 @@ namespace kickcat

int32_t writeData(uint16_t address, void const* data, uint16_t to_write);

std::shared_ptr<AbstractSPI> spi_interface_; // TODO shared ptr like link in bus.h
std::shared_ptr<AbstractSPI> spi_interface_; // TODO shared ptr like link in bus.h
};


Expand Down
4 changes: 2 additions & 2 deletions lib/slave/include/kickcat/ESC/XMC4800.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,13 +6,13 @@
namespace kickcat
{
constexpr uint32_t ECAT0_BASE_ADDRESS = 0x54010000;
constexpr uint32_t ECAT0_END_ADDRESS = 0x5401FFFF;
constexpr uint32_t ECAT0_END_ADDRESS = 0x5401FFFF;


class XMC4800 final : public AbstractESC
{
public:
XMC4800() = default;
XMC4800() = default;
~XMC4800() = default;


Expand Down
88 changes: 69 additions & 19 deletions lib/slave/include/kickcat/FSM.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,55 +2,105 @@
#define SLAVE_STACK_INCLUDE_FSM_H_

#include <cstdarg>
#include <map>
#include <string>
#include "PDO.h"
#include "kickcat/AbstractESC.h"
#include "kickcat/protocol.h"


namespace kickcat
{
constexpr uint8_t NUMBER_OF_STATES = 4;

// TODO: to rename ESM
namespace FSM
{
class StateMachine;

struct ALControl
{
uint16_t value;

State getRequestedState()
{
return static_cast<State>(value & State::MASK_STATE);
}
};

struct ALStatus
{
uint16_t al_status;
uint16_t al_status_code;
uint16_t al_watchdog;
bool validOutputData{false};

State getState()
{
return static_cast<State>(al_status & State::MASK_STATE);
}

bool has_expired_watchdog()
{
return not(al_watchdog & 0x1);
}

static ALStatus build(uint8_t state, uint8_t statusCode = StatusCode::NO_ERROR)
{
if (statusCode == NO_ERROR)
{
return ALStatus{state, statusCode, 0};
}
else
{
return ALStatus{static_cast<uint16_t>(state | State::ERROR_ACK), statusCode, 0};
}
}
};

class AbstractState

{
friend StateMachine;

public:
AbstractState(uint8_t id);
virtual ~AbstractState() = default;

uint8_t id();
AbstractState(uint8_t id, AbstractESC& esc, PDO& pdo);
void setMailbox(mailbox::response::Mailbox* mbx);

protected:
uint8_t id_;
AbstractESC& esc_;
PDO& pdo_;
mailbox::response::Mailbox* mbx_;

virtual ALStatus routine(ALStatus currentStatus,
ALControl alControl); //return al_status and al_status_code

virtual ALStatus routineInternal(ALStatus currentStatus,
ALControl alControl) = 0; //return al_status and al_status_code

virtual void onEntry(uint8_t fromState)
{
(void)fromState;
};

private:
virtual std ::tuple<uint16_t, uint16_t> routine(
uint16_t al_control,
uint16_t al_status,
uint16_t al_status_code) = 0; //return al_status and al_status_code
virtual void onEntry(uint8_t fromState);
uint8_t id();
};

class StateMachine
class StateMachine final
{
public:
StateMachine(AbstractESC& esc, std::array<FSM::AbstractState*, 4>&& states);
StateMachine(AbstractESC& esc, std::array<FSM::AbstractState*, NUMBER_OF_STATES>&& states);
void setOutputDataValid(bool isValid);
void start();
void play();

protected:
AbstractState* getState(uint8_t id);

private:
AbstractState* findState(uint8_t id);

AbstractESC& esc_;
AbstractState* currentState_;
std::array<FSM::AbstractState*, 4> states_;
uint16_t al_status_{State::INIT};
uint16_t al_status_code_{NO_ERROR};
std::array<FSM::AbstractState*, NUMBER_OF_STATES> states_;
ALStatus status_{};
};
}
}
Expand Down
39 changes: 7 additions & 32 deletions lib/slave/include/kickcat/Slave2.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,43 +7,18 @@
#include "kickcat/Mailbox.h"
#include "kickcat/SlaveFSM.h"


// TODO: to rename file
namespace kickcat
{
class Slave final
{
public:
Slave(AbstractESC* esc, PDO* pdo)
: esc_{esc}
, pdo_{pdo}
{
}

void set_mailbox(mailbox::response::Mailbox* mbx)
{
mbx_ = mbx;
init_.setMailbox(mbx);
preOp_.setMailbox(mbx);
safeOP_.setMailbox(mbx);
OP_.setMailbox(mbx);
}

void start()
{
stateMachine.start();
}

void routine()
{
if (mbx_)
{
mbx_->receive();
mbx_->process();
mbx_->send();
}


stateMachine.play();
}
Slave(AbstractESC* esc, PDO* pdo);

void set_mailbox(mailbox::response::Mailbox* mbx);
void start();
void routine();

private:
AbstractESC* esc_;
Expand Down
Loading

0 comments on commit e298ff4

Please sign in to comment.