// RobotBuilder Version: 2.0
//
// This file was generated by RobotBuilder. It contains sections of
// code that are automatically generated and assigned by robotbuilder.
// These sections will be updated in the future when you export to
// Java from RobotBuilder. Do not put any code or make any change in
// the blocks indicating autogenerated code or it will be lost on an
// update. Deleting the comments indicating the section will prevent
// it from being updated in the future.


package org.usfirst.frc157.ProtoBot2017.subsystems;

import org.usfirst.frc157.ProtoBot2017.RobotMap; 
import com.ctre.CANTalon;

import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.DigitalInput;


/**
 *
 */
public class Helix extends Subsystem {

//    private final CANTalon helixMotorRight = RobotMap.helixMotorRight;
//    private final CANTalon helixMotorLeft = RobotMap.helixMotorLeft;
    private double motorSpeed = 0.5;

    // Put methods for controlling this subsystem
    // here. Call these from Commands.

    private String name;
    private CANTalon motor;
    private MotorSense motorInverted;
    
    public enum MotorSense
    {
    	NORMAL,
    	INVERTED
    }
    
    public enum helixCommand
    {
        LOAD,
        IDLE,
        UNLOAD
    }
       

//    public enum helixCommand
//    {
//        LOADRIGHT,
//        LOADLEFT,
//        IDLERIGHT,
//        IDLELEFT
//    }

    public Helix(String name, CANTalon motor)
    {
    	this(name, motor, MotorSense.NORMAL);
    }

    public Helix(String name, CANTalon motor, MotorSense motorInverted)
    {
    	this.name = name;
    	this.motor = motor;
    	this.motorInverted = motorInverted;
    	if(this.motorInverted == MotorSense.INVERTED)
    	{
    		motorSpeed = -1.0 * motorSpeed; // if inverted flip motor speed
    	}
        System.out.println(name + " helix created");
        
        motor.changeControlMode(CANTalon.TalonControlMode.PercentVbus);
    }        

    public String getName()
    {
    	return name;
    }
    
//    public Helix()
//    {
//        System.out.println(name + "helix");
//        
//        helixMotorRight.changeControlMode(CANTalon.TalonControlMode.PercentVbus);
//        helixMotorLeft.changeControlMode(CANTalon.TalonControlMode.PercentVbus);
//     }        

    public void load()
    {
    	System.out.println("Load " + name + " helix");
    	motor.set(motorSpeed);
    }

    public void unload()
    {
    	System.out.println("Unoad " + name + " helix");
    	motor.set(-motorSpeed);
    }

    public void idle()
    {
    	System.out.println("Idle " + name + " helix");
    	motor.set(0.0);
    }

    public void operation(helixCommand operation)
    {
    	switch(operation)
    	{
    	case LOAD:
    		load();
    		break;
    	case UNLOAD:
    		unload();
    		break;
    	case IDLE:
    		idle();
    		break;
    	}
    }
    
    // Right Motor+
    
//    public void loadRight()
//    {
//        System.out.println("going up right");
//        
//        helixMotorRight.set(motorSpeed);
//    }
//    public void loadLeft()
//    {
//        helixMotorLeft.set(-motorSpeed);
//    }
//    public void unloadRight()
//    {
//        helixMotorRight.set(-motorSpeed);
//    }
//    public void unloadLeft()
//    {
//        helixMotorLeft.set(motorSpeed);
//    }
//    public void idleRight()
//    {
//        helixMotorRight.set(0.0);        
//    }
//    public void idleLeft()
//    {
//        helixMotorLeft.set(0.0);
//    }
    
    
    @Override
    protected void initDefaultCommand()
    {
        // TODO Auto-generated method stub
        
    }
}
    

@@ -1181,16 +1181,26 @@
 INVERTED
+,%0A    %09OFF
 %0A    %7D%0A 
@@ -1825,24 +1825,119 @@
 peed%0A    %09%7D%0A
+    %09else if (this.motorInverted == MotorSense.INVERTED)%0A    %09%7B%0A    %09%09motorSpeed = 0.0;%0A    %09%7D%0A
         Syst

Add Helix OFF to support a non-operating/non-present helix