Initial Commit

.gitignore

@@ -0,0 +1,53 @@
+# Prerequisites
+*.d
+
+# Object files
+*.o
+*.ko
+*.obj
+*.elf
+
+# Linker output
+*.ilk
+*.map
+*.exp
+
+# Precompiled Headers
+*.gch
+*.pch
+
+# Libraries
+*.lib
+*.a
+*.la
+*.lo
+
+# Shared objects (inc. Windows DLLs)
+*.dll
+*.so
+*.so.*
+*.dylib
+
+# Executables
+*.exe
+*.out
+*.app
+*.i*86
+*.x86_64
+*.hex
+
+# Debug files
+*.dSYM/
+*.su
+*.idb
+*.pdb
+
+# Kernel Module Compile Results
+*.mod*
+*.cmd
+.tmp_versions/
+modules.order
+Module.symvers
+Mkfile.old
+dkms.conf
+

SConscript

@@ -0,0 +1,14 @@
+# for module compiling
+import os
+from building import *
+
+cwd  = GetCurrentDir()
+objs = []
+list = os.listdir(cwd)
+
+for d in list:
+    path = os.path.join(cwd, d)
+    if os.path.isfile(os.path.join(path, 'SConscript')):
+        objs = objs + SConscript(os.path.join(d, 'SConscript'))
+
+Return('objs')

chassis/SConscript

@@ -0,0 +1,8 @@
+from building import *
+
+cwd   = GetCurrentDir()
+src	  = Glob('*.c')
+PATH  = [cwd]
+group = DefineGroup('robots', src, depend = [], CPPPATH = PATH)
+
+Return('group')

chassis/chassis.c

@@ -0,0 +1,93 @@
+#include "chassis.h"
+
+#define DBG_SECTION_NAME  "chassis"
+#define DBG_LEVEL         DBG_LOG
+#include <rtdbg.h>
+
+chassis_t chassis_create(wheel_t* c_wheels, kinematics_t c_kinematics)
+{
+    // Malloc memory for new chassis
+    chassis_t new_chassis = (chassis_t) rt_malloc(sizeof(struct chassis));
+    if(new_chassis == RT_NULL)
+    {
+        LOG_E("Falied to allocate memory for chassis\n");
+        return RT_NULL;
+    }
+
+    new_chassis -> c_wheels      = c_wheels;
+    new_chassis -> c_kinematics  = c_kinematics;
+
+    return new_chassis;
+}
+
+void chassis_destroy(chassis_t chas)
+{
+    for(int i = 0; i < chas->c_kinematics->total_wheels; i++)
+    {
+        LOG_I("Free wheel %d", i);
+        wheel_destroy(chas->c_wheels[i]);
+    }
+    kinematics_destroy(chas->c_kinematics);
+
+    rt_free(chas);
+}
+
+rt_err_t chassis_enable(chassis_t chas)
+{
+    for(int i = 0; i < chas->c_kinematics->total_wheels; i++)
+    {
+        LOG_I("Enabling wheel %d", i);
+        wheel_enable(chas->c_wheels[i]);
+    }
+
+    return RT_EOK;
+}
+
+rt_err_t chassis_disable(chassis_t chas)
+{
+    // TODO
+
+    return RT_EOK;
+}
+
+rt_err_t chassis_reset(chassis_t chas)
+{
+    // TODO
+
+    return RT_EOK;
+}
+
+rt_err_t chassis_set_velocity(chassis_t chas, struct velocity target_velocity)
+{
+    rt_int16_t* res_rpm = kinematics_get_rpm(*chas->c_kinematics, target_velocity);
+    chassis_set_rpm(chas, res_rpm);
+
+    return RT_EOK;
+}
+
+rt_err_t chassis_set_rpm(chassis_t chas, rt_int16_t target_rpm[])
+{
+    // Set new speed
+    for(int i = 0; i < chas->c_kinematics->total_wheels; i++)
+    {
+        LOG_I("Set wheel %d speed %d rpm", i, target_rpm[i]);
+        wheel_set_rpm(chas->c_wheels[i], target_rpm[i]);
+    }
+
+    for(int i = 0; i < chas->c_kinematics->total_wheels; i++)
+    {
+        wheel_update(chas->c_wheels[i]);
+    }
+
+    return RT_EOK;
+}
+
+rt_err_t chassis_update(chassis_t chas)
+{
+    // TODO
+    for(int i = 0; i < chas->c_kinematics->total_wheels; i++)
+    {
+        wheel_update(chas->c_wheels[i]);
+    }
+    return RT_EOK;
+}

chassis/chassis.h

@@ -0,0 +1,28 @@
+#ifndef __CHASSIS_H__
+#define __CHASSIS_H__
+
+#include <kinematics.h>
+#include <wheel.h>
+
+typedef struct chassis *chassis_t;
+
+struct chassis
+{
+    wheel_t*        c_wheels;
+    kinematics_t    c_kinematics;
+    struct velocity c_velocity;
+};
+
+chassis_t   chassis_create(wheel_t* c_wheel, kinematics_t c_kinematics);
+void        chassis_destroy(chassis_t chas);
+
+rt_err_t    chassis_enable(chassis_t chas);
+rt_err_t    chassis_disable(chassis_t chas);
+rt_err_t    chassis_reset(chassis_t chas);
+
+rt_err_t    chassis_set_velocity(chassis_t chas, struct velocity target_velocity);
+rt_err_t    chassis_set_rpm(chassis_t chas, rt_int16_t target_rpm[]);
+
+rt_err_t    chassis_update(chassis_t chas);
+
+#endif

