Rewrite--pt 1 working, it seems

Re-wrote code from scratch b/c of debugging issues. part 1 (on ground w/ normal force) seems to be working.