Skip to content

Commit 8bd5bee

Browse files
mongonta0716mongonta0716
authored
Merge pull request #1 from mongonta0716/SCSEasing
functional enhancement
2 parents ce996b9 + 7cb16af commit 8bd5bee

File tree

12 files changed

+419
-39
lines changed

12 files changed

+419
-39
lines changed

‎README.md‎

Lines changed: 22 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,2 +1,24 @@
11
# stackchan-arduino
22
Stack Chan Library for ArduinoFramework
3+
4+
# 使い方
5+
6+
platformio.iniのlib_depsに下記の記述を追加してください。(そのうち公開します。)
7+
8+
```
9+
lib_deps =
10+
m5stack/M5Unified@0.1.16
11+
arminjo/ServoEasing@^2.4.0
12+
madhephaestus/ESP32Servo @ 0.13.0
13+
bblanchon/ArduinoJson @ ^6
14+
tobozo/YAMLDuino
15+
https://github.com/mongonta0716/stackchan-arduino.git
16+
https://github.com/mongonta0716/SCServo
17+
lib_ldf_mode = deep ; これを忘れるとリンクエラーになります。
18+
```
19+
20+
# 設定ファイル
21+
- [SC_BasicConfig.yaml](./data/yaml/SC_BasicConfig.yaml)<br>ｽﾀｯｸﾁｬﾝを起動するために必要な基本的なパラメータを集めています。
22+
- [SC_SecConfig.yaml](./data/yaml/SC_SecConfig.yaml)<br>個人情報用の設定ファイルです。(WiFiやAPIKey等)
23+
- [SC_ExConfig.yaml](./data/yaml/SC_ExConfig.yaml)<br>アプリケーション側で、設定を追加したいときに使用します。使い方はexamples/Advanceを参照してください。
24+

‎data/yaml/SC_BasicConfig.yaml‎

Lines changed: 49 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,49 @@
1+
servo:
2+
pin:
3+
# ServoPin
4+
# Core1 PortA X:22,Y:21 PortC X:16,Y:17
5+
# Core2 PortA X:33,Y:32 PortC X:13,Y:14
6+
# Stack-chanPCB Core1 X:5,Y:2 Core2 X:19,Y27
7+
# When using SCS0009, x:RX, y:TX (not used).(StackchanRT Version:Core1 x16,y17, Core2: x13,y14)
8+
x: 17
9+
y: 18
10+
offset:
11+
# Specified by +- from 90 degree during servo initialization
12+
x: 0
13+
y: 0
14+
speed:
15+
normal_mode:
16+
interval_min: 3000
17+
interval_max: 6000
18+
move_min: 500
19+
move_max: 1500
20+
sing_mode:
21+
interval_min: 500
22+
interval_max: 1000
23+
move_min: 500
24+
move_max: 1000
25+
bluetooth:
26+
device_name: "M5StackBTSPK"
27+
starting_state: false
28+
start_volume: 100
29+
auto_power_off_time: 0 # Core2 Only. time(msec) of auto power off(0 is disable.)
30+
balloon:
31+
font_language: "CN" # "JA or CN or Default"
32+
lyrics: # A maximum of 10 can be specified.
33+
- "こんにちは"
34+
- "Hello"
35+
- "Bonjour"
36+
- "你好"
37+
- "私はスタックチャン"
38+
- "我是Stack-chan"
39+
- "I'm Stack-chan"
40+
- "Je suis Stack-chan"
41+
led_lr: 0 # 0:stereo, 1:left_only, 2:right_only
42+
led_pin: 15 # GoBottom1:15 GoBottom2:25
43+
takao_base: false # Whether to use takaobase to feed power from the rear connector.(Stack-chan_Takao_Base https://ssci.to/8905)
44+
servo_type: "PWM" # "PWM": SG90PWMServo, "SCS": Feetech SCS0009
45+
extend_config_filename: "/yaml/SC_ExConfig.yaml" # Configuration file for the application.
46+
extend_config_filesize: 2048 # Buffer size for feature extensions
47+
secret_config_filename: "/yaml/SC_SecConfig.yaml" # Configuration file for the File for personal information.
48+
secret_config_filesize: 2048 # Buffer size for personal information.
49+
secret_info_show: true # Whether personal information is output to the log or not.

‎data/yaml/SC_ExtConfig.yaml‎