controller/SConscript

@@ -0,0 +1,14 @@
+# for module compiling
+import os
+from building import *
+
+cwd  = GetCurrentDir()
+objs = []
+list = os.listdir(cwd)
+
+for d in list:
+    path = os.path.join(cwd, d)
+    if os.path.isfile(os.path.join(path, 'SConscript')):
+        objs = objs + SConscript(os.path.join(d, 'SConscript'))
+
+Return('objs')

encoder/SConscript

@@ -0,0 +1,8 @@
+from building import *
+
+cwd   = GetCurrentDir()
+src	  = Glob('*.c')
+PATH  = [cwd]
+group = DefineGroup('robots', src, depend = [], CPPPATH = PATH)
+
+Return('group')

encoder/encoder.c

@@ -0,0 +1,102 @@
+#include <rtdevice.h>
+#include <encoder.h>
+
+#define DBG_SECTION_NAME  "encoder"
+#define DBG_LEVEL         DBG_LOG
+#include <rtdbg.h>
+
+void encoder_isr(void *args)
+{
+    rt_uint16_t* pulse_count = (rt_uint16_t*)args;
+    (*pulse_count)++;
+    // LOG_D("Count %d", *pulse_count);
+}
+
+encoder_t encoder_create(rt_base_t pin, rt_uint16_t pulse_revol)
+{
+    // Malloc memory and initialize pulse_count and pin
+    encoder_t new_encoder = (encoder_t)rt_malloc(sizeof(struct encoder));
+    if(new_encoder == RT_NULL)
+    {
+        LOG_E("Failed to malloc memory for new encoder\n");
+        return RT_NULL;
+    }
+
+    new_encoder->pulse_count = 0;
+    new_encoder->pin = pin;
+    new_encoder->pulse_revol = pulse_revol;
+    new_encoder->last_count = 0;
+
+    return new_encoder;
+}
+
+void encoder_destroy(encoder_t enc)
+{
+    LOG_D("Free Encoder");
+
+    rt_free(enc);
+}
+
+rt_err_t encoder_enable(encoder_t enc)
+{
+    LOG_D("Enabling encoder");
+
+    // Attach interrupts
+    rt_pin_mode(enc->pin, PIN_MODE_INPUT_PULLUP);
+    rt_pin_attach_irq(enc->pin, PIN_IRQ_MODE_FALLING, encoder_isr, &(enc->pulse_count));
+    enc->last_time = rt_tick_get();
+
+    return rt_pin_irq_enable(enc->pin, PIN_IRQ_ENABLE);
+}
+
+rt_err_t encoder_disable(encoder_t enc)
+{
+    LOG_D("Diabling encoder");
+
+    return rt_pin_detach_irq(enc->pin);
+}
+
+rt_int16_t encoder_read(encoder_t enc)
+{
+    return enc->pulse_count;
+}
+
+rt_err_t encoder_reset(encoder_t enc)
+{
+    enc -> pulse_count = 0;
+    return RT_EOK;
+}
+
+rt_int16_t encoder_measure_cps(encoder_t enc)
+{
+    // TODO
+    // return count per second
+    if((rt_tick_get() - enc->last_time) < rt_tick_from_millisecond(enc->sample_time))
+    {
+        // LOG_D("Encoder waiting ... ");
+        // LOG_D("Current count per second %d", enc->cps);
+        return enc->cps;
+    }
+
+    enc->cps = (enc->pulse_count - enc->last_count) * 1000 / enc->sample_time;
+    enc->last_count = enc->pulse_count;
+    enc->last_time = rt_tick_get();
+    // LOG_D("Current count per second %d", enc->cps);
+
+    return enc->cps;
+}
+
+rt_int16_t encoder_measure_rpm(encoder_t enc)
+{
+    // return resolution per minute
+    // LOG_D("Current count per revolution %d",  enc->pulse_revol);
+
+    rt_int16_t res_rpm = encoder_measure_cps(enc) * 60 / enc->pulse_revol;
+    // LOG_D("Current revol per minute %d", res_rpm);
+    return res_rpm;
+}
+
+void encoder_set_sample_time(encoder_t enc, rt_uint16_t sample_time)
+{
+    enc->sample_time = sample_time;
+}

encoder/encoder.h

