diff --git a/lib/Chassis/src/chassis.cpp b/lib/Chassis/src/chassis.cpp index fb0200b..0739b8a 100644 --- a/lib/Chassis/src/chassis.cpp +++ b/lib/Chassis/src/chassis.cpp @@ -117,8 +117,8 @@ void Chassis::UpdateMotors(void) */ void Chassis::SetWheelSpeeds(float leftSpeedCMperSec, float rightSpeedCMperSec) { - leftMotor.SetTargetSpeed(leftSpeedCMperSec * LEFT_TICKS_PER_CM); - rightMotor.SetTargetSpeed(rightSpeedCMperSec * RIGHT_TICKS_PER_CM); + leftMotor.SetTargetSpeed(leftSpeedCMperSec * LEFT_TICKS_PER_CM * SECONDS_PER_CONTROL_LOOP); + rightMotor.SetTargetSpeed(rightSpeedCMperSec * RIGHT_TICKS_PER_CM * SECONDS_PER_CONTROL_LOOP); } /** diff --git a/lib/Chassis/src/chassis.h b/lib/Chassis/src/chassis.h index ddb109e..c6b481a 100644 --- a/lib/Chassis/src/chassis.h +++ b/lib/Chassis/src/chassis.h @@ -19,6 +19,7 @@ protected: * avoid rounding errors. */ const uint16_t CONTROL_LOOP_PERIOD_MS = 20; + const float SECONDS_PER_CONTROL_LOOP = 1. / CONTROL_LOOP_PERIOD_MS; protected: /**