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); } }