Device/motor.c

+#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

Device/motor.h

+#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

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); 
 }

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