@@ -0,0 +1,35 @@
+#ifndef __ENCODER_H__
+#define __ENCODER_H__
+
+#include <rtthread.h>
+
+struct encoder
+{
+    rt_base_t       pin;            /* interrupt pin  */
+    rt_int16_t      pulse_count;    /* absolute pulse number in total */
+    rt_uint16_t     pulse_revol;    /* pulse number per revolution */
+    rt_uint16_t     sample_time;    /* sample time in microseconds for speed measurement */
+    rt_tick_t       last_time;
+    rt_int16_t      last_count;
+    rt_int16_t      cps;
+};
+
+typedef struct encoder *encoder_t;
+
+encoder_t     encoder_create(rt_base_t pin, rt_uint16_t pulse_revol);
+void          encoder_destroy(encoder_t enc);
+
+rt_err_t      encoder_enable(encoder_t enc);                                     /* start measurement */
+rt_err_t      encoder_disable(encoder_t enc);                                    /* stop measurement */
+rt_err_t      encoder_reset(encoder_t enc);                                      /* set pulse_count to 0 */
+
+void          encoder_set_sample_time(encoder_t enc, rt_uint16_t sample_time);                  /* set sample time */
+
+/** rpm = pulse_count / sample_time(ms) / pulse_revol * 1000 * 60 **/
+rt_int16_t   encoder_read(encoder_t enc);                                       /* return pulse_count */
+rt_int16_t   encoder_measure_cps(encoder_t enc);                               /* pulse_count per second */
+rt_int16_t   encoder_measure_rpm(encoder_t enc);                               /* revolutions per minute */
+
+// Encoder Thread
+
+#endif

gimbal/SConscript

@@ -0,0 +1,8 @@
+from building import *
+
+cwd   = GetCurrentDir()
+src	  = Glob('*.c')
+PATH  = [cwd]
+group = DefineGroup('robots', src, depend = [], CPPPATH = PATH)
+
+Return('group')

gimbal/gimbal.c

@@ -0,0 +1,156 @@
+
+/*
+ reference to Robomaster 2019 
+*/
+#include "gimbal.h"
+
+#define     DBG_SECTION_NAME    "gimbal"
+#define     DBG_LEVEL           DBG_LOG
+#include <rtdbg.h>
+
+
+//gimbal_t gimbal_find (const char * name)
+//{
+//    struct object * object;
+//    object = object_find (name, Object_Class_Gimbal);
+
+//    return (gimbal_t)
+//    object;
+//}
+
+
+rt_err_t gimbal_pitch_enable (struct gimbal * gimbal)
+{
+    if (gimbal == NULL)
+        return - RT_EINVAL;
+
+    //to fill in controller_enable
+    //controller_enable(&(gimbal->ctrl[PITCH_MOTOR_INDEX]));
+    return RT_EOK;
+}
+
+
+rt_err_t gimbal_pitch_disable (struct gimbal * gimbal)
+{
+    if (gimbal == NULL)
+        return - RT_EINVAL;
+
+    //to fill in controller_disable
+    //controller_disable(&(gimbal->ctrl[PITCH_MOTOR_INDEX]));
+    return RT_EOK;
+}
+
+
+rt_err_t gimbal_yaw_enable (struct gimbal * gimbal)
+{
+    if (gimbal == NULL)
+        return - RT_EINVAL;
+
+    //controller_enable(&(gimbal->ctrl[YAW_MOTOR_INDEX]));
+    return RT_EOK;
+}
+
+
+rt_err_t gimbal_yaw_disable (struct gimbal * gimbal)
+{
+    if (gimbal == NULL)
+        return - RT_EINVAL;
+
+    //controller_disable(&(gimbal->ctrl[YAW_MOTOR_INDEX]));
+    return RT_EOK;
+}
+
+
+rt_err_t gimbal_disable (struct gimbal * gimbal)
+{
+
+    if (gimbal == NULL)
+        return - RT_EINVAL;
+
+    rt_err_t err_y = 0, err_p = 0;
+
+    err_y = gimbal_yaw_disable (gimbal);
+    err_p = gimbal_pitch_disable (gimbal);
+
+    if ((err_y == RT_EOK) && (err_p == RT_EOK))
+    {
+        return RT_EOK;
+    }
+    else 
+    {
+        return - RT_ERRTR;
+    }
+}
+
+
+rt_err_t gimbal_enable (struct gimbal * gimbal)
+{
+
+    if (gimbal == NULL)
+        return - RT_EINVAL;
+
+    rt_err_t err_y = 0, err_p = 0;
+
+    err_y = gimbal_yaw_enable (gimbal);
+    err_p = gimbal_pitch_enable (gimbal);
+
+    if ((err_y == RT_EOK) && (err_p == RT_EOK))
+    {
+        return RT_EOK;
+    }
+    else 
+    {
+        return - RT_ERRTR;
+    }
+}
+
+
+rt_err_t gimbal_set_pitch_angle (struct gimbal * gimbal, float pitch)
+{
+    if (gimbal == NULL)
+        return - RT_EINVAL;
+
+    if (gimbal->mode.bit.pitch_mode == GYRO_MODE)
+    {
+
+    }
+    else 
+    {
+
+    }
+
+    return RT_EOK;
+}
+
+
+rt_err_t gimbal_set_yaw_angle (struct gimbal * gimbal, float yaw, uint8_t mode) //yaw mode：
+{ //1.clockwise 2.anticlockwise
+
+    if (gimbal == NULL)
+        return - RT_EINVAL;
+
+    if (gimbal->mode.bit.yaw_mode == GYRO_MODE)
+    {
+
+    }
+    else 
+    {
+
+    }
+
+    return RT_EOK;
+}
+
+
+rt_err_t gimbal_rate_update (struct gimbal * gimbal, float yaw_rate, float pitch_rate)
+{
+    if (gimbal == NULL)
+        return - RT_EINVAL;
+
+    gimbal->sensor.rate.yaw_rate = yaw_rate;
+    gimbal->sensor.rate.pitch_rate = pitch_rate;
+
+    return RT_EOK;
+}
+
+

