Been messing with the code for a few days off and on, realizing that it is quite inconsistent and jumpy compared to the original Arduino code. I originally thought this coudl be because my AVR was running at 8MHz vs. the 16MHz of the Arduino, but it turns out it was some issues with the serial communication and the internal RC oscillator's inaccuracy. This problem has been solved for the time being by using an external 8MHz crystal for the system clock and changing the required fuses. All the fuse changes can be done through the Makefile, type "make set_ext_osc_fast" to do this (CAUTION: the fuse settings in the Makefile are for an ATmega8515! make sure to change them for your target AVR mcu!).