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