package org.usfirst.frc.team1736.robot;


public class RobotPoseCalculator {
	
	public RobotPoseCalculator(){
		return;
	}
	
	public void update(){
		double WheelSpeedOno;
		double WheelSpeedDos;
		double WheelSpeedTres;
		double WheelSpeedCuatro;
		double Vx;
		double Vy;
		double netSpeed;
		
		//Calculate wheel speeds in radians per second
		WheelSpeedOno =RobotState.frontLeftWheelVelocity_rpm * 2.0 * Math.PI / 60; 
		WheelSpeedDos =RobotState.frontRightWheelVelocity_rpm * 2.0 * Math.PI / 60;
		WheelSpeedTres =RobotState.rearLeftWheelVelocity_rpm * 2.0 * Math.PI / 60;
		WheelSpeedCuatro =RobotState.rearRightWheelVelocity_rpm * 2.0 * Math.PI / 60;
	
		//Calculate translational velocity x/y components via inverse mechanum kinematic equations
		Vx = (WheelSpeedOno + WheelSpeedDos + WheelSpeedTres + WheelSpeedCuatro) * 0.17 / 4;
		Vy  = (WheelSpeedOno - WheelSpeedDos + WheelSpeedTres - WheelSpeedCuatro) * 0.17 / 4;
	
		//Calculate net speed vector with pythagorean theorem
		netSpeed = Math.sqrt(Vx*Vx+Vy*Vy);
		
		//Store results into state variables
		RobotState.robotNetSpeed_ftpers = netSpeed;
		RobotState.robotFwdRevVel_ftpers = Vx;
		RobotState.robotStrafeVel_ftpers = Vy;		
	}
}

@@ -157,156 +157,518 @@
 %0D%0A%09%09
-double WheelSpeedOno;%0D%0A%09%09double WheelSpeedDos;%0D%0A%09%09double WheelSpeedTres;%0D%0A%09%09double WheelSpeedCuatro;%0D%0A%09%09double Vx;%0D%0A%09%09double Vy;%0D%0A%09%09double netSpeed;
+//The calculations referenced herein are derived from this helpful paper:%0D%0A%09%09// http://www.academia.edu/4557426/KINEMATICS_MODELLING_OF_MECANUM_WHEELED_MOBILE_PLATFORM%0D%0A%09%09%0D%0A%09%09double WheelSpeedOno;%0D%0A%09%09double WheelSpeedDos;%0D%0A%09%09double WheelSpeedTres;%0D%0A%09%09double WheelSpeedCuatro;%0D%0A%09%09double Vx;%0D%0A%09%09double Vy;%0D%0A%09%09double netSpeed;%0D%0A%09%09%0D%0A%09%09///////////////////////////////////////////////////////////////////////////%0D%0A%09%09// Speed calculations%0D%0A%09%09///////////////////////////////////////////////////////////////////////////
 %0D%0A%09%09
@@ -1582,16 +1582,330 @@
 s = Vy;%09
+%0D%0A%09%09%0D%0A%09%09///////////////////////////////////////////////////////////////////////////%0D%0A%09%09// Distance calculations%0D%0A%09%09///////////////////////////////////////////////////////////////////////////%0D%0A%09%09RobotState.robotNetDistance_ft = netSpeed;%0D%0A%09%09RobotState.robotFwdRevDist_ft = Vx;%0D%0A%09%09RobotState.robotStrafeDist_ft = Vy;
 %09%0D%0A%09%7D%0D%0A%7D

comment WIP