Manual calculation of motor speed

Programming Support PROS Support
1 
void cutomTest()
{
    double encNow = 0, encLast = 0;
    uint32_t nowTime = pros::millis(), lastTime = pros::millis();
    while (1)
    {
        int loop = pros::millis() - lastTime;
        encNow = chassis->getEnc(0);
        double speed = (encNow - encLast) / loop * 1000 * 60 / 360;
        chassis->set(127, 127);
        std::cout << loop << " " << speed << std::endl;
        encLast = encNow;
        lastTime = pros::millis();
        pros::Task::delay_until(&nowTime, 10);
    }
}

image
The official speed is filtered, a little delayed.
This method is suitable for the use of voltage loops.
If the official speed loop is used, getEnc () will also be delayed.

2 Likes