Lines changed: 14 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,14 @@
1+
# 各アプリケーションで利用する設定を書きます。
2+
# '#'を使うとコメントとして働きます。
3+
app_parameters1:
4+
item1: "アイテム1のデータ" # 文字列
5+
item2: 123456789 # 数値
6+
item3: True # True or False or 0 or 1
7+
8+
app_parameters2:
9+
item4: "リストのサンプル"
10+
list_str: # リストの書き方1
11+
- "文字列1"
12+
- "文字列2"
13+
- "文字列3"
14+
list_num: [ 1, -2, 3, -4] # リストの書き方2

‎data/yaml/SC_SecConfig.yaml‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,8 @@
1+
# 個人情報用の設定ファイル
2+
wifi:
3+
ssid: "YOUR_WIFI_SSID"
4+
password: "YOUR_WIFI_PASSWORD"
5+
apikey:
6+
stt: "YOUR_APIKEY" # ApiKey of SpeechToText Service
7+
aiservice: "YOUR_APIKEY" # ApiKey of AIService
8+
tts: "YOUR_APIKEY" # ApiKey of TextToSpeech Service

‎examples/Basic/platformio.ini‎

Lines changed: 29 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,29 @@
1+
[platformio]
2+
default_envs = m5stack-core2
3+
4+
[env]
5+
platform = espressif32 @ 6.5.0
6+
framework = arduino
7+
upload_speed = 1500000
8+
monitor_speed = 115200
9+
board_build.f_flash = 80000000L
10+
board_build.filesystem = spiffs
11+
board_build.partitions = default_16MB.csv
12+
build_flags = -DCORE_DEBUG_LEVEL=4
13+
lib_deps =
14+
15+
lib_deps =
16+
m5stack/M5Unified@0.1.16
17+
arminjo/ServoEasing@^2.4.0
18+
madhephaestus/ESP32Servo @ 0.13.0
19+
bblanchon/ArduinoJson @ ^6
20+
tobozo/YAMLDuino
21+
https://github.com/mongonta0716/stackchan-arduino.git
22+
https://github.com/mongonta0716/SCServo
23+
lib_ldf_mode = deep ; これを忘れるとリンクエラーになります。
24+
25+
[env:m5stack-core2]
26+
board = m5stack-conre2
27+
28+
[env:m5stack-cores3]
29+
board = m5stack-cores3

‎examples/Basic/src/Stackchan_ex_config.cpp‎

Lines changed: 53 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,53 @@
1+
2+
#include "Stackchan_ex_config.h"
3+
4+
StackchanExConfig::StackchanExConfig() {};
5+
StackchanExConfig::~StackchanExConfig() {};
6+
7+
void StackchanExConfig::loadExtendConfig(fs::FS& fs, const char *yaml_filename, uint32_t yaml_size) {
8+
M5_LOGI("----- StackchanExConfig::loadConfig:%s\n", yaml_filename);
9+
File file = fs.open(yaml_filename);
10+
if (file) {
11+
DynamicJsonDocument doc(yaml_size);
12+
auto err = deserializeYml( doc, file);
13+
if (err) {
14+
M5_LOGE("yaml file read error: %s\n", yaml_filename);
15+
M5_LOGE("error%s\n", err.c_str());
16+
}
17+
serializeJsonPretty(doc, Serial);
18+
setExtendSettings(doc);
19+
}
20+
}
21+
22+
void StackchanExConfig::setExtendSettings(DynamicJsonDocument doc) {
23+
JsonObject app_param1 = doc["app_parameters1"];
24+
_ex_parameters.item1 = doc["app_parameters1"]["item1"].as<String>(); // 文字列はこのように記述
25+
_ex_parameters.item2 = app_param1["item2"]; // 数値
26+
_ex_parameters.item3 = app_param1["item3"].as<bool>(); // True/False/0/1
27+
JsonObject app_param2 = doc["app_parameters2"];
28+
_item4 = app_param2["item4"].as<String>();
29+
JsonArray list_str = app_param2["list_str"];
30+
_list_str_count = list_str.size();
31+
for (int i=0; i<_list_str_count; i++) {
32+
_list_str[i] = list_str[i].as<String>();
33+
}
34+
JsonArray list_num = app_param2["list_num"];
35+
_list_num_count = list_num.size();
36+
for (int i=0; i<_list_num_count; i++) {
37+
_list_num[i] = list_num[i];
38+
}
39+
}
40+
41+
42+
void StackchanExConfig::printExtParameters(void) {
43+
M5_LOGI("item1:%s", _ex_parameters.item1.c_str());
44+
M5_LOGI("item2:%d", _ex_parameters.item2);
45+
M5_LOGI("item3:%s", _ex_parameters.item3 ? "true":"false");
46+
M5_LOGI("item4:%s", _item4.c_str());
47+
for (int i=0; i<_list_str_count; i++) {
48+
M5_LOGI("list_str[%d]: %s", i, _list_str[i].c_str());
49+
}
50+
for (int i=0; i<_list_num_count; i++) {
51+
M5_LOGI("list_num[%d]: %d", i, _list_num[i]);
52+
}
53+
}

