Revision 61b0791a
| components/bluetooth/bluetooth-connector.cpp | ||
|---|---|---|
| 63 | 63 |
break; |
| 64 | 64 |
} |
| 65 | 65 |
iwrap->bluetoothIwrapSendCommand(cmd); |
| 66 |
BaseThread::sleep(MS2ST(5000)); // waiting connection establish
|
|
| 66 |
BaseThread::sleep(MS2ST(1000)); // waiting connection establish
|
|
| 67 | 67 |
break; |
| 68 | 68 |
} |
| 69 | 69 |
} |
| components/bluetooth/bluetooth-wiimote.cpp | ||
|---|---|---|
| 3 | 3 |
|
| 4 | 4 |
#include <amiro/bluetooth/bluetooth-wiimote.hpp> |
| 5 | 5 |
|
| 6 |
#include <global.hpp> |
|
| 7 |
|
|
| 6 | 8 |
using namespace chibios_rt; |
| 7 | 9 |
using namespace amiro; |
| 8 | 10 |
|
| 11 |
extern Global global; |
|
| 12 |
|
|
| 9 | 13 |
/* |
| 10 | 14 |
* Class constructor |
| 11 | 15 |
*/ |
| ... | ... | |
| 39 | 43 |
size_t length; |
| 40 | 44 |
msg_t msg; |
| 41 | 45 |
|
| 42 |
static uint8_t button_up; |
|
| 43 |
static uint8_t button_down; |
|
| 44 |
static uint8_t button_right; |
|
| 45 |
static uint8_t button_left; |
|
| 46 |
static uint8_t button_plus; |
|
| 47 |
static uint8_t button_home; |
|
| 48 |
static uint8_t button_minus; |
|
| 49 |
static uint8_t button_A; |
|
| 50 |
static uint8_t button_B; |
|
| 51 |
static uint8_t button_1; |
|
| 52 |
static uint8_t button_2; |
|
| 53 |
|
|
| 54 | 46 |
msg = mailbox.fetch((msg_t*) &recv_descriptor, TIME_INFINITE); |
| 55 | 47 |
if ((msg == RDY_RESET) || stopflag) |
| 56 | 48 |
return RDY_RESET; |
| ... | ... | |
| 58 | 50 |
buffer = recv_descriptor->bluetoothDescriptorGetPayload(); |
| 59 | 51 |
length = recv_descriptor->bluetoothDescriptorGetPayloadLength(); |
| 60 | 52 |
|
| 61 |
if (buffer[0] == 0xA1 && buffer[1] == 0x31) {
|
|
| 62 |
accelerometer.x_axis = (buffer[4] << 2) + ((buffer[2] & 0x60) >> 5) - 0x1EC; |
|
| 63 |
accelerometer.y_axis = (buffer[5] << 2) + ((buffer[3] & 0x20) >> 4) - 0x1EA; |
|
| 64 |
accelerometer.z_axis = (buffer[6] << 2) + ((buffer[3] & 0x40) >> 5) - 0x1EE; |
|
| 65 |
|
|
| 66 |
if (buffer[3] & 0x80) { // Press home to return button reporting
|
|
| 67 |
bluetoothWiimoteDataBtn(); |
|
| 68 |
accelerometer.x_axis = 0; |
|
| 69 |
accelerometer.y_axis = 0; |
|
| 70 |
accelerometer.z_axis = 0; |
|
| 71 |
} |
|
| 72 |
|
|
| 73 |
} else if (buffer[0] == 0xA1 && buffer[1] == 0x30) {
|
|
| 74 |
button_up = (buffer[2] & 0x08) >> 3; |
|
| 75 |
button_down = (buffer[2] & 0x04) >> 2; |
|
| 76 |
button_right = (buffer[2] & 0x02) >> 1; |
|
| 77 |
button_left = (buffer[2] & 0x01) >> 0; |
|
| 78 |
button_plus = (buffer[2] & 0x10) >> 4; |
|
| 79 |
button_home = (buffer[3] & 0x80) >> 7; |
|
| 80 |
button_minus = (buffer[3] & 0x10) >> 4; |
|
| 81 |
button_A = (buffer[3] & 0x08) >> 3; |
|
| 82 |
button_B = (buffer[3] & 0x04) >> 2; |
|
| 83 |
button_1 = (buffer[3] & 0x02) >> 1; |
|
| 84 |
button_2 = (buffer[3] & 0x01) >> 0; |
|
| 85 |
|
|
| 86 |
if (button_up) |
|
| 87 |
chSequentialStreamPut((BaseSequentialStream*) &SD1, 'U'); |
|
| 88 |
|
|
| 89 |
if (button_down) |
|
| 90 |
chSequentialStreamPut((BaseSequentialStream*) &SD1, 'D'); |
|
| 91 |
|
|
| 92 |
if (button_right) |
|
| 93 |
chSequentialStreamPut((BaseSequentialStream*) &SD1, 'R'); |
|
| 94 |
|
|
| 95 |
if (button_left) |
|
| 96 |
chSequentialStreamPut((BaseSequentialStream*) &SD1, 'L'); |
|
| 53 |
if (buffer[0] == 0xA1 && (buffer[1] == 0x30 || buffer[1] == 0x31)) {
|
|
| 54 |
buttons.left = (buffer[2] & 0x01) ? 1 : 0; |
|
| 55 |
buttons.right = (buffer[2] & 0x02) ? 1 : 0; |
|
| 56 |
buttons.down = (buffer[2] & 0x04) ? 1 : 0; |
|
| 57 |
buttons.up = (buffer[2] & 0x08) ? 1 : 0; |
|
| 58 |
buttons.plus = (buffer[2] & 0x10) ? 1 : 0; |
|
| 59 |
buttons.two = (buffer[3] & 0x01) ? 1 : 0; |
|
| 60 |
buttons.one = (buffer[3] & 0x02) ? 1 : 0; |
|
| 61 |
buttons.B = (buffer[3] & 0x04) ? 1 : 0; |
|
| 62 |
buttons.A = (buffer[3] & 0x08) ? 1 : 0; |
|
| 63 |
buttons.minus = (buffer[3] & 0x10) ? 1 : 0; |
|
| 64 |
buttons.home = (buffer[3] & 0x80) ? 1 : 0; |
|
| 65 |
|
|
| 66 |
accelerometer.x_axis = (buffer[4] << 2) + ((buffer[2] & 0x60) >> 5) - 0x1FF; |
|
| 67 |
accelerometer.y_axis = (buffer[5] << 2) + ((buffer[3] & 0x20) >> 4) - 0x1FF; |
|
| 68 |
accelerometer.z_axis = (buffer[6] << 2) + ((buffer[3] & 0x40) >> 5) - 0x1FF; |
|
| 97 | 69 |
|
| 98 |
if (button_plus) |
|
| 99 |
chSequentialStreamPut((BaseSequentialStream*) &SD1, '+'); |
|
| 100 |
|
|
| 101 |
if (button_home) |
|
| 102 |
chSequentialStreamPut((BaseSequentialStream*) &SD1, 'H'); |
|
| 103 |
|
|
| 104 |
if (button_minus) |
|
| 105 |
chSequentialStreamPut((BaseSequentialStream*) &SD1, '-'); |
|
| 106 |
|
|
| 107 |
if (button_A) |
|
| 108 |
chSequentialStreamPut((BaseSequentialStream*) &SD1, 'A'); |
|
| 109 |
|
|
| 110 |
if (button_B) |
|
| 111 |
chSequentialStreamPut((BaseSequentialStream*) &SD1, 'B'); |
|
| 112 |
|
|
| 113 |
if (button_1) |
|
| 114 |
chSequentialStreamPut((BaseSequentialStream*) &SD1, '1'); |
|
| 115 |
|
|
| 116 |
if (button_2) |
|
| 117 |
chSequentialStreamPut((BaseSequentialStream*) &SD1, '2'); |
|
| 118 |
|
|
| 119 |
if (button_minus && button_plus) // Press minus and plue to return accelerometer reporting |
|
| 120 | 70 |
bluetoothWiimoteDataBtnAcc(); |
| 121 | 71 |
} else {
|
| 122 |
chSequentialStreamWrite((BaseSequentialStream*) &SD1, buffer, length);
|
|
| 72 |
chSequentialStreamWrite((BaseSequentialStream*) &global.sercanmux1, buffer, length);
|
|
| 123 | 73 |
} |
| 124 | 74 |
|
| 125 | 75 |
msg = iwrap->transport.bluetoothTransportGetStorageMailbox()->post((msg_t) recv_descriptor, TIME_INFINITE); |
| ... | ... | |
| 179 | 129 |
return &accelerometer; |
| 180 | 130 |
} |
| 181 | 131 |
|
| 132 |
BluetoothWiimote::Buttons * BluetoothWiimote::getButtons() {
|
|
| 133 |
return &buttons; |
|
| 134 |
} |
|
| 135 |
|
|
| 182 | 136 |
/* |
| 183 | 137 |
* @brief : On-off LEDs and Motor of Wiimote. |
| 184 | 138 |
* |
| devices/PowerManagement/main.cpp | ||
|---|---|---|
| 858 | 858 |
return; |
| 859 | 859 |
} |
| 860 | 860 |
|
| 861 |
void shellRequestWiiSteering(BaseSequentialStream* chp, int argc, char *argv[]) {
|
|
| 862 |
// if Wii steering is currently active, stop it |
|
| 863 |
if (global.userThread.getCurrenState() == UserThread::WII_STEERING) {
|
|
| 864 |
global.userThread.setNextState(UserThread::IDLE); |
|
| 865 |
} |
|
| 866 |
// check arguments and (try to) enable Wii steering |
|
| 867 |
else {
|
|
| 868 |
// if arguments invalid |
|
| 869 |
if (argc < 1 || argc > 2 || global.userThread.setWiiAddress(argv[0]) != RDY_OK) {
|
|
| 870 |
chprintf(chp, "Warning: invalid arguments\n"); |
|
| 871 |
chprintf(chp, "Usage: %s\n", "wii_steering <address> [<deadzone>]"); |
|
| 872 |
chprintf(chp, "\n"); |
|
| 873 |
chprintf(chp, "\taddress\n"); |
|
| 874 |
chprintf(chp, "bluetooth address of the Wiimote controller to pair with.\n"); |
|
| 875 |
chprintf(chp, "\tdeadzone\n"); |
|
| 876 |
chprintf(chp, "deadzone to set for the Wiimote controller [default = 10%%].\n"); |
|
| 877 |
return; |
|
| 878 |
} |
|
| 879 |
else {
|
|
| 880 |
// set deadzone |
|
| 881 |
const float deadzone = global.userThread.setWiiDeadzone((argc == 2) ? std::atof(argv[1]) : 0.1f); |
|
| 882 |
chprintf(chp, "deadzone set to %u%%\n", (unsigned int)(deadzone * 100.0f)); |
|
| 883 |
|
|
| 884 |
// start Wii steering behaviour |
|
| 885 |
global.userThread.setNextState(UserThread::WII_STEERING); |
|
| 886 |
} |
|
| 887 |
} |
|
| 888 |
return; |
|
| 889 |
} |
|
| 890 |
|
|
| 861 | 891 |
static const ShellCommand commands[] = {
|
| 862 | 892 |
{"shutdown", shellRequestShutdown},
|
| 863 | 893 |
{"check", shellRequestCheck},
|
| ... | ... | |
| 876 | 906 |
{"print_vcnl", shellRequestPrintVCNL},
|
| 877 | 907 |
{"shell_board", shellSwitchBoardCmd},
|
| 878 | 908 |
{"get_bootloader_info", shellRequestGetBootloaderInfo},
|
| 909 |
{"wii_steering", shellRequestWiiSteering},
|
|
| 879 | 910 |
{NULL, NULL}
|
| 880 | 911 |
}; |
| 881 | 912 |
|
| devices/PowerManagement/userthread.cpp | ||
|---|---|---|
| 3 | 3 |
#include "global.hpp" |
| 4 | 4 |
#include <array> |
| 5 | 5 |
#include <chprintf.h> |
| 6 |
#include <cmath> |
|
| 6 | 7 |
|
| 7 | 8 |
using namespace amiro; |
| 8 | 9 |
|
| 9 | 10 |
extern Global global; |
| 10 | 11 |
|
| 11 |
uint16_t touch;
|
|
| 12 |
std::array<uint16_t, 8> proximity;
|
|
| 13 |
std::array<float, 8> proxNormalized;
|
|
| 12 |
volatile UserThread::State current_state;
|
|
| 13 |
volatile UserThread::State next_state;
|
|
| 14 |
types::kinematic kinematic;
|
|
| 14 | 15 |
|
| 15 |
bool running;
|
|
| 16 |
namespace obstacle_avoidance {
|
|
| 16 | 17 |
|
| 17 | 18 |
uint16_t constexpr proxThresholdLow = 0x0000; |
| 18 | 19 |
uint16_t constexpr proxThresholdHigh = 0x1000; |
| ... | ... | |
| 40 | 41 |
/* w_z [µrad/s] */ 0 |
| 41 | 42 |
}; |
| 42 | 43 |
|
| 43 |
|
|
| 44 |
|
|
| 45 | 44 |
inline uint8_t ProxId2LedId(const uint8_t proxId) {
|
| 46 | 45 |
return (proxId < 4) ? proxId+4 : proxId-4; |
| 47 | 46 |
} |
| ... | ... | |
| 57 | 56 |
} |
| 58 | 57 |
} |
| 59 | 58 |
|
| 59 |
} /* namespace obstacle_avoidance */ |
|
| 60 |
|
|
| 61 |
namespace wii_steering {
|
|
| 62 |
|
|
| 63 |
BluetoothWiimote wiimote(&global.wt12, RX_TX); |
|
| 64 |
BluetoothSerial btserial(&global.wt12, RX_TX); |
|
| 65 |
|
|
| 66 |
float deadzone; |
|
| 67 |
char bt_address[18] = {'\0'};
|
|
| 68 |
float wiimoteCalib[3] = {0.0f};
|
|
| 69 |
|
|
| 70 |
uint32_t constexpr maxTranslation = 500e3; |
|
| 71 |
uint32_t constexpr maxRotation = 3.1415927f * 1000000.0f * 2.0f; |
|
| 72 |
|
|
| 73 |
} |
|
| 74 |
|
|
| 60 | 75 |
UserThread::UserThread() : |
| 61 | 76 |
chibios_rt::BaseStaticThread<USER_THREAD_STACK_SIZE>() |
| 62 | 77 |
{
|
| ... | ... | |
| 69 | 84 |
msg_t |
| 70 | 85 |
UserThread::main() |
| 71 | 86 |
{
|
| 72 |
uint8_t sensor = 0;
|
|
| 73 |
float factor_x = 0.0f;
|
|
| 74 |
float factor_wz = 0.0f;
|
|
| 75 |
types::kinematic kinematic = defaultKinematic;
|
|
| 87 |
/*
|
|
| 88 |
* initialize some variables
|
|
| 89 |
*/
|
|
| 90 |
current_state = IDLE;
|
|
| 76 | 91 |
|
| 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; |
|
| 92 |
/* |
|
| 93 |
* set all LEDs black (off) |
|
| 94 |
*/ |
|
| 95 |
for (uint8_t led = 0; led < 8; ++led) {
|
|
| 96 |
global.robot.setLightColor(led, Color(Color::BLACK)); |
|
| 97 |
} |
|
| 98 |
|
|
| 99 |
/* |
|
| 100 |
* thread loop |
|
| 101 |
*/ |
|
| 102 |
while (!this->shouldTerminate()) {
|
|
| 103 |
/* |
|
| 104 |
* handle changes of the state |
|
| 105 |
*/ |
|
| 106 |
if (next_state != current_state) {
|
|
| 107 |
switch (current_state) {
|
|
| 108 |
case IDLE: |
|
| 109 |
{
|
|
| 110 |
if (next_state == OBSTACLE_AVOIDANCE) {
|
|
| 111 |
// set all LEDs to white for one second |
|
| 112 |
for (uint8_t led = 0; led < 8; ++led) {
|
|
| 113 |
global.robot.setLightColor(led, Color(Color::WHITE)); |
|
| 114 |
} |
|
| 115 |
this->sleep(MS2ST(1000)); |
|
| 116 |
for (uint8_t led = 0; led < 8; ++led) {
|
|
| 117 |
global.robot.setLightColor(led, Color(Color::BLACK)); |
|
| 101 | 118 |
} |
| 119 |
} |
|
| 120 |
/* if (this->next_state == WII_STEERING) */ else {
|
|
| 121 |
// setup bluetooth |
|
| 122 |
wii_steering::wiimote.bluetoothWiimoteListen(wii_steering::bt_address); |
|
| 123 |
wii_steering::btserial.bluetoothSerialListen("ALL");
|
|
| 124 |
|
|
| 125 |
// set LEDs: front = green; rear = red; sides = blue |
|
| 126 |
global.robot.setLightColor(constants::LightRing::LED_NNW, Color(Color::GREEN)); |
|
| 127 |
global.robot.setLightColor(constants::LightRing::LED_NNE, Color(Color::GREEN)); |
|
| 128 |
global.robot.setLightColor(constants::LightRing::LED_SSW, Color(Color::RED)); |
|
| 129 |
global.robot.setLightColor(constants::LightRing::LED_SSE, Color(Color::RED)); |
|
| 130 |
global.robot.setLightColor(constants::LightRing::LED_WNW, Color(Color::BLUE)); |
|
| 131 |
global.robot.setLightColor(constants::LightRing::LED_WSW, Color(Color::BLUE)); |
|
| 132 |
global.robot.setLightColor(constants::LightRing::LED_ENE, Color(Color::BLUE)); |
|
| 133 |
global.robot.setLightColor(constants::LightRing::LED_ESE, Color(Color::BLUE)); |
|
| 134 |
|
|
| 135 |
chprintf((BaseSequentialStream*)&global.sercanmux1, "press buttons '1' and '2' to connect\n"); |
|
| 136 |
} |
|
| 137 |
break; |
|
| 138 |
} |
|
| 139 |
case OBSTACLE_AVOIDANCE: |
|
| 140 |
{
|
|
| 141 |
if (next_state == IDLE) {
|
|
| 142 |
// stop the robot |
|
| 143 |
kinematic = {0, 0, 0, 0, 0, 0};
|
|
| 144 |
global.robot.setTargetSpeed(kinematic); |
|
| 102 | 145 |
|
| 103 | 146 |
// set all LEDs to white for one second |
| 104 | 147 |
for (uint8_t led = 0; led < 8; ++led) {
|
| 105 |
global.robot.setLightColor(led, Color(Color::WHITE));
|
|
| 148 |
global.robot.setLightColor(led, Color(Color::WHITE)); |
|
| 106 | 149 |
} |
| 107 | 150 |
this->sleep(MS2ST(1000)); |
| 108 | 151 |
for (uint8_t led = 0; led < 8; ++led) {
|
| 109 |
global.robot.setLightColor(led, Color(Color::BLACK));
|
|
| 152 |
global.robot.setLightColor(led, Color(Color::BLACK)); |
|
| 110 | 153 |
} |
| 154 |
} |
|
| 155 |
/* if (this->next_state == WII_STEERING) */ else {
|
|
| 156 |
// must turn off obstacle avoidance first |
|
| 157 |
chprintf((BaseSequentialStream*)&global.sercanmux1, "ERROR: turn off obstacle avoidance first!\n"); |
|
| 158 |
next_state = OBSTACLE_AVOIDANCE; |
|
| 159 |
} |
|
| 160 |
break; |
|
| 111 | 161 |
} |
| 162 |
case WII_STEERING: {
|
|
| 163 |
if (next_state == IDLE) {
|
|
| 164 |
// stop the robot |
|
| 165 |
kinematic = {0, 0, 0, 0, 0, 0};
|
|
| 166 |
global.robot.setTargetSpeed(kinematic); |
|
| 112 | 167 |
|
| 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 |
|
| 168 |
// disconnect from Wiimote controller |
|
| 169 |
wii_steering::wiimote.bluetoothWiimoteDisconnect(wii_steering::bt_address); |
|
| 170 |
wii_steering::btserial.bluetoothSerialStop(); |
|
| 171 |
wii_steering::wiimote.bluetoothWiimoteStop(); |
|
| 172 |
|
|
| 173 |
// set all LEDs to black |
|
| 174 |
for (uint8_t led = 0; led < 8; ++led) {
|
|
| 175 |
global.robot.setLightColor(led, Color(Color::BLACK)); |
|
| 120 | 176 |
} |
| 177 |
} |
|
| 178 |
/* if (this->next_state == OBSTACLE_AVOIDANCE) */ else {
|
|
| 179 |
// must turn off wii steering first |
|
| 180 |
chprintf((BaseSequentialStream*)&global.sercanmux1, "ERROR: turn off wii steering first!\n"); |
|
| 181 |
next_state = WII_STEERING; |
|
| 182 |
} |
|
| 183 |
break; |
|
| 184 |
} |
|
| 185 |
} |
|
| 186 |
current_state = next_state; |
|
| 187 |
} |
|
| 121 | 188 |
|
| 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); |
|
| 189 |
// sleep here so the loop is executed as quickly as possible |
|
| 190 |
this->sleep(CAN::UPDATE_PERIOD); |
|
| 191 |
|
|
| 192 |
/* |
|
| 193 |
* exeute behaviour depending on the current state |
|
| 194 |
*/ |
|
| 195 |
switch (current_state) {
|
|
| 196 |
case IDLE: |
|
| 197 |
{
|
|
| 198 |
// read touch sensors |
|
| 199 |
if (global.mpr121.getButtonStatus() == 0x0F) {
|
|
| 200 |
next_state = OBSTACLE_AVOIDANCE; |
|
| 201 |
} |
|
| 202 |
break; |
|
| 203 |
} |
|
| 204 |
case OBSTACLE_AVOIDANCE: |
|
| 205 |
{
|
|
| 206 |
// read touch sensors |
|
| 207 |
if (global.mpr121.getButtonStatus() == 0x0F) {
|
|
| 208 |
next_state = IDLE; |
|
| 209 |
break; |
|
| 210 |
} |
|
| 211 |
|
|
| 212 |
// initialize some variables |
|
| 213 |
uint8_t sensor = 0; |
|
| 214 |
std::array<uint16_t, 8> proximity; |
|
| 215 |
std::array<float, 8> proxNormalized; |
|
| 216 |
float factor_x = 0.0f; |
|
| 217 |
float factor_wz = 0.0f; |
|
| 218 |
|
|
| 219 |
// read proximity values |
|
| 220 |
for (sensor = 0; sensor < 8; ++sensor) {
|
|
| 221 |
proximity[sensor] = global.vcnl4020[sensor].getProximityScaledWoOffset(); |
|
| 222 |
} |
|
| 223 |
|
|
| 224 |
// normalize proximity values |
|
| 225 |
for (sensor = 0; sensor < 8; ++sensor) {
|
|
| 226 |
register uint16_t prox = proximity[sensor]; |
|
| 227 |
// limit to high treshold |
|
| 228 |
if (prox > obstacle_avoidance::proxThresholdHigh) |
|
| 229 |
prox = obstacle_avoidance::proxThresholdHigh; |
|
| 230 |
// limit to low threshold |
|
| 231 |
else if (prox < obstacle_avoidance::proxThresholdLow) |
|
| 232 |
prox = obstacle_avoidance::proxThresholdLow; |
|
| 233 |
// apply low threshold |
|
| 234 |
prox -= obstacle_avoidance::proxThresholdLow; |
|
| 235 |
// normalize to [0, 1] |
|
| 236 |
proxNormalized[sensor] = float(prox) / float(obstacle_avoidance::proxRange); |
|
| 237 |
} |
|
| 238 |
|
|
| 239 |
// map the sensor values to the top LEDs |
|
| 240 |
for (sensor = 0; sensor < 8; ++sensor) {
|
|
| 241 |
global.robot.setLightColor(obstacle_avoidance::ProxId2LedId(sensor), obstacle_avoidance::Prox2Color(proxNormalized[sensor])); |
|
| 242 |
} |
|
| 243 |
|
|
| 244 |
// evaluate NAM |
|
| 245 |
for (sensor = 0; sensor < 8; ++sensor) {
|
|
| 246 |
factor_x += proxNormalized[sensor] * obstacle_avoidance::namMatrix[sensor][0]; |
|
| 247 |
factor_wz += proxNormalized[sensor] * obstacle_avoidance::namMatrix[sensor][1]; |
|
| 248 |
} |
|
| 249 |
|
|
| 250 |
// set motor commands |
|
| 251 |
kinematic = obstacle_avoidance::defaultKinematic; |
|
| 252 |
kinematic.x += (factor_x * obstacle_avoidance::baseTranslation) + 0.5f; |
|
| 253 |
kinematic.w_z += (factor_wz * obstacle_avoidance::baseRotation) + 0.5f; |
|
| 254 |
global.robot.setTargetSpeed(kinematic); |
|
| 255 |
|
|
| 256 |
break; |
|
| 257 |
} |
|
| 258 |
case WII_STEERING: |
|
| 259 |
{
|
|
| 260 |
// if not yet connected to the Wiimote controller |
|
| 261 |
if (!wii_steering::wiimote.bluetoothWiimoteIsConnected()) {
|
|
| 262 |
// try to connect |
|
| 263 |
chprintf((BaseSequentialStream*)&global.sercanmux1, "connecting...\n"); |
|
| 264 |
wii_steering::wiimote.bluetoothWiimoteConnect(wii_steering::bt_address); |
|
| 265 |
|
|
| 266 |
if (wii_steering::wiimote.bluetoothWiimoteIsConnected()) {
|
|
| 267 |
chprintf((BaseSequentialStream*)&global.sercanmux1, "connection established\n"); |
|
| 268 |
chprintf((BaseSequentialStream*)&global.sercanmux1, "\n"); |
|
| 269 |
chprintf((BaseSequentialStream*)&global.sercanmux1, "Wiimote control:\n"); |
|
| 270 |
chprintf((BaseSequentialStream*)&global.sercanmux1, "\tpress 'home' to calibrate\n"); |
|
| 271 |
chprintf((BaseSequentialStream*)&global.sercanmux1, "\thold 'A' to steer\n"); |
|
| 272 |
} |
|
| 273 |
} |
|
| 274 |
// steer AMiRo using the Wiimote controller like a joystick |
|
| 275 |
else {
|
|
| 276 |
// initialize some variables |
|
| 277 |
float wiimoteAcc[3] = {0.0f, 0.0f, 0.0f};
|
|
| 278 |
|
|
| 279 |
// get Wiimote accelerometer data |
|
| 280 |
wiimoteAcc[0] = wii_steering::wiimote.getAccelerometer()->x_axis; |
|
| 281 |
wiimoteAcc[1] = wii_steering::wiimote.getAccelerometer()->y_axis; |
|
| 282 |
wiimoteAcc[2] = wii_steering::wiimote.getAccelerometer()->z_axis; |
|
| 283 |
|
|
| 284 |
// calibrate accelerometer offset |
|
| 285 |
if (wii_steering::wiimote.getButtons()->home) {
|
|
| 286 |
wii_steering::wiimoteCalib[0] = wiimoteAcc[0]; |
|
| 287 |
wii_steering::wiimoteCalib[1] = wiimoteAcc[1] + 100.0f; |
|
| 288 |
wii_steering::wiimoteCalib[2] = wiimoteAcc[2]; |
|
| 289 |
|
|
| 290 |
chprintf((BaseSequentialStream*)&global.sercanmux1, "accelerometer calibrated:\n"); |
|
| 291 |
chprintf((BaseSequentialStream*)&global.sercanmux1, "\tX = %d\n", (int32_t)wii_steering::wiimoteCalib[0]); |
|
| 292 |
chprintf((BaseSequentialStream*)&global.sercanmux1, "\tY = %d\n", (int32_t)wii_steering::wiimoteCalib[1]); |
|
| 293 |
chprintf((BaseSequentialStream*)&global.sercanmux1, "\tZ = %d\n", (int32_t)wii_steering::wiimoteCalib[2]); |
|
| 294 |
} |
|
| 295 |
|
|
| 296 |
for (uint8_t axis = 0; axis < 3; ++axis) {
|
|
| 297 |
// apply calibration values |
|
| 298 |
wiimoteAcc[axis] -= wii_steering::wiimoteCalib[axis]; |
|
| 299 |
|
|
| 300 |
// normalize to (-1, 1) |
|
| 301 |
wiimoteAcc[axis] /= 100.0f; |
|
| 302 |
|
|
| 303 |
// limit to 1G |
|
| 304 |
if (wiimoteAcc[axis] > 1.0f) {
|
|
| 305 |
wiimoteAcc[axis] = 1.0f; |
|
| 306 |
} else if (wiimoteAcc[axis] < -1.0f) {
|
|
| 307 |
wiimoteAcc[axis] = -1.0f; |
|
| 137 | 308 |
} |
| 138 | 309 |
|
| 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])); |
|
| 310 |
// apply deadzone |
|
| 311 |
if (std::fabs(wiimoteAcc[axis]) < wii_steering::deadzone) {
|
|
| 312 |
wiimoteAcc[axis] = 0.0f; |
|
| 144 | 313 |
} |
| 145 | 314 |
|
| 146 | 315 |
/* |
| 147 |
* evaluate NAM |
|
| 316 |
* the value is now in (-1 .. -deazone, 0, deadzone .. 1) |
|
| 317 |
* note the gaps [-deadzone .. 0] and [0 .. deadzone] |
|
| 148 | 318 |
*/ |
| 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]; |
|
| 319 |
|
|
| 320 |
// normalize (deadzone, 1) to (0, 1) and (-1, -deadzone) tpo (-1, 0) |
|
| 321 |
if (wiimoteAcc[axis] > 0) {
|
|
| 322 |
wiimoteAcc[axis] -= wii_steering::deadzone; |
|
| 323 |
} else if (wiimoteAcc[axis] < 0){
|
|
| 324 |
wiimoteAcc[axis] += wii_steering::deadzone; |
|
| 154 | 325 |
} |
| 326 |
wiimoteAcc[axis] *= (1.0f / (1.0f - wii_steering::deadzone)); |
|
| 327 |
} |
|
| 155 | 328 |
|
| 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); |
|
| 329 |
// only move when A is pressed |
|
| 330 |
if (wii_steering::wiimote.getButtons()->A) {
|
|
| 331 |
// set kinematic relaive to maximum speeds |
|
| 332 |
kinematic.x = wii_steering::maxTranslation * wiimoteAcc[2]; |
|
| 333 |
kinematic.w_z = wii_steering::maxRotation * wiimoteAcc[0] * ((wiimoteAcc[2] < 0.0f) ? 1.0f : -1.0f); |
|
| 334 |
} else {
|
|
| 335 |
kinematic = {0, 0, 0, 0, 0, 0};
|
|
| 336 |
} |
|
| 337 |
|
|
| 338 |
// set speed |
|
| 339 |
global.robot.setTargetSpeed(kinematic); |
|
| 163 | 340 |
} |
| 164 | 341 |
|
| 165 |
this->sleep(MS2ST(10)); |
|
| 342 |
break; |
|
| 343 |
} |
|
| 166 | 344 |
} |
| 345 |
} |
|
| 346 |
|
|
| 347 |
// stop the robot |
|
| 348 |
kinematic = {0, 0, 0, 0, 0, 0};
|
|
| 349 |
global.robot.setTargetSpeed(kinematic); |
|
| 167 | 350 |
|
| 168 | 351 |
return RDY_OK; |
| 169 | 352 |
} |
| 170 | 353 |
|
| 354 |
void |
|
| 355 |
UserThread::setNextState(const UserThread::State state) |
|
| 356 |
{
|
|
| 357 |
next_state = state; |
|
| 358 |
return; |
|
| 359 |
} |
|
| 360 |
|
|
| 361 |
UserThread::State |
|
| 362 |
UserThread::getCurrenState() const |
|
| 363 |
{
|
|
| 364 |
return current_state; |
|
| 365 |
} |
|
| 366 |
|
|
| 367 |
msg_t |
|
| 368 |
UserThread::setWiiAddress(const char* address) |
|
| 369 |
{
|
|
| 370 |
if (strlen(address) != 17) {
|
|
| 371 |
return RDY_RESET; |
|
| 372 |
} |
|
| 373 |
else {
|
|
| 374 |
strcpy(wii_steering::bt_address, address); |
|
| 375 |
return RDY_OK; |
|
| 376 |
} |
|
| 377 |
} |
|
| 378 |
|
|
| 379 |
float |
|
| 380 |
UserThread::setWiiDeadzone(const float deadzone) |
|
| 381 |
{
|
|
| 382 |
// check for negative value and limit to zero |
|
| 383 |
float dz = (deadzone < 0.0f) ? 0.0f : deadzone; |
|
| 384 |
|
|
| 385 |
// if value is >1, range is assumed to be (0, 100) |
|
| 386 |
if (dz > 1.0f) {
|
|
| 387 |
// limit to 100 |
|
| 388 |
if (dz > 100.0f) {
|
|
| 389 |
dz = 100.0f; |
|
| 390 |
} |
|
| 391 |
dz /= 100.0f; |
|
| 392 |
} |
|
| 393 |
|
|
| 394 |
// set value and return it |
|
| 395 |
wii_steering::deadzone = dz; |
|
| 396 |
return dz; |
|
| 397 |
} |
|
| 398 |
|
|
| devices/PowerManagement/userthread.h | ||
|---|---|---|
| 9 | 9 |
class UserThread : public chibios_rt::BaseStaticThread<USER_THREAD_STACK_SIZE> |
| 10 | 10 |
{
|
| 11 | 11 |
public: |
| 12 |
|
|
| 13 |
enum State {
|
|
| 14 |
IDLE, |
|
| 15 |
OBSTACLE_AVOIDANCE, |
|
| 16 |
WII_STEERING, |
|
| 17 |
}; |
|
| 18 |
|
|
| 19 |
public: |
|
| 12 | 20 |
explicit UserThread(); |
| 13 | 21 |
|
| 14 | 22 |
virtual ~UserThread(); |
| 15 | 23 |
|
| 16 | 24 |
virtual msg_t main(); |
| 25 |
|
|
| 26 |
void setNextState(const State state); |
|
| 27 |
|
|
| 28 |
State getCurrenState() const; |
|
| 29 |
|
|
| 30 |
msg_t setWiiAddress(const char* address); |
|
| 31 |
|
|
| 32 |
float setWiiDeadzone(const float deadzone); |
|
| 17 | 33 |
}; |
| 18 | 34 |
|
| 19 | 35 |
} // end of namespace amiro |
| include/amiro/bluetooth/bluetooth-wiimote.hpp | ||
|---|---|---|
| 33 | 33 |
}; |
| 34 | 34 |
Accelerometer *getAccelerometer(); |
| 35 | 35 |
|
| 36 |
struct Buttons {
|
|
| 37 |
uint16_t left : 1; |
|
| 38 |
uint16_t right : 1; |
|
| 39 |
uint16_t down : 1; |
|
| 40 |
uint16_t up : 1; |
|
| 41 |
uint16_t plus : 1; |
|
| 42 |
uint16_t two : 1; |
|
| 43 |
uint16_t one : 1; |
|
| 44 |
uint16_t B : 1; |
|
| 45 |
uint16_t A : 1; |
|
| 46 |
uint16_t minus : 1; |
|
| 47 |
uint16_t home : 1; |
|
| 48 |
}; |
|
| 49 |
Buttons* getButtons(); |
|
| 50 |
|
|
| 36 | 51 |
protected: |
| 37 | 52 |
virtual msg_t main(void); |
| 38 | 53 |
|
| ... | ... | |
| 50 | 65 |
uint8_t stopflag; |
| 51 | 66 |
|
| 52 | 67 |
Accelerometer accelerometer; |
| 68 |
Buttons buttons; |
|
| 53 | 69 |
}; |
| 54 | 70 |
} |
| 55 | 71 |
|
Also available in: Unified diff