Skip to content

Commit eaae139

Browse files
ken-nakamuraken-nakamura
authored
Merge pull request #1 from ken-nakamura/main
Initial commit
2 parents 1cfbe61 + 8f9930b commit eaae139

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

45 files changed

+5185
-2
lines changed

‎LICENSE.BSD

Lines changed: 29 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,29 @@
1+
BSD 3-Clause License
2+
3+
Copyright (c) 2018, Japan Embedded Systems Technology Association(JASA)
4+
All rights reserved.
5+
6+
Redistribution and use in source and binary forms, with or without
7+
modification, are permitted provided that the following conditions are met:
8+
9+
* Redistributions of source code must retain the above copyright notice, this
10+
list of conditions and the following disclaimer.
11+
12+
* Redistributions in binary form must reproduce the above copyright notice,
13+
this list of conditions and the following disclaimer in the documentation
14+
and/or other materials provided with the distribution.
15+
16+
* Neither the name of the copyright holder nor the names of its
17+
contributors may be used to endorse or promote products derived from
18+
this software without specific prior written permission.
19+
20+
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
21+
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
22+
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
23+
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
24+
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
25+
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
26+
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
27+
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
28+
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
29+
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

‎LICENSE.MIT

Lines changed: 21 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,21 @@
1+
Copyright (c) 2017 M5Stack
2+
Copyright (c) 2021 JASA(Japan Embedded Systems Technology Association)
3+
4+
Permission is hereby granted, free of charge, to any person obtaining a copy
5+
of this software and associated documentation files (the "Software"), to deal
6+
in the Software without restriction, including without limitation the rights
7+
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
8+
copies of the Software, and to permit persons to whom the Software is
9+
furnished to do so, subject to the following conditions:
10+
11+
The above copyright notice and this permission notice shall be included in all
12+
copies or substantial portions of the Software.
13+
14+
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
15+
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
16+
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
17+
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
18+
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
19+
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
20+
SOFTWARE.
21+

‎README.md

