update command

chassis/chassis.c

@@ -1,4 +1,5 @@
 #include "chassis.h"
+#include <command.h>
 
 #define DBG_SECTION_NAME  "chassis"
 #define DBG_LEVEL         DBG_LOG
@@ -92,46 +93,40 @@ rt_err_t chassis_update(chassis_t chas)
     return RT_EOK;
 }
 
-rt_err_t chassis_parse_command(chassis_t chas, rt_int8_t cmd, void *args)
+static void chassis_stop(command_info_t info)
+{
+    rt_kprintf("stop cmd\n");
+}
+static void chassis_forward(command_info_t info)
 {
-    // TODO
-    struct velocity target_vel;
-    
-    LOG_D("received command: cmd:%d arg:%d", cmd, *((rt_uint8_t *)args));
 
-    switch (cmd)
-    {
-    case CHASSIS_FORWARD_ANALOG:
-        target_vel.linear_x = (float)*((rt_uint8_t *)args) / 100.0f;   // m/s
-        target_vel.linear_y = 0;
-        target_vel.angular_z = 0;
-        break;
-    case CHASSIS_BACKWARD_ANALOG:
-        target_vel.linear_x = -(float)*((rt_uint8_t *)args) / 100.0f;
-        target_vel.linear_y = 0;
-        target_vel.angular_z = 0;
-        break;
-    case CHASSIS_TURN_LEFT_ANALOG:
-        target_vel.linear_x = 0.00f;
-        target_vel.linear_y = 0;
-        target_vel.angular_z = (float)*((rt_uint8_t *)args);
-        break;
-    case CHASSIS_TURN_RIGHT_ANALOG:
-        target_vel.linear_x = 0.00f;
-        target_vel.linear_y = 0;
-        target_vel.angular_z = -(float)*((rt_uint8_t *)args);
-        break;
-    case CHASSIS_STOP:
-        target_vel.linear_x = 0.00f;
-        target_vel.linear_y = 0;
-        target_vel.angular_z = 0;
-        break;
-    default:
-        return RT_ERROR;
-        break;
-    }
+}
+static void chassis_backward(command_info_t info)
+{
 
-    chassis_set_velocity(chas, target_vel);
+}
+static void chassis_turn_left(command_info_t info)
+{
+
+}
+static void chassis_turn_right(command_info_t info)
+{
+
+}
+static void chassis_move_left(command_info_t info)
+{
+
+}
+static void chassis_move_right(command_info_t info)
+{
 
-    return RT_EOK;
 }
+
+static void chassis_command_register(void)
+{
+    // TODO
+    command_register(COMMAND_CAR_STOP, chassis_stop);
+    
+}
+
+INIT_APP_EXPORT(chassis_command_register);

chassis/chassis.h

@@ -4,15 +4,6 @@
 #include <kinematics.h>
 #include <wheel.h>
 
-enum chassis_command
-{
-    CHASSIS_FORWARD_ANALOG,
-    CHASSIS_BACKWARD_ANALOG,
-    CHASSIS_TURN_LEFT_ANALOG,
-    CHASSIS_TURN_RIGHT_ANALOG,
-    CHASSIS_STOP,
-};
-
 typedef struct chassis *chassis_t;
 
 struct chassis
@@ -34,6 +25,4 @@ rt_err_t    chassis_set_rpm(chassis_t chas, rt_int16_t target_rpm[]);
 
 rt_err_t    chassis_update(chassis_t chas);
 
-rt_err_t    chassis_parse_command(chassis_t chas, rt_int8_t cmd, void *args);
-
 #endif

motor/servo.c

