Skip to content

Commit 7a0dc12

Browse files
tildezerotildezero
committed
update to taproot 2.0
1 parent ff8c2a0 commit 7a0dc12

File tree

8 files changed

+57
-48
lines changed

8 files changed

+57
-48
lines changed

‎ut-robomaster/src/main.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -25,7 +25,7 @@ static void initializeIo(src::Drivers *drivers)
2525
drivers->schedulerTerminalHandler.init();
2626
drivers->djiMotorTerminalSerialHandler.init();
2727
drivers->bmi088.initialize(IMU_SAMPLE_FREQUENCY, IMU_KP, IMU_KI);
28-
drivers->bmi088.requestRecalibration();
28+
drivers->bmi088.requestCalibration();
2929
}
3030

3131
// Anything that you would like to be called place here. It will be called

‎ut-robomaster/src/robots/sentry/sentry_control.hpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -34,7 +34,7 @@ class SentryControl : CommonControlManual
3434
CommandAgitatorContinuous rotateAgitatorL{drivers, &agitatorL, BarrelId::STANDARD1};
3535
CommandAgitatorContinuous rotateAgitatorR{drivers, &agitatorR, BarrelId::STANDARD2};
3636

37-
CommandSentryPosition sentryPosition{drivers, &chassis};
37+
CommandSentryPosition sentryPosition{drivers, &chassis, &turret};
3838
CommandSentryAim sentryAim{drivers, &turret};
3939

4040
// Mappings

‎ut-robomaster/src/subsystems/odometry/observer_displacement.cpp‎

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -29,8 +29,10 @@ bool ChassisDisplacementObserver::getVelocityChassisDisplacement(
2929

3030
// TODO: Depending on when this subsystem gets initialized,
3131
// the first time this function runs, deltaT might be large
32-
auto nowTime = imu->getPrevIMUDataReceivedTime(); // Units of us
33-
auto dt = (nowTime - lastTime) / 1e6f; // Want units of s
32+
// auto nowTime = tap::arch::clock::getTimeMicroseconds(); // units of us
33+
auto nowTime = tap::arch::clock::getTimeMilliseconds(); // ms
34+
auto dt = (nowTime - lastTime) / 1000.0f; // sec
35+
// auto dt = (nowTime - lastTime) / 1e6f; // Want units of s
3436

3537
// z is 0 since we're moving on the x-y plane and gravity affects z
3638
Vector3f nowAcc{imu->getAx(), imu->getAy(), 0.0f};

‎ut-robomaster/src/subsystems/odometry/observer_displacement.hpp‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,7 @@
22
#define SUBSYSTEMS_ODOMETRY_OBSERVER_DISPLACEMENT_HPP_
33

44
#include "tap/algorithms/odometry/chassis_displacement_observer_interface.hpp"
5+
#include "tap/architecture/clock.hpp"
56

67
#include "subsystems/chassis/chassis_subsystem.hpp"
78

‎ut-robomaster/src/subsystems/turret/double_yaw_motor.cpp‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -34,8 +34,8 @@ void DoubleYawMotor::reset()
3434

3535
void DoubleYawMotor::updateMotorAngle()
3636
{
37-
float encoderAngle = static_cast<float>(motor1.getEncoderUnwrapped()) /
38-
DjiMotor::ENC_RESOLUTION / M3508.gearRatio / YAW_REDUCTION;
37+
float encoderAngle = static_cast<float>(motor1.getEncoder()->getPosition().getWrappedValue()) /
38+
DjiMotorEncoder::ENC_RESOLUTION / M3508.gearRatio / YAW_REDUCTION;
3939
currentAngle.setWrappedValue(encoderAngle);
4040
}
4141

@@ -64,8 +64,8 @@ void DoubleYawMotor::setVelocity(float velocity, float dt)
6464

6565
float DoubleYawMotor::getCurrentVelocity()
6666
{
67-
float rpm1 = motor1.getShaftRPM(); // rev / m
68-
float rpm2 = motor2.getShaftRPM(); // rev / m
67+
float rpm1 = motor1.getInternalEncoder().getShaftRPM(); // rev / m
68+
float rpm2 = motor2.getInternalEncoder().getShaftRPM(); // rev / m
6969
float currentVelocity = (rpm1 + rpm2) / 2.0f / 60.0f / M3508.gearRatio; // rev / s
7070
return currentVelocity;
7171
}

‎ut-robomaster/src/subsystems/turret/turret_motor.cpp‎

Lines changed: 7 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -27,13 +27,14 @@ void TurretMotor::reset()
2727

2828
void TurretMotor::updateMotorAngle()
2929
{
30-
uint16_t encoderValue = motor.getEncoderWrapped();
30+
// uint16_t encoderValue = motor.getEncoderWrapped();
31+
uint16_t encoderValue = motor.getEncoder()->getPosition().getWrappedValue();
3132
if (lastUpdatedEncoderValue != encoderValue)
3233
{
3334
lastUpdatedEncoderValue = encoderValue;
3435

3536
unwrappedAngle = static_cast<float>(encoderValue) * M_TWOPI /
36-
static_cast<float>(DjiMotor::ENC_RESOLUTION);
37+
static_cast<float>(DjiMotorEncoder::ENC_RESOLUTION);
3738
currentAngle.setWrappedValue(unwrappedAngle);
3839
}
3940
}
@@ -44,8 +45,10 @@ void TurretMotor::setAngle(float desiredAngle, float dt)
4445

4546
float positionControllerError = WrappedFloat(currentAngle.getWrappedValue(), 0, M_TWOPI)
4647
.minDifference(setpoint.getWrappedValue());
47-
float output =
48-
pid.runController(positionControllerError, (M_TWOPI / 60.0f) * motor.getShaftRPM(), dt);
48+
float output = pid.runController(
49+
positionControllerError,
50+
(M_TWOPI / 60.0f) * motor.getInternalEncoder().getShaftRPM(),
51+
dt);
4952

5053
motor.setDesiredOutput(output);
5154
}

