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
|
#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
#define FRAME_HZ 100
#define FRAME_MS (1000 / FRAME_HZ)
#define FRAME_TICKS (FRAME_MS / APX_TICK_PERIOD_MS)
#define REDRAW_FRAME_HZ 40
#define REDRAW_FRAME_MS (1000 / REDRAW_FRAME_HZ)
#define REDRAW_FRAME_TICKS (REDRAW_FRAME_MS / APX_TICK_PERIOD_MS)
#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} {
systemTask.PushMessage(Pinetime::System::Messages::DisableSleeping);
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);
// ser2 = lv_chart_add_series(chart, LV_COLOR_GREEN);
// ser3 = lv_chart_add_series(chart, LV_COLOR_YELLOW);
lv_chart_init_points(chart, ser1, 0);
// lv_chart_init_points(chart, ser2, 0);
// lv_chart_init_points(chart, ser3, 0);
lv_chart_refresh(chart); /*Required after direct set*/
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);
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() {
bool new_record = false;
bool jump_started_or_ended = false;
double X; double Y; double_t Z;
double G = motionController.GXYZ(X,Y,Z);
// lv_chart_set_next(chart, ser2, motionController.Y());
// lv_chart_set_next(chart, ser3, motionController.Z());
TickType_t current_time = xTaskGetTickCount();
if (started) {
TickType_t current_frame_ms = current_time - last_frame_time;
double G_scaled = G * G_SCALE;
if (G_scaled < 1.000) {
if (!jumping) {
jump_started_or_ended = true;
current_jump_speed = 0;
current_jump_length = 0;
jumping = true;
}
double jump_ratio = 1.0 - G_scaled;
double jump_ratio *= jump_ratio;
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;
} else {
if (jumping) {
double best_jump_length = records[0].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 = current_jump_length;
new_record = 1;
}
jumping = false;
jump_started_or_ended = true;
}
}
}
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;
lv_chart_set_next(chart, ser1, 1000 - ((int16_t)G_uint16));
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 (jump_started_or_ended) {
if (new_record) {
uint32_t a = (current_jump_length*100000.0);
lv_label_set_text_fmt(recordLabel, "%d.%03d", a/1000, a%1000);
motorController.RunForDuration(35);
}
lv_color_t color;
if (jumping) {
color = LV_COLOR_CYAN;
} else {
double best_jump_length = records[0].jump_length;
if (current_jump_length * 100 > best_jump_length) {
uint32_t a = (current_jump_length*100000.0);
lv_label_set_text_fmt(lastLabel, "%d.%03d", a/1000, a%1000);
if (!new_record && current_jump_length > 0.05) {
motorController.RunForDuration(11);
}
}
color = LV_COLOR_ORANGE;
}
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
}
|
