ank/RBE2001-lab2
1
Fork 0
Code Releases Activity

second order ish IK

Browse source
This commit is contained in:
Andy Killorin 2026-01-29 09:00:26 -05:00
parent a126b577b7
commit 09bce37a9a
Signed by: ank
GPG key ID: 23F9463ECB67FE8C
2 changed files with 25 additions and 6 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

23
lib/Chassis/src/chassis.cpp
View file

@ -139,7 +139,22 @@ void Chassis::SetMotorEfforts(int16_t left, int16_t right)
Twist Chassis::CalcOdomFromWheelMotion(void)
{
Twist velocity;
Twist delta;
float leftDelta = (float) leftMotor.speed * (float) CONTROL_LOOP_PERIOD_MS / 1000.0;
float rightDelta = (float) rightMotor.speed * (float) CONTROL_LOOP_PERIOD_MS / 1000.0;
float distance = (leftDelta + rightDelta) / 2.0;
distance /= LEFT_TICKS_PER_CM;
float deltaHeading = (rightDelta - leftDelta)
/ LEFT_TICKS_PER_CM
/ (3.14 * ROBOT_RADIUS);
delta.u = distance;
delta.omega = deltaHeading;
/**
* TODO: Calculate velocities from wheel motion, which are held in leftMotor.spped and rightMotor.speed.
* Note that you might want to calculate the deltas instead of speeds (to save some floating point maths).
@ -148,9 +163,9 @@ Twist Chassis::CalcOdomFromWheelMotion(void)
*/
#ifdef __NAV_DEBUG__
TeleplotPrint("u", velocity.u);
TeleplotPrint("omega", velocity.omega);
TeleplotPrint("u", delta.u);
TeleplotPrint("omega", delta.omega);
#endif
return velocity;
return delta;
}

6
src/robot-nav.cpp
View file

@ -10,12 +10,16 @@ void Robot::UpdatePose(const Twist& twist)
* TODO: Add your FK algorithm to update currPose here.
*/
float transitHeading = currPose.theta + (twist.omega * 0.5);
currPose.x += twist.u * cos(transitHeading);
currPose.y += twist.u * sin(transitHeading);
#ifdef __NAV_DEBUG__
TeleplotPrint("x", currPose.x);
TeleplotPrint("y", currPose.y);
TeleplotPrint("theta", currPose.theta);
#endif
}
/**