diff --git a/Device/motor.c b/Device/motor.c
index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..433694e4f2f4fa3115f310d8d5bd2f5ea29e016d 100644
--- a/Device/motor.c
+++ b/Device/motor.c
@@ -0,0 +1,43 @@
+#include "motor.h"
+#include "oled.h"
+
+void set_motor_voltage(uint8_t id, motor_t *motor1, motor_t *motor2, motor_t *motor3, motor_t *motor4) {
+ int16_t sp1, sp2, sp3, sp4;
+ uint16_t tx_id, can_id;
+ sp1 = sp2 = sp3 = sp4 = 0;
+ tx_id = id;
+
+ if (motor1) { sp1 = correct_output(motor1); }
+ if (motor2) { sp2 = correct_output(motor2); }
+ if (motor3) { sp3 = correct_output(motor3); }
+ if (motor4) { sp4 = correct_output(motor4); }
+
+ can1_transmit(tx_id, sp1, sp2, sp3, sp4);
+}
+
+
+static int16_t correct_output(motor_t *motor) {
+ return motor->voltage;
+}
+
+uint8_t get_motor_data(motor_t *motor) {
+ uint8_t buf[CAN_DATA_SIZE];
+ can1_read(motor->can_info.rx_id, buf);
+ oled_printf(1,1,"Val: %i", buf[2]);
+
+ switch (motor->type) {
+ case GM6020:
+ get_6020_data(motor, buf);
+ return 1;
+ default:
+ return 0;
+ }
+}
+
+
+static void get_6020_data(motor_t* motor, uint8_t buf[CAN_DATA_SIZE]){
+ motor->info.GM6020_info.angle = (int16_t)(buf[0] << 8 | buf[1]);
+ motor->info.GM6020_info.speed = (int16_t)(buf[2] << 8 | buf[3]);
+ motor->info.GM6020_info.current = (int16_t)(buf[4] << 8 | buf[5]);
+ motor->info.GM6020_info.temp = (int8_t)(buf[6]);
+}
\ No newline at end of file
diff --git a/Device/motor.h b/Device/motor.h
index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..08409589f7bcb07327dff6e0d18c9dbf308526fb 100644
--- a/Device/motor.h
+++ b/Device/motor.h
@@ -0,0 +1,71 @@
+#ifndef _MOTOR_H_
+#define _MOTOR_H_
+
+#include "bsp_can.h"
+#include "bsp_pwm.h"
+#include "pid.h"
+
+#define CAN_TX1_ID 0x200
+#define CAN_TX2_ID 0x1FF
+
+#define CAN_RX1_START 0x201
+#define CAN_RX2_START 0x205
+
+typedef enum {
+ /* can motors */
+ M3508,
+ GM6020,
+ M2006,
+ general_motor,
+ /* pwm motors */
+ M2305,
+} motor_type_t;
+//
+
+typedef struct {
+ uint16_t rx_id;
+ uint16_t tx_id;
+ uint8_t can_id;
+} can_info_t;
+
+
+
+/**
+ * @struct GM6020_info_t
+ * @brief store GM6020 motor data
+ * @var rx_id sensor CAN receive id
+ * @var tx_id sensor CAN transmit id
+ * @var can_id CAN id chosen from [CAN1, CAN2]
+ * @var angle most recent angle data
+ * @var current_get actual current / torque output
+ * @var current_set target current / torque output
+ */
+typedef struct {
+ int16_t angle;
+ int16_t speed;
+ int16_t current;
+ int8_t temp;
+} GM6020_info_t;
+
+typedef union{
+ GM6020_info_t GM6020_info;
+} motor_info_t;
+
+// Une struct par moteur
+typedef struct
+{
+ can_info_t can_info; // Can ID information
+ motor_info_t info; // Specific info about the motor
+ motor_type_t type; // Type du moteur, ex:GM6020
+ pid_struct_t pid; // Controler
+ float target;
+ float voltage;
+} motor_t;
+
+//id:0 -> 0x200, 1 -> 0x1ff
+void set_motor_voltage(uint8_t id, motor_t *motor1, motor_t *motor2, motor_t *motor3, motor_t *motor4);
+
+static int16_t correct_output(motor_t *motor);
+uint8_t get_motor_data(motor_t *motor);
+static void get_6020_data(motor_t* motor, uint8_t buf[CAN_DATA_SIZE]);
+#endif
diff --git a/Driver/bsp_can.c b/Driver/bsp_can.c
index 0df78112513c7c596d43b75827b516674fefbfde..d91f60676710154ad741de41942ba7d78bb15115 100644
--- a/Driver/bsp_can.c
+++ b/Driver/bsp_can.c
@@ -18,8 +18,8 @@
#include "bsp_can.h"
#include "bsp_led.h"
-moto_info_t motor_info[MOTOR_MAX_NUM];
uint16_t can_cnt;
+uint8_t can1_rx_buffer[MOTOR_MAX_NUM][CAN_DATA_SIZE];
/**
* @brief init can filter, start can, enable can rx interrupt
@@ -55,22 +55,19 @@ void can_user_init(CAN_HandleTypeDef* hcan )
*/
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
- CAN_RxHeaderTypeDef rx_header;
- uint8_t rx_data[8];
+ CAN_RxHeaderTypeDef rx_header;
+ /* get device id ahead of time */
+ rx_header.StdId = (CAN_RI0R_STID & hcan->Instance->sFIFOMailBox[CAN_RX_FIFO0].RIR) >> CAN_TI0R_STID_Pos;
+
if(hcan->Instance == CAN1)
{
- HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &rx_header, rx_data); //receive can data
- }
- if ((rx_header.StdId >= FEEDBACK_ID_BASE)
- && (rx_header.StdId < FEEDBACK_ID_BASE + MOTOR_MAX_NUM)) // judge the can id
- {
- can_cnt ++;
- uint8_t index = rx_header.StdId - FEEDBACK_ID_BASE; // get motor index by can_id
- motor_info[index].rotor_angle = ((rx_data[0] << 8) | rx_data[1]);
- motor_info[index].rotor_speed = ((rx_data[2] << 8) | rx_data[3]);
- motor_info[index].torque_current = ((rx_data[4] << 8) | rx_data[5]);
- motor_info[index].temp = rx_data[6];
- }
+ if ((rx_header.StdId >= FEEDBACK_ID_BASE) && (rx_header.StdId < FEEDBACK_ID_BASE + MOTOR_MAX_NUM)) // judge the can id
+ {
+ can_cnt ++;
+ uint8_t index = rx_header.StdId - FEEDBACK_ID_BASE; // get motor index by can_id
+ HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &rx_header, can1_rx_buffer[index]); //receive can data
+ }
+ }
if (can_cnt == 500)
{
can_cnt = 0;
@@ -78,29 +75,41 @@ void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
}
}
+
+
+void can1_transmit(uint16_t id, int16_t msg1, int16_t msg2, int16_t msg3, int16_t msg4) {
+ can_transmit(id, msg1, msg2, msg3, msg4);
+}
+
+void can1_read(uint16_t id, uint8_t buf[CAN_DATA_SIZE]) {
+ uint8_t idx = id - FEEDBACK_ID_BASE;
+ memcpy(buf, can1_rx_buffer[idx], CAN_DATA_SIZE);
+}
+
/**
* @brief send motor control message through can bus
* @param id_range to select can control id 0x1ff or 0x2ff
* @param motor voltage 1,2,3,4 or 5,6,7
* @retval None
*/
-void set_motor_voltage(uint8_t id_range, int16_t v1, int16_t v2, int16_t v3, int16_t v4)
+void can_transmit(uint8_t id, int16_t val1, int16_t val2, int16_t val3, int16_t val4)
{
CAN_TxHeaderTypeDef tx_header;
uint8_t tx_data[8];
- tx_header.StdId = (id_range == 0)?(0x200):(0x1ff);
+ tx_header.StdId = (id == 0)?(0x200):(0x1ff);
tx_header.IDE = CAN_ID_STD;
tx_header.RTR = CAN_RTR_DATA;
tx_header.DLC = 8;
+ tx_header.TransmitGlobalTime = DISABLE;
- tx_data[0] = (v1>>8)&0xff;
- tx_data[1] = (v1)&0xff;
- tx_data[2] = (v2>>8)&0xff;
- tx_data[3] = (v2)&0xff;
- tx_data[4] = (v3>>8)&0xff;
- tx_data[5] = (v3)&0xff;
- tx_data[6] = (v4>>8)&0xff;
- tx_data[7] = (v4)&0xff;
+ tx_data[0] = (val1>>8)&0xff;
+ tx_data[1] = (val1)&0xff;
+ tx_data[2] = (val2>>8)&0xff;
+ tx_data[3] = (val2)&0xff;
+ tx_data[4] = (val3>>8)&0xff;
+ tx_data[5] = (val3)&0xff;
+ tx_data[6] = (val4>>8)&0xff;
+ tx_data[7] = (val4)&0xff;
HAL_CAN_AddTxMessage(&hcan1, &tx_header, tx_data,(uint32_t*)CAN_TX_MAILBOX0);
}
diff --git a/Driver/bsp_can.h b/Driver/bsp_can.h
index 04cee92f1dd387cdef4127fccb86940a1ccaca6d..7f8b125909ff05b403ebb1c1a3f672c84ba126da 100644
--- a/Driver/bsp_can.h
+++ b/Driver/bsp_can.h
@@ -19,11 +19,14 @@
#define __BSP_CAN
#include "can.h"
+#include <stdio.h>
+#include <string.h>
-#define FEEDBACK_ID_BASE 0x201
+#define FEEDBACK_ID_BASE 0x204
#define CAN_CONTROL_ID_BASE 0x200
#define CAN_CONTROL_ID_EXTEND 0x1ff
#define MOTOR_MAX_NUM 7
+#define CAN_DATA_SIZE 8
typedef struct
{
@@ -36,5 +39,7 @@ typedef struct
}moto_info_t;
void can_user_init(CAN_HandleTypeDef* hcan);
-void set_motor_voltage(uint8_t id_range, int16_t v1, int16_t v2, int16_t v3, int16_t v4);
+void can1_transmit(uint16_t id, int16_t val1, int16_t val2, int16_t val3, int16_t val4);
+void can1_read(uint16_t id, uint8_t buf[CAN_DATA_SIZE]);
+void can_transmit(uint8_t id_range, int16_t val1, int16_t val2, int16_t val3, int16_t val4);
#endif