init commit

modules/motor/HT04.c

@@ -0,0 +1,55 @@
+#include "HT04.h"
+#include "memory.h"
+
+joint_instance joint_motor_info[HT_MOTOR_CNT];
+
+static uint16_t float_to_uint(float x, float x_min, float x_max, uint8_t bits)
+{
+    float span = x_max - x_min;
+    float offset = x_min;
+    return (uint16_t) ((x-offset)*((float)((1<<bits)-1))/span);
+}
+
+static float uint_to_float(int x_int, float x_min, float x_max, int bits)
+{
+    float span = x_max - x_min;
+    float offset = x_min;
+    return ((float)x_int)*span/((float)((1<<bits)-1)) + offset;
+}
+
+void JointControl(joint_instance* _instance,float current)
+{
+    uint16_t tmp;
+    LIMIT_MIN_MAX(current,  T_MIN,  T_MAX);
+    tmp = float_to_uint(current, T_MIN, T_MAX, 12);
+    _instance->motor_can_instace.rx_buff[6] = tmp>>8;
+    _instance->motor_can_instace.rx_buff[7] = tmp&0xff;
+    CANTransmit(_instance);
+}
+
+void SetJointMode(joint_mode cmd,joint_instance* _instance)
+{
+    static uint8_t buf[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x00};
+    buf[7]=(uint8_t)cmd;
+    memcpy(_instance->motor_can_instace.rx_buff,buf,8*sizeof(uint8_t));
+    CANTransmit(&_instance->motor_can_instace);
+}
+
+void DecodeJoint(can_instance* motor_instance)
+{
+    uint16_t tmp;
+    for (size_t i = 0; i < HT_MOTOR_CNT; i++)
+    {
+        if(&joint_motor_info[i].motor_can_instace==motor_instance)
+        {
+            tmp = (motor_instance->rx_buff[1] << 8) | motor_instance->rx_buff[2];
+            joint_motor_info[i].last_ecd=joint_motor_info[i].ecd;
+            joint_motor_info[i].ecd=uint_to_float(tmp,P_MAX,P_MIN,16);
+            tmp = (motor_instance->rx_buff[3] << 4) | (motor_instance->rx_buff[4] >> 4);
+            joint_motor_info[i].speed_rpm= uint_to_float(tmp,V_MAX,V_MIN,12);
+            tmp=((motor_instance->rx_buff[4]&0xf)<<8) | motor_instance->rx_buff[5];
+            joint_motor_info[i].given_current=uint_to_float(tmp,T_MAX,T_MIN,12);
+            break;
+        }
+    }
+}

modules/motor/HT04.h

@@ -0,0 +1,38 @@
+#ifndef HT04_H
+#define HT04_H
+
+#include "struct_typedef.h"
+#include "bsp_can.h"
+
+#define HT_MOTOR_CNT 4
+
+#define P_MIN -95.5f    // Radians
+#define P_MAX 95.5f
+#define V_MIN -45.0f    // Rad/s
+#define V_MAX 45.0f
+#define T_MIN -18.0f
+#define T_MAX 18.0f
+
+typedef struct //HT04
+{
+    float last_ecd;
+    float ecd;
+    float speed_rpm;
+    float given_current;
+    can_instance motor_can_instace;
+} joint_instance;
+
+typedef enum
+{
+    CMD_MOTOR_MODE    = 0xfc,
+    CMD_RESET_MODE    = 0xfd,
+    CMD_ZERO_POSITION = 0xfe
+} joint_mode;
+
+
+void JointControl(joint_instance* _instance,float current);
+
+void SetJointMode(joint_mode cmd,joint_instance* _instance);
+
+void DecodeJoint(can_instance* motor_instance);
+#endif // !HT04_H#define HT04_H

modules/motor/LK9025.c

@@ -0,0 +1,48 @@
+#include"LK9025.h"
+
+static driven_instance driven_motor_info[2];
+
+void LKMotroInit(uint16_t motor_id,uint16_t rx_id,CAN_HandleTypeDef* hcan)
+{
+    static uint8_t idx;
+    driven_motor_info[idx].motor_can_instance.can_handle=hcan;
+    driven_motor_info[idx].motor_can_instance.can_module_callback=DecodeDriven;
+    driven_motor_info[idx].motor_can_instance.rx_id=rx_id;
+    driven_motor_info[idx].motor_can_instance.tx_id=motor_id;
+    CANRegister(&driven_motor_info[idx].motor_can_instance);
+}
+
+void DrivenControl(int16_t motor1_current,int16_t motor2_current)
+{
+    // LIMIT_MIN_MAX(motor1_current,  I_MIN,  I_MAX);
+    // LIMIT_MIN_MAX(motor2_current,  I_MIN,  I_MAX);
+    driven_motor_info[0].motor_can_instance.tx_buff[0] = motor1_current;
+    driven_motor_info[0].motor_can_instance.tx_buff[1] = motor1_current>>8;
+    driven_motor_info[0].motor_can_instance.tx_buff[2] = motor2_current;
+    driven_motor_info[0].motor_can_instance.tx_buff[3] = motor2_current>>8;
+    CANTransmit(&driven_motor_info[0].motor_can_instance);
+}
+
+void SetDrivenMode(driven_mode cmd,uint16_t motor_id)
+{
+    static uint8_t buf[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x00};
+    // code goes here ...
+    
+    // CANTransmit(driven_mode)
+}
+
+void DecodeDriven(can_instance* _instance)
+{
+    for (size_t i = 0; i < LK_MOTOR_CNT; i++)
+    {
+        if(&driven_motor_info[i].motor_can_instance==_instance)
+        {
+            driven_motor_info[i].last_ecd = driven_motor_info[i].ecd;                                   
+            driven_motor_info[i].ecd = (uint16_t)((_instance->rx_buff[7]<<8) | _instance->rx_buff[6]);             
+            driven_motor_info[i].speed_rpm = (uint16_t)(_instance->rx_buff[5] << 8 | _instance->rx_buff[4]);      
+            driven_motor_info[i].given_current = (uint16_t)(_instance->rx_buff[3] << 8 | _instance->rx_buff[2]);  
+            driven_motor_info[i].temperate = _instance->rx_buff[1];
+            break;
+        }
+    }
+}

modules/motor/LK9025.h

@@ -0,0 +1,36 @@
+#ifndef LK9025_H
+#define LK9025_H
+
+#include "struct_typedef.h"
+#include "bsp_can.h"
+
+#define LK_MOTOR_CNT 2
+#define I_MIN -2000
+#define I_MAX 2000
+
+#define LIMIT_MIN_MAX(x,min,max) (x) = (((x)<=(min))?(min):(((x)>=(max))?(max):(x)))
+
+typedef struct //9025
+{
+    uint16_t last_ecd;
+    uint16_t ecd;
+    int16_t speed_rpm;
+    int16_t given_current;
+    uint8_t temperate;
+    can_instance motor_can_instance;
+} driven_instance;
+
+typedef enum
+{
+    unused = 0,
+} driven_mode;
+
+void LKMotroInit(uint16_t motor_id,uint16_t rx_id,CAN_HandleTypeDef* hcan);
+
+void DrivenControl(int16_t motor1_current,int16_t motor2_current);
+
+void SetDrivenMode(driven_mode cmd,uint16_t motor_id);
+
+void DecodeDriven(can_instance* _instance);
+
+#endif // LK9025_H