‎ut-robomaster/src/subsystems/turret/turret_subsystem.cpp‎

Lines changed: 36 additions & 33 deletions
Original file line numberDiff line numberDiff line change
@@ -37,45 +37,48 @@ void TurretSubsystem::refresh()
3737
// who needs hardware checks?
3838
// setAmputated(!hardwareOk());
3939

40+
#if defined(TARGET_HERO)
4041
Remote* remote = &drivers->remote;
4142
float h = remote->getChannel(Remote::Channel::LEFT_HORIZONTAL);
4243
float v = remote->getChannel(Remote::Channel::LEFT_VERTICAL);
4344
yaw.setOutput(h);
4445
pitch.motor.setDesiredOutput(GM6020.maxOutput * v);
46+
return;
47+
#endif
48+
49+
yaw.updateMotorAngle();
50+
pitch.updateMotorAngle();
51+
52+
#if defined(TARGET_STANDARD) || defined(TARGET_HERO)
53+
yawEncoder.update();
54+
55+
if (!isCalibrated && yawEncoder.isOnline())
56+
{
57+
baseYaw = yawEncoder.getAngle() - YAW_OFFSET - yaw.getAngle();
58+
isCalibrated = true;
59+
60+
setTargetWorldAngles(getCurrentLocalYaw() + getChassisYaw(), getCurrentLocalPitch());
61+
}
62+
#else
63+
if (!isCalibrated && !isAmputated())
64+
{
65+
baseYaw = -YAW_OFFSET;
66+
isCalibrated = true;
67+
68+
setTargetWorldAngles(getCurrentLocalYaw() + getChassisYaw(), getCurrentLocalPitch());
69+
}
70+
#endif
4571

46-
// yaw.updateMotorAngle();
47-
// pitch.updateMotorAngle();
48-
49-
// #if defined(TARGET_STANDARD) || defined(TARGET_HERO)
50-
// yawEncoder.update();
51-
52-
// if (!isCalibrated && yawEncoder.isOnline())
53-
// {
54-
// baseYaw = yawEncoder.getAngle() - YAW_OFFSET - yaw.getAngle();
55-
// isCalibrated = true;
56-
57-
// setTargetWorldAngles(getCurrentLocalYaw() + getChassisYaw(), getCurrentLocalPitch());
58-
// }
59-
// #else
60-
// if (!isCalibrated && !isAmputated())
61-
// {
62-
// baseYaw = -YAW_OFFSET;
63-
// isCalibrated = true;
64-
65-
// setTargetWorldAngles(getCurrentLocalYaw() + getChassisYaw(), getCurrentLocalPitch());
66-
// }
67-
// #endif
68-
69-
// if (isCalibrated && !drivers->isKillSwitched())
70-
// {
71-
// yaw.setAngle(-baseYaw + getTargetLocalYaw(), DT);
72-
// pitch.setAngle((PITCH_OFFSET + getTargetLocalPitch()) * PITCH_REDUCTION, DT);
73-
// }
74-
// else
75-
// {
76-
// yaw.reset();
77-
// pitch.reset();
78-
// }
72+
if (isCalibrated && !drivers->isKillSwitched())
73+
{
74+
yaw.setAngle(-baseYaw + getTargetLocalYaw(), DT);
75+
pitch.setAngle((PITCH_OFFSET + getTargetLocalPitch()) * PITCH_REDUCTION, DT);
76+
}
77+
else
78+
{
79+
yaw.reset();
80+
pitch.reset();
81+
}
7982
}
8083

8184
void TurretSubsystem::inputTargetData(Vector3f position, Vector3f velocity, Vector3f acceleration)

‎ut-robomaster/src/utils/motors/motor_controller.cpp‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -50,15 +50,15 @@ bool MotorController::isOnline() { return motor.isMotorOnline(); }
5050

5151
float MotorController::measurePosition()
5252
{
53-
int64_t encoderVal = motor.getEncoderUnwrapped();
54-
float units = static_cast<float>(encoderVal) / DjiMotor::ENC_RESOLUTION;
53+
int64_t encoderVal = motor.getEncoder()->getPosition().getUnwrappedValue();
54+
float units = static_cast<float>(encoderVal) / DjiMotorEncoder::ENC_RESOLUTION;
5555
float turns = units / constants.gearRatio; // revs
5656
return turns;
5757
}
5858

5959
float MotorController::measureVelocity()
6060
{
61-
int16_t rpm = motor.getShaftRPM() / constants.gearRatio;
61+
int16_t rpm = motor.getInternalEncoder().getShaftRPM() / constants.gearRatio;
6262
float rps = rpm / 60.0f; // revs / sec
6363
return rps;
6464
}

0 commit comments

Comments
 (0)