1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
|
#include "displayapp/screens/Motion.h"
#include <lvgl/lvgl.h>
#include "displayapp/DisplayApp.h"
// #if (portTICK_PERIOD_MS == 0)
#define APX_TICK_PERIOD_MS 1
// #else
// #define APX_TICK_PERIOD_MS portTICK_PERIOD_MS
// #endif
#if configTICK_RATE_HZ == 1024
#define FRAME_HZ 100
#define FRAME_MS 10
#define FRAME_TICKS 10
#define REDRAW_FRAME_HZ 40
#define REDRAW_FRAME_MS (1024 / REDRAW_FRAME_HZ)
#define REDRAW_FRAME_TICKS REDRAW_FRAME_MS
#else
#error "Unsupported configTICK_RATE_HZ"
#endif
#define G_SCALE 0.001
using namespace Pinetime::Applications::Screens;
Motion::Motion(Pinetime::Applications::DisplayApp* app, System::SystemTask& systemTask, Controllers::MotionController& motionController, Controllers::MotorController& motorController)
: Screen(app), motionController {motionController}, motorController {motorController}, systemTask {systemTask} {
{
lv_obj_t *box;
box = lv_obj_create(lv_scr_act(), nullptr);
lv_obj_set_size(box, 20, 200);
lv_obj_align(box, nullptr, LV_ALIGN_IN_LEFT_MID, 40, 0);
lv_obj_set_style_local_text_color(box, LV_LABEL_PART_MAIN, LV_STATE_DEFAULT, LV_COLOR_GRAY);
}
systemTask.PushMessage(Pinetime::System::Messages::DisableSleeping);
#ifndef JUMPSCORE_NO_CHART
chart = lv_chart_create(lv_scr_act(), NULL);
lv_obj_set_size(chart, 100, 100);
lv_obj_align(chart, NULL, LV_ALIGN_IN_BOTTOM_MID, 0, 0);
lv_chart_set_type(chart, LV_CHART_TYPE_LINE); /*Show lines and points too*/
// lv_chart_set_series_opa(chart, LV_OPA_70); /*Opacity of the data series*/
// lv_chart_set_series_width(chart, 4); /*Line width and point radious*/
lv_chart_set_range(chart, 0, 1000);
lv_chart_set_update_mode(chart, LV_CHART_UPDATE_MODE_SHIFT);
lv_chart_set_point_count(chart, 6);
/*Add 3 data series*/
ser1 = lv_chart_add_series(chart, LV_COLOR_RED);
lv_chart_init_points(chart, ser1, 0);
lv_chart_refresh(chart); /*Required after direct set*/
#endif
label = lv_label_create(lv_scr_act(), NULL);
lv_label_set_text_static(label, labelText);
// lv_label_set_text_fmt(label, "X #FF0000 %d# Y #008000 %d# Z #FFFF00 %d#", 0, 0, 0);
// lv_label_set_align(label, LV_LABEL_ALIGN_CENTER);
lv_obj_align(label, NULL, LV_ALIGN_IN_TOP_MID, 0, 0);
// lv_label_set_recolor(label, true);
lastLabel = lv_label_create(lv_scr_act(), NULL);
lv_obj_align(lastLabel, label, LV_ALIGN_OUT_BOTTOM_MID, 0, 0);
recordLabel = lv_label_create(lv_scr_act(), NULL);
lv_obj_align(recordLabel, lastLabel, LV_ALIGN_OUT_BOTTOM_MID, 0, 0);
#ifdef DEBUG_JUMPSCORE_APP
infoLabel = lv_label_create(lv_scr_act(), NULL);
lv_obj_align(infoLabel, recordLabel, LV_ALIGN_OUT_BOTTOM_MID, 0, 0);
#endif
taskRefresh = lv_task_create(RefreshTaskCallback, FRAME_MS, LV_TASK_PRIO_MID, this);
}
Motion::~Motion() {
lv_task_del(taskRefresh);
lv_obj_clean(lv_scr_act());
systemTask.PushMessage(Pinetime::System::Messages::EnableSleeping);
}
void Motion::Refresh() {
double X; double Y; double_t Z;
double G = systemTask.ReadGXYZ(X,Y,Z);
TickType_t current_time = xTaskGetTickCount();
if (started) {
TickType_t current_frame_ms = current_time - last_frame_time;
#ifdef DEBUG_JUMPSCORE_APP
frame_count++;
total_frames_ms += current_frame_ms;
if (current_frame_ms > max_frame_ms) {
max_frame_ms = current_frame_ms;
}
if (frame_count % 300 == 0) {
avg_frame_ms = total_frames_ms/300;
lv_label_set_text_fmt(infoLabel, "%dM %dA", max_frame_ms, avg_frame_ms);
total_frames_ms = 0;
max_frame_ms = 0;
}
#endif
double G_scaled = G * G_SCALE;
if (G_scaled < 1.000) {
if (!jumping) {
current_jump_speed = 0;
current_jump_length = 0;
jumping = true;
color = LV_COLOR_CYAN;
}
double jump_ratio = 1.0 - G_scaled;
double current_jump_accel = jump_ratio * 9.8;
double current_frame_s = current_frame_ms * 0.001;
current_jump_speed += current_jump_accel * current_frame_s;
current_jump_length += current_jump_speed * current_frame_s * jump_ratio;
} else {
if (jumping) {
if (current_jump_length * 100 > best_jump_length) {
last_jump_length = current_jump_length;
if (current_jump_length >= best_jump_length) {
records[4] = records[3];
records[3] = records[2];
records[2] = records[1];
records[1] = records[0];
records[0].jump_length = best_jump_length = current_jump_length;
new_record = 1;
}
}
jumping = false;
color = LV_COLOR_ORANGE;
}
}
}
last_frame_time = current_time;
if (started) {
if (((TickType_t)(current_time - last_redraw_frame_time)) < REDRAW_FRAME_TICKS) {
return;
}
} else {
started = true;
}
last_redraw_frame_time = current_time;
{
uint16_t G_uint16 = G < 0xffff ? G : 0xffff;
#ifndef JUMPSCORE_NO_CHART
lv_chart_set_next(chart, ser1, 1000 - ((int16_t)G_uint16));
#endif
labelText[4] = '0'+(G_uint16%10); G_uint16 /= 10;
labelText[3] = '0'+(G_uint16%10); G_uint16 /= 10;
labelText[2] = '0'+(G_uint16%10); G_uint16 /= 10;
labelText[1] = '0'+(G_uint16%10); G_uint16 /= 10;
labelText[0] = '0'+G_uint16;
}
lv_label_set_text_static(label, labelText);
if (new_record) {
uint32_t a = (best_jump_length*100000.0);
lv_label_set_text_fmt(recordLabel, "%d.%03d", a/1000, a%1000);
motorController.RunForDuration(35);
}
if (last_jump_length > 0) {
uint32_t a = (last_jump_length*100000.0);
lv_label_set_text_fmt(lastLabel, "%d.%03d", a/1000, a%1000);
if (!new_record && last_jump_length > 0.05) {
motorController.RunForDuration(11);
}
last_jump_length = 0;
}
new_record = false;
lv_obj_set_style_local_text_color(lastLabel, LV_LABEL_PART_MAIN, LV_STATE_DEFAULT, color);
#if 0
lv_label_set_text_fmt(label,
"X #FF0000 %d# Y #008000 %d# Z #FFFF00 %d#",
motionController.X() / 0x10,
motionController.Y() / 0x10,
motionController.Z() / 0x10);
lv_obj_align(label, NULL, LV_ALIGN_IN_TOP_MID, 0, 10);
#endif
}
|