gimbal/gimbal.h

@@ -0,0 +1,94 @@
+
+/*
+ reference to Robomaster 2019 
+*/
+#ifndef __GIMBAL_H__
+#define __GIMBAL_H__
+
+//#include "ro_object.h"
+#include "string.h"
+#include "ro_errco.h"
+
+#include "rtthread.h"
+
+#define     YAW_MOTOR_INDEX     0
+#define     PITCH_MOTOR_INDEX   1
+
+#define     ENCODER_MODE        (0u)
+#define     GYRO_MODE           (1u)
+
+#define     YAW_CLOCKWISE       (1u)
+#define     YAW_ANTICLOCKWISE   (2u)
+
+typedef struct gimbal * gimbal_t;
+
+
+struct gimbal_param
+{
+    rt_int16_t      pitch_ecd_center;
+    rt_int16_t      yaw_ecd_center;
+};
+
+
+struct gimbal_p_y
+{
+/* unit: degree */
+    float           yaw;
+    float           pitch;
+};
+
+struct gimbal_rate
+{
+/* unit: degree/s */
+    float           yaw_rate;
+    float           pitch_rate;
+};
+
+struct gimbal_sensor
+{
+    struct gimbal_p_y gyro_angle;
+    struct gimbal_rate rate;
+};
+
+struct gimbal
+{
+    //struct object parent;
+    struct gimbal_param param; //storage ecd/Gyro data when gimbal in center position
+    union 
+    {                                                   //for gimbal added Gyro to detect whether 
+      rt_uint8_t      state;                              //gimbla turn over the angle limitted
+      struct 
+     {
+        rt_uint8_t      yaw_mode        : 1;
+        rt_uint8_t      pitch_mode      : 1;
+     } bit;
+    } mode;
+
+    struct gimbal_sensor sensor; //storage Gyro data for gimbal added Gyro
+    struct gimbal_p_y ecd_angle; //storage encoder data
+    struct gimbal_p_y gyro_target_angle;
+    struct gimbal_p_y ecd_target_angle;
+//  struct motor_device motor[2];
+//  struct cascade cascade[2];    //cascade pid para for yaw/pit
+//  struct cascade_feedback cascade_fdb[2];   //feedback data
+//  struct controller ctrl[2];                //yaw/pit controller algorithm
+};
+
+
+gimbal_t gimbal_find (const char * name);
+
+rt_err_t gimbal_enable (struct gimbal * gimbal);
+rt_err_t gimbal_disable (struct gimbal * gimbal);
+
+rt_err_t gimbal_pitch_enable (struct gimbal * gimbal);
+rt_err_t gimbal_pitch_disable (struct gimbal * gimbal);
+rt_err_t gimbal_yaw_enable (struct gimbal * gimbal);
+rt_err_t gimbal_yaw_disable (struct gimbal * gimbal);
+
+rt_err_t gimbal_set_pitch_angle (struct gimbal * gimbal, float pitch);
+rt_err_t gimbal_set_yaw_angle (struct gimbal * gimbal, float yaw, uint8_t mode);        //yaw mode  
+rt_err_t gimbal_rate_update (struct gimbal * gimbal, float yaw_rate, float pitch_rate); //1.clockwise 2.anticlockwise
+
+
+#endif //  __GIMBAL_H__
+

kinematics/SConscript

@@ -0,0 +1,8 @@
+from building import *
+
+cwd   = GetCurrentDir()
+src	  = Glob('*.c')
+PATH  = [cwd]
+group = DefineGroup('robots', src, depend = [], CPPPATH = PATH)
+
+Return('group')

kinematics/kinematics.c

