Skip to content

Commit 7b87a64

Browse files
mongonta0716mongonta0716
committed
1st commit
1 parent 03d77f9 commit 7b87a64

9 files changed

+590
-0
lines changed

‎CMakeLists.txt

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,9 @@
1+
set(COMPONENT_ADD_INCLUDEDIRS
2+
src
3+
)
4+
file(GLOB SRCS
5+
src/*.cpp
6+
)
7+
set(COMPONENT_SRCS ${SRCS})
8+
9+
register_component()

‎component.mk

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,7 @@
1+
#
2+
# Main Makefile. This is basically the same as a component makefile.
3+
#
4+
# (Uses default behaviour of compiling all source files in directory, adding 'include' to include path.)
5+
6+
COMPONENT_SRCDIRS := src
7+
COMPONENT_ADD_INCLUDEDIRS := src

‎library.json

Lines changed: 23 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,23 @@
1+
{
2+
"name": "Stackchan-arduino",
3+
"keywords": "stackchan, servo, M5Stack",
4+
"description": "An M5Stack library for Stackchan",
5+
"repository": {
6+
"type": "git",
7+
"url": "https://github.com/mongonta0716/stackchan-arduino.git"
8+
},
9+
"authors": [
10+
{
11+
"name": "Takao Akaki",
12+
"email": "[email protected]",
13+
"url": "https://mongonta0716.github.io/",
14+
"maintainer": true
15+
}
16+
],
17+
"license": "MIT",
18+
"dependencies": {
19+
},
20+
"version": "0.0.1",
21+
"frameworks": "arduino",
22+
"platforms": "espressif32"
23+
}

‎library.properties

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,10 @@
1+
name=Stackchan_arduino
2+
version=0.0.1
3+
author=Takao Akaki
4+
maintainer=Takao Akaki<[email protected]>
5+
sentence=Stackchan library for M5Stack
6+
paragraph=See more on http://M5Stack.com
7+
category=Device Control
8+
url=https://platformio.org/lib/show/4529/M5Stack-Avatar
9+
architectures=esp32
10+
includes=Stackchan_servo.h, Stackchan_system_config.h

‎src/Stackchan_Takao_Base.hpp

Lines changed: 34 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,34 @@
1+
#ifndef STACKCHAN_TAKAO_BASE_HPP
2+
#define STACKCHAN_TAKAO_BASE_HPP
3+
4+
5+
#include <M5Unified.h>
6+
#include "Stackchan_servo.h"
7+
8+
enum PowerStatus {
9+
SidePower,
10+
BackPower,
11+
Battery
12+
};
13+
14+
uint8_t checkTakaoBasePowerStatus(m5::Power_Class* power, StackchanSERVO* servo ) {
15+
bool last_ext_output = power->getExtOutput();
16+
if (power->Axp192.getACINVoltage() > 1.0f) {
17+
power->setExtOutput(true);
18+
power->setLed(80);
19+
return PowerStatus::SidePower;
20+
}
21+
while (servo->isMoving()) {delay(1);} // サーボが動いている間は待機（そうしないとサーボの動きが乱れる。）
22+
power->setExtOutput(false); // 後側のUSB-Cの状態を把握するためにfalseにする必要があります。
23+
delay(500);
24+
if (power->Axp192.getBatteryDischargeCurrent() > 3.0f) {
25+
power->setExtOutput(true);
26+
power->setLed(0);
27+
return PowerStatus::Battery;
28+
}
29+
power->setExtOutput(false);
30+
power->setLed(80);
31+
return PowerStatus::BackPower;
32+
}
33+
34+
#endif // STACKCHAN_TAKAO_BASE_HPP

‎src/Stackchan_servo.cpp

Lines changed: 182 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,182 @@
1+
// Copyright (c) Takao Akaki
2+
#include "Stackchan_servo.h"
3+
4+
#include <ServoEasing.hpp>
5+
6+
long convertSCS0009Pos(int16_t degree) {
7+
//Serial.printf("Degree: %d\n", degree);
8+
return map(degree, 0, 300, 1023, 0);
9+
}
10+
11+
StackchanSERVO::StackchanSERVO() {}
12+
13+
StackchanSERVO::~StackchanSERVO() {}
14+
15+
16+
void StackchanSERVO::attachServos() {
17+
if (_servo_type == SCS) {
18+
// SCS0009
19+
Serial2.begin(1000000, SERIAL_8N1, _init_param.servo[AXIS_X].pin, _init_param.servo[AXIS_Y].pin);
20+
_sc.pSerial = &Serial2;
21+
_sc.WritePos(AXIS_X + 1, convertSCS0009Pos(_init_param.servo[AXIS_X].start_degree + _init_param.servo[AXIS_X].offset), 1000);
22+
_sc.WritePos(AXIS_Y + 1, convertSCS0009Pos(_init_param.servo[AXIS_Y].start_degree + _init_param.servo[AXIS_Y].offset), 1000);
23+
vTaskDelay(1000/portTICK_PERIOD_MS);
24+
} else {
25+
// SG90 PWM
26+
if (_servo_x.attach(_init_param.servo[AXIS_X].pin,
27+
_init_param.servo[AXIS_X].start_degree + _init_param.servo[AXIS_X].offset,
28+
DEFAULT_MICROSECONDS_FOR_0_DEGREE,
29+
DEFAULT_MICROSECONDS_FOR_180_DEGREE)) {
30+
Serial.print("Error attaching servo x");
31+
}
32+
if (_servo_y.attach(_init_param.servo[AXIS_Y].pin,
33+
_init_param.servo[AXIS_Y].start_degree + _init_param.servo[AXIS_Y].offset,
34+
DEFAULT_MICROSECONDS_FOR_0_DEGREE,
35+
DEFAULT_MICROSECONDS_FOR_180_DEGREE)) {
36+
Serial.print("Error attaching servo x");
37+
}
38+
39+
_servo_x.setEasingType(EASE_QUADRATIC_IN_OUT);
40+
_servo_y.setEasingType(EASE_QUADRATIC_IN_OUT);
41+
}
42+
}
43+
44+
void StackchanSERVO::begin(stackchan_servo_initial_param_s init_param) {
45+
_init_param = init_param;
46+
attachServos();
47+
}
48+
49+
void StackchanSERVO::begin(int servo_pin_x, int16_t start_degree_x, int16_t offset_x,
50+
int servo_pin_y, int16_t start_degree_y, int16_t offset_y,
51+
ServoType servo_type) {
52+
_init_param.servo[AXIS_X].pin = servo_pin_x;
53+
_init_param.servo[AXIS_X].start_degree = start_degree_x;
54+
_init_param.servo[AXIS_X].offset = offset_x;
55+
_init_param.servo[AXIS_Y].pin = servo_pin_y;
56+
_init_param.servo[AXIS_Y].start_degree = start_degree_y;
57+
_init_param.servo[AXIS_Y].offset = offset_y;
58+
_servo_type = servo_type;
59+
attachServos();
60+
}
61+
62+
void StackchanSERVO::moveX(int x, uint32_t millis_for_move) {
63+
if (_servo_type == SCS) {
64+
_sc.WritePos(AXIS_X + 1, convertSCS0009Pos(x + _init_param.servo[AXIS_X].offset), millis_for_move);
65+
_isMoving = true;
66+
vTaskDelay(millis_for_move/portTICK_PERIOD_MS);
67+
_isMoving = false;
68+
} else {
69+
if (millis_for_move == 0) {
70+
_servo_x.easeTo(x + _init_param.servo[AXIS_X].offset);
71+
} else {
72+
_servo_x.easeToD(x + _init_param.servo[AXIS_X].offset, millis_for_move);
73+
}
74+
_isMoving = true;
75+
synchronizeAllServosStartAndWaitForAllServosToStop();
76+
_isMoving = false;
77+
}
78+
}
79+
80+
void StackchanSERVO::moveX(servo_param_s servo_param_x) {
81+
_init_param.servo[AXIS_X].offset = servo_param_x.offset;
82+
moveX(servo_param_x.degree, servo_param_x.millis_for_move);
83+
}
84+
85+
void StackchanSERVO::moveY(int y, uint32_t millis_for_move) {
86+
if (_servo_type == SCS) {
87+
_sc.WritePos(AXIS_Y + 1, convertSCS0009Pos(y + _init_param.servo[AXIS_Y].offset), millis_for_move);
88+
_isMoving = true;
89+
vTaskDelay(millis_for_move/portTICK_PERIOD_MS);
90+
_isMoving = false;
91+
} else {
92+
if (millis_for_move == 0) {
93+
_servo_y.easeTo(y + _init_param.servo[AXIS_Y].offset);
94+
} else {
95+
_servo_y.easeToD(y + _init_param.servo[AXIS_Y].offset, millis_for_move);
96+
}
97+
_isMoving = true;
98+
synchronizeAllServosStartAndWaitForAllServosToStop();
99+
_isMoving = false;
100+
}
101+
}
102+
103+
void StackchanSERVO::moveY(servo_param_s servo_param_y) {
104+
_init_param.servo[AXIS_Y].offset = servo_param_y.offset;
105+
moveY(servo_param_y.degree, servo_param_y.millis_for_move);
106+
}
107+
void StackchanSERVO::moveXY(int x, int y, uint32_t millis_for_move) {
108+
if (_servo_type == SCS) {
109+
_sc.WritePos(AXIS_X + 1, convertSCS0009Pos(x + _init_param.servo[AXIS_X].offset), millis_for_move);
110+
_sc.WritePos(AXIS_Y + 1, convertSCS0009Pos(y + _init_param.servo[AXIS_Y].offset), millis_for_move);
111+
_isMoving = true;
112+
vTaskDelay(millis_for_move/portTICK_PERIOD_MS);
113+
_isMoving = false;
114+
} else {
115+
_servo_x.setEaseToD(x + _init_param.servo[AXIS_X].offset, millis_for_move);
116+
_servo_y.setEaseToD(y + _init_param.servo[AXIS_Y].offset, millis_for_move);
117+
_isMoving = true;
118+
synchronizeAllServosStartAndWaitForAllServosToStop();
119+
_isMoving = false;
120+
}
121+
}
122+
123+
void StackchanSERVO::moveXY(servo_param_s servo_param_x, servo_param_s servo_param_y) {
124+
if (_servo_type == SCS) {
125+
_sc.WritePos(AXIS_X + 1, convertSCS0009Pos(servo_param_x.degree + servo_param_x.offset), servo_param_x.millis_for_move);
126+
_sc.WritePos(AXIS_Y + 1, convertSCS0009Pos(servo_param_y.degree + servo_param_y.offset), servo_param_y.millis_for_move);
127+
_isMoving = true;
128+
vTaskDelay(max(servo_param_x.millis_for_move, servo_param_y.millis_for_move)/portTICK_PERIOD_MS);
129+
_isMoving = false;
130+
} else {
131+
if (servo_param_x.degree != 0) {
132+
_servo_x.setEaseToD(servo_param_x.degree + servo_param_x.offset, servo_param_x.millis_for_move);
133+
}
134+
if (servo_param_y.degree != 0) {
135+
_servo_y.setEaseToD(servo_param_y.degree + servo_param_y.offset, servo_param_y.millis_for_move);
136+
}
137+
_isMoving = true;
138+
synchronizeAllServosStartAndWaitForAllServosToStop();
139+
_isMoving = false;
140+
}
141+
}
142+
143+
void StackchanSERVO::motion(Motion motion_number) {
144+
if (motion_number == nomove) return;
145+
moveXY(90, 75, 500);
146+
switch(motion_number) {
147+
case greet:
148+
moveY(90, 1000);
149+
moveY(75, 1000);
150+
break;
151+
case laugh:
152+
for (int i=0; i<5; i++) {
153+
moveY(80, 500);
154+
moveY(60, 500);
155+
}
156+
break;
157+
case nod:
158+
for (int i=0; i<5; i++) {
159+
moveY(90, 1000);
160+
moveY(60, 1000);
161+
}
162+
break;
163+
case refuse:
164+
for (int i=0; i<2; i++) {
165+
moveX(70, 500);
166+
moveX(110, 500);
167+
}
168+
break;
169+
case test:
170+
moveX(45, 1000);
171+
moveX(135, 1000);
172+
moveX(90, 1000);
173+
moveY(50, 1000);
174+
moveY(90, 1000);
175+
break;
176+
default:
177+
Serial.printf("invalid motion number: %d\n", motion_number);
178+
break;
179+
}
180+
delay(1000);
181+
moveXY(_init_param.servo[AXIS_X].start_degree, _init_param.servo[AXIS_Y].degree, 1000);
182+
}

‎src/Stackchan_servo.h

Lines changed: 72 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,72 @@
1+
// Copyright (c) 2022 Takao Akaki
2+
3+
#ifndef _STACKCHAN_SERVO_H_
4+
#define _STACKCHAN_SERVO_H_
5+
6+
// コンパイル時にServoEasing.hppをIncludeしてくださいという警告が出ますが、hppにすると二重定義のリンクエラーが出ます。
7+
// その対処でStackchan_servo.hはh, Stackchan_servo.cppはhppをincludeしています。
8+
#define SUPPRESS_HPP_WARNING
9+
#include <ServoEasing.h>
10+
#include <SCServo.h>
11+
12+
enum Motion {
13+
nomove, // 動かない
14+
greet, // 挨拶
15+
laugh, // 笑う
16+
nod, // うなづく
17+
refuse, // 首を横に振る（拒絶）
18+
test = 99, // テスト用
19+
};
20+
21+
enum ServoAxis {
22+
AXIS_X,
23+
AXIS_Y
24+
};
25+
26+
enum ServoType {
27+
PWM,
28+
SCS
29+
};
30+
31+
typedef struct ServoParam {
32+
int pin; // サーボのピン番号
33+
int16_t start_degree; // 初期角度
34+
int16_t offset; // オフセット（90°からの+-）
35+
int16_t degree; // 角度
36+
uint32_t millis_for_move; // 移動時間(msec)
37+
uint16_t lower_limit; // サーボ角度の下限
38+
uint16_t upper_limit; // サーボ角度の上限
39+
} servo_param_s;
40+
41+
42+
typedef struct StackchanServo{
43+
servo_param_s servo[2];
44+
} stackchan_servo_initial_param_s;
45+
46+
class StackchanSERVO {
47+
protected:
48+
ServoType _servo_type;
49+
SCSCL _sc;
50+
ServoEasing _servo_x;
51+
ServoEasing _servo_y;
52+
void attachServos();
53+
stackchan_servo_initial_param_s _init_param;
54+
bool _isMoving;
55+
public:
56+
StackchanSERVO();
57+
~StackchanSERVO();
58+
void begin(stackchan_servo_initial_param_s init_params);
59+
void begin(int servo_pin_x, int16_t start_degree_x, int16_t offset_x,
60+
int servo_pin_y, int16_t start_degree_y, int16_t offset_y,
61+
ServoType servo_type=PWM);
62+
void moveX(int x, uint32_t millis_for_move = 0);
63+
void moveY(int y, uint32_t millis_for_move = 0);
64+
void moveXY(int x, int y, uint32_t millis_for_move);
65+
void moveX(servo_param_s servo_param_x);
66+
void moveY(servo_param_s servo_param_y);
67+
void moveXY(servo_param_s servo_param_x, servo_param_s servo_param_y);
68+
void motion(Motion motion_no);
69+
bool isMoving() { return _isMoving; }
70+
};
71+
72+
#endif // _STACKCHAN_SERVO_H_

0 commit comments

Comments
 (0)