Skip to content

Commit 5b4a619

Browse files
GitshaoxiangGitshaoxiang
authored
Merge pull request #233 from Gitshaoxiang/update-example
update example
2 parents f106485 + f14f2cb commit 5b4a619

Some content is hidden

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

64 files changed

+11999
-0
lines changed

‎examples/Face/CALCULATOR/CALCULATOR.ino

Lines changed: 204 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,204 @@
1+
#include <M5Stack.h>
2+
3+
#define KEYBOARD_I2C_ADDR 0X08
4+
#define KEYBOARD_INT 5
5+
6+
float Num1 = 0;
7+
float Num2 = 0;
8+
float Num3 = 0;
9+
float Num4 = 0;
10+
char Fun;
11+
char Fun2;
12+
char Fun3;
13+
int CalNum = 0;
14+
int SumFlag = 0;
15+
float Sum;
16+
String Datain;
17+
uint8_t key_val;
18+
19+
20+
void setup()
21+
{
22+
M5.begin();
23+
Wire.begin();
24+
M5.Lcd.setTextFont(4);
25+
pinMode(KEYBOARD_INT, INPUT_PULLUP);
26+
CalNum = 1;
27+
Draw_Screen();
28+
}
29+
30+
31+
void loop()
32+
{
33+
if(digitalRead(KEYBOARD_INT) == LOW)
34+
{
35+
Wire.requestFrom(KEYBOARD_I2C_ADDR, 1); // request 1 byte from keyboard
36+
37+
while (Wire.available())
38+
{
39+
uint8_t key_val = Wire.read(); // receive a byte as character
40+
if(key_val != 0)
41+
{
42+
if(isDigit(key_val) || key_val == '.')
43+
{
44+
if( SumFlag == 1 )
45+
{
46+
StoreData();
47+
}
48+
Datain += (char)key_val;
49+
IsNumber();
50+
}
51+
52+
if( key_val == 'A' || key_val == '+' || key_val == '-' || key_val == '*' || key_val == '/' || key_val == '=' )
53+
{
54+
FunKey(key_val);
55+
}
56+
}
57+
}
58+
}
59+
}
60+
61+
62+
void FunKey( uint8_t key_in )
63+
{
64+
switch (key_in)
65+
{
66+
case 'A':
67+
M5.Lcd.clear();
68+
Draw_Screen();
69+
Num1 = 0;
70+
Num2 = 0;
71+
Sum = 0;
72+
Datain = " ";
73+
CalNum = 1;
74+
break;
75+
76+
case '=':
77+
if( Fun == '+' )
78+
{
79+
Sum = Num1 + Num2;
80+
}
81+
82+
if( Fun == '-' )
83+
{
84+
Sum = Num1 - Num2;
85+
}
86+
87+
if( Fun == '*' )
88+
{
89+
Sum = Num1 * Num2;
90+
}
91+
92+
if( Fun == '/' )
93+
{
94+
Sum = Num1 / Num2;
95+
}
96+
97+
M5.Lcd.setCursor(105,190);
98+
M5.Lcd.print(Sum);
99+
SumFlag = 1;
100+
break;
101+
102+
case '+':
103+
Fun = '+';
104+
CalNum += 1 ;
105+
Datain = " ";
106+
M5.Lcd.setCursor(20, 140);
107+
M5.Lcd.print(" ");
108+
M5.Lcd.setCursor(20, 140);
109+
M5.Lcd.print(Fun);
110+
break;
111+
112+
case '-':
113+
Fun = '-';
114+
CalNum += 1;
115+
Datain = " ";
116+
M5.Lcd.setCursor(20, 140);
117+
M5.Lcd.print(" ");
118+
M5.Lcd.setCursor(20, 140);
119+
M5.Lcd.print(Fun);
120+
break;
121+
122+
case '*':
123+
Fun = '*';
124+
CalNum += 1;
125+
Datain = " ";
126+
M5.Lcd.setCursor(20, 140);
127+
M5.Lcd.print(" ");
128+
M5.Lcd.setCursor(20, 140);
129+
M5.Lcd.print(Fun);
130+
break;
131+
132+
case '/':
133+
Fun = '/';
134+
CalNum += 1;
135+
Datain = " ";
136+
M5.Lcd.setCursor(20, 140);
137+
M5.Lcd.print(" ");
138+
M5.Lcd.setCursor(20, 140);
139+
M5.Lcd.print(Fun);
140+
break;
141+
142+
default:
143+
break;
144+
}
145+
}
146+
147+
148+
void StoreData( void )
149+
{
150+
Num1 = Sum;
151+
Num2 = 0;
152+
SumFlag = 0;
153+
Datain = " ";
154+
M5.Lcd.setCursor(20, 140);
155+
M5.Lcd.print(Fun);
156+
M5.Lcd.setCursor(70, 100);
157+
M5.Lcd.print(" ");
158+
M5.Lcd.setCursor(70, 140);
159+
M5.Lcd.print(" ");
160+
M5.Lcd.setCursor(70, 100);
161+
M5.Lcd.print(Num1);
162+
M5.Lcd.setCursor(105,190);
163+
M5.Lcd.print(" ");
164+
}
165+
166+
167+
void IsNumber( void )
168+
{
169+
if( CalNum == 1 )
170+
{
171+
Num1 = Datain.toFloat();
172+
M5.Lcd.setCursor(70,100);
173+
M5.Lcd.print(Num1);
174+
}
175+
176+
if( CalNum >= 2 )
177+
{
178+
Num2 = Datain.toFloat();
179+
M5.Lcd.setCursor(70, 140);
180+
M5.Lcd.print(Num2);
181+
}
182+
}
183+
184+
185+
void DrawBox( int LocX, int LocY, int W, int H )
186+
{
187+
M5.Lcd.drawLine( LocX, LocY, LocX + W, LocY, TFT_WHITE);
188+
M5.Lcd.drawLine( LocX, LocY + H, LocX + W, LocY + H, TFT_WHITE);
189+
M5.Lcd.drawLine( LocX, LocY, LocX, LocY + H, TFT_WHITE);
190+
M5.Lcd.drawLine( LocX + W, LocY, LocX + W, LocY + H, TFT_WHITE);
191+
}
192+
193+
void Draw_Screen( void )
194+
{
195+
M5.Lcd.clear();
196+
M5.Lcd.drawLine( 1, 180, 320, 180, TFT_WHITE);
197+
M5.Lcd.setCursor( 15,20 );
198+
M5.Lcd.print( "FRACTION CALCULATOR" );
199+
DrawBox( 50, 95, 250, 32 );
200+
DrawBox( 10, 135, 30, 32 );
201+
DrawBox( 50, 135, 250, 32 );
202+
M5.Lcd.setCursor(10,190);
203+
M5.Lcd.print("SUM =");
204+
}