@@ -0,0 +1,170 @@
+#include "kinematics.h"
+
+#define DBG_SECTION_NAME  "kinematics"
+#define DBG_LEVEL         DBG_LOG
+#include <rtdbg.h>
+
+kinematics_t kinematics_create(enum base k_base, float length_x, float length_y, float wheel_radius)
+{
+    kinematics_t new_kinematics = (kinematics_t)rt_malloc(sizeof(struct kinematics));
+    if(new_kinematics == RT_NULL)
+    {
+        LOG_E("Failed to malloc memory for kinematics\n");
+        return RT_NULL;
+    }
+
+    new_kinematics->k_base       = k_base;
+    new_kinematics->length_x     = length_x;
+    new_kinematics->length_y     = length_y;
+    new_kinematics->wheel_radius = wheel_radius;
+
+    if(k_base == TWO_WD)
+    {
+        new_kinematics->total_wheels = 2;
+    }
+    if(k_base == FOUR_WD)
+    {
+        new_kinematics->total_wheels = 4;
+    }
+    if(k_base == ACKERMANN)
+    {
+        new_kinematics->total_wheels = 3;
+    }
+    if(k_base == MECANUM)
+    {
+        new_kinematics->total_wheels = 4;
+    }
+
+    return new_kinematics;
+}
+
+void kinematics_destroy(kinematics_t kinematics)
+{
+    LOG_I("Free Kinematics");
+    rt_free(kinematics);
+}
+
+rt_err_t kinematics_reset(kinematics_t kin)
+{
+    // TODO
+
+    return RT_EOK;
+}
+
+/* Return desired rpm for each motor given target velocity */
+rt_int16_t* kinematics_get_rpm(struct kinematics kin, struct velocity target_vel)
+{
+    // TODO
+    struct rpm cal_rpm;
+    rt_int16_t* res_rpm = (rt_int16_t*) rt_malloc(sizeof(rt_int16_t) * 4);
+    for(int i = 0; i < 4; i++)
+    {
+        res_rpm[i] = 0;
+    }
+
+    float linear_vel_x_mins;
+    float linear_vel_y_mins;
+    float angular_vel_z_mins;
+    float tangential_vel;
+
+    float x_rpm;
+    float y_rpm;
+    float tan_rpm;
+
+    if(kin.k_base == TWO_WD || kin.k_base == FOUR_WD ||  kin.k_base == ACKERMANN)
+    {
+        target_vel.linear_y = 0;
+    }
+
+    //convert m/s to m/min
+    linear_vel_x_mins = target_vel.linear_x * 60;
+    linear_vel_y_mins = target_vel.linear_y * 60;
+
+    //convert rad/s to rad/min
+    angular_vel_z_mins = target_vel.angular_z * 60;
+
+    tangential_vel = angular_vel_z_mins * ((kin.length_x / 2) + (kin.length_y / 2));
+
+    x_rpm   = linear_vel_x_mins / kin.wheel_radius;
+    y_rpm   = linear_vel_y_mins / kin.wheel_radius;
+    tan_rpm = tangential_vel / kin.wheel_radius;
+
+    // front-left motor
+    cal_rpm.motor1 = x_rpm - y_rpm - tan_rpm;
+
+    // front-right motor
+    cal_rpm.motor2 = x_rpm + y_rpm + tan_rpm;
+
+    // rear-left motor
+    cal_rpm.motor3 = x_rpm + y_rpm - tan_rpm;
+
+    // rear-right motor
+    cal_rpm.motor4 = x_rpm - y_rpm + tan_rpm;
+
+    // rt_kprintf("cal_rpm[1]: %d\n", cal_rpm.motor1);
+    // rt_kprintf("cal_rpm[2]: %d\n", cal_rpm.motor2);
+    // rt_kprintf("cal_rpm[3]: %d\n", cal_rpm.motor3);
+    // rt_kprintf("cal_rpm[4]: %d\n", cal_rpm.motor4);
+
+    if(kin.k_base == TWO_WD)
+    {
+        res_rpm[0] = cal_rpm.motor3;
+        res_rpm[1] = cal_rpm.motor4;
+    }
+    if(kin.k_base == FOUR_WD)
+    {
+        res_rpm[0] = cal_rpm.motor1;
+        res_rpm[1] = cal_rpm.motor2;
+        res_rpm[2] = cal_rpm.motor3;
+        res_rpm[3] = cal_rpm.motor4;
+    }
+    if(kin.k_base == ACKERMANN)
+    {
+        res_rpm[0] = target_vel.angular_z;
+        res_rpm[1] = cal_rpm.motor3;
+        res_rpm[2] = cal_rpm.motor4;
+    }
+    if(kin.k_base == MECANUM)
+    {
+        res_rpm[0] = cal_rpm.motor1;
+        res_rpm[1] = cal_rpm.motor2;
+        res_rpm[2] = cal_rpm.motor3;
+        res_rpm[3] = cal_rpm.motor4;
+    }
+
+    return res_rpm;
+}
+
+/* Return current velocity given rpm of each motor */
+struct velocity kinematics_get_velocity(struct kinematics kin, struct rpm current_rpm)
+{
+    // TODO
+    struct velocity res_vel;
+
+    int total_wheels = 0;
+    if(kin.k_base == TWO_WD) total_wheels = 2;
+    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;
+
+    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_radius; // 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_radius; // 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_radius) / ((kin.length_x / 2) + (kin.length_y / 2)); //  rad/s
+
+    return res_vel;
+}

kinematics/kinematics.h

