feat:add motor and encoder based on can-bus and add a kinematics

encoder/can_encoder.c

@@ -0,0 +1,83 @@
+/*
+ * Copyright (c) 2019, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2019-11-14     Soil_L       The first version
+ */
+#include "can_encoder.h"
+
+#define DBG_SECTION_NAME  "can_encoder"
+#define DBG_LEVEL         DBG_LOG
+#include <rtdbg.h>
+
+rt_sem_t rx_sem_g;
+
+rt_err_t encoder_isr(rt_device_t dev, rt_size_t size)
+{
+    rt_sem_release(rx_sem_g);
+    return RT_EOK;
+}
+
+static rt_err_t can_encoder_enable(void *enc)
+{
+    can_encoder_t enc_sub = (can_encoder_t)enc;
+
+    volatile rt_err_t res;
+
+    //Avoid repeated opening 
+    if(!(enc_sub->can_dev->flag & RT_DEVICE_FLAG_ACTIVATED))
+    {
+        res = rt_device_open(enc_sub->can_dev,RT_DEVICE_FLAG_INT_RX|RT_DEVICE_FLAG_INT_TX);
+        res = rt_device_control(enc_sub->can_dev,RT_CAN_CMD_SET_BAUD,(void *)enc_sub->Baud);
+        res = rt_device_control(enc_sub->can_dev,RT_CAN_CMD_SET_FILTER,&enc_sub->cfg);
+    }
+    res = rt_device_set_rx_indicate(enc_sub->can_dev,encoder_isr);
+
+    return RT_EOK;
+}
+
+static rt_err_t can_encoder_disable(void *enc)
+{
+    return RT_EOK;
+}
+
+static rt_err_t can_encoder_destroy(void *enc)
+{
+    can_encoder_t enc_sub = (can_encoder_t)enc;
+
+    volatile rt_err_t res;
+    res = rt_device_close(enc_sub->can_dev);
+    rt_sem_delete(*(enc_sub->rx_sem));
+    rt_free(enc_sub);
+
+    return RT_EOK;
+}
+
+can_encoder_t can_encoder_create(char *can,struct rt_can_filter_config cfg,rt_uint32_t Baud,rt_sem_t* rx_sem)
+{
+    can_encoder_t new_encoder = (can_encoder_t)encoder_create(sizeof(struct can_encoder),0);
+    if(new_encoder == RT_NULL)
+    {
+        return RT_NULL;
+    }
+
+    new_encoder->can_dev = (rt_device_t)rt_device_find(can);
+    if (new_encoder->can_dev == RT_NULL)
+    {
+        rt_free(new_encoder);
+        LOG_E("Falied to find device on %s", can);
+        return RT_NULL;
+    }
+
+    new_encoder->cfg = cfg;
+    new_encoder->enc.enable = can_encoder_enable;
+    new_encoder->enc.disable = can_encoder_disable;
+    new_encoder->enc.destroy = can_encoder_destroy;
+    new_encoder->Baud = Baud;
+    new_encoder->rx_sem = rx_sem;
+    rx_sem_g = *rx_sem;
+    return new_encoder;
+}

encoder/can_encoder.h

@@ -0,0 +1,28 @@
+/*
+ * Copyright (c) 2019, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2019-11-14     Soil_L       The first version
+ */
+#ifndef _CAN_ENCODER_
+#define _CAN_ENCODER_
+
+#include "encoder.h"
+
+struct can_encoder
+{
+	struct encoder              enc;
+	rt_device_t                 can_dev;
+	struct rt_can_filter_config cfg;
+	rt_uint32_t                 Baud;
+	rt_sem_t                    *rx_sem;
+	rt_uint32_t                 rpm;
+};
+typedef struct can_encoder *can_encoder_t;
+
+can_encoder_t can_encoder_create(char *can,struct rt_can_filter_config cfg,rt_uint32_t Baud,rt_sem_t* rx_sem);
+
+#endif  //CAN_ENCODER_

