package org.pihisamurai;

import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

public class SpeedController {
	
	Robot robot;
	private int box;
	
	SpeedController(Robot robot){
		this.robot = robot;
		
		SmartDashboard.putNumber("Turn Speed 0 Boxes", 5.1);
		SmartDashboard.putNumber("Turn Speed 1 Boxes", 5.1);
		SmartDashboard.putNumber("Turn Speed 2 Boxes", 4.5);
		SmartDashboard.putNumber("Turn Speed 3 Boxes", 3.5);
		SmartDashboard.putNumber("Turn Speed 4 Boxes", 2.5);
		SmartDashboard.putNumber("Turn Speed 5 Boxes", 2.0);
		SmartDashboard.putNumber("Turn Speed 6 Boxes", 1.5);

		SmartDashboard.putNumber("Stick change rate 0 boxes", 2.0);
		SmartDashboard.putNumber("Stick change rate 1 boxes", 2.0);
		SmartDashboard.putNumber("Stick change rate 2 boxes", 1.5);
		SmartDashboard.putNumber("Stick change rate 3 boxes", 0.7);
		SmartDashboard.putNumber("Stick change rate 4 boxes", 0.5);
		SmartDashboard.putNumber("Stick change rate 5 boxes", 0.3);
		SmartDashboard.putNumber("Stick change rate 6 boxes", 0.2);
		
		SmartDashboard.putNumber("Angle stick rate 0 boxes", 2.0);
		SmartDashboard.putNumber("Angle stick rate 1 boxes", 2.0);
		SmartDashboard.putNumber("Angle stick rate 2 boxes", 1.25);
		SmartDashboard.putNumber("Angle stick rate 3 boxes", 1.2);
		SmartDashboard.putNumber("Angle stick rate 4 boxes", 0.8);
		SmartDashboard.putNumber("Angle stick rate 5 boxes", 0.6);
		SmartDashboard.putNumber("Angle stick rate 6 boxes", 0.3);
	}
	
	public int getBoxCount(){
		return box;
	}
	
	public void setBoxCount(int box){
		this.box = box;
		SmartDashboard.putNumber("Box Count", box);
	}
	
	double turnSpeedModifier = 1;
	
	public void setTurnSpeedModifier(double a){
		turnSpeedModifier = a;
	}
	
	public void update(){
		double turnSpeed =  SmartDashboard.getNumber("Turn Speed " + box + " Boxes");
		double accelLimit =  SmartDashboard.getNumber("Stick change rate " + box + " boxes");
		double turnAccelLimit = SmartDashboard.getNumber("Angle stick rate " + box + " boxes");

		robot.drivetrain.setPrimaryLimit(accelLimit);
		robot.drivetrain.setStrafeLimit(accelLimit);
		robot.drivetrain.setAngleAccelLimit(turnAccelLimit);
		
		robot.drivetrain.setMaxTurnSpeed(turnSpeed*turnSpeedModifier);
	}
}

@@ -256,35 +256,35 @@
 Speed 0 Boxes%22, 
-5.1
+4.0
 );%0D%0A%09%09SmartDashb
@@ -324,11 +324,11 @@
 s%22, 
-5.1
+4.0
 );%0D%0A
@@ -376,17 +376,17 @@
 Boxes%22, 
-4
+3
 .5);%0D%0A%09%09
@@ -426,33 +426,33 @@
 eed 3 Boxes%22, 3.
-5
+0
 );%0D%0A%09%09SmartDashb
@@ -490,17 +490,17 @@
 xes%22, 2.
-5
+0
 );%0D%0A%09%09Sm
@@ -536,35 +536,35 @@
 Speed 5 Boxes%22, 
-2.0
+1.5
 );%0D%0A%09%09SmartDashb
@@ -598,25 +598,25 @@
 6 Boxes%22, 1.
-5
+3
 );%0D%0A%0D%0A%09%09Smar
@@ -1605,55 +1605,8 @@
 x;%0D%0A
-%09%09SmartDashboard.putNumber(%22Box Count%22, box);%0D%0A
 %09%7D%0D%0A
@@ -1745,16 +1745,69 @@
 date()%7B%0D
+%0A%0D%0A%09%09SmartDashboard.putNumber(%22Box Count%22, box);%0D%0A%09%09%0D
 %0A%09%09doubl
@@ -2141,34 +2141,29 @@
 StrafeLimit(
-accelLimit
+999.0
 );%0D%0A%09%09robot.

Update default speed config values