Skip to content

Commit 330e178

Browse files
sogwmssogwms
committed
update command
1 parent c8f953a commit 330e178

File tree

8 files changed

+238
-93
lines changed

8 files changed

+238
-93
lines changed

‎chassis/chassis.c‎

Lines changed: 33 additions & 38 deletions
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,5 @@
11
#include "chassis.h"
2+
#include <command.h>
23

34
#define DBG_SECTION_NAME "chassis"
45
#define DBG_LEVEL DBG_LOG
@@ -92,46 +93,40 @@ rt_err_t chassis_update(chassis_t chas)
9293
return RT_EOK;
9394
}
9495

95-
rt_err_t chassis_parse_command(chassis_t chas, rt_int8_t cmd, void *args)
96+
static void chassis_stop(command_info_t info)
97+
{
98+
rt_kprintf("stop cmd\n");
99+
}
100+
static void chassis_forward(command_info_t info)
96101
{
97-
// TODO
98-
struct velocity target_vel;
99-
100-
LOG_D("received command: cmd:%d arg:%d", cmd, *((rt_uint8_t *)args));
101102

102-
switch (cmd)
103-
{
104-
case CHASSIS_FORWARD_ANALOG:
105-
target_vel.linear_x = (float)*((rt_uint8_t *)args) / 100.0f; // m/s
106-
target_vel.linear_y = 0;
107-
target_vel.angular_z = 0;
108-
break;
109-
case CHASSIS_BACKWARD_ANALOG:
110-
target_vel.linear_x = -(float)*((rt_uint8_t *)args) / 100.0f;
111-
target_vel.linear_y = 0;
112-
target_vel.angular_z = 0;
113-
break;
114-
case CHASSIS_TURN_LEFT_ANALOG:
115-
target_vel.linear_x = 0.00f;
116-
target_vel.linear_y = 0;
117-
target_vel.angular_z = (float)*((rt_uint8_t *)args);
118-
break;
119-
case CHASSIS_TURN_RIGHT_ANALOG:
120-
target_vel.linear_x = 0.00f;
121-
target_vel.linear_y = 0;
122-
target_vel.angular_z = -(float)*((rt_uint8_t *)args);
123-
break;
124-
case CHASSIS_STOP:
125-
target_vel.linear_x = 0.00f;
126-
target_vel.linear_y = 0;
127-
target_vel.angular_z = 0;
128-
break;
129-
default:
130-
return RT_ERROR;
131-
break;
132-
}
103+
}
104+
static void chassis_backward(command_info_t info)
105+
{
133106

134-
chassis_set_velocity(chas, target_vel);
107+
}
108+
static void chassis_turn_left(command_info_t info)
109+
{
110+
111+
}
112+
static void chassis_turn_right(command_info_t info)
113+
{
114+
115+
}
116+
static void chassis_move_left(command_info_t info)
117+
{
118+
119+
}
120+
static void chassis_move_right(command_info_t info)
121+
{
135122

136-
return RT_EOK;
137123
}
124+
125+
static void chassis_command_register(void)
126+
{
127+
// TODO
128+
command_register(COMMAND_CAR_STOP, chassis_stop);
129+
130+
}
131+
132+
INIT_APP_EXPORT(chassis_command_register);

‎chassis/chassis.h‎

Lines changed: 0 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -4,15 +4,6 @@
44
#include <kinematics.h>
55
#include <wheel.h>
66

7-
enum chassis_command
8-
{
9-
CHASSIS_FORWARD_ANALOG,
10-
CHASSIS_BACKWARD_ANALOG,
11-
CHASSIS_TURN_LEFT_ANALOG,
12-
CHASSIS_TURN_RIGHT_ANALOG,
13-
CHASSIS_STOP,
14-
};
15-
167
typedef struct chassis *chassis_t;
178

189
struct chassis
@@ -34,6 +25,4 @@ rt_err_t chassis_set_rpm(chassis_t chas, rt_int16_t target_rpm[]);
3425

3526
rt_err_t chassis_update(chassis_t chas);
3627

37-
rt_err_t chassis_parse_command(chassis_t chas, rt_int8_t cmd, void *args);
38-
3928
#endif

‎motor/servo.c‎