@@ -0,0 +1,79 @@
+#include "servo.h"
+
+#define DBG_SECTION_NAME    "servo"
+#define DBG_LEVEL           DBG_LOG
+#include <rtdbg.h>
+
+/**
+ * @brief create a servo object
+ * @param pwm pwm device name
+ * @param channel pwm channel
+ * @param angle servo angle range
+ * @param pluse_min the minimum of pwm pluse width that servo can receive   (unit:ns
+ * @param pluse_max the maximum of pwm pluse width that servo can receive   (unit:ns
+ * @return the pointer of structure servo if success
+ */
+servo_t servo_create(const char * pwm, int channel, float angle, rt_uint32_t pluse_min, rt_uint32_t pluse_max)
+{
+    // Malloc memory for new servo
+    servo_t new_servo = (servo_t) rt_malloc(sizeof(struct servo));
+    if(new_servo == RT_NULL)
+    {
+        LOG_E("Falied to allocate memory");
+        return RT_NULL;
+    }
+
+    new_servo->pwmdev = (struct rt_device_pwm *)rt_device_find(pwm);
+    if (new_servo->pwmdev == RT_NULL)
+    {
+        LOG_E("Falied to find device on %s", pwm);
+        servo_destroy(new_servo);
+        return RT_NULL;
+    }
+    new_servo->channel = channel;
+    new_servo->angle_maximum = angle;
+    if (pluse_max == RT_NULL || pluse_min == RT_NULL)
+    {
+        new_servo->pluse_maximum = SERVO_DEFAULT_PULSE_MAX;
+        new_servo->pluse_minimum = SERVO_DEFAULT_PULSE_MIN;
+    }
+    else
+    {
+        new_servo->pluse_maximum = pluse_max;
+        new_servo->pluse_minimum = pluse_min;
+    }
+
+    return new_servo;
+}
+
+rt_err_t servo_destroy(servo_t servo)
+{
+    RT_ASSERT(servo != RT_NULL);
+
+    rt_free(servo);
+
+    return RT_EOK;
+}
+
+rt_err_t servo_enable(servo_t servo)
+{
+    RT_ASSERT(servo != RT_NULL);
+
+    return rt_pwm_enable(servo->pwmdev, servo->channel);
+}
+
+rt_err_t servo_disable(servo_t servo)
+{
+    RT_ASSERT(servo != RT_NULL);
+
+    return rt_pwm_disable(servo->pwmdev, servo->channel);
+}
+
+rt_err_t servo_set_angle(servo_t servo, float angle)
+{
+    RT_ASSERT(servo != RT_NULL);
+    
+    rt_uint32_t set_point = servo->pluse_minimum + (servo->pluse_maximum - servo->pluse_minimum) * angle / servo->angle_maximum;
+    
+    return rt_pwm_set(servo->pwmdev, servo->channel, SERVO_PERIOD, set_point);
+}

motor/servo.h

@@ -0,0 +1,29 @@
+#ifndef __SERVO_H__
+#define __SERVO_H__
+
+#include <rtthread.h>
+#include <rtdevice.h>
+
+// 20ms; 1.5ms; +-1ms
+#define SERVO_PERIOD                    20000000
+#define SERVO_DEFAULT_PULSE_MAX         2500000
+#define SERVO_DEFAULT_PULSE_MIN         500000
+
+typedef struct servo *servo_t;
+
+struct servo
+{
+    struct rt_device_pwm    *pwmdev;
+    int                     channel;
+    float                   angle_maximum;
+    rt_uint32_t             pluse_maximum;
+    rt_uint32_t             pluse_minimum;
+};
+
+servo_t  servo_create(const char * pwm, int channel, float angle, rt_uint32_t pluse_min, rt_uint32_t pluse_max);
+rt_err_t servo_destroy(servo_t servo);
+rt_err_t servo_enable(servo_t servo);
+rt_err_t servo_disable(servo_t servo);
+rt_err_t servo_set_angle(servo_t servo, float angle);
+
+#endif //  __SERVO_H__

protocol/command.c

@@ -4,12 +4,27 @@
 #define DBG_LEVEL         DBG_LOG
 #include <rtdbg.h>
 
-#define TABLE_MAX_SIZE      32
+// Thread
+#define THREAD_PRIORITY             ((RT_THREAD_PRIORITY_MAX / 3) + 1)
+#define THREAD_STACK_SIZE           512
+#define THREAD_TIMESLICE            10
+static rt_thread_t trd_cmd = RT_NULL;
 
+// Message Queue
+#define MAX_MSGS                    16
+static rt_mq_t msg_cmd = RT_NULL;
+
+// Table
+#define TABLE_MAX_SIZE              32
+struct command
+{
+    rt_int16_t   robot_cmd;
+    void         (*handler)(command_info_t info);
+};
 static struct command command_table[TABLE_MAX_SIZE];
-static uint8_t command_table_size = 0;
+static uint16_t command_table_size = 0;
 