‎examples/Modules/BALA2/BALA2.ino

Lines changed: 169 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,169 @@
1+
/*
2+
Description: BALA2 stand and run in balance.
3+
Note: click the device power button and Long press button B will setup calibration mode.
4+
at calibration mode Press the A/C key to increase or decrease the correction value.
5+
When it is adjusted to a satisfactory value, press the B key to save the parameter.
6+
*/
7+
8+
#define M5STACK_MPU6886
9+
10+
#include <M5Stack.h>
11+
#include "freertos/FreeRTOS.h"
12+
#include "imu_filter.h"
13+
#include "MadgwickAHRS.h"
14+
#include "bala.h"
15+
#include "pid.h"
16+
#include "calibration.h"
17+
18+
extern uint8_t bala_img[41056];
19+
static void PIDTask(void *arg);
20+
static void draw_waveform();
21+
22+
static float angle_point = -1.5;
23+
24+
float kp = 24.0f, ki = 0.0f, kd = 90.0f;
25+
float s_kp = 15.0f, s_ki = 0.075f, s_kd = 0.0f;
26+
27+
bool calibration_mode = false;
28+
29+
Bala bala;
30+
31+
PID pid(angle_point, kp, ki, kd);
32+
PID speed_pid(0, s_kp, s_ki, s_kd);
33+
34+
// the setup routine runs once when M5Stack starts up
35+
void setup(){
36+
// Initialize the M5Stack object
37+
38+
M5.begin(true, false, false, false);
39+
40+
Serial.begin(115200);
41+
M5.IMU.Init();
42+
43+
int16_t x_offset, y_offset, z_offset;
44+
float angle_center;
45+
calibrationInit();
46+
47+
if (M5.BtnB.isPressed()) {
48+
calibrationGryo();
49+
calibration_mode = true;
50+
}
51+
52+
calibrationGet(&x_offset, &y_offset, &z_offset, &angle_center);
53+
Serial.printf("x: %d, y: %d, z:%d, angle: %.2f", x_offset, y_offset, z_offset, angle_center);
54+
55+
angle_point = angle_center;
56+
pid.SetPoint(angle_point);
57+
58+
SemaphoreHandle_t i2c_mutex;;
59+
i2c_mutex = xSemaphoreCreateMutex();
60+
bala.SetMutex(&i2c_mutex);
61+
ImuTaskStart(x_offset, y_offset, z_offset, &i2c_mutex);
62+
xTaskCreatePinnedToCore(PIDTask, "pid_task", 4 * 1024, NULL, 4, NULL, 1);
63+
64+
M5.Lcd.drawJpg(bala_img, 41056);
65+
if (calibration_mode) {
66+
M5.Lcd.setCursor(0, 0);
67+
M5.Lcd.printf("calibration mode");
68+
}
69+
}
70+
71+
// the loop routine runs over and over again forever
72+
void loop() {
73+
static uint32_t next_show_time = 0;
74+
vTaskDelay(pdMS_TO_TICKS(5));
75+
76+
if(millis() > next_show_time) {
77+
draw_waveform();
78+
next_show_time = millis() + 10;
79+
}
80+
81+
M5.update();
82+
if (M5.BtnA.wasPressed()) {
83+
angle_point += 0.25;
84+
pid.SetPoint(angle_point);
85+
}
86+
87+
if (M5.BtnB.wasPressed()) {
88+
if (calibration_mode) {
89+
calibrationSaveCenterAngle(angle_point);
90+
}
91+
92+
}
93+
94+
if (M5.BtnC.wasPressed()) {
95+
angle_point -= 0.25;
96+
pid.SetPoint(angle_point);
97+
}
98+
}
99+
100+
static void PIDTask(void *arg) {
101+
float bala_angle;
102+
float motor_speed = 0;
103+
104+
int16_t pwm_speed;
105+
int16_t pwm_output;
106+
int16_t pwm_angle;
107+
108+
int32_t encoder = 0;
109+
int32_t last_encoder = 0;
110+
uint32_t last_ticks = 0;
111+
112+
pid.SetOutputLimits(1023, -1023);
113+
pid.SetDirection(-1);
114+
115+
speed_pid.SetIntegralLimits(40, -40);
116+
speed_pid.SetOutputLimits(1023, -1023);
117+
speed_pid.SetDirection(1);
118+
119+
for(;;) {
120+
vTaskDelayUntil(&last_ticks, pdMS_TO_TICKS(5));
121+
122+
// in imu task update, update freq is 200HZ
123+
bala_angle = getAngle();
124+
125+
// Get motor encoder value
126+
bala.UpdateEncoder();
127+
128+
encoder = bala.wheel_left_encoder + bala.wheel_right_encoder;
129+
// motor_speed filter
130+
motor_speed = 0.8 * motor_speed + 0.2 * (encoder - last_encoder);
131+
last_encoder = encoder;
132+
133+
if(fabs(bala_angle) < 70) {
134+
pwm_angle = (int16_t)pid.Update(bala_angle);
135+
pwm_speed = (int16_t)speed_pid.Update(motor_speed);
136+
pwm_output = pwm_speed + pwm_angle;
137+
if(pwm_output > 1023) { pwm_output = 1023; }
138+
if(pwm_output < -1023) { pwm_output = -1023; }
139+
bala.SetSpeed(pwm_output, pwm_output);
140+
} else {
141+
pwm_angle = 0;
142+
bala.SetSpeed(0, 0);
143+
bala.SetEncoder(0, 0);
144+
speed_pid.SetIntegral(0);
145+
}
146+
}
147+
}
148+
149+
static void draw_waveform() {
150+
#define MAX_LEN 120
151+
#define X_OFFSET 100
152+
#define Y_OFFSET 95
153+
#define X_SCALE 3
154+
static int16_t val_buf[MAX_LEN] = {0};
155+
static int16_t pt = MAX_LEN - 1;
156+
val_buf[pt] = constrain((int16_t)(getAngle() * X_SCALE), -50, 50);
157+
158+
if (--pt < 0) {
159+
pt = MAX_LEN - 1;
160+
}
161+
162+
for (int i = 1; i < (MAX_LEN); i++) {
163+
uint16_t now_pt = (pt + i) % (MAX_LEN);
164+
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);
165+
if (i < MAX_LEN - 1) {
166+
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);
167+
}
168+
}
169+
}

0 commit comments

Comments
 (0)