Lines changed: 79 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,79 @@
1+
#include "servo.h"
2+
3+
#define DBG_SECTION_NAME "servo"
4+
#define DBG_LEVEL DBG_LOG
5+
#include <rtdbg.h>
6+
7+
/**
8+
* @brief create a servo object
9+
* @param pwm pwm device name
10+
* @param channel pwm channel
11+
* @param angle servo angle range
12+
* @param pluse_min the minimum of pwm pluse width that servo can receive (unit:ns
13+
* @param pluse_max the maximum of pwm pluse width that servo can receive (unit:ns
14+
* @return the pointer of structure servo if success
15+
*/
16+
servo_t servo_create(const char * pwm, int channel, float angle, rt_uint32_t pluse_min, rt_uint32_t pluse_max)
17+
{
18+
// Malloc memory for new servo
19+
servo_t new_servo = (servo_t) rt_malloc(sizeof(struct servo));
20+
if(new_servo == RT_NULL)
21+
{
22+
LOG_E("Falied to allocate memory");
23+
return RT_NULL;
24+
}
25+
26+
new_servo->pwmdev = (struct rt_device_pwm *)rt_device_find(pwm);
27+
if (new_servo->pwmdev == RT_NULL)
28+
{
29+
LOG_E("Falied to find device on %s", pwm);
30+
servo_destroy(new_servo);
31+
return RT_NULL;
32+
}
33+
new_servo->channel = channel;
34+
new_servo->angle_maximum = angle;
35+
if (pluse_max == RT_NULL || pluse_min == RT_NULL)
36+
{
37+
new_servo->pluse_maximum = SERVO_DEFAULT_PULSE_MAX;
38+
new_servo->pluse_minimum = SERVO_DEFAULT_PULSE_MIN;
39+
}
40+
else
41+
{
42+
new_servo->pluse_maximum = pluse_max;
43+
new_servo->pluse_minimum = pluse_min;
44+
}
45+
46+
return new_servo;
47+
}
48+
49+
rt_err_t servo_destroy(servo_t servo)
50+
{
51+
RT_ASSERT(servo != RT_NULL);
52+
53+
rt_free(servo);
54+
55+
return RT_EOK;
56+
}
57+
58+
rt_err_t servo_enable(servo_t servo)
59+
{
60+
RT_ASSERT(servo != RT_NULL);
61+
62+
return rt_pwm_enable(servo->pwmdev, servo->channel);
63+
}
64+
65+
rt_err_t servo_disable(servo_t servo)
66+
{
67+
RT_ASSERT(servo != RT_NULL);
68+
69+
return rt_pwm_disable(servo->pwmdev, servo->channel);
70+
}
71+
72+
rt_err_t servo_set_angle(servo_t servo, float angle)
73+
{
74+
RT_ASSERT(servo != RT_NULL);
75+
76+
rt_uint32_t set_point = servo->pluse_minimum + (servo->pluse_maximum - servo->pluse_minimum) * angle / servo->angle_maximum;
77+
78+
return rt_pwm_set(servo->pwmdev, servo->channel, SERVO_PERIOD, set_point);
79+
}

‎motor/servo.h‎

Lines changed: 29 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,29 @@
1+
#ifndef __SERVO_H__
2+
#define __SERVO_H__
3+
4+
#include <rtthread.h>
5+
#include <rtdevice.h>
6+
7+
// 20ms; 1.5ms; +-1ms
8+
#define SERVO_PERIOD 20000000
9+
#define SERVO_DEFAULT_PULSE_MAX 2500000
10+
#define SERVO_DEFAULT_PULSE_MIN 500000
11+
12+
typedef struct servo *servo_t;
13+
14+
struct servo
15+
{
16+
struct rt_device_pwm *pwmdev;
17+
int channel;
18+
float angle_maximum;
19+
rt_uint32_t pluse_maximum;
20+
rt_uint32_t pluse_minimum;
21+
};
22+
23+
servo_t servo_create(const char * pwm, int channel, float angle, rt_uint32_t pluse_min, rt_uint32_t pluse_max);
24+
rt_err_t servo_destroy(servo_t servo);
25+
rt_err_t servo_enable(servo_t servo);
26+
rt_err_t servo_disable(servo_t servo);
27+
rt_err_t servo_set_angle(servo_t servo, float angle);
28+
29+
#endif // __SERVO_H__

‎protocol/command.c‎

Lines changed: 65 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -4,12 +4,27 @@
44
#define DBG_LEVEL DBG_LOG
55
#include <rtdbg.h>
66

7-
#define TABLE_MAX_SIZE 32
7+
// Thread
8+
#define THREAD_PRIORITY ((RT_THREAD_PRIORITY_MAX / 3) + 1)
9+
#define THREAD_STACK_SIZE 512
10+
#define THREAD_TIMESLICE 10
11+
static rt_thread_t trd_cmd = RT_NULL;
812

13+
// Message Queue
14+
#define MAX_MSGS 16
15+
static rt_mq_t msg_cmd = RT_NULL;
16+
17+
// Table
18+
#define TABLE_MAX_SIZE 32
19+
struct command
20+
{
21+
rt_int16_t robot_cmd;
22+
void (*handler)(command_info_t info);
23+
};
924
static struct command command_table[TABLE_MAX_SIZE];
10-
static uint8_t command_table_size = 0;
25+
static uint16_t command_table_size = 0;
1126

