diff --git a/f4/can/include/can.h b/f4/can/include/can.h
new file mode 100644
index 0000000000000000000000000000000000000000..c5c8b91af398736415ce3be4a82011fde5d33a79
--- /dev/null
+++ b/f4/can/include/can.h
@@ -0,0 +1,104 @@
+#ifndef CAN_F4_H
+#define CAN_F4_H
+
+#include "stm32f4xx_hal.h"
+#include "comm.h"
+
+typedef struct
+{
+ unsigned char irq_priority;
+ unsigned char irq_subpriority;
+}TCAN_IRQ_Priorities;
+
+typedef enum
+{
+ CAN_10KBPS,
+ CAN_20KBPS,
+ CAN_50KBPS,
+ CAN_83K3BPS,
+ CAN_100KBPS,
+ CAN_125KBPS,
+ CAN_250KBPS,
+ CAN_500KBPS,
+ CAN_1000KBPS
+} CAN_SPEED;
+
+typedef enum
+{
+ VERSION_1,
+ VERSION_2,
+ VERSION_NULL
+} DynamixelVersion;
+
+typedef struct TDynamixelMessage
+{
+ unsigned char received_bytes;
+/**
+ * \brief
+ *
+ */
+ unsigned char rx_buffer[MAX_DYN_SLAVE_RX_BUFFER_LEN];
+/**
+ * \brief
+ *
+ */
+ unsigned char Len_L;
+/**
+ * \brief
+ *
+ */
+ unsigned char Len_H;
+/**
+ * \brief
+ *
+ */
+ unsigned int length;
+/**
+ * \brief
+ *
+ */
+ DynamixelVersion version;
+/**
+ * \brief
+ *
+ */
+ unsigned char packet_init;
+/**
+ * \brief
+ *
+ */
+ unsigned char packet_incomplete;
+/**
+ * \brief
+ *
+ */
+ unsigned char send_packet;
+/**
+ * \brief
+ *
+ */
+ unsigned char iterations;
+/**
+ * \brief
+ *
+ */
+}TDynamixelMessage;
+
+/* public functions */
+void can_init(TComm *comm_dev,CAN_InitTypeDef *conf,TCAN_IRQ_Priorities *priorities);
+void can_config(TComm *comm_dev,CAN_InitTypeDef *conf);
+void can_set_priorities(TComm *comm_dev,TCAN_IRQ_Priorities *priorities);
+void can_set_bitrate(TComm *comm_dev,CAN_SPEED bitrate);
+void can_set_filter(TComm *comm_dev,CAN_FilterTypeDef *filter);
+void can_set_can_id(TComm *comm_dev,unsigned int id);
+/* IRQ functions */
+unsigned char can_send_irq(unsigned char first_byte);
+unsigned char can_enable_tx_irq(void);
+unsigned char can_receive_irq(void);
+unsigned char can_cancel_receive_irq(void);
+/* DMA functions */
+unsigned char can_send_dma(unsigned char *data,unsigned short int length);
+unsigned char can_receive_dma(unsigned char *data,unsigned short int length);
+unsigned char can_cancel_receive_dma(void);
+
+#endif
diff --git a/f4/can/include/can_common.h b/f4/can/include/can_common.h
new file mode 100644
index 0000000000000000000000000000000000000000..f7eabaff39d5986f886369ab1df4663a2b8ff2ad
--- /dev/null
+++ b/f4/can/include/can_common.h
@@ -0,0 +1,22 @@
+#ifndef CAN_COMMON_F1_H
+#define CAN_COMMON_F1_H
+
+typedef struct
+{
+ unsigned char irq_priority;
+ unsigned char irq_subpriority;
+}TCAN_IRQ_Priorities;
+
+enum CAN_SPEED { //only applies when APB1 = 36Mhz
+ CAN_10KBPS,
+ CAN_20KBPS,
+ CAN_50KBPS,
+ CAN_83K3BPS,
+ CAN_100KBPS,
+ CAN_125KBPS,
+ CAN_250KBPS,
+ CAN_500KBPS,
+ CAN_1000KBPS
+};
+
+#endif
diff --git a/f4/can/src/can.c b/f4/can/src/can.c
new file mode 100644
index 0000000000000000000000000000000000000000..d8eb65395df2498e0c04f794fc8d7341d9fa1ea8
--- /dev/null
+++ b/f4/can/src/can.c
@@ -0,0 +1,442 @@
+#include "can.h"
+
+#define CAN CAN1
+#define CAN_ENABLE_CLK __HAL_RCC_CAN1_CLK_ENABLE()
+#define CAN_IRQnRX CAN1_RX0_IRQn
+#define CAN_IRQnTX CAN1_TX_IRQn
+
+#define CAN_TX_PIN GPIO_PIN_9
+#define CAN_TX_GPIO_PORT GPIOB
+#define CAN_ENABLE_TX_GPIO_CLK __HAL_RCC_GPIOB_CLK_ENABLE()
+
+#define CAN_RX_PIN GPIO_PIN_8
+#define CAN_RX_GPIO_PORT GPIOB
+#define CAN_ENABLE_RX_GPIO_CLK __HAL_RCC_GPIOB_CLK_ENABLE()
+
+// private variables
+CAN_HandleTypeDef CANHandle;
+CAN_FilterTypeDef CANFilterConfig;
+CAN_TxHeaderTypeDef CAN_txHeader;
+uint8_t *CAN_txMessage;
+uint32_t TxMailbox;
+CAN_RxHeaderTypeDef CAN_rxHeader;
+uint8_t CAN_rxMessage[8];
+
+TComm *can_comm_dev;
+unsigned char can_dma_phase;
+unsigned char *can_dma_phase_data;
+unsigned short int can_dma_phase_length;
+unsigned short int can_dma_phase_write_ptr;
+
+unsigned char can_missing_bytes_data[255];
+unsigned short int can_missing_bytes_length;
+unsigned short int can_missing_bytes_write_ptr;
+
+unsigned short int can_send_lenght;
+unsigned short int can_send_iterations;
+unsigned char can_send_data[255];
+
+TDynamixelMessage message;
+
+void can_init(TComm *comm_dev,CAN_InitTypeDef *conf,TCAN_IRQ_Priorities *priorities)
+{
+ CANHandle.Instance = CAN;
+ can_config(comm_dev,conf);
+
+ GPIO_InitTypeDef GPIO_InitStructure;
+
+ /* Enable GPIO clock */
+ CAN_ENABLE_TX_GPIO_CLK;
+ CAN_ENABLE_RX_GPIO_CLK;
+
+ CAN_ENABLE_CLK;
+
+ /**CAN GPIO Configuration
+ PB8 ------> CAN_RX
+ PB9 ------> CAN_TX
+ */
+ /* Configure CAN Tx and Rx as alternate function push-pull */
+ GPIO_InitStructure.Pin = CAN_TX_PIN|CAN_RX_PIN;
+ GPIO_InitStructure.Mode = GPIO_MODE_AF_PP;
+ GPIO_InitStructure.Pull = GPIO_NOPULL;
+ GPIO_InitStructure.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
+ GPIO_InitStructure.Alternate = GPIO_AF9_CAN1;
+ HAL_GPIO_Init(CAN_TX_GPIO_PORT, &GPIO_InitStructure);
+
+ can_set_priorities(comm_dev,priorities);
+
+ HAL_CAN_Init(&CANHandle);
+
+ /* Initialize the comm structure */
+ comm_dev->send_irq=can_send_irq;
+ comm_dev->enable_tx_irq=can_enable_tx_irq;
+ comm_dev->receive_irq=can_receive_irq;
+ comm_dev->cancel_receive_irq=can_cancel_receive_irq;
+ if(comm_dev->use_dma)
+ {
+ comm_dev->send_dma=can_send_dma;
+ comm_dev->receive_dma=can_receive_dma;
+ comm_dev->cancel_receive_dma=can_cancel_receive_dma;
+ }
+ comm_dev->irq_send_cb=0x00000000;
+ comm_dev->irq_receive_cb=0x00000000;
+ comm_dev->dma_send_cb=0x00000000;
+ comm_dev->dma_receive_cb=0x00000000;
+
+ // /* initialize internal variables */
+ can_comm_dev=comm_dev;
+
+ can_dma_phase=0x00;
+ can_dma_phase_data=0x00;
+ can_dma_phase_length=0;
+ can_dma_phase_write_ptr=0;
+ can_missing_bytes_length = 0;
+ can_missing_bytes_write_ptr = 0;
+ can_send_lenght = 0;
+ can_send_iterations = 0;
+
+ message.packet_init = 0x00;
+ message.packet_incomplete = 0x00;
+ message.send_packet = 0x00;
+ message.iterations = 0;
+ ///////////////////////////////////////////////////////////////////////////
+ //default tx message Configuration
+ CAN_txHeader.DLC = 1;
+ CAN_txHeader.IDE = CAN_ID_STD;
+ CAN_txHeader.RTR = CAN_RTR_DATA;
+ CAN_txHeader.StdId = 0x00;
+ CAN_txHeader.ExtId = 0x01;
+ //default configration for the filer -> none
+ CANFilterConfig.FilterBank = 0;
+ CANFilterConfig.FilterMode = CAN_FILTERMODE_IDMASK;
+ CANFilterConfig.FilterScale = CAN_FILTERSCALE_32BIT;
+ CANFilterConfig.FilterIdHigh = 0x0000;
+ CANFilterConfig.FilterIdLow = 0x0000;
+ CANFilterConfig.FilterMaskIdHigh = 0x0000;
+ CANFilterConfig.FilterMaskIdLow = 0x0000;
+ CANFilterConfig.FilterFIFOAssignment = 0;
+ CANFilterConfig.FilterActivation = ENABLE;
+ CANFilterConfig.SlaveStartFilterBank = 0;
+ //configure the filter
+ HAL_CAN_ConfigFilter(&CANHandle, &CANFilterConfig);
+ //start the can device
+ HAL_CAN_Start(&CANHandle);
+ //Activate default interrupt when message in FIFO0
+ HAL_CAN_ActivateNotification(&CANHandle, CAN_IT_RX_FIFO0_MSG_PENDING);
+}
+
+void can_config(TComm *comm_dev,CAN_InitTypeDef *conf)
+{
+ CANHandle.Init.Prescaler = conf->Prescaler;
+ CANHandle.Init.Mode = conf->Mode;
+ CANHandle.Init.SyncJumpWidth = conf->SyncJumpWidth;
+ CANHandle.Init.TimeSeg1 = conf->TimeSeg1;
+ CANHandle.Init.TimeSeg2 = conf->TimeSeg2;
+ CANHandle.Init.TimeTriggeredMode = conf->TimeTriggeredMode;
+ CANHandle.Init.AutoBusOff = conf->AutoBusOff;
+ CANHandle.Init.AutoWakeUp = conf->AutoWakeUp;
+ CANHandle.Init.AutoRetransmission = conf->AutoRetransmission;
+ CANHandle.Init.ReceiveFifoLocked = conf->ReceiveFifoLocked;
+ CANHandle.Init.TransmitFifoPriority = conf->TransmitFifoPriority;
+}
+
+void can_set_priorities(TComm *comm_dev,TCAN_IRQ_Priorities *priorities)
+{
+ HAL_NVIC_SetPriority(CAN_IRQnRX, priorities->irq_priority, priorities->irq_subpriority);
+ HAL_NVIC_EnableIRQ(CAN_IRQnRX);
+ HAL_NVIC_SetPriority(CAN_IRQnTX, priorities->irq_priority, priorities->irq_subpriority);
+ HAL_NVIC_EnableIRQ(CAN_IRQnTX);
+}
+
+// This only works on APB1 clock of 42Mhz
+void can_set_bitrate(TComm *comm_dev,CAN_SPEED bitrate)
+{
+ switch (bitrate) {
+ case CAN_10KBPS:
+ CANHandle.Init.Prescaler = 280;
+ CANHandle.Init.TimeSeg1 = CAN_BS1_12TQ;
+ CANHandle.Init.TimeSeg2 = CAN_BS2_2TQ;
+ CANHandle.Init.SyncJumpWidth = CAN_SJW_1TQ;
+ break;
+ case CAN_20KBPS:
+ CANHandle.Init.Prescaler = 140;
+ CANHandle.Init.TimeSeg1 = CAN_BS1_12TQ;
+ CANHandle.Init.TimeSeg2 = CAN_BS2_2TQ;
+ CANHandle.Init.SyncJumpWidth = CAN_SJW_1TQ;
+ break;
+ case CAN_50KBPS:
+ CANHandle.Init.Prescaler = 56;
+ CANHandle.Init.TimeSeg1 = CAN_BS1_12TQ;
+ CANHandle.Init.TimeSeg2 = CAN_BS2_2TQ;
+ CANHandle.Init.SyncJumpWidth = CAN_SJW_1TQ;
+ break;
+ case CAN_83K3BPS:
+ CANHandle.Init.Prescaler = 28;
+ CANHandle.Init.TimeSeg1 = CAN_BS1_15TQ;
+ CANHandle.Init.TimeSeg2 = CAN_BS2_2TQ;
+ CANHandle.Init.SyncJumpWidth = CAN_SJW_1TQ;
+ break;
+ case CAN_100KBPS:
+ CANHandle.Init.Prescaler = 28;
+ CANHandle.Init.TimeSeg1 = CAN_BS1_12TQ;
+ CANHandle.Init.TimeSeg2 = CAN_BS2_2TQ;
+ CANHandle.Init.SyncJumpWidth = CAN_SJW_1TQ;
+ break;
+ case CAN_125KBPS:
+ CANHandle.Init.Prescaler = 21;
+ CANHandle.Init.TimeSeg1 = CAN_BS1_13TQ;
+ CANHandle.Init.TimeSeg2 = CAN_BS2_2TQ;
+ CANHandle.Init.SyncJumpWidth = CAN_SJW_1TQ;
+ break;
+ case CAN_250KBPS:
+ CANHandle.Init.Prescaler = 12;
+ CANHandle.Init.TimeSeg1 = CAN_BS1_11TQ;
+ CANHandle.Init.TimeSeg2 = CAN_BS2_2TQ;
+ CANHandle.Init.SyncJumpWidth = CAN_SJW_1TQ;
+ break;
+ case CAN_500KBPS:
+ CANHandle.Init.Prescaler = 6;
+ CANHandle.Init.TimeSeg1 = CAN_BS1_11TQ;
+ CANHandle.Init.TimeSeg2 = CAN_BS2_2TQ;
+ CANHandle.Init.SyncJumpWidth = CAN_SJW_1TQ;
+ break;
+ case CAN_1000KBPS:
+ CANHandle.Init.Prescaler = 3;
+ CANHandle.Init.TimeSeg1 = CAN_BS1_11TQ;
+ CANHandle.Init.TimeSeg2 = CAN_BS2_2TQ;
+ CANHandle.Init.SyncJumpWidth = CAN_SJW_1TQ;
+ break;
+ }
+}
+
+void can_set_filter(TComm *comm_dev,CAN_FilterTypeDef *filter)
+{
+ HAL_CAN_ConfigFilter(&CANHandle, filter);
+}
+
+void can_set_can_id(TComm *comm_dev,unsigned int id)
+{
+ CAN_txHeader.StdId = id;
+ //CANHandle.pTxMsg->StdId = id;
+}
+
+/**
+ * HAL_CAN_ErrorCallback
+ * Callback function for CAN errors.
+ * @param CANHandle Pointer to a CAN_HandleTypeDef structure that contains
+ * the configuration information for a given CAN peripheral.
+ */
+void HAL_CAN_ErrorCallback(CAN_HandleTypeDef* CANHandle)
+{
+ //HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4,GPIO_PIN_SET);
+}
+
+
+void HAL_CAN_TxMailbox0CompleteCallback(CAN_HandleTypeDef* CANHandle)
+{
+ if (can_send_lenght != 0)
+ {
+ if (can_send_lenght > 8)
+ {
+ CAN_txHeader.DLC = 8;
+ for (uint8_t i=0;i<8;i++)
+ {
+ CAN_txMessage[i] = can_send_data[i+(8*can_send_iterations)];
+ //CANHandle->pTxMsg->Data[i] = can_send_data[i+(8*can_send_iterations)];
+ }
+ can_send_lenght = can_send_lenght - 8;
+ can_send_iterations++;
+ //HAL_CAN_Transmit_IT(CANHandle);
+ HAL_CAN_AddTxMessage(CANHandle, &CAN_txHeader, CAN_txMessage, &TxMailbox);
+ }
+ else
+ {
+ //CANHandle->pTxMsg->DLC = can_send_lenght;
+ for (uint8_t i=0;i<can_send_lenght;i++)
+ {
+ CAN_txMessage[i] = can_send_data[i+(8*can_send_iterations)];
+ //CANHandle->pTxMsg->Data[i] = can_send_data[i+(8*can_send_iterations)];
+ }
+ can_send_lenght = 0;
+ can_send_iterations = 0;
+ //HAL_CAN_Transmit_IT(CANHandle);
+ HAL_CAN_AddTxMessage(CANHandle, &CAN_txHeader, CAN_txMessage, &TxMailbox);
+ uint8_t byte;
+ comm_do_dma_send(can_comm_dev);
+ comm_do_irq_send(can_comm_dev,&byte);
+ }
+ }
+}
+
+void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef* CANHandle)
+{
+ HAL_CAN_GetRxMessage(CANHandle, CAN_RX_FIFO0, &CAN_rxHeader, CAN_rxMessage);
+ if(can_missing_bytes_length == 0){
+ for(uint8_t i=0;i<CAN_rxHeader.DLC;i++)
+ {
+ if(!can_dma_phase)
+ {
+ comm_do_irq_receive(can_comm_dev,CAN_rxMessage[i]);
+ }
+ else
+ {
+ if(can_dma_phase_length<5){
+ can_dma_phase_data[can_dma_phase_write_ptr]=CAN_rxMessage[i];
+ can_dma_phase_length--;
+ can_dma_phase_write_ptr++;
+ if(can_dma_phase_length==0){
+ comm_do_dma_receive(can_comm_dev);
+ }
+ }
+ }
+ }
+ if(can_dma_phase_length>4){
+ for(uint8_t i=0;i<CAN_rxHeader.DLC;i++)
+ can_missing_bytes_data[i] = CAN_rxMessage[i];
+ can_missing_bytes_length = can_dma_phase_length-4;
+ can_missing_bytes_write_ptr = 7;
+ }
+ }
+ else
+ {
+ if(can_missing_bytes_length<=8){
+ for(uint8_t i=0;i<CAN_rxHeader.DLC;i++)
+ {
+ can_missing_bytes_write_ptr++;
+ can_missing_bytes_length--;
+ can_missing_bytes_data[can_missing_bytes_write_ptr] = CAN_rxMessage[i];
+ }
+ for(uint8_t i=0;i<=can_missing_bytes_write_ptr;i++)
+ {
+ if(!can_dma_phase)
+ comm_do_irq_receive(can_comm_dev,can_missing_bytes_data[i]);
+ else
+ {
+ can_dma_phase_data[can_dma_phase_write_ptr]=can_missing_bytes_data[i];
+ can_dma_phase_length--;
+ can_dma_phase_write_ptr++;
+ if(can_dma_phase_length==0){
+ comm_do_dma_receive(can_comm_dev);
+ can_missing_bytes_length = 0;
+ can_missing_bytes_write_ptr = 0;
+ }
+ }
+ }
+ }
+ else
+ {
+ for(uint8_t i=0;i<CAN_rxHeader.DLC;i++)
+ {
+ can_missing_bytes_write_ptr++;
+ can_missing_bytes_length--;
+ can_missing_bytes_data[can_missing_bytes_write_ptr] = CAN_rxMessage[i];
+ }
+ }
+ }
+}
+
+// interrupt handlers
+void CAN1_RX0_IRQHandler(void)
+{
+ /* USER CODE BEGIN USB_LP_CAN1_RX0_IRQn 0 */
+
+ /* USER CODE END USB_LP_CAN1_RX0_IRQn 0 */
+ HAL_CAN_IRQHandler(&CANHandle);
+ /* USER CODE BEGIN USB_LP_CAN1_RX0_IRQn 1 */
+
+ /* USER CODE END USB_LP_CAN1_RX0_IRQn 1 */
+}
+
+void CAN1_TX_IRQHandler(void)
+{
+ /* USER CODE BEGIN USB_HP_CAN1_TX_IRQn 0 */
+
+ /* USER CODE END USB_HP_CAN1_TX_IRQn 0 */
+ HAL_CAN_IRQHandler(&CANHandle);
+ /* USER CODE BEGIN USB_HP_CAN1_TX_IRQn 1 */
+
+ /* USER CODE END USB_HP_CAN1_TX_IRQn 1 */
+}
+
+unsigned char can_send_irq(unsigned char first_byte)
+{
+ return 0x00;
+}
+
+unsigned char can_enable_tx_irq(void)
+{
+ return 0x00;
+}
+
+unsigned char can_receive_irq(void)
+{
+ can_dma_phase=0x00;
+ can_dma_phase_write_ptr=0x00;
+ //HAL_CAN_Receive_IT(&CANHandle,CAN_FIFO0);
+ HAL_CAN_GetRxMessage(&CANHandle, CAN_RX_FIFO0, &CAN_rxHeader, CAN_rxMessage);
+ return 0x00;
+}
+
+unsigned char can_cancel_receive_irq(void)
+{
+ return 0x00;
+}
+
+/* DMA functions */
+unsigned char can_send_dma(unsigned char *data,unsigned short int length)
+{
+ __HAL_CAN_CLEAR_FLAG(&CANHandle,CAN_FLAG_RQCP0);
+ __HAL_CAN_CLEAR_FLAG(&CANHandle,CAN_FLAG_TXOK0);
+ if(length > 8){
+ CAN_txHeader.DLC = 8;
+ for (uint8_t i=0;i<length;i++)
+ {
+ can_send_data[i] = data[i];
+ }
+ can_send_lenght = length - 8;
+ can_send_iterations = 1;
+ // for (uint8_t i=0;i<8;i++)
+ // {
+ // CANHandle.pTxMsg->Data[i] = data[i];
+ // }
+ //HAL_CAN_Transmit_IT(&CANHandle);
+ HAL_CAN_AddTxMessage(&CANHandle, &CAN_txHeader, data, &TxMailbox);
+ }
+ else{
+ CAN_txHeader.DLC = length;
+ // for (uint8_t i=0;i<length;i++)
+ // {
+ // CANHandle.pTxMsg->Data[i] = data[i];
+ // }
+ can_send_lenght = 0;
+ can_send_iterations = 0;
+ //HAL_CAN_Transmit_IT(&CANHandle);
+ HAL_CAN_AddTxMessage(&CANHandle, &CAN_txHeader, data, &TxMailbox);
+ uint8_t byte;
+ comm_do_dma_send(can_comm_dev);
+ comm_do_irq_send(can_comm_dev,&byte);
+ }
+ return 0x00;
+}
+
+unsigned char can_receive_dma(unsigned char *data,unsigned short int length)
+{
+ can_dma_phase=0x01;
+ can_dma_phase_data=data;
+ can_dma_phase_length=length;
+ can_dma_phase_write_ptr=0;
+
+ return 0x00;
+}
+
+unsigned char can_cancel_receive_dma(void)
+{
+ can_dma_phase=0x00;
+ can_dma_phase_data=0x00;
+ can_dma_phase_length=0;
+ can_dma_phase_write_ptr=0;
+ //can_missing_bytes_length=0;
+ //can_missing_bytes_write_ptr=0;
+
+ return 0x00;
+}