encoder/encoder.c

@@ -6,6 +6,7 @@
  * Change Logs:
  * Date           Author       Notes
  * 2019-08-26     sogwms       The first version
+ * 2019-11-14     Soil_L       motified for the need of can encoder
  */
 
 #include <rtdevice.h>
@@ -96,10 +97,16 @@ rt_int16_t encoder_measure_cps(encoder_t enc)
 rt_int16_t encoder_measure_rpm(encoder_t enc)
 {
     RT_ASSERT(enc != RT_NULL);
-
-    // return resolution per minute
-    rt_int16_t res_rpm = encoder_measure_cps(enc) * 60 / enc->pulse_revol;
-
+    rt_int16_t res_rpm;
+    if(enc->sample_time == 0)
+    {
+    	res_rpm = enc->pulse_count;
+    }
+    else
+    {
+    	// return resolution per minute
+    	res_rpm = encoder_measure_cps(enc) * 60 / enc->pulse_revol;
+    }
     return res_rpm;
 }
 

kinematics/kinematics.c

@@ -6,6 +6,7 @@
  * Change Logs:
  * Date           Author       Notes
  * 2019-07-17     Wu Han       The first version
+ * 2019-11-14     Soil_L       add four wheel omnidirectional chassis
  */
 
 #include "kinematics.h"
@@ -46,7 +47,11 @@ kinematics_t kinematics_create(enum base k_base, float length_x, float length_y,
     {
         new_kinematics->total_wheels = 4;
     }
-
+    if(k_base == FOUR_WD_ALLDIR)
+    {
+        new_kinematics->total_wheels = 4;
+    }
+
     return new_kinematics;
 }
 
@@ -136,6 +141,13 @@ void kinematics_get_rpm(struct kinematics kin, struct velocity target_vel, rt_in
         res_rpm[2] = cal_rpm.motor3;
         res_rpm[3] = cal_rpm.motor4;
     }