12-
rt_err_t command_register(rt_int8_t cmd, rt_err_t (*handler)(rt_int8_t cmd, void *param))
27+
rt_err_t command_register(rt_int16_t cmd, void (*handler)(command_info_t info))
1328
{
1429
if (command_table_size > TABLE_MAX_SIZE - 1)
1530
{
@@ -18,41 +33,71 @@ rt_err_t command_register(rt_int8_t cmd, rt_err_t (*handler)(rt_int8_t cmd, void
1833
}
1934
command_table[command_table_size].robot_cmd = cmd;
2035
command_table[command_table_size].handler = handler;
21-
command_table_size += 1;
36+
command_table_size++;
2237

2338
return RT_EOK;
2439
}
2540

26-
rt_err_t command_unregister(rt_int8_t cmd)
41+
rt_err_t command_unregister(rt_int16_t cmd)
2742
{
28-
// TODO
43+
for (int i = 0; i < command_table_size; i++)
44+
{
45+
if (command_table[i].robot_cmd == cmd)
46+
{
47+
for (int j = i; j < command_table_size - 1; j++)
48+
{
49+
rt_memcpy(&command_table[j], &command_table[j+1], sizeof(struct command));
50+
}
51+
command_table_size--;
52+
break;
53+
}
54+
}
2955

3056
return RT_EOK;
3157
}
3258

33-
rt_err_t command_handle(rt_int8_t cmd, void *param)
59+
rt_err_t command_send(command_info_t info)
60+
{
61+
return rt_mq_send(msg_cmd, info, sizeof(struct command_info));
62+
}
63+
64+
static void command_thread_entry(void *param)
3465
{
35-
// look-up table and call callback
36-
for (uint16_t i = 0; i < command_table_size; i++)
66+
struct command_info info;
67+
68+
while (1)
3769
{
38-
if (command_table[i].robot_cmd == cmd)
70+
rt_mq_recv(msg_cmd, &info, sizeof(struct command_info), RT_WAITING_FOREVER);
71+
// look-up table and call callback
72+
for (int i = 0; i < command_table_size; i++)
3973
{
40-
command_table[i].handler(command_table[i].robot_cmd, param);
41-
return RT_EOK;
74+
if (command_table[i].robot_cmd == info.cmd)
75+
{
76+
command_table[i].handler(&info);
77+
break;
78+
}
4279
}
4380
}
44-
45-
return RT_ERROR;
4681
}
4782

48-
rt_err_t command_send(command_sender_t sender, rt_int8_t cmd, void *param, rt_uint16_t len)
83+
void command_init(void)
4984
{
50-
if (sender->send != RT_NULL)
85+
msg_cmd = rt_mq_create("command", sizeof(struct command_info), MAX_MSGS, RT_IPC_FLAG_FIFO);
86+
if (msg_cmd == RT_NULL)
5187
{
52-
sender->send(cmd, param, len);
88+
LOG_E("Failed to creat meassage queue");
89+
return;
5390
}
54-
55-
return RT_EOK;
56-
}
5791

92+
trd_cmd = rt_thread_create("command",
93+
command_thread_entry, RT_NULL,
94+
THREAD_STACK_SIZE,
95+
THREAD_PRIORITY, THREAD_TIMESLICE);
5896

97+
if (trd_cmd == RT_NULL)
98+
{
99+
LOG_E("Failed to creat thread");
100+
return;
101+
}
102+
rt_thread_startup(trd_cmd);
103+
}

‎protocol/command.h‎

Lines changed: 10 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -12,25 +12,21 @@
1212
#define COMMAND_CAR_TURNRIGHT 5
1313
#define COMMAND_GET_CAR_SPEED 6
1414

15-
#define COMMAND_CONTROLLER_VIBRATE 100
15+
// #define COMMAND_CAR_FORWARD_WITH_PARAM
1616

17-
typedef struct command_sender *command_sender_t;
17+
#define COMMAND_RC_VIBRATE -1
1818

19-
struct command_sender
19+
struct command_info
2020
{
21-
char *name;
22-
void (*send)(rt_int8_t cmd, void *param, rt_uint16_t len);
21+
void *target;
22+
rt_int16_t cmd;
23+
void *param;
2324
};
2425

25-
struct command
26-
{
27-
rt_int8_t robot_cmd;
28-
rt_err_t (*handler)(rt_int8_t cmd, void *param);
29-
};
26+
typedef struct command_info *command_info_t;
3027

31-
rt_err_t command_register(rt_int8_t cmd, rt_err_t (*handler)(rt_int8_t cmd, void *param));
32-
rt_err_t command_unregister(rt_int8_t cmd);
33-
rt_err_t command_handle(rt_int8_t cmd, void *param);
34-
rt_err_t command_send(command_sender_t sender, rt_int8_t cmd, void *param, rt_uint16_t len);
28+
rt_err_t command_register(rt_int16_t cmd, void (*handler)(command_info_t info));
29+
rt_err_t command_unregister(rt_int16_t cmd);
30+
rt_err_t command_send(command_info_t info);
3531

3632
#endif

0 commit comments

Comments
 (0)