Update main.c

Changes to add LUT use
Use of Clamp, MIN, MAX to reduce code space
Some minor logic changes to reduce code use and (IMO) make it easier to read

NOTE: Changed the pid calculation routine to return int32_t. It was using float, but no float types are used so it was a waste.  Since int32_t is used for internal variables, it makes sense to use it, and you can then at least down-cast it to smaller types.