osPriority Thread::get_priority() {
osPriority ret;
_mutex.lock();
ret = osThreadGetPriority(_tid);
_mutex.unlock();
return ret;
}
osPriority Thread::get_priority() {
return osThreadGetPriority(_tid);
}
/**
* Initializes the console
*/
void Task_SerialConsole(void const* args) {
const osThreadId mainID = (const osThreadId)args;
// Store the thread's ID
const osThreadId threadID = Thread::gettid();
ASSERT(threadID != nullptr);
// Store our priority so we know what to reset it to after running a command
const osPriority threadPriority = osThreadGetPriority(threadID);
// Initalize the console buffer and save the char buffer's starting address
std::shared_ptr<Console> console = Console::Instance();
// Set the console username to whoever the git author is
console->changeUser(git_head_author);
// Let everyone know we're ok
LOG(INIT,
"Serial console ready!\r\n"
" Thread ID: %u, Priority: %d",
threadID, threadPriority);
// Signal back to main and wait until we're signaled to continue
osSignalSet(mainID, MAIN_TASK_CONTINUE);
Thread::signal_wait(SUB_TASK_CONTINUE, osWaitForever);
// Display RoboJackets if we're up and running at this point during startup
console->ShowLogo();
// Print out the header to show the user we're ready for input
console->PrintHeader();
// Set the title of the terminal window
console->SetTitle("RoboJackets");
while (true) {
// Execute any active iterative command
execute_iterative_command();
// If there is a new command to handle, parse and process it
if (console->CommandReady() == true) {
// Increase the thread's priority first so we can make sure the
// scheduler will select it to run
osStatus tState =
osThreadSetPriority(threadID, osPriorityAboveNormal);
ASSERT(tState == osOK);
// Execute the command
NVIC_DisableIRQ(UART0_IRQn);
size_t rxLen = console->rxBuffer().size() + 1;
char rx[rxLen];
memcpy(rx, console->rxBuffer().c_str(), rxLen - 1);
rx[rxLen - 1] = '\0';
execute_line(rx);
NVIC_EnableIRQ(UART0_IRQn);
// Now, reset the priority of the thread to its idle state
tState = osThreadSetPriority(threadID, threadPriority);
ASSERT(tState == osOK);
console->CommandHandled();
}
// Check if a system stop is requested
if (console->IsSystemStopRequested() == true) break;
// Yield to other threads when not needing to execute anything
Thread::yield();
}
}
/**
* initializes the motion controller thread
*/
void Task_Controller(const void* args) {
const auto mainID = reinterpret_cast<osThreadId>(const_cast<void*>(args));
// Store the thread's ID
const auto threadID = Thread::gettid();
ASSERT(threadID != nullptr);
// Store our priority so we know what to reset it to after running a command
const auto threadPriority = osThreadGetPriority(threadID);
(void)threadPriority; // disable warning if unused
// signal back to main and wait until we're signaled to continue
osSignalSet(mainID, MAIN_TASK_CONTINUE);
Thread::signal_wait(SUB_TASK_CONTINUE, osWaitForever);
std::array<int16_t, 5> duty_cycles{};
pidController.setPidValues(3.0, 10, 2, 30, 0);
// initialize timeout timer
commandTimeoutTimer = make_unique<RtosTimerHelper>(
[&]() { commandTimedOut = true; }, osTimerPeriodic);
while (true) {
// note: the 4th value is not an encoder value. See the large comment
// below for an explanation.
std::array<int16_t, 5> enc_deltas{};
// zero out command if we haven't gotten an updated target in a while
if (commandTimedOut || Battery::globBatt->isBattCritical()) {
duty_cycles = {0, 0, 0, 0, 0};
}
auto statusByte = FPGA::Instance->set_duty_get_enc(
duty_cycles.data(), duty_cycles.size(), enc_deltas.data(),
enc_deltas.size());
/*
* The time since the last update is derived with the value of
* WATCHDOG_TIMER_CLK_WIDTH in robocup.v
*
* The last encoder reading (5th one) from the FPGA is the watchdog
* timer's tick since the last SPI transfer.
*
* Multiply the received tick count by:
* (1/18.432) * 2 * (2^WATCHDOG_TIMER_CLK_WIDTH)
*
* This will give you the duration since the last SPI transfer in
* microseconds (us).
*
* For example, if WATCHDOG_TIMER_CLK_WIDTH = 6, here's how you would
* convert into time assuming the fpga returned a reading of 1265 ticks:
* time_in_us = [ 1265 * (1/18.432) * 2 * (2^6) ] = 8784.7us
*
* The precision would be in increments of the multiplier. For
* this example, that is:
* time_precision = 6.94us
*
*/
const float dt = enc_deltas.back() * (1 / 18.432e6) * 2 * 128;
// const float dt = 1.0f / (CONTROL_LOOP_WAIT_MS / 1000.0f);
// take first 4 encoder deltas
std::array<int16_t, 4> driveMotorEnc;
for (auto i = 0; i < 4; i++) {
driveMotorEnc[i] = enc_deltas[i];
enc_counts[i] += enc_deltas[i];
}
Eigen::Vector4d errors{};
Eigen::Vector4d wheelVelsOut{};
Eigen::Vector4d targetWheelVelsOut{};
// run PID controller to determine what duty cycles to use to drive the
// motors.
std::array<int16_t, 4> driveMotorDutyCycles = pidController.run(
driveMotorEnc, dt, &errors, &wheelVelsOut, &targetWheelVelsOut);
// assign the duty cycles, zero out motors that the fpga returns an
// error for
static_assert(wheelStallDetection.size() == driveMotorDutyCycles.size(),
"wheelStallDetection Size should be the same as "
"driveMotorDutyCycles");
for (std::size_t i = 0; i < driveMotorDutyCycles.size(); i++) {
const auto& vel = driveMotorDutyCycles[i];
bool didStall = wheelStallDetection[i].stall_update(
duty_cycles[i], wheelVelsOut[i]);
const bool hasError = (statusByte & (1 << i)) || didStall;
duty_cycles[i] = (hasError ? 0 : vel);
}
// limit duty cycle values, while keeping sign (+ or -)
for (auto dc : duty_cycles) {
if (std::abs(dc) > FPGA::MAX_DUTY_CYCLE) {
dc = copysign(FPGA::MAX_DUTY_CYCLE, dc);
}
}
// dribbler duty cycle
// duty_cycles[4] = dribblerSpeed;
duty_cycles[4] = get_damped_drib_duty_cycle();
Thread::wait(CONTROL_LOOP_WAIT_MS);
}
}
