Skip to content

Commit c61ddc7

Browse files
author
guoyongchao
committed
Update controller
1 parent 02df055 commit c61ddc7

File tree

10 files changed

+348
-11
lines changed

10 files changed

+348
-11
lines changed

‎controller/SConscript‎

Lines changed: 5 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -1,14 +1,8 @@
1-
# for module compiling
2-
import os
31
from building import *
42

5-
cwd = GetCurrentDir()
6-
objs = []
7-
list = os.listdir(cwd)
3+
cwd = GetCurrentDir()
4+
src = Glob('*.c')
5+
PATH = [cwd]
6+
group = DefineGroup('robots', src, depend = ['PKG_USING_ROBOTS'], CPPPATH = PATH)
87

9-
for d in list:
10-
path = os.path.join(cwd, d)
11-
if os.path.isfile(os.path.join(path, 'SConscript')):
12-
objs = objs + SConscript(os.path.join(d, 'SConscript'))
13-
14-
Return('objs')
8+
Return('group')

‎controller/command.c‎

Lines changed: 58 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,58 @@
1+
#include <command.h>
2+
3+
#define DBG_SECTION_NAME "command"
4+
#define DBG_LEVEL DBG_LOG
5+
#include <rtdbg.h>
6+
7+
#define TABLE_MAX_SIZE 32
8+
9+
static struct command command_table[TABLE_MAX_SIZE];
10+
static uint8_t command_table_size = 0;
11+
12+
rt_err_t command_register(rt_int8_t cmd, rt_err_t (*handler)(rt_int8_t cmd, void *param))
13+
{
14+
if (command_table_size > TABLE_MAX_SIZE - 1)
15+
{
16+
LOG_E("Command table voerflow");
17+
return RT_ERROR;
18+
}
19+
command_table[command_table_size].robot_cmd = cmd;
20+
command_table[command_table_size].handler = handler;
21+
command_table_size += 1;
22+
23+
return RT_EOK;
24+
}
25+
26+
rt_err_t command_unregister(rt_int8_t cmd)
27+
{
28+
// TODO
29+
30+
return RT_EOK;
31+
}
32+
33+
rt_err_t command_handle(rt_int8_t cmd, void *param)
34+
{
35+
// look-up table and call callback
36+
for (uint16_t i = 0; i < command_table_size; i++)
37+
{
38+
if (command_table[i].robot_cmd == cmd)
39+
{
40+
command_table[i].handler(command_table[i].robot_cmd, param);
41+
return RT_EOK;
42+
}
43+
}
44+
45+
return RT_ERROR;
46+
}
47+
48+
rt_err_t command_send(command_sender_t sender, rt_int8_t cmd, void *param, rt_uint16_t len)
49+
{
50+
if (sender->send != RT_NULL)
51+
{
52+
sender->send(cmd, param, len);
53+
}
54+
55+
return RT_EOK;
56+
}
57+
58+

‎controller/command.h‎

Lines changed: 36 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,36 @@
1+
#ifndef __COMMAND_H__
2+
#define __COMMAND_H__
3+
4+
#include <rtthread.h>
5+
6+
// command
7+
#define COMMAND_NONE 0
8+
#define COMMAND_CAR_STOP 1
9+
#define COMMAND_CAR_FORWARD 2
10+
#define COMMAND_CAR_BACKWARD 3
11+
#define COMMAND_CAR_TURNLEFT 4
12+
#define COMMAND_CAR_TURNRIGHT 5
13+
#define COMMAND_GET_CAR_SPEED 6
14+
15+
#define COMMAND_CONTROLLER_VIBRATE 100
16+
17+
typedef struct command_sender *command_sender_t;
18+
19+
struct command_sender
20+
{
21+
char *name;
22+
void (*send)(rt_int8_t cmd, void *param, rt_uint16_t len);
23+
};
24+
25+
struct command
26+
{
27+
rt_int8_t robot_cmd;
28+
rt_err_t (*handler)(rt_int8_t cmd, void *param);
29+
};
30+
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);
35+
36+
#endif

‎controller/include/controller.h‎

Whitespace-only changes.

‎controller/include/ps2/ps2_controller.h‎

Whitespace-only changes.

‎controller/ps2.c‎

