Skip to content

Commit 5d83db7

Browse files
EeeeBinEeeeBin
committed
Add BaseX example
1 parent 1c0ffeb commit 5d83db7

File tree

3 files changed

+445
-0
lines changed

3 files changed

+445
-0
lines changed

‎examples/Modules/BaseX/BaseX.cpp

Lines changed: 231 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,231 @@
1+
/*
2+
* @Author: Sorzn
3+
* @Date: 2019-12-12 14:33:45
4+
* @LastEditTime: 2019-12-13 15:58:32
5+
* @Description: M5Stack project
6+
* @FilePath: /M5Stack/examples/Modules/BaseX/BaseX.cpp
7+
*/
8+
9+
#include "M5Stack.h"
10+
#include "BaseX.h"
11+
12+
#define MAX(in, max) (((in) > (max)) ? (in) : (max))
13+
#define MIN(in, min) (((in) < (min)) ? (in) : (min))
14+
#define CONSTRAIN(in, min, max) MAX(min, MIN(in, max))
15+
16+
uint8_t BASE_X::CheckPos(uint8_t pos)
17+
{
18+
pos = pos - 1;
19+
pos = CONSTRAIN(pos, 0, 3);
20+
return pos;
21+
}
22+
23+
/**
24+
* @description:
25+
* @param pos: 1 ~ 4
26+
* @param mode: NORMAL, POSITION LOCK, SPEED LOCK
27+
* @return: None
28+
*/
29+
void BASE_X::SetMode(uint8_t pos, uint8_t mode)
30+
{
31+
pos = CheckPos(pos);
32+
M5.I2C.writeByte(BASE_X_ADDR, BASE_X_CONFIG_ADDR + (0x10 * pos), mode);
33+
}
34+
35+
/**
36+
* @description:
37+
* @param pos: 1 ~ 4
38+
* @return: encoder value
39+
*/
40+
int32_t BASE_X::GetEncoderValue(uint8_t pos)
41+
{
42+
uint8_t addr;
43+
uint8_t read_buf[4] = {0, 0, 0, 0};
44+
45+
pos = CheckPos(pos);
46+
addr = BASE_X_ENCODER_ADDR + 4*pos;
47+
M5.I2C.readBytes(BASE_X_ADDR, addr, 4, read_buf);
48+
return (read_buf[0] << 24) | (read_buf[1] << 16) | (read_buf[2] << 8) | read_buf[3];
49+
}
50+
51+
/**
52+
* @description:
53+
* @param pos: 1 ~ 4
54+
* @param encoder: INT32_MIN ~ INT32_MAX
55+
* @return: None
56+
*/
57+
void BASE_X::SetEncoderValue(uint8_t pos, int32_t encoder)
58+
{
59+
uint8_t addr;
60+
uint8_t write_buf[4] = {0, 0, 0, 0};
61+
62+
pos = CheckPos(pos);
63+
addr = BASE_X_ENCODER_ADDR + 4*pos;
64+
write_buf[0] = encoder >> 24;
65+
write_buf[1] = encoder >> 16;
66+
write_buf[2] = encoder >> 8;
67+
write_buf[3] = encoder & 0xff;
68+
69+
M5.I2C.writeBytes(BASE_X_ADDR, addr, write_buf, 4);
70+
}
71+
72+
/**
73+
* @description:
74+
* @param pos: 1 ~ 4
75+
* @param duty: Set motor speed, -127 ~ 127
76+
* @return:
77+
*/
78+
void BASE_X::SetMotorSpeed(uint8_t pos, int8_t duty)
79+
{
80+
uint8_t addr;
81+
pos = CheckPos(pos);
82+
addr = BASE_X_PWM_DUTY_ADDR + pos;
83+
84+
M5.I2C.writeByte(BASE_X_ADDR, addr, duty);
85+
}
86+
87+
/**
88+
* @description:
89+
* @param pos: 1 ~ 4
90+
* @return Motor run speed, -127 ~ 127:
91+
*/
92+
int8_t BASE_X::GetMotorSpeed(uint8_t pos)
93+
{
94+
uint8_t read_data;
95+
uint8_t addr;
96+
pos = CheckPos(pos);
97+
addr = BASE_X_PWM_DUTY_ADDR + pos;
98+
99+
M5.I2C.readByte(BASE_X_ADDR, addr, &read_data);
100+
return read_data;
101+
}
102+
103+
104+
/**
105+
* @description:
106+
* @param pos: 1 ~ 4
107+
* @return: Motor encoder increments every 20 ms
108+
*/
109+
int8_t BASE_X::GetMotorSpeed20MS(uint8_t pos)
110+
{
111+
uint8_t read_data;
112+
uint8_t addr;
113+
pos = CheckPos(pos);
114+
addr = BASE_X_SPEED_ADDR + pos;
115+
116+
M5.I2C.readByte(BASE_X_ADDR, addr, &read_data);
117+
return read_data;
118+
}
119+
120+
void BASE_X::SetPositionPID(uint8_t pos, uint8_t kp, uint8_t ki, uint8_t kd)
121+
{
122+
uint8_t write_buf[3] = {0, 0, 0};
123+
uint8_t addr;
124+
pos = CheckPos(pos);
125+
126+
addr = BASE_X_CONFIG_ADDR + pos * 0x10 + 0x01;
127+
write_buf[0] = kp;
128+
write_buf[1] = ki;
129+
write_buf[2] = kd;
130+
131+
M5.I2C.writeBytes(BASE_X_ADDR, addr, write_buf, 3);
132+
}
133+
134+
/**
135+
* @description:
136+
* @param pos: 1 ~ 4
137+
* @param position_point: in POSITION mode, motor will lock in this value, INT32_MIN ~ INT32_MAX
138+
* @return: None
139+
*/
140+
void BASE_X::SetPositionPoint(uint8_t pos, int32_t position_point)
141+
{
142+
uint8_t addr;
143+
uint8_t write_buf[4] = {0, 0, 0, 0};
144+
145+
pos = CheckPos(pos);
146+
addr = BASE_X_CONFIG_ADDR + pos * 0x10 + 0x04;
147+
write_buf[0] = position_point & 0xff;
148+
write_buf[1] = position_point >> 8;
149+
write_buf[2] = position_point >> 16;
150+
write_buf[3] = position_point >> 24;
151+
152+
// Serial.printf(" %x %x %x %x \r\n", write_buf[0], write_buf[1], write_buf[2], write_buf[3]);
153+
M5.I2C.writeBytes(BASE_X_ADDR, addr, write_buf, 4);
154+
}
155+
156+
/**
157+
* @description:
158+
* @param pos: 1 ~ 4
159+
* @param max_pwm: 0 ~ 127, POSITION mode, max speed
160+
* @return:
161+
*/
162+
void BASE_X::SetPostionPIDMaxSpeed(uint8_t pos, uint8_t max_pwm)
163+
{
164+
uint8_t addr;
165+
pos = CheckPos(pos);
166+
addr = BASE_X_CONFIG_ADDR + pos * 0x10 + 0x08;
167+
168+
M5.I2C.writeByte(BASE_X_ADDR, addr, max_pwm);
169+
}
170+
171+
void BASE_X::SetSpeedPID(uint8_t pos, uint8_t kp, uint8_t ki, uint8_t kd)
172+
{
173+
uint8_t write_buf[3] = {0, 0, 0};
174+
uint8_t addr;
175+
pos = CheckPos(pos);
176+
177+
addr = BASE_X_CONFIG_ADDR + pos * 0x10 + 0x09;
178+
write_buf[0] = kp;
179+
write_buf[1] = ki;
180+
write_buf[2] = kd;
181+
182+
M5.I2C.writeBytes(BASE_X_ADDR, addr, write_buf, 3);
183+
}
184+
185+
/**
186+
* @description:
187+
* @param pos: 1 ~ 4
188+
* @param speed_point: speed_point is lock GetMotorSpeed20MS(), not GetMotorSpeed(), maybe -20 ~ 20
189+
* @return: None
190+
*/
191+
void BASE_X::SetSpeedPoint(uint8_t pos, int8_t speed_point)
192+
{
193+
uint8_t addr;
194+
pos = CheckPos(pos);
195+
addr = BASE_X_CONFIG_ADDR + pos * 0x10 + 0x0c;
196+
197+
M5.I2C.writeByte(BASE_X_ADDR, addr, (uint8_t)speed_point);
198+
}
199+
200+
/**
201+
* @description:
202+
* @param pos: 1 ~ 2
203+
* @param angle: 0 ~ 180
204+
* @return: None
205+
*/
206+
void BASE_X::SetServoAngle(uint8_t pos, uint8_t angle)
207+
{
208+
uint8_t addr;
209+
pos = CheckPos(pos);
210+
addr = BASE_X_SERVO_ANGLE_ADDR + pos;
211+
212+
M5.I2C.writeByte(BASE_X_ADDR, addr, angle);
213+
}
214+
215+
/**
216+
* @description:
217+
* @param pos: 1 ~ 2
218+
* @param width: 500 ~ 2500
219+
* @return: None
220+
*/
221+
void BASE_X::SetServoPulseWidth(uint8_t pos, uint16_t width)
222+
{
223+
uint8_t addr;
224+
uint8_t write_buf[2] = {0, 0};
225+
226+
pos = CheckPos(pos);
227+
addr = BASE_X_SERVO_PULSE_ADDR + pos * 0x02;
228+
write_buf[0] = width >> 8;
229+
write_buf[1] = width & 0xff;
230+
M5.I2C.writeBytes(BASE_X_ADDR, addr, write_buf, 2);
231+
}