‎examples/Basic/src/Stackchan_ex_config.h‎

Lines changed: 42 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,42 @@
1+
#ifndef __STACKCHAN_EX_CONFIG_H__
2+
#define __STACKCHAN_EX_CONFIG_H__
3+
4+
#include <Stackchan_system_config.h>
5+
6+
typedef struct ExConfig {
7+
String item1;
8+
int item2;
9+
bool item3;
10+
} ex_config_s;
11+
12+
13+
// StackchanSystemConfigを継承します。
14+
class StackchanExConfig : public StackchanSystemConfig
15+
{
16+
protected:
17+
ex_config_s _ex_parameters;
18+
String _item4;
19+
uint8_t _list_str_count;
20+
String _list_str[10];
21+
uint8_t _list_num_count;
22+
int _list_num[10];
23+
24+
public:
25+
StackchanExConfig();
26+
~StackchanExConfig();
27+
28+
void loadExtendConfig(fs::FS& fs, const char *yaml_filename, uint32_t yaml_size) override;
29+
void setExtendSettings(DynamicJsonDocument doc) override;
30+
void printExtParameters(void) override;
31+
ex_config_s getExConfig() { return _ex_parameters; }
32+
uint8_t getListStrCount() { return _list_str_count; }
33+
String getListStr(uint8_t no) { return _list_str[no]; }
34+
uint8_t getListNumCount() { return _list_num_count; }
35+
int getListNum(uint8_t no) { return _list_num[no]; }
36+
void setItem1(String item1) { _ex_parameters.item1 = item1; }
37+
void setItem2(int item2) { _ex_parameters.item2 = item2; }
38+
void setItem3(bool item3) { _ex_parameters.item3 = item3; }
39+
void setExConfig(ex_config_s config) { _ex_parameters = config; }
40+
};
41+
42+
#endif // __STACKCHAN_EX_CONFIG_H__

‎examples/Basic/src/main.cpp‎

Lines changed: 21 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,21 @@
1+
#include <Arduino.h>
2+
#include <M5Unified.h>
3+
#include <SD.h>
4+
#include "Stackchan_ex_config.h"
5+
6+
StackchanExConfig config;
7+
8+
void setup() {
9+
auto cfg = M5.config();
10+
M5.begin(cfg);
11+
M5.Log.setLogLevel(m5::log_target_display, ESP_LOG_INFO);
12+
M5.Log.setEnableColor(m5::log_target_serial, false);
13+
SD.begin(GPIO_NUM_4, SPI, 25000000);
14+
delay(2000);
15+
config.loadConfig(SD, "/yaml/SC_BasicConfig.yaml");
16+
17+
}
18+
19+
void loop() {
20+
// put your main code here, to run repeatedly:
21+
}

‎src/Stackchan_servo.cpp‎

Lines changed: 34 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -8,6 +8,19 @@ long convertSCS0009Pos(int16_t degree) {
88
return map(degree, 0, 300, 1023, 0);
99
}
1010

11+
// シリアルサーボ用のEasing関数
12+
float quadraticEaseInOut(float p) {
13+
//return p;
14+
if(p < 0.5)
15+
{
16+
return 2 * p * p;
17+
}
18+
else
19+
{
20+
return (-2 * p * p) + (4 * p) - 1;
21+
}
22+
}
23+
1124
StackchanSERVO::StackchanSERVO() {}
1225