Lines changed: 159 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,159 @@
1+
/*********************************************************
2+
Change From YFRobot. www.yfrobot.com
3+
**********************************************************/
4+
5+
#include "ps2.h"
6+
#include "ps2_table.h"
7+
8+
#define DBG_SECTION_NAME "ps2"
9+
#define DBG_LEVEL DBG_LOG
10+
#include <rtdbg.h>
11+
12+
#define HAL_CS_H() rt_pin_write(PS2_DEFAULT_CS_PIN, PIN_HIGH)
13+
#define HAL_CS_L() rt_pin_write(PS2_DEFAULT_CS_PIN, PIN_LOW)
14+
#define HAL_CLK_H() rt_pin_write(PS2_DEFAULT_CLK_PIN, PIN_HIGH)
15+
#define HAL_CLK_L() rt_pin_write(PS2_DEFAULT_CLK_PIN, PIN_LOW)
16+
#define HAL_DO_H() rt_pin_write(PS2_DEFAULT_DO_PIN, PIN_HIGH)
17+
#define HAL_DO_L() rt_pin_write(PS2_DEFAULT_DO_PIN, PIN_LOW)
18+
#define HAL_GET_DI() rt_pin_read(PS2_DEFAULT_DI_PIN)
19+
20+
#define THREAD_DELAY_TIME 50
21+
22+
#define THREAD_PRIORITY 16
23+
#define THREAD_STACK_SIZE 1024
24+
#define THREAD_TIMESLICE 10
25+
26+
#define KEEP_TIME() _delay_us(16);
27+
28+
static uint8_t light_mode = PS2_NO_MODE;
29+
static rt_thread_t tid_ps2 = RT_NULL;
30+
static struct ps2_table table[PS2_TABLE_SIZE] = PS2_DEFAULT_TABLE;
31+
32+
static void _delay_us(uint16_t us)
33+
{
34+
for (int i = 0; i < us; i++)
35+
{
36+
for (int j = 0; j < 0x1F;)
37+
j++;
38+
}
39+
}
40+
41+
static uint8_t _transfer(uint8_t data)
42+
{
43+
uint8_t temp = 0;
44+
45+
for (uint16_t i = 0x01; i < 0x0100; i <<= 1)
46+
{
47+
if (i & data)
48+
HAL_DO_H();
49+
else
50+
HAL_DO_L();
51+
52+
HAL_CLK_H();
53+
KEEP_TIME();
54+
HAL_CLK_L();
55+
if (HAL_GET_DI())
56+
temp = i | temp;
57+
KEEP_TIME();
58+
HAL_CLK_H();
59+
}
60+
61+
return temp;
62+
}
63+
64+
static void transfer(const uint8_t *pb_send, uint8_t *pb_recv, uint8_t len)
65+
{
66+
HAL_CS_L();
67+
_delay_us(16);
68+
for (uint8_t i = 0; i < len; i++)
69+
{
70+
pb_recv[i] = _transfer(pb_send[i]);
71+
}
72+
HAL_CS_H();
73+
_delay_us(16);
74+
}
75+
76+
int ps2_scan(ps2_ctrl_data_t pt)
77+
{
78+
uint8_t temp[9] = {0};
79+
80+
temp[0] = 0x01;
81+
temp[1] = 0x42;
82+
temp[3] = 0;
83+
temp[4] = 0;
84+
85+
transfer(temp, temp, 9);
86+
87+
pt->button = temp[3] | (temp[4] << 8);
88+
pt->right_stick_x = temp[5];
89+
pt->right_stick_y = temp[6];
90+
pt->left_stick_x = temp[7];
91+
pt->left_stick_y = temp[8];
92+
93+
if (temp[2] == 0x5A)
94+
{
95+
light_mode = temp[1];
96+
return 1;
97+
}
98+
else
99+
{
100+
light_mode = PS2_NO_MODE;
101+
}
102+
103+
return 0;
104+
}
105+
106+
/**
107+
* @return PS2_GREEN_MDOE or PS2_RED_MDOE or other(no connect)
108+
*/
109+
int ps2_read_light(void)
110+
{
111+
return light_mode;
112+
}
113+
114+
static void ps2_thread_entry(void *param)
115+
{
116+
struct ps2_ctrl_data ctrl_data;
117+
118+
while (1)
119+
{
120+
rt_thread_mdelay(THREAD_DELAY_TIME);
121+
ps2_scan(&ctrl_data);
122+
// look-up table and send standard command
123+
for (int i = 0; i < PS2_TABLE_SIZE; i++)
124+
{
125+
if (!(ctrl_data.button & table[i].ps2_cmd))
126+
{
127+
if (table[i].standard_cmd != COMMAND_NONE)
128+
{
129+
command_handle(table[i].standard_cmd, RT_NULL);
130+
}
131+
}
132+
}
133+
}
134+
}
135+
136+
void ps2_init(void)
137+
{
138+
rt_pin_mode(PS2_DEFAULT_CS_PIN, PIN_MODE_OUTPUT);
139+
rt_pin_mode(PS2_DEFAULT_CLK_PIN, PIN_MODE_OUTPUT);
140+
rt_pin_mode(PS2_DEFAULT_DO_PIN, PIN_MODE_OUTPUT);
141+
rt_pin_mode(PS2_DEFAULT_DI_PIN, PIN_MODE_INPUT);
142+
143+
HAL_CS_H();
144+
HAL_CLK_H();
145+
146+
tid_ps2 = rt_thread_create("ps2",
147+
ps2_thread_entry, RT_NULL,
148+
THREAD_STACK_SIZE,
149+
THREAD_PRIORITY, THREAD_TIMESLICE);
150+
151+
if (tid_ps2 != RT_NULL)
152+
{
153+
rt_thread_startup(tid_ps2);
154+
}
155+
else
156+
{
157+
LOG_E("Can't create thread for ps2");
158+
}
159+
}

