More work on PSO.
No actual movement done yet.

Signed-off-by: loganperreault <perreault.logan@gmail.com>