Got calculate_speed working, speed parameter can be read to get current motor speed. Still more cleanup needed. Next step is to get regulate_speed working