+    else if(kin.k_base == FOUR_WD_ALLDIR) //FRONT:0   BACK:1   LEFT:2   RIGHT:3
+    {
+        res_rpm[0] = x_rpm + tan_rpm;
+        res_rpm[1]  = -x_rpm + tan_rpm;
+        res_rpm[2]  = y_rpm + tan_rpm;
+        res_rpm[3] = -y_rpm + tan_rpm;
+    }
     else
     {
         return;
@@ -158,25 +170,43 @@ void kinematics_get_velocity(struct kinematics kin, struct rpm current_rpm, stru
     if(kin.k_base == FOUR_WD) total_wheels = 4;
     if(kin.k_base == ACKERMANN) total_wheels = 2;
     if(kin.k_base == MECANUM) total_wheels = 4;
+    if(kin.k_base == FOUR_WD_ALLDIR) total_wheels = 4;
 
     float average_rps_x;
     float average_rps_y;
     float average_rps_a;
 
-    //convert average revolutions per minute to revolutions per second
-    average_rps_x = ((float)(current_rpm.motor1 + current_rpm.motor2 + current_rpm.motor3 + current_rpm.motor4) / total_wheels) / 60; // RPM
-    res_vel.linear_x = average_rps_x * kin.wheel_cir; // m/s
-
-    //convert average revolutions per minute in y axis to revolutions per second
-    average_rps_y = ((float)(-current_rpm.motor1 + current_rpm.motor2 + current_rpm.motor3 - current_rpm.motor4) / total_wheels) / 60; // RPM
-    if(kin.k_base == MECANUM)
-        res_vel.linear_y = average_rps_y * kin.wheel_cir; // m/s
-    else
-        res_vel.linear_y = 0;
-
-    //convert average revolutions per minute to revolutions per second
-    average_rps_a = ((float)(-current_rpm.motor1 + current_rpm.motor2 - current_rpm.motor3 + current_rpm.motor4) / total_wheels) / 60;
-    res_vel.angular_z =  (average_rps_a * kin.wheel_cir) / ((kin.length_x / 2) + (kin.length_y / 2)); //  rad/s
-
+    if(kin.k_base == TWO_WD || kin.k_base == FOUR_WD ||  kin.k_base == ACKERMANN)
+    {
+      //convert average revolutions per minute to revolutions per second
+      average_rps_x = ((float)(current_rpm.motor1 + current_rpm.motor2 + current_rpm.motor3 + current_rpm.motor4) / total_wheels) / 60; // RPM
+      res_vel.linear_x = average_rps_x * kin.wheel_cir; // m/s
+
+      //convert average revolutions per minute in y axis to revolutions per second
+      average_rps_y = ((float)(-current_rpm.motor1 + current_rpm.motor2 + current_rpm.motor3 - current_rpm.motor4) / total_wheels) / 60; // RPM
+      if(kin.k_base == MECANUM)
+          res_vel.linear_y = average_rps_y * kin.wheel_cir; // m/s
+      else
+          res_vel.linear_y = 0;
+
+      //convert average revolutions per minute to revolutions per second
+      average_rps_a = ((float)(-current_rpm.motor1 + current_rpm.motor2 - current_rpm.motor3 + current_rpm.motor4) / total_wheels) / 60;
+      res_vel.angular_z =  (average_rps_a * kin.wheel_cir) / ((kin.length_x / 2) + (kin.length_y / 2)); //  rad/s
+    }
+    if(kin.k_base == FOUR_WD_ALLDIR)//FRONT:motor1  BACK:motor2  LEFT:motor3  RIGHT:motor4
+    {
+      //convert average revolutions per minute to revolutions per second
+      average_rps_x = ((float)(current_rpm.motor1 - current_rpm.motor2) / 2) / 60; // RPM
+      res_vel.linear_x = average_rps_x * kin.total_wheels; // m/s
+
+      //convert average revolutions per minute in y axis to revolutions per second
+      average_rps_y = ((float)(current_rpm.motor3 - current_rpm.motor4) / 2) / 60; // RPM
+      res_vel.linear_y = average_rps_y * kin.wheel_cir; // m/s
+
+      //convert average revolutions per minute to revolutions per second
+      average_rps_a = ((float)(current_rpm.motor1 + current_rpm.motor2 + current_rpm.motor3 + current_rpm.motor4 
+      							- 2*60*(average_rps_x+average_rps_y)) / total_wheels) / 60;
+      res_vel.angular_z =  (average_rps_a * kin.wheel_cir) / ((kin.length_x / 2) + (kin.length_y / 2)); //  rad/s
+    }
     rt_memcpy(velocity, &res_vel, sizeof(struct velocity));
 }

kinematics/kinematics.h

@@ -6,6 +6,7 @@
  * Change Logs:
  * Date           Author       Notes
  * 2019-07-17     Wu Han       The first version
+ * 2019-11-14     Soil_L       add four wheel omnidirectional
  */
 
 #ifndef __KINEMATICS_H__
@@ -17,7 +18,8 @@ enum base {
     TWO_WD = 0, 
     FOUR_WD, 
     ACKERMANN, 
-    MECANUM
+    MECANUM,
+    FOUR_WD_ALLDIR
 };
 
 // rad/min

motor/can_motor.c

@@ -0,0 +1,96 @@
+/*
+ * Copyright (c) 2019, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2019-11-14     Soil_L       The first version
+ */
+#include "can_motor.h"
+
+#define DBG_SECTION_NAME  "can_motor"
+#define DBG_LEVEL         DBG_LOG
+#include <rtdbg.h>
+
+
+static rt_err_t can_motor_enable(void *mot)
+{
+    can_motor_t mot_sub = (can_motor_t)mot;
+
+    //Avoid repeated opening 
+    if(!(mot_sub->can_dev->flag & RT_DEVICE_FLAG_ACTIVATED))
+    {
+        rt_err_t res = rt_device_open(mot_sub->can_dev,RT_DEVICE_FLAG_INT_TX | RT_DEVICE_FLAG_INT_RX );
+        res = rt_device_control(mot_sub->can_dev,RT_CAN_CMD_SET_BAUD,(void *)mot_sub->Baud);
+        res = rt_device_control(mot_sub->can_dev,RT_CAN_CMD_SET_FILTER,&mot_sub->cfg);
+    }
+    return RT_EOK;
+}
+
+static rt_err_t can_motor_disable(void *mot)
+{
+    can_motor_t mot_sub = (can_motor_t)mot;
+
+    rt_err_t res = rt_device_close(mot_sub->can_dev);
+
+    return RT_EOK;
+}
+
+static rt_err_t can_motor_reset(void *mot)
+{
+    return RT_EOK;
+}
+
+static rt_err_t can_motor_set_speed(void *mot, rt_int16_t thousands)
+{
+    can_motor_t mot_sub = (can_motor_t)mot;
+
+    if(mot_sub->min_num > thousands)
+    {
+        thousands = mot_sub->min_num;
+    }
+    else if(mot_sub->max_num < thousands)
+    {
+        thousands = mot_sub->max_num;
+    }
+
+    mot_sub->msg->data[(mot_sub->mot_id -1)*2] = thousands>>8;
+    mot_sub->msg->data[(mot_sub->mot_id -1)*2+1] = thousands;
+    rt_size_t  size = rt_device_write(mot_sub->can_dev, 0, mot_sub->msg, sizeof(*(mot_sub->msg)) );
+
+    return RT_EOK;
+}
+
+
+can_motor_t can_motor_create(char *can,struct rt_can_filter_config cfg,rt_uint32_t Baud,rt_can_msg_t msg,rt_int16_t max_num,rt_int16_t min_num,rt_uint8_t mot_id)
+{
+    RT_ASSERT(max_num>min_num)
+
+    can_motor_t new_motor = (can_motor_t)motor_create(sizeof(struct can_motor));
+    if (new_motor == RT_NULL)
+    {
+        return RT_NULL;
+    }
+
+    new_motor->can_dev = (rt_device_t)rt_device_find(can);
+    if (new_motor->can_dev == RT_NULL)
+    {
+        rt_free(new_motor);
+        LOG_E("Falied to find device on %s", can);
+        return RT_NULL;
+    }
+
+    new_motor->cfg = cfg;
+    new_motor->Baud = Baud;
+    new_motor->msg = msg;
+    new_motor->max_num = max_num;
+    new_motor->min_num = min_num;
+    new_motor->mot_id = mot_id;
+    new_motor->mot.enable = can_motor_enable;
+    new_motor->mot.set_speed = can_motor_set_speed;
+    new_motor->mot.reset = can_motor_reset;
+    new_motor->mot.disable = can_motor_disable;
+
+    return new_motor;
+}

motor/can_motor.h

@@ -0,0 +1,30 @@
+/*
+ * Copyright (c) 2019, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2019-11-14     Soil_L       The first version
+ */
+#ifndef _CAN_MOTOR_H_
+#define _CAN_MOTOR_H_
+
+#include "motor.h"
+
+struct can_motor
+{
+    struct motor                mot;
+    rt_device_t                 can_dev;
+    struct rt_can_filter_config cfg;
+    rt_uint32_t                 Baud;
+    rt_can_msg_t                msg;  //the pointer of message defined by the users,it decide the number of the can motor in the can net
+    rt_int16_t                  max_num;
+    rt_int16_t                  min_num;
+    rt_uint8_t                  mot_id;	//the id of the can motor in the can net
+};
+typedef struct can_motor *can_motor_t;
+
+can_motor_t can_motor_create(char *can,struct rt_can_filter_config cfg,rt_uint32_t Baud,rt_can_msg_t msg,rt_int16_t max_num,rt_int16_t min_num,rt_uint8_t mot_id);
+
+#endif // __CAN_MOTOR_H__