-rt_err_t command_register(rt_int8_t cmd, rt_err_t (*handler)(rt_int8_t cmd, void *param))
+rt_err_t command_register(rt_int16_t cmd, void (*handler)(command_info_t info))
 {
     if (command_table_size > TABLE_MAX_SIZE - 1)
     {
@@ -18,41 +33,71 @@ rt_err_t command_register(rt_int8_t cmd, rt_err_t (*handler)(rt_int8_t cmd, void
     }
     command_table[command_table_size].robot_cmd = cmd;
     command_table[command_table_size].handler = handler;
-    command_table_size += 1;
+    command_table_size++;
 
     return RT_EOK;
 }
 
-rt_err_t command_unregister(rt_int8_t cmd)
+rt_err_t command_unregister(rt_int16_t cmd)
 {
-    // TODO
+    for (int i = 0; i < command_table_size; i++)
+    {
+        if (command_table[i].robot_cmd == cmd)
+        {
+            for (int j = i; j < command_table_size - 1; j++)
+            {
+                rt_memcpy(&command_table[j], &command_table[j+1], sizeof(struct command));
+            }
+            command_table_size--;
+            break;
+        }
+    }
 
     return RT_EOK;
 }
 
-rt_err_t command_handle(rt_int8_t cmd, void *param)
+rt_err_t command_send(command_info_t info)
+{
+    return rt_mq_send(msg_cmd, info, sizeof(struct command_info));
+}
+
+static void command_thread_entry(void *param)
 {
-    // look-up table and call callback
-    for (uint16_t i = 0; i < command_table_size; i++)
+    struct command_info info;
+
+    while (1)
     {
-        if (command_table[i].robot_cmd == cmd)
+        rt_mq_recv(msg_cmd, &info, sizeof(struct command_info), RT_WAITING_FOREVER);
+        // look-up table and call callback
+        for (int i = 0; i < command_table_size; i++)
         {
-            command_table[i].handler(command_table[i].robot_cmd, param);
-            return RT_EOK;
+            if (command_table[i].robot_cmd == info.cmd)
+            {
+                command_table[i].handler(&info);
+                break;
+            }
         }
     }
-
-    return RT_ERROR;
 }
 
-rt_err_t command_send(command_sender_t sender, rt_int8_t cmd, void *param, rt_uint16_t len)
+void command_init(void)
 {
-    if (sender->send != RT_NULL)
+    msg_cmd = rt_mq_create("command", sizeof(struct command_info), MAX_MSGS, RT_IPC_FLAG_FIFO);
+    if (msg_cmd == RT_NULL)
     {
-        sender->send(cmd, param, len);
+        LOG_E("Failed to creat meassage queue");
+        return;
     }
-    
-    return RT_EOK;
-}
 
+    trd_cmd = rt_thread_create("command",
+                              command_thread_entry, RT_NULL,
+                              THREAD_STACK_SIZE,
+                              THREAD_PRIORITY, THREAD_TIMESLICE);
 
+    if (trd_cmd == RT_NULL)
+    {
+        LOG_E("Failed to creat thread");
+        return;
+    }
+    rt_thread_startup(trd_cmd);
+}

protocol/command.h

@@ -12,25 +12,21 @@
 #define COMMAND_CAR_TURNRIGHT           5
 #define COMMAND_GET_CAR_SPEED           6
 
-#define COMMAND_CONTROLLER_VIBRATE      100
+// #define COMMAND_CAR_FORWARD_WITH_PARAM
 
-typedef struct command_sender *command_sender_t;
+#define COMMAND_RC_VIBRATE              -1
 
-struct command_sender
+struct command_info
 {
-    char *name;
-    void (*send)(rt_int8_t cmd, void *param, rt_uint16_t len);
+    void       *target;
+    rt_int16_t cmd;
+    void       *param;
 };
 
-struct command
-{
-    rt_int8_t   robot_cmd;
-    rt_err_t    (*handler)(rt_int8_t cmd, void *param);
-};
+typedef struct command_info *command_info_t;
 
-rt_err_t command_register(rt_int8_t cmd, rt_err_t (*handler)(rt_int8_t cmd, void *param));
-rt_err_t command_unregister(rt_int8_t cmd);
-rt_err_t command_handle(rt_int8_t cmd, void *param);
-rt_err_t command_send(command_sender_t sender, rt_int8_t cmd, void *param, rt_uint16_t len);
+rt_err_t command_register(rt_int16_t cmd, void (*handler)(command_info_t info));
+rt_err_t command_unregister(rt_int16_t cmd);
+rt_err_t command_send(command_info_t info);
 
 #endif

protocol/ps2.c

@@ -9,9 +9,9 @@ Change From YFRobot. www.yfrobot.com
 #define DBG_LEVEL         DBG_LOG
 #include <rtdbg.h>
 
-#define THREAD_DELAY_TIME           50
+#define THREAD_DELAY_TIME           30
 
-#define THREAD_PRIORITY             16
+#define THREAD_PRIORITY             ((RT_THREAD_PRIORITY_MAX / 3) + 2)
 #define THREAD_STACK_SIZE           1024
 #define THREAD_TIMESLICE            10
 
@@ -26,6 +26,8 @@ static rt_base_t ps2_clk_pin;
 static rt_base_t ps2_do_pin; 
 static rt_base_t ps2_di_pin;
 
+static struct command_info cmd_info;
+
 static void hal_cs_high(void)
 {
     rt_pin_write(ps2_cs_pin, PIN_HIGH);
@@ -140,7 +142,6 @@ int ps2_read_light(void)
 static void ps2_thread_entry(void *param)
 {
     struct ps2_ctrl_data ctrl_data;
-
     while (1)
     {
         rt_thread_mdelay(THREAD_DELAY_TIME);   
@@ -152,7 +153,8 @@ static void ps2_thread_entry(void *param)
             {
                 if (table[i].standard_cmd != COMMAND_NONE)
                 {
-                    command_handle(table[i].standard_cmd, RT_NULL);
+                    cmd_info.cmd = table[i].standard_cmd;
+                    command_send(&cmd_info);
                 }
             }
         }
@@ -162,25 +164,33 @@ static void ps2_thread_entry(void *param)
         {
             if (table[PS2_ROCKER_LX].standard_cmd != COMMAND_NONE)
             {
-                command_handle(table[PS2_ROCKER_LX].standard_cmd, &ctrl_data.left_stick_x);
+                cmd_info.cmd = table[PS2_ROCKER_LX].standard_cmd;
+                cmd_info.param = &ctrl_data.left_stick_x;
+                command_send(&cmd_info);
             }
             if (table[PS2_ROCKER_LY].standard_cmd != COMMAND_NONE)
             {
-                command_handle(table[PS2_ROCKER_LY].standard_cmd, &ctrl_data.left_stick_y);
+                cmd_info.cmd = table[PS2_ROCKER_LY].standard_cmd;
+                cmd_info.param = &ctrl_data.left_stick_y;
+                command_send(&cmd_info);
             }
             if (table[PS2_ROCKER_RX].standard_cmd != COMMAND_NONE)
             {
-                command_handle(table[PS2_ROCKER_RX].standard_cmd, &ctrl_data.right_stick_x);
+                cmd_info.cmd = table[PS2_ROCKER_RX].standard_cmd;
+                cmd_info.param = &ctrl_data.right_stick_x;
+                command_send(&cmd_info);
             }
             if (table[PS2_ROCKER_RY].standard_cmd != COMMAND_NONE)
             {
-                command_handle(table[PS2_ROCKER_RY].standard_cmd, &ctrl_data.right_stick_y);
+                cmd_info.cmd = table[PS2_ROCKER_RY].standard_cmd;
+                cmd_info.param = &ctrl_data.right_stick_y;
+                command_send(&cmd_info);
             }
         }
     }
 }
 
-void ps2_init(rt_base_t cs_pin, rt_base_t clk_pin, rt_base_t do_pin, rt_base_t di_pin)
+void ps2_init(rt_base_t cs_pin, rt_base_t clk_pin, rt_base_t do_pin, rt_base_t di_pin, void *target)
 {
     ps2_cs_pin = cs_pin;
     ps2_clk_pin = clk_pin;
@@ -195,6 +205,8 @@ void ps2_init(rt_base_t cs_pin, rt_base_t clk_pin, rt_base_t do_pin, rt_base_t d
     hal_cs_high();
     hal_clk_high();
 
+    cmd_info.target = target;
+
     tid_ps2 = rt_thread_create("ps2",
                           ps2_thread_entry, RT_NULL,
                           THREAD_STACK_SIZE,

protocol/ps2.h

@@ -47,7 +47,7 @@ struct ps2_ctrl_data
     uint8_t right_stick_y;
 };
 
-void ps2_init(rt_base_t cs_pin, rt_base_t clk_pin, rt_base_t do_pin, rt_base_t di_pin);
+void ps2_init(rt_base_t cs_pin, rt_base_t clk_pin, rt_base_t do_pin, rt_base_t di_pin, void *target);
 int  ps2_scan(ps2_ctrl_data_t pt);
 int  ps2_read_light(void);