Updated COG init function. PWM needs to be updated still. Other small updates.