Lines changed: 9 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,2 +1,9 @@
1-
# openel-arduino
2-
OpenEL for Arduino
1+
# OpenEL
2+
3+
OpenEL(Open Embedded Library) is a unified API(Application Programming Interface) for actuators and sensors. The specifications and implementation have been developed by [JASA(Japan Embedded Systems Technology Association)](https://www.jasa.or.jp/data/english/) since 2011.
4+
5+
As of November 7, 2021, version 3.2 is available.
6+
7+
[https://openel.github.io/](https://openel.github.io/)
8+
[https://github.com/openel/](https://github.com/openel/)
9+

‎examples/BALA2/BALA2.ino

Lines changed: 296 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,296 @@
1+
/*
2+
* MIT License
3+
*
4+
* Copyright (c) 2017 M5Stack
5+
* Copyright (c) 2021 JASA(Japan Embedded Systems Technology Association)
6+
*
7+
* Permission is hereby granted, free of charge, to any person obtaining a copy
8+
* of this software and associated documentation files (the "Software"), to deal
9+
* in the Software without restriction, including without limitation the rights
10+
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
11+
* copies of the Software, and to permit persons to whom the Software is
12+
* furnished to do so, subject to the following conditions:
13+
*
14+
* The above copyright notice and this permission notice shall be included in all
15+
* copies or substantial portions of the Software.
16+
*
17+
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
18+
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
19+
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
20+
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
21+
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
22+
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
23+
* SOFTWARE.
24+
*/
25+
26+
#define M5STACK_MPU6886
27+
28+
#include <BlynkSimpleEsp32_BLE.h>
29+
#include <BLEDevice.h>
30+
#include <BLEServer.h>
31+
32+
#include <M5Stack.h>
33+
#include <Preferences.h>
34+
#include <math.h>
35+
#include "freertos/FreeRTOS.h"
36+
#include "freertos/semphr.h"
37+
#include "calibration.h"
38+
#include "imu_filter.h"
39+
#include "MadgwickAHRS.h"
40+
#include "pid.h"
41+
#include "openEL.hpp"
42+
#include "Actuator.hpp"
43+
#include "Sensor.hpp"
44+
45+
#define BLYNK_PRINT Serial
46+
#define BLYNK_USE_DIRECT_CONNECT
47+
#define AUTH "My Blynk Auth Token" // Your Blynk Auth Token
48+
49+
Actuator *motor_l = 0;
50+
Actuator *motor_r = 0;
51+
Sensor *IMU = 0;
52+
Sensor *BMM = 0;
53+
Property imu_property;
54+
ReturnCode ret;
55+
56+
char auth[] = AUTH;
57+
float value;
58+
float value_list[2048];
59+
int32_t num;
60+
uint32_t tim;
61+
62+
extern uint8_t openel_img[8455];
63+
64+
static float angle_point = -1.5;
65+
66+
static int16_t pwm_offset = 0;
67+
static int16_t left_offset = 0;
68+
static int16_t right_offset = 0;
69+
70+
float kp = 24.0f, ki = 0.0f, kd = 90.0f;
71+
float s_kp = 15.0f, s_ki = 0.075f, s_kd = 0.0f;
72+
73+
PID pid(angle_point, kp, ki, kd);
74+
PID speed_pid(0, s_kp, s_ki, s_kd);
75+
76+
// the setup routine runs once when M5Stack starts up
77+
void setup() {
78+
M5.begin(true, false, true, true);
79+
Blynk.setDeviceName("Blynk_bala2");
80+
Blynk.begin(auth);
81+
82+
HALId halid1, halid2, halid3, halid4;
83+
halid1.deviceKindId = 1;
84+
halid1.vendorId = 9;
85+
halid1.productId = 1;
86+
halid1.instanceId = 1;
87+
88+
halid2.deviceKindId = 1;
89+
halid2.vendorId = 9;
90+
halid2.productId = 1;
91+
halid2.instanceId = 2;
92+
93+
halid3.deviceKindId = 2;
94+
halid3.vendorId = 9;
95+
halid3.productId = 2;
96+
halid3.instanceId = 1;
97+
98+
halid4.deviceKindId = 2;
99+
halid4.vendorId = 9;
100+
halid4.productId = 3;
101+
halid4.instanceId = 1;
102+
103+
motor_l = new Actuator(halid1);
104+
motor_r = new Actuator(halid2);
105+
IMU = new Sensor(halid3);
106+
BMM = new Sensor(halid4);
107+
108+
M5.Lcd.clear();
109+
110+
int16_t x_offset, y_offset, z_offset;
111+
float angle_center;
112+
calibrationInit();
113+
114+
calibrationGet(&x_offset, &y_offset, &z_offset, &angle_center);
115+
116+
angle_point = angle_center;
117+
pid.SetPoint(angle_point);
118+
119+
SemaphoreHandle_t i2c_mutex;
120+
i2c_mutex = xSemaphoreCreateMutex();
121+
122+
xSemaphoreTake(i2c_mutex, portMAX_DELAY);
123+
ret = motor_l->Init();
124+
ret = motor_r->Init();
125+
ret = IMU->Init();
126+
ret = BMM->Init();
127+
xSemaphoreGive(i2c_mutex);
128+
129+
ImuTaskStart(x_offset, y_offset, z_offset, &i2c_mutex);
130+
xTaskCreatePinnedToCore(PIDTask, "pid_task", 4 * 1024, &i2c_mutex, 4, NULL, 1);
131+
132+
M5.Lcd.drawJpg(openel_img, 8455);
133+
}
134+
135+
// the loop routine runs over and over again forever
136+
void loop() {
137+
static uint32_t next_show_time = 0;
138+
139+
Blynk.run();
140+
vTaskDelay(pdMS_TO_TICKS(10));
141+
142+
if(millis() > next_show_time) {
143+
draw_waveform();
144+
next_show_time = millis() + 10;
145+
}
146+
M5.update();
147+
}
148+
149+
// Blynk Virtual PIN Data
150+
BLYNK_WRITE(V0) {
151+
int x = param[0].asInt();
152+
int y = param[1].asInt();
153+
bool flag;
154+
155+
if(y > 0) {
156+
move(200);
157+
flag = true;
158+
} else if (y < 0) {
159+
move(-200);
160+
flag = true;
161+
} else {
162+
move(0);
163+
flag = false;
164+
}
165+
166+
if (x > 0) {
167+
if (flag == true) {
168+
turn(-100);
169+
} else {
170+
rotate(-200);
171+
}
172+
} else if (x < 0) {
173+
if (flag == true) {
174+
turn(100);
175+
} else {
176+
rotate(200);
177+
}
178+
} else {
179+
rotate(0);
180+
}
181+
}
182+
183+
static void PIDTask(void *arg) {
184+
float bala_angle;
185+
float motor_speed = 0;
186+
float motor_encoder_l;
187+
float motor_encoder_r;
188+
189+
int16_t pwm_speed;
190+
int16_t pwm_output_l;
191+
int16_t pwm_output_r;
192+
int16_t pwm_angle;
193+
194+
int32_t encoder = 0;
195+
int32_t last_encoder = 0;
196+
uint32_t last_ticks = 0;
197+
198+
SemaphoreHandle_t i2c_lock = *(SemaphoreHandle_t*)arg;
199+
200+
pid.SetOutputLimits(1023, -1023);
201+
pid.SetDirection(-1);
202+
203+
speed_pid.SetIntegralLimits(40, -40);
204+
speed_pid.SetOutputLimits(1023, -1023);
205+
speed_pid.SetDirection(1);
206+
207+
for(;;) {
208+
// in imu task update, update freq is 200HZ
209+
bala_angle = getAngle();
210+
211+
// Get motor encoder value
212+
xSemaphoreTake(i2c_lock, portMAX_DELAY);
213+
motor_l->GetValue(HAL_REQUEST_POSITION_CONTROL, &motor_encoder_l);
214+
motor_r->GetValue(HAL_REQUEST_POSITION_CONTROL, &motor_encoder_r);
215+
xSemaphoreGive(i2c_lock);
216+
217+
encoder = (int32_t)(motor_encoder_l + motor_encoder_r);
218+
// motor_speed filter
219+
motor_speed = 0.8 * motor_speed + 0.2 * (encoder - last_encoder);
220+
last_encoder = encoder;
221+
222+
if(fabs(bala_angle) < 70) {
223+
pwm_angle = (int16_t)pid.Update(bala_angle);
224+
pwm_speed = (int16_t)speed_pid.Update(motor_speed);
225+
pwm_output_l = pwm_speed + pwm_angle + pwm_offset + left_offset;
226+
if(pwm_output_l > 1023) { pwm_output_l = 1023; }
227+
if(pwm_output_l < -1023) { pwm_output_l = -1023; }
228+
pwm_output_r = pwm_speed + pwm_angle + pwm_offset + right_offset;
229+
if(pwm_output_r > 1023) { pwm_output_r = 1023; }
230+
if(pwm_output_r < -1023) { pwm_output_r = -1023; }
231+
xSemaphoreTake(i2c_lock, portMAX_DELAY);
232+
motor_l->SetValue(HAL_REQUEST_VELOCITY_CONTROL, pwm_output_l);
233+
motor_r->SetValue(HAL_REQUEST_VELOCITY_CONTROL, pwm_output_r);
234+
xSemaphoreGive(i2c_lock);
235+
} else {
236+
pwm_angle = 0;
237+
xSemaphoreTake(i2c_lock, portMAX_DELAY);
238+
motor_l->SetValue(HAL_REQUEST_POSITION_CONTROL, 0);
239+
motor_r->SetValue(HAL_REQUEST_POSITION_CONTROL, 0);
240+
xSemaphoreGive(i2c_lock);
241+
xSemaphoreTake(i2c_lock, portMAX_DELAY);
242+
motor_l->SetValue(HAL_REQUEST_VELOCITY_CONTROL, 0);
243+
motor_r->SetValue(HAL_REQUEST_VELOCITY_CONTROL, 0);
244+
xSemaphoreGive(i2c_lock);
245+
speed_pid.SetIntegral(0);
246+
}
247+
vTaskDelayUntil(&last_ticks, pdMS_TO_TICKS(10));
248+
}
249+
}
250+
251+
static void draw_waveform() {
252+
#define MAX_LEN 120
253+
#define X_OFFSET 100
254+
#define Y_OFFSET 95
255+
#define X_SCALE 3
256+
static int16_t val_buf[MAX_LEN] = {0};
257+
static int16_t pt = MAX_LEN - 1;
258+
val_buf[pt] = constrain((int16_t)(getAngle() * X_SCALE), -50, 50);
259+
260+
if (--pt < 0) {
261+
pt = MAX_LEN - 1;
262+
}
263+
264+
for (int i = 1; i < (MAX_LEN); i++) {
265+
uint16_t now_pt = (pt + i) % (MAX_LEN);
266+
M5.Lcd.drawLine(i + X_OFFSET, val_buf[(now_pt + 1) % MAX_LEN] + Y_OFFSET, i + 1 + X_OFFSET, val_buf[(now_pt + 2) % MAX_LEN] + Y_OFFSET, TFT_BLACK);
267+
if (i < MAX_LEN - 1) {
268+
M5.Lcd.drawLine(i + X_OFFSET, val_buf[now_pt] + Y_OFFSET, i + 1 + X_OFFSET, val_buf[(now_pt + 1) % MAX_LEN] + Y_OFFSET, TFT_GREEN);
269+
}
270+
}
271+
}
272+
273+
void move(int16_t speed) {
274+
pwm_offset = speed;
275+
}
276+
277+
void stop() {
278+
pwm_offset = 0;
279+
left_offset = 0;
280+
right_offset = 0;
281+
}
282+
283+
void turn(int16_t speed) {
284+
if (speed > 0) {
285+
left_offset = speed;
286+
right_offset = 0;
287+
} else if (speed < 0) {
288+
left_offset = 0;
289+
right_offset = -speed;
290+
}
291+
}
292+
293+
void rotate(int16_t speed) {
294+
left_offset = speed;
295+
right_offset = -speed;
296+
}

0 commit comments

Comments
 (0)