Example #1
0
        public SmartDrive()
        {
            turnPID = new SWave_PID(Constants.Drive_TurnP, 0, Constants.Drive_TurnD, -0.5, 0.5);
            frontBackPID = new SWave_PID(Constants.Drive_AlignBackP, 0, Constants.Drive_AlignBackD, -0.25, 0.25);
            sidePID = new SWave_PID(Constants.Drive_AlignSideP, 0, Constants.Drive_AlignSideD, -0.5, 0.5);
            drive = new Drive();
            frontBack = new SWave_AnalogueUltrasonic(Constants.ChannelAnalogue_BackUltrasonic, Constants.UltraScaling);
            side = new SWave_AnalogueUltrasonic(Constants.ChannelAnalogue_SideUltrasonic, Constants.UltraScaling);
            gyro = new Gyro(Constants.ChannelAnalogue_Gyro);
            rotateTrigger = new SWave_EdgeTrigger(true, true);
            fieldCentricToggle = new SWave_Toggle();

            DriveSpeeds = new point(0, 0);
            TurnSetpoint = 0; Rotation = 0;
            FieldCentric = true; StrafeBackButton = false; StrafeLeftButton = false; StrafeForwardButton = false; StrafeRightButton = false;
            AlignNoodle = false; AlignLoad = false; AlignOutput = false;
        }
        /// <summary>
        /// Instantiates the Motor COntrollers, Encoders, Gyroscope.
        /// </summary>
        public Drive()
        {
            #region Instantiating
            L1 = new RampMotor<Talon>(Constants.Drive_L1Channel);
            L2 = new RampMotor<Talon>(Constants.Drive_L2Channel);
            L3 = new RampMotor<Talon>(Constants.Drive_L3Channel);
            R1 = new RampMotor<Talon>(Constants.Drive_R1Channel);
            R2 = new RampMotor<Talon>(Constants.Drive_R2Channel);
            R3 = new RampMotor<Talon>(Constants.Drive_R3Channel);

            LShift = new Solenoid(Constants.Drive_LShiftChannel);
            RShift = new Solenoid(Constants.Drive_RShiftChannel);

            LEncode = new Encoder(Constants.Drive_LEncoderAChannel, Constants.Drive_LEncoderBChannel);
            REncode = new Encoder(Constants.Drive_REncoderAChannel, Constants.Drive_REncoderBChannel);

            LSpeedFilter = new InputFilter(0);
            RSpeedFilter = new InputFilter(0);

            gyro = new Gyro(Constants.Drive_GyroChannel);
            #endregion

            //Invert the Right Side Motors and Encoder
            R1.Inverted = true;
            R2.Inverted = true;
            R3.Inverted = true;

            REncode.ReverseDirection = true;

            L1.MaxChange = Constants.Drive_MaxPowerChange;
            L2.MaxChange = Constants.Drive_MaxPowerChange;
            L3.MaxChange = Constants.Drive_MaxPowerChange;
            R1.MaxChange = Constants.Drive_MaxPowerChange;
            R2.MaxChange = Constants.Drive_MaxPowerChange;
            R3.MaxChange = Constants.Drive_MaxPowerChange;

            gyro.Reset();

            LShift.Set(false);
            RShift.Set(false);

            //Secondary Motors are set to the same max acceleration as the primary motors by default, until we get these numbers tuned.
            shiftingMotorValue = Constants.Drive_MaxPowerChange;
        }
 private static void AddGyro(string key, Gyro gyro)
 {
     GyroKeys.Add(key);
     Gyros.Add(key, gyro);
 }
        private static void InitializeDriveTrain()
        {
            FrontRight = new CANJaguar(22);
            FrontLeft = new CANJaguar(21);
            RearRight = new CANJaguar(23);
            RearLeft = new CANJaguar(20);

            DriveTrain = new List<CANJaguar>
            {
                FrontRight,
                FrontLeft,
                RearRight,
                RearLeft
            };

            PivotGyro = new Gyro(0);
            PitchGyro = new Gyro(1);
            PivotGyro.Sensitivity = 0.007;
            PitchGyro.Sensitivity = 0.007;

            Gyros = new List<Gyro>
            {
                PivotGyro,
                PitchGyro
            };
        }