‎examples/Modules/BaseX/BaseX.h

Lines changed: 60 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,60 @@
1+
/*
2+
* @Author: Sorzn
3+
* @Date: 2019-12-12 14:33:50
4+
* @LastEditTime: 2019-12-13 15:47:59
5+
* @Description: M5Stack project
6+
* @FilePath: /M5Stack/examples/Modules/BaseX/BaseX.h
7+
*/
8+
9+
#ifndef __BASE_X_H__
10+
#define __BASE_X_H__
11+
12+
#include "Arduino.h"
13+
14+
#define BASE_X_ADDR (0x22)
15+
16+
#define BASE_X_SERVO_ANGLE_ADDR (0x00)
17+
#define BASE_X_SERVO_PULSE_ADDR (0x10)
18+
#define BASE_X_PWM_DUTY_ADDR (0x20)
19+
#define BASE_X_ENCODER_ADDR (0x30)
20+
#define BASE_X_SPEED_ADDR (0x40)
21+
22+
/*
23+
| 0 | 1 | 2 | 3 | 4, 5, 6, 7 | 8 | 9 | 10 | 11 | 12 |
24+
| mod | position-p | position-i | position-d | position-point | position-max-speed | speed-p | speed-i | speed-d | speed-point |
25+
*/
26+
#define BASE_X_CONFIG_ADDR (0x50)
27+
28+
#define NORMAL_MODE (0x00)
29+
#define POSITION_MODE (0x01)
30+
#define SPEED_MODE (0x02)
31+
32+
33+
class BASE_X
34+
{
35+
public:
36+
void SetMode(uint8_t pos, uint8_t mode);
37+
38+
int32_t GetEncoderValue(uint8_t pos);
39+
void SetEncoderValue(uint8_t pos, int32_t encode);
40+
41+
void SetMotorSpeed(uint8_t pos, int8_t duty);
42+
int8_t GetMotorSpeed(uint8_t pos);
43+
44+
int8_t GetMotorSpeed20MS(uint8_t pos);
45+
46+
void SetPositionPID(uint8_t pos, uint8_t kp, uint8_t ki, uint8_t kd);
47+
void SetPositionPoint(uint8_t pos, int32_t position_point);
48+
void SetPostionPIDMaxSpeed(uint8_t pos, uint8_t max_pwm);
49+
50+
void SetSpeedPID(uint8_t pos, uint8_t kp, uint8_t ki, uint8_t kd);
51+
void SetSpeedPoint(uint8_t pos, int8_t speed_point);
52+
53+
void SetServoAngle(uint8_t pos, uint8_t angle);
54+
void SetServoPulseWidth(uint8_t pos, uint16_t width);
55+
56+
private:
57+
uint8_t CheckPos(uint8_t pos);
58+
};
59+
60+
#endif

0 commit comments

Comments
 (0)