@@ -0,0 +1,48 @@
+#ifndef __KINEMATICS_H__
+#define __KINEMATICS_H__
+
+#include <rtthread.h>
+
+enum base {
+    TWO_WD = 0, 
+    FOUR_WD, 
+    ACKERMANN, 
+    MECANUM
+};
+
+// rad/min
+struct rpm
+{
+    int motor1;
+    int motor2;
+    int motor3;
+    int motor4;
+};
+
+// m/s
+struct velocity
+{
+    float linear_x;
+    float linear_y;
+    float angular_z;
+};
+
+typedef struct kinematics *kinematics_t;
+
+struct kinematics
+{
+    enum base   k_base;
+    float       length_x;
+    float       length_y;
+    float       wheel_radius;
+    int         total_wheels;
+};
+
+kinematics_t    kinematics_create(enum base k_base, float length_x, float length_y, float wheel_radius);
+void            kinematics_destroy(kinematics_t kinematics);
+rt_err_t        kinematics_reset(kinematics_t kin);
+
+rt_int16_t*     kinematics_get_rpm(struct kinematics kin, struct velocity target_vel);
+struct velocity kinematics_get_velocity(struct kinematics kin, struct rpm current_rpm);
+
+#endif

motor/SConscript

@@ -0,0 +1,8 @@
+from building import *
+
+cwd   = GetCurrentDir()
+src	  = Glob('*.c')
+PATH  = [cwd]
+group = DefineGroup('robots', src, depend = [], CPPPATH = PATH)
+
+Return('group')

motor/motor.c

@@ -0,0 +1,68 @@
+#include "motor.h"
+
+#define DBG_SECTION_NAME  "motor"
+#define DBG_LEVEL         DBG_LOG
+#include <rtdbg.h>
+
+motor_t motor_create(int (*init)(void), int (*enable)(void), int (*disable)(void), int (*set_speed)(rt_int8_t percentage), enum motor_type type)
+{
+    // 1. Malloc memory
+    motor_t new_motor = (motor_t)rt_malloc(sizeof(struct motor));
+    if(new_motor == RT_NULL)
+    {
+        LOG_E("Falied to allocate memory for dc motor");
+        return RT_NULL;
+    }
+    
+    new_motor->type      = type;
+	new_motor->init      = init;
+    new_motor->enable    = enable;
+    new_motor->disable   = disable;
+    new_motor->set_speed = set_speed;
+
+	new_motor->init();
+
+    return new_motor;
+}
+
+rt_err_t motor_enable(motor_t mot)
+{
+    // Enable PWM
+    LOG_D("Enabling motor");
+    mot->enable();
+
+    return RT_EOK;
+}
+
+rt_err_t motor_disable(motor_t mot)
+{
+    // Disable PWM
+    LOG_D("Disabling motor");
+    mot->disable();
+
+    return RT_EOK;
+}
+
+void motor_run(motor_t mot, rt_int8_t pwm)
+{
+    // Set speed (pwm) to desired value
+    // LOG_D("Set motor speed %d pwm", pwm);
+    mot->set_speed(pwm);
+}
+
+void motor_stop(motor_t mot)
+{
+    // Set Speed to 0
+    motor_run(mot, 0);
+}
+
+void motor_destroy(motor_t mot)
+{
+    // Disable first
+    motor_disable(mot);
+
+    // Free memory
+    LOG_D("Free motor");
+
+    rt_free(mot);
+}

motor/motor.h

@@ -0,0 +1,39 @@
+#ifndef __MOTOR_H__
+#define __MOTOR_H__
+
+#include <rtthread.h>
+#include <rtdevice.h>
+
+typedef struct motor *motor_t;
+
+enum motor_type
+{
+    SERVO = 0,
+    DC_MOTOR,
+    // STEPPER_MOTOR,
+    // CAN_MOTOR,
+};
+
+struct motor
+{
+    enum    motor_type                  type;
+    int     (*init)(void);
+    int     (*enable)(void);
+    int     (*disable)(void);
+    int     (*set_speed)(rt_int8_t percentage);
+};
+
+motor_t   motor_create(int (*init)(void),
+                       int (*enable)(void), 
+                       int (*disable)(void),
+                       int (*set_speed)(rt_int8_t percentage),
+                       enum motor_type type);
+void      motor_destroy(motor_t mot);
+
+rt_err_t  motor_enable(motor_t mot);
+rt_err_t  motor_disable(motor_t mot);
+
+void      motor_run(motor_t motor, rt_int8_t pwm);
+void      motor_stop(motor_t motor);
+
+#endif

pid/SConscript

@@ -0,0 +1,8 @@
+from building import *
+
+cwd   = GetCurrentDir()
+src	  = Glob('*.c')
+PATH  = [cwd]
+group = DefineGroup('robots', src, depend = [], CPPPATH = PATH)
+
+Return('group')

pid/pid.c

