Fixed a bug where sin should have been used instead of cos
Fixed a bug where sin should have been used instead of cos

BCLab-UNM/SwarmBaseCode-ROS,BCLab-UNM/SwarmBaseCode-ROS,BCLab-UNM/SwarmBaseCode-ROS,BCLab-UNM/SwarmBaseCode-ROS,BCLab-UNM/SwarmBaseCode-ROS,BCLab-UNM/SwarmBaseCode-ROS