amiro-os / devices / PowerManagement / userthread.cpp @ 2f3e64c4
History | View | Annotate | Download (4.788 KB)
1 | 58fe0e0b | Thomas Schöpping | #include "userthread.h" |
---|---|---|---|
2 | |||
3 | #include "global.hpp" |
||
4 | #include <array> |
||
5 | 2f3e64c4 | Thomas Schöpping | #include <chprintf.h> |
6 | 58fe0e0b | Thomas Schöpping | |
7 | using namespace amiro; |
||
8 | |||
9 | extern Global global;
|
||
10 | |||
11 | uint16_t touch; |
||
12 | std::array<uint16_t, 8> proximity;
|
||
13 | std::array<float, 8> proxNormalized; |
||
14 | |||
15 | bool running;
|
||
16 | |||
17 | uint16_t constexpr proxThresholdLow = 0x0000;
|
||
18 | b4885314 | Thomas Schöpping | uint16_t constexpr proxThresholdHigh = 0x1000;
|
19 | 58fe0e0b | Thomas Schöpping | uint16_t constexpr proxRange = proxThresholdHigh - proxThresholdLow; |
20 | |||
21 | std::array< std::array<float, 2>, 8> constexpr namMatrix = { |
||
22 | /* x w_z */
|
||
23 | std::array<float, 2>/* SSW */{ 0.00f, 0.00f}, |
||
24 | std::array<float, 2>/* WSW */{ 0.25f, -0.25f}, |
||
25 | std::array<float, 2>/* WNW */{-0.75f, -0.50f}, |
||
26 | std::array<float, 2>/* NNW */{-0.75f, -1.00f}, |
||
27 | std::array<float, 2>/* NNE */{-0.75f, 1.00f}, |
||
28 | std::array<float, 2>/* ENE */{-0.75f, 0.50f}, |
||
29 | std::array<float, 2>/* ESE */{ 0.25f, 0.25f}, |
||
30 | std::array<float, 2>/* SSE */{ 0.00f, 0.00f} |
||
31 | }; |
||
32 | b4885314 | Thomas Schöpping | uint32_t constexpr baseTranslation = 100e3; // 2cm/s |
33 | 58fe0e0b | Thomas Schöpping | uint32_t constexpr baseRotation = 1e6; // 1rad/s |
34 | types::kinematic constexpr defaultKinematic = { |
||
35 | /* x [µm/s] */ baseTranslation,
|
||
36 | /* y [µm/s] */ 0, |
||
37 | /* z [µm/s] */ 0, |
||
38 | /* w_x [µrad/s] */ 0, |
||
39 | /* w_y [µrad/s] */ 0, |
||
40 | /* w_z [µrad/s] */ 0 |
||
41 | }; |
||
42 | |||
43 | |||
44 | |||
45 | inline uint8_t ProxId2LedId(const uint8_t proxId) { |
||
46 | return (proxId < 4) ? proxId+4 : proxId-4; |
||
47 | } |
||
48 | |||
49 | Color Prox2Color(const float prox) { |
||
50 | float p = 0.0f; |
||
51 | if (prox < 0.5f) { |
||
52 | p = 2.0f * prox; |
||
53 | return Color(0x00, p*0xFF, (1.0f-p)*0xFF); |
||
54 | } else {
|
||
55 | p = 2.0f * (prox - 0.5f); |
||
56 | return Color(p*0xFF, (1.0f-p)*0xFF, 0x00); |
||
57 | } |
||
58 | } |
||
59 | |||
60 | UserThread::UserThread() : |
||
61 | chibios_rt::BaseStaticThread<USER_THREAD_STACK_SIZE>() |
||
62 | { |
||
63 | } |
||
64 | |||
65 | UserThread::~UserThread() |
||
66 | { |
||
67 | } |
||
68 | |||
69 | msg_t |
||
70 | UserThread::main() |
||
71 | { |
||
72 | uint8_t sensor = 0;
|
||
73 | float factor_x = 0.0f; |
||
74 | float factor_wz = 0.0f; |
||
75 | types::kinematic kinematic = defaultKinematic; |
||
76 | |||
77 | for (uint8_t led = 0; led < 8; ++led) { |
||
78 | global.robot.setLightColor(led, Color(Color::BLACK)); |
||
79 | } |
||
80 | running = false;
|
||
81 | |||
82 | while (!this->shouldTerminate()) |
||
83 | { |
||
84 | /*
|
||
85 | * read touch sensor values
|
||
86 | */
|
||
87 | touch = global.mpr121.getButtonStatus(); |
||
88 | |||
89 | /*
|
||
90 | * evaluate touch input
|
||
91 | */
|
||
92 | if (touch == 0x0F) { |
||
93 | if (running) {
|
||
94 | // stop the robot
|
||
95 | running = false;
|
||
96 | kinematic = {0, 0, 0, 0, 0, 0}; |
||
97 | global.robot.setTargetSpeed(kinematic); |
||
98 | } else {
|
||
99 | // start the robot
|
||
100 | running = true;
|
||
101 | } |
||
102 | |||
103 | // set all LEDs to white for one second
|
||
104 | for (uint8_t led = 0; led < 8; ++led) { |
||
105 | global.robot.setLightColor(led, Color(Color::WHITE)); |
||
106 | } |
||
107 | this->sleep(MS2ST(1000)); |
||
108 | for (uint8_t led = 0; led < 8; ++led) { |
||
109 | global.robot.setLightColor(led, Color(Color::BLACK)); |
||
110 | } |
||
111 | } |
||
112 | |||
113 | if (running) {
|
||
114 | /*
|
||
115 | * read proximity values
|
||
116 | */
|
||
117 | for (sensor = 0; sensor < 8; ++sensor) { |
||
118 | proximity[sensor] = global.vcnl4020[sensor].getProximityScaledWoOffset(); |
||
119 | //proxNormalized[sensor] += 2.0f * (proxNormalized[sensor] * (1.0f - proxNormalized[sensor])); // non linearity
|
||
120 | } |
||
121 | |||
122 | /*
|
||
123 | * normalize proximity values
|
||
124 | */
|
||
125 | for (sensor = 0; sensor < 8; ++sensor) { |
||
126 | register uint16_t prox = proximity[sensor];
|
||
127 | // limit to high treshold
|
||
128 | if (prox > proxThresholdHigh)
|
||
129 | prox = proxThresholdHigh; |
||
130 | // limit to low threshold
|
||
131 | else if (prox < proxThresholdLow) |
||
132 | prox = proxThresholdLow; |
||
133 | // apply low threshold
|
||
134 | prox -= proxThresholdLow; |
||
135 | // normalize to [0, 1]
|
||
136 | proxNormalized[sensor] = float(prox) / float(proxRange); |
||
137 | } |
||
138 | |||
139 | /*
|
||
140 | * map the sensor values to the top LEDs
|
||
141 | */
|
||
142 | for (sensor = 0; sensor < 8; ++sensor) { |
||
143 | global.robot.setLightColor(ProxId2LedId(sensor), Prox2Color(proxNormalized[sensor])); |
||
144 | } |
||
145 | |||
146 | /*
|
||
147 | * evaluate NAM
|
||
148 | */
|
||
149 | factor_x = 0.0f; |
||
150 | factor_wz = 0.0f; |
||
151 | for (sensor = 0; sensor < 8; ++sensor) { |
||
152 | factor_x += proxNormalized[sensor] * namMatrix[sensor][0];
|
||
153 | factor_wz += proxNormalized[sensor] * namMatrix[sensor][1];
|
||
154 | } |
||
155 | |||
156 | /*
|
||
157 | * set motor commands
|
||
158 | */
|
||
159 | kinematic = defaultKinematic; |
||
160 | kinematic.x += (factor_x * baseTranslation) + 0.5f; |
||
161 | kinematic.w_z += (factor_wz * baseRotation) + 0.5f; |
||
162 | global.robot.setTargetSpeed(kinematic); |
||
163 | } |
||
164 | |||
165 | b4885314 | Thomas Schöpping | this->sleep(MS2ST(10)); |
166 | 58fe0e0b | Thomas Schöpping | } |
167 | |||
168 | return RDY_OK;
|
||
169 | } |