@@ -0,0 +1,128 @@
+#include "pid.h"
+
+#define DBG_SECTION_NAME  "pid"
+#define DBG_LEVEL         DBG_LOG
+#include <rtdbg.h>
+
+pid_control_t pid_create(void)
+{
+    // TODO
+    // Malloc memory and initialize PID
+    pid_control_t new_pid = (pid_control_t)rt_malloc(sizeof(struct pid_control));
+    if(new_pid == RT_NULL)
+    {
+        LOG_E("Failed to malloc memory for PID\n");
+    }
+
+    pid_set_kp(new_pid, 0.05f);
+    pid_set_ki(new_pid, 0.01f);
+    pid_set_kd(new_pid, 0.02f);
+
+    new_pid->maximum = 100;
+    new_pid->minimum = 20;
+
+    new_pid->p_error = 10.0f;
+    new_pid->i_error = 1.0f;
+    new_pid->d_error = 0.2f;
+
+    new_pid->integral = 0.0f;
+    new_pid->error = 0.0f;
+    new_pid->error_l = 0.0f;
+    new_pid->error_ll = 0.0f;
+
+    new_pid->out = 0.0f;
+    new_pid->last_out = 0.0f;
+
+    return new_pid;
+}
+
+void pid_destroy(pid_control_t pid)
+{
+    LOG_D("Free PID");
+    rt_free(pid);
+}
+
+rt_err_t pid_reset(pid_control_t pid)
+{
+    // TODO
+    rt_memset(pid, 0, sizeof(pid_control_t));
+    return RT_EOK;
+}
+
+rt_err_t pid_set_kp(pid_control_t pid, float kp)
+{
+    pid->kp = kp;
+    return RT_EOK;
+}
+
+rt_err_t pid_set_ki(pid_control_t pid, float ki)
+{
+    pid->ki = ki;
+    return RT_EOK;
+}
+
+rt_err_t pid_set_kd(pid_control_t pid, float kd)
+{
+    pid->kd = kd;
+    return RT_EOK;
+}
+
+rt_err_t pid_set_target(pid_control_t pid, rt_int16_t set_point)
+{
+    // TODO
+    pid->set_point = set_point;
+
+    return RT_EOK;
+}
+
+rt_err_t pid_set_sample_time(pid_control_t pid, rt_uint16_t sample_time)
+{
+    // TODO
+    pid->sample_time = sample_time;
+    return RT_EOK;
+}
+
+float pid_update(pid_control_t pid, rt_int16_t current_point)
+{
+    // TODO
+    if((rt_tick_get() - pid->last_time) < rt_tick_from_millisecond(pid->sample_time))
+    {
+        LOG_D("PID waiting ... ");
+        return pid->last_out;
+    }
+
+    pid->last_time = rt_tick_get();
+    pid->error = pid->set_point - current_point;
+
+    pid->integral += pid->error;
+
+    pid->p_error = pid->kp * pid->error;
+    pid->i_error = pid->ki * pid->integral;
+    pid->d_error = pid->kd * (pid->error - 2 * pid->error_l + pid->error_ll);
+
+    pid->out = pid->p_error + pid->i_error + pid->d_error;
+    if (pid->out > pid->maximum)
+    {
+        pid->out = pid->maximum;
+    }
+    if (pid->out < pid->minimum)
+    {
+        pid->out = pid->minimum;
+    }
+
+    pid->last_out = pid->out;
+    pid->error_ll = pid->error_l;
+    pid->error_l = pid->error;
+
+    rt_kprintf("%d - %d\n", current_point, pid->set_point);
+    //LOG_D("PID current: %d : setpoint %d - P%d I%d D%d - [%d]", current_point, pid->set_point, (int)(pid->p_error + 0.5f), (int)(pid->i_error + 0.5f), (int)(pid->d_error + 0.5f), (int)(pid->out + 0.5f));
+    // LOG_D("PID P Error: %d", (int)(pid->p_error + 0.5f));
+    // LOG_D("PID I Error: %d", (int)(pid->i_error + 0.5f));
+    // LOG_D("PID D Error: %d", (int)(pid->d_error + 0.5f));
+    // LOG_D("PID Last Out: %d", (int)(pid->last_out + 0.5f));
+    // LOG_D("PID Out: %d", (int)(pid->out + 0.5f));
+
+    return pid->out;
+}
+
+// PID Thread

pid/pid.h

@@ -0,0 +1,47 @@
+#ifndef __PID_H__
+#define __PID_H__
+
+#include <rtthread.h>
+
+typedef struct pid_control *pid_control_t;
+
+struct pid_control
+{
+    float       kp;
+    float       ki;
+    float       kd;
+
+    float       minimum;
+    float       maximum;
+    rt_int16_t  set_point;
+    rt_uint16_t sample_time;    // unit:ms
+
+    float       p_error;
+    float       i_error;
+    float       d_error;
+
+    float integral;
+    float error;
+    float error_l;
+    float error_ll;
+    rt_tick_t last_time;
+
+    float out;
+    float last_out;
+};
+
+pid_control_t   pid_create(void);
+void            pid_destroy(pid_control_t pid);
+rt_err_t        pid_reset(pid_control_t pid);
+
+rt_err_t        pid_set_kp(pid_control_t pid, float kp);
+rt_err_t        pid_set_ki(pid_control_t pid, float ki);
+rt_err_t        pid_set_kd(pid_control_t pid, float kd);
+rt_err_t        pid_set_target(pid_control_t pid, rt_int16_t set_point);
+rt_err_t        pid_set_sample_time(pid_control_t pid, rt_uint16_t sample_time);
+
+float           pid_update(pid_control_t pid, rt_int16_t current_point);
+
+// PID Thread
+
+#endif

wheel/SConscript

