package com.team254.frc2015.sim;

import static org.junit.Assert.*;

import org.junit.Test;

public class CanGrabber {

    @Test
    public void test() {
        DCMotor motor = DCMotor.makeTransmission(DCMotor.makeRS550(), 2, 80.0,
                .8);
        double total_time = 0.0;
        double arm_com = 1.0;
        double arm_mass = .3;
        double constant_spring_assist_torque = 0.0; // torque provided through
                                                     // the whole range of
                                                     // motion
        double over_center_spring_assist_torque = 100.0; // torque provided
                                                         // proportional to sin
                                                         // of angle (direction
                                                         // of gravity)
        // lower
        while (motor.getPosition() < Math.PI / 2 && total_time < 1.0) {
            double gravity_torque = 9.8 * arm_com * arm_mass
                    * Math.sin(motor.getPosition());
            motor.step(
                    12.0,
                    .4,
                    gravity_torque + constant_spring_assist_torque
                            + over_center_spring_assist_torque
                            * Math.sin(motor.getPosition()), 0.001);
            total_time += .001;
        }
        System.out.println("Time: " + total_time + ", Angle: "
                + motor.getPosition());

        // raise
        total_time = 0.0;
        while (motor.getPosition() > 0 && total_time < 1.0) {
            double gravity_torque = 9.8 * arm_com * arm_mass
                    * Math.sin(motor.getPosition());
            motor.step(
                    -12.0,
                    .4,
                    gravity_torque + constant_spring_assist_torque
                            + over_center_spring_assist_torque
                            * Math.sin(motor.getPosition()), 0.001);
            total_time += .001;
        }
        if (total_time >= 1.0) {
            System.out.println("CAN NOT RAISE ARM!");
        }
        System.out.println("Time: " + total_time + ", Angle: "
                + motor.getPosition());
    }
}

@@ -215,11 +215,11 @@
 keRS
-550
+775
 (), 
@@ -341,16 +341,50 @@
 s = .3;%0A
+        double battery_r = 0.018;%0A
         
@@ -646,10 +646,8 @@
 e = 
-10
 0.0;
@@ -1093,32 +1093,101 @@
 getPosition());%0A
+            double current = motor.getCurrent() * 2; // Two peacocks%0A
             moto
@@ -1214,24 +1214,46 @@
         12.0
+ - current * battery_r
 ,%0A          
@@ -1453,32 +1453,33 @@
 osition()), 0.00
+0
 1);%0A            
@@ -1485,32 +1485,33 @@
  total_time += .
+0
 001;%0A        %7D%0A 
@@ -1824,32 +1824,101 @@
 getPosition());%0A
+            double current = motor.getCurrent() * 2; // Two peacocks%0A
             moto
@@ -1950,16 +1950,38 @@
    -12.0
+ + current * battery_r
 ,%0A      
@@ -2193,16 +2193,17 @@
 )), 0.00
+0
 1);%0A    
@@ -2225,16 +2225,17 @@
 ime += .
+0
 001;%0A   

Tweak peacock model