1326
StackchanSERVO::~StackchanSERVO() {}
@@ -21,6 +34,7 @@ void StackchanSERVO::attachServos() {
2134
_sc.WritePos(AXIS_X + 1, convertSCS0009Pos(_init_param.servo[AXIS_X].start_degree + _init_param.servo[AXIS_X].offset), 1000);
2235
_sc.WritePos(AXIS_Y + 1, convertSCS0009Pos(_init_param.servo[AXIS_Y].start_degree + _init_param.servo[AXIS_Y].offset), 1000);
2336
vTaskDelay(1000/portTICK_PERIOD_MS);
37+
2438
} else {
2539
// SG90 PWM
2640
if (_servo_x.attach(_init_param.servo[AXIS_X].pin,
@@ -39,6 +53,8 @@ void StackchanSERVO::attachServos() {
3953
_servo_x.setEasingType(EASE_QUADRATIC_IN_OUT);
4054
_servo_y.setEasingType(EASE_QUADRATIC_IN_OUT);
4155
}
56+
_last_degree_x = _init_param.servo[AXIS_X].start_degree;
57+
_last_degree_y = _init_param.servo[AXIS_Y].start_degree;
4258
}
4359

4460
void StackchanSERVO::begin(stackchan_servo_initial_param_s init_param) {
@@ -75,6 +91,7 @@ void StackchanSERVO::moveX(int x, uint32_t millis_for_move) {
7591
synchronizeAllServosStartAndWaitForAllServosToStop();
7692
_isMoving = false;
7793
}
94+
_last_degree_x = x;
7895
}
7996

8097
void StackchanSERVO::moveX(servo_param_s servo_param_x) {
@@ -98,6 +115,7 @@ void StackchanSERVO::moveY(int y, uint32_t millis_for_move) {
98115
synchronizeAllServosStartAndWaitForAllServosToStop();
99116
_isMoving = false;
100117
}
118+
_last_degree_y = y;
101119
}
102120

103121
void StackchanSERVO::moveY(servo_param_s servo_param_y) {
@@ -106,10 +124,18 @@ void StackchanSERVO::moveY(servo_param_s servo_param_y) {
106124
}
107125
void StackchanSERVO::moveXY(int x, int y, uint32_t millis_for_move) {
108126
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);
127+
int increase_degree_x = x - _last_degree_x;
128+
int increase_degree_y = y - _last_degree_y;
129+
uint32_t division_time = millis_for_move / SERIAL_EASE_DIVISION;
111130
_isMoving = true;
112-
vTaskDelay(millis_for_move/portTICK_PERIOD_MS);
131+
//M5_LOGI("SCS: %d, %d, %d", increase_degree_x, increase_degree_y, division_time);
132+
for (float f=0.0f; f<1.0f; f=f+(1.0f/SERIAL_EASE_DIVISION)) {
133+
int x_pos = _last_degree_x + increase_degree_x * quadraticEaseInOut(f);
134+
int y_pos = _last_degree_y + increase_degree_y * quadraticEaseInOut(f);
135+
_sc.WritePos(AXIS_X + 1, convertSCS0009Pos(x_pos + _init_param.servo[AXIS_X].offset), division_time);
136+
_sc.WritePos(AXIS_Y + 1, convertSCS0009Pos(y_pos + _init_param.servo[AXIS_Y].offset), division_time);
137+
//vTaskDelay(division_time);
138+
}
113139
_isMoving = false;
114140
} else {
115141
_servo_x.setEaseToD(x + _init_param.servo[AXIS_X].offset, millis_for_move);
@@ -118,6 +144,9 @@ void StackchanSERVO::moveXY(int x, int y, uint32_t millis_for_move) {
118144
synchronizeAllServosStartAndWaitForAllServosToStop();
119145
_isMoving = false;
120146
}
147+
_last_degree_x = x;
148+
_last_degree_y = y;
149+
//M5_LOGI("SCS: %d, %d", _last_degree_x, _last_degree_y);
121150
}
122151

123152
void StackchanSERVO::moveXY(servo_param_s servo_param_x, servo_param_s servo_param_y) {
@@ -138,6 +167,8 @@ void StackchanSERVO::moveXY(servo_param_s servo_param_x, servo_param_s servo_par
138167
synchronizeAllServosStartAndWaitForAllServosToStop();
139168
_isMoving = false;
140169
}
170+
_last_degree_x = servo_param_x.degree;
171+
_last_degree_y = servo_param_y.degree;
141172
}
142173

143174
void StackchanSERVO::motion(Motion motion_number) {

‎src/Stackchan_servo.h‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -10,6 +10,8 @@
1010
#include <ESP32Servo.h>
1111
#include <ServoEasing.h>
1212
#include <SCServo.h>
13+
#include <M5Unified.h>
14+
#define SERIAL_EASE_DIVISION 5 // シリアルサーボのEasing分割数
1315

1416
enum Motion {
1517
nomove, // 動かない
@@ -54,6 +56,8 @@ class StackchanSERVO {
5456
void attachServos();
5557
stackchan_servo_initial_param_s _init_param;
5658
bool _isMoving;
59+
int _last_degree_x; // 前回のX軸の角度
60+
int _last_degree_y; // 前回のY軸の角度
5761
public:
5862
StackchanSERVO();
5963
~StackchanSERVO();

0 commit comments

Comments
 (0)