@@ -0,0 +1,8 @@
+from building import *
+
+cwd   = GetCurrentDir()
+src	  = Glob('*.c')
+PATH  = [cwd]
+group = DefineGroup('robots', src, depend = [], CPPPATH = PATH)
+
+Return('group')

wheel/wheel.c

@@ -0,0 +1,116 @@
+#include "wheel.h"
+
+#define DBG_SECTION_NAME  "wheel"
+#define DBG_LEVEL         DBG_LOG
+#include <rtdbg.h>
+
+wheel_t wheel_create(motor_t w_motor, encoder_t w_encoder, pid_control_t w_pid, float radius, rt_uint16_t gear_ratio)
+{
+    // 1. Malloc memory for wheel
+    wheel_t new_wheel = (wheel_t) rt_malloc(sizeof(struct wheel));
+    if(new_wheel == RT_NULL)
+    {
+        LOG_E("Falied to allocate memory for dc wheel");
+        return RT_NULL;
+    }
+
+    // 2. Initialize wheel
+    new_wheel -> w_motor    = w_motor;
+    new_wheel -> w_encoder  = w_encoder;
+    new_wheel -> w_pid      = w_pid;
+    new_wheel -> radius     = radius;
+    new_wheel -> gear_ratio = gear_ratio;
+
+    return new_wheel;
+}
+
+void wheel_destroy(wheel_t wheel)
+{
+    LOG_D("Free wheel");
+
+    motor_destroy(wheel->w_motor);
+    encoder_destroy(wheel->w_encoder);
+    pid_destroy(wheel->w_pid);
+
+    rt_free(wheel);
+}
+
+rt_err_t wheel_enable(wheel_t whl)
+{
+    // TODO
+    LOG_D("Enabling wheel");
+
+    // Enable PWM for motor
+    motor_enable(whl->w_motor);
+
+    // Enable Encoder's interrupt
+    encoder_enable(whl->w_encoder);
+    rt_thread_mdelay(whl->w_encoder->sample_time);
+
+    // Enable PID
+
+    return RT_EOK;
+}
+
+rt_err_t wheel_disable(wheel_t whl)
+{
+    // TODO
+    LOG_D("Disabling wheel");
+
+    // Disable PWM for motor
+    motor_disable(whl->w_motor);
+
+    // Disable Encoder's interrupt
+
+    // Disable PID
+
+    return RT_EOK;
+}
+
+rt_err_t wheel_reset(wheel_t whl)
+{
+    // TODO
+
+    return RT_EOK;
+}
+
+/** speed = rpm x 60 x 2 x PI x radius **/
+rt_err_t wheel_set_speed(wheel_t whl, double speed)
+{
+    return wheel_set_rpm(whl, (rt_int16_t) (speed / 60.0 / 2.0 / whl->radius / PI));
+}
+
+rt_err_t wheel_set_rpm(wheel_t whl, rt_int16_t rpm)
+{
+    LOG_D("Set wheel speed %d rpm", rpm);
+    pid_set_target(whl->w_pid, rpm);
+    if(whl->w_pid->set_point == rpm)
+    {
+        return RT_EOK;
+    }
+    else
+    {
+        return RT_ERROR;
+    }
+}
+
+void wheel_update(wheel_t whl)
+{
+    // TODO
+    // Get current rpm
+    rt_int16_t current_point = encoder_measure_rpm(whl->w_encoder);
+
+    // PID calculation
+    rt_int8_t pwm = pid_update(whl->w_pid, current_point);
+
+    // Set PWM
+    motor_run(whl->w_motor, pwm);
+}
+
+void wheel_stop(wheel_t whl)
+{
+    motor_stop(whl->w_motor);
+}
+
+// Maximum Speed
+// Wheel Thread

wheel/wheel.h

@@ -0,0 +1,41 @@
+#ifndef __WHEEL_H__
+#define __WHEEL_H__
+
+#include <rtthread.h>
+#include <encoder.h>
+#include <motor.h>
+#include <pid.h>
+
+#define PI 3.1415926
+
+typedef struct wheel *wheel_t;
+
+struct wheel
+{
+    motor_t         w_motor;
+    encoder_t       w_encoder;
+    pid_control_t           w_pid;
+
+    rt_uint16_t     rpm;
+    float           radius;
+    rt_uint16_t     gear_ratio;
+};
+
+wheel_t     wheel_create(motor_t w_motor, encoder_t w_encoder, pid_control_t w_pid, float radius, rt_uint16_t gear_ratio);
+void        wheel_destroy(wheel_t wheel);
+
+rt_err_t    wheel_enable(wheel_t whl);
+rt_err_t    wheel_disable(wheel_t whl);
+rt_err_t    wheel_reset(wheel_t whl);
+
+/** speed = rpm x 60 x 2 x PI x radius **/
+rt_err_t    wheel_set_speed(wheel_t whl, double speed);
+rt_err_t    wheel_set_rpm(wheel_t whl, rt_int16_t rpm);
+
+void        wheel_update(wheel_t whl);
+void        wheel_stop(wheel_t whl);
+
+// Maximum Speed
+// Wheel Thread
+
+#endif