diff --git a/lib/Chassis/src/chassis.cpp b/lib/Chassis/src/chassis.cpp index fb0200b..6f57fc4 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 * CONTROL_LOOP_PERIOD_S); + rightMotor.SetTargetSpeed(rightSpeedCMperSec * RIGHT_TICKS_PER_CM * CONTROL_LOOP_PERIOD_S); } /** diff --git a/lib/Chassis/src/chassis.h b/lib/Chassis/src/chassis.h index ddb109e..4f00c2f 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 CONTROL_LOOP_PERIOD_S = CONTROL_LOOP_PERIOD_MS / 1000.; protected: /**