void Task_Controller_UpdateTarget(Eigen::Vector3f targetVel) {
pidController.setTargetVel(targetVel);
// reset timeout
commandTimedOut = false;
if (commandTimeoutTimer)
commandTimeoutTimer->start(COMMAND_TIMEOUT_INTERVAL);
}
/**
* initializes the motion controller thread
*/
void Task_Controller(void const* args) {
const osThreadId mainID = (const osThreadId)args;
// Store the thread's ID
osThreadId threadID = Thread::gettid();
ASSERT(threadID != nullptr);
// Store our priority so we know what to reset it to after running a command
osPriority threadPriority = osThreadGetPriority(threadID);
MPU6050 imu(RJ_I2C_SDA, RJ_I2C_SCL);
imu.setBW(MPU6050_BW_256);
imu.setGyroRange(MPU6050_GYRO_RANGE_250);
imu.setAcceleroRange(MPU6050_ACCELERO_RANGE_2G);
imu.setSleepMode(false);
char testResp;
if ((testResp = imu.testConnection())) {
float resultRatio[6];
imu.selfTest(resultRatio);
LOG(INIT,
"IMU self test results:\r\n"
" Accel (X,Y,Z):\t(%2.2f%%, %2.2f%%, %2.2f%%)\r\n"
" Gyro (X,Y,Z):\t(%2.2f%%, %2.2f%%, %2.2f%%)",
resultRatio[0], resultRatio[1], resultRatio[2], resultRatio[3],
resultRatio[4], resultRatio[5]);
LOG(INIT, "Control loop ready!\r\n Thread ID: %u, Priority: %d",
((P_TCB)threadID)->task_id, threadPriority);
} else {
LOG(SEVERE,
"MPU6050 not found!\t(response: 0x%02X)\r\n Falling back to "
"sensorless control loop.",
testResp);
}
// signal back to main and wait until we're signaled to continue
osSignalSet(mainID, MAIN_TASK_CONTINUE);
Thread::signal_wait(SUB_TASK_CONTINUE, osWaitForever);
array<int16_t, 5> duty_cycles{};
// pidController.setPidValues(1.5, 0.05, 0); // TODO: tune pid values
pidController.setPidValues(0.8, 0.05, 0);
// initialize timeout timer
commandTimeoutTimer = make_unique<RtosTimerHelper>(
[&]() { commandTimedOut = true; }, osTimerPeriodic);
while (true) {
// imu.getGyro(gyroVals);
// imu.getAccelero(accelVals);
// note: the 4th value is not an encoder value. See the large comment
// below for an explanation.
array<int16_t, 5> enc_deltas{};
// zero out command if we haven't gotten an updated target in a while
if (commandTimedOut) duty_cycles = {0, 0, 0, 0, 0};
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 * 64;
// take first 4 encoder deltas
array<int16_t, 4> driveMotorEnc;
for (int i = 0; i < 4; i++) driveMotorEnc[i] = enc_deltas[i];
// run PID controller to determine what duty cycles to use to drive the
// motors.
array<int16_t, 4> driveMotorDutyCycles =
pidController.run(driveMotorEnc, dt);
for (int i = 0; i < 4; i++) duty_cycles[i] = driveMotorDutyCycles[i];
// limit duty cycle values, while keeping sign (+ or -)
for (int16_t& 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;
#if 0
// log duty cycle values
printf("duty cycles: ");
for (int i = 0; i < 4; i++) {
printf("%d, ", duty_cycles[i]);
}
printf("\r\n");
#endif
Thread::wait(CONTROL_LOOP_WAIT_MS);
}
}
void Task_Controller_UpdateOffsets(int16_t ax, int16_t ay, int16_t az,
int16_t gx, int16_t gy, int16_t gz) {
pidController.startGyro(ax, ay, az, gx, gy, gz);
}
/**
* 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);
}
}