‎controller/ps2.h‎

Lines changed: 61 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,61 @@
1+
#ifndef __PS2_H__
2+
#define __PS2_H__
3+
4+
#include <rtthread.h>
5+
#include <rtdevice.h>
6+
#include <command.h>
7+
8+
// PIN
9+
#define PS2_DEFAULT_CS_PIN 28 // GET_PIN(B, 12)
10+
#define PS2_DEFAULT_CLK_PIN 29 // GET_PIN(B, 13)
11+
#define PS2_DEFAULT_DO_PIN 31 // GET_PIN(B, 15)
12+
#define PS2_DEFAULT_DI_PIN 30 // GET_PIN(B, 14)
13+
14+
// COMMAND
15+
#define PS2_CMD_VIBRATE 1
16+
17+
// MODE
18+
#define PS2_NO_MODE 0
19+
#define PS2_GREEN_MODE 0x41
20+
#define PS2_RED_MODE 0x73
21+
22+
// KEY
23+
#define PS2_BTN_SELECT (1 << 0)
24+
#define PS2_BTN_L3 (1 << 1)
25+
#define PS2_BTN_R3 (1 << 2)
26+
#define PS2_BTN_START (1 << 3)
27+
#define PS2_BTN_UP (1 << 4)
28+
#define PS2_BTN_RIGHT (1 << 5)
29+
#define PS2_BTN_DOWN (1 << 6)
30+
#define PS2_BTN_LEFT (1 << 7)
31+
#define PS2_BTN_L2 (1 << 8)
32+
#define PS2_BTN_R2 (1 << 9)
33+
#define PS2_BTN_L1 (1 << 10)
34+
#define PS2_BTN_R1 (1 << 11)
35+
#define PS2_BTN_TRIANGLE (1 << 12)
36+
#define PS2_BTN_CICLE (1 << 13)
37+
#define PS2_BTN_FORK (1 << 14)
38+
#define PS2_BTN_SQUARE (1 << 15)
39+
40+
#define PS2_ROCKER_LX (9)
41+
#define PS2_ROCKER_LY (10)
42+
#define PS2_ROCKER_RX (11)
43+
#define PS2_ROCKER_RY (12)
44+
45+
typedef struct ps2_ctrl_data *ps2_ctrl_data_t;
46+
typedef struct ps2 *ps2_t;
47+
48+
struct ps2_ctrl_data
49+
{
50+
uint16_t button; // 16
51+
uint8_t left_stick_x;
52+
uint8_t left_stick_y;
53+
uint8_t right_stick_x;
54+
uint8_t right_stick_y;
55+
};
56+
57+
void ps2_init(void);
58+
int ps2_scan(ps2_ctrl_data_t pt);
59+
int ps2_read_light(void);
60+
61+
#endif

‎controller/ps2/ps2_controler.c‎

Whitespace-only changes.

‎controller/ps2_table.h‎

Lines changed: 29 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,29 @@
1+
#include "ps2.h"
2+
3+
#define PS2_TABLE_SIZE 16
4+
#define PS2_DEFAULT_TABLE \
5+
{ \
6+
{PS2_BTN_SELECT ,COMMAND_CAR_STOP}, \
7+
{PS2_BTN_L3 ,COMMAND_NONE}, \
8+
{PS2_BTN_R3 ,COMMAND_NONE}, \
9+
{PS2_BTN_START ,COMMAND_NONE}, \
10+
{PS2_BTN_UP ,COMMAND_CAR_FORWARD}, \
11+
{PS2_BTN_RIGHT ,COMMAND_CAR_TURNRIGHT}, \
12+
{PS2_BTN_DOWN ,COMMAND_CAR_BACKWARD}, \
13+
{PS2_BTN_LEFT ,COMMAND_CAR_TURNLEFT}, \
14+
{PS2_BTN_L2 ,COMMAND_NONE}, \
15+
{PS2_BTN_R2 ,COMMAND_NONE}, \
16+
{PS2_BTN_L1 ,COMMAND_NONE}, \
17+
{PS2_BTN_R1 ,COMMAND_NONE}, \
18+
{PS2_BTN_TRIANGLE ,COMMAND_NONE}, \
19+
{PS2_BTN_CICLE ,COMMAND_NONE}, \
20+
{PS2_BTN_FORK ,COMMAND_NONE}, \
21+
{PS2_BTN_SQUARE ,COMMAND_NONE} \
22+
}
23+
24+
struct ps2_table
25+
{
26+
int ps2_cmd;
27+
int standard_cmd;
28+
};
29+

‎controller/src/controller.c‎

Whitespace-only changes.

0 commit comments

Comments
 (0)