// 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
// C++ 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.



#include "Commands/TimedShoot.h"
#include "commands/ShooterDefault.h"
#include "Robot.h"

// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTOR

TimedShoot::TimedShoot(double timeout) : frc::TimedCommand(timeout) {
    ShootSpeed = 2000;
    // Use requires() here to declare subsystem dependencies
    // eg. requires(Robot::chassis.get());
    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=REQUIRES
    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=REQUIRES
}

    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTOR
TimedShoot::TimedShoot(double timeout, double speed) : frc::TimedCommand(timeout) {
    ShootSpeed = speed;
    Requires(Robot::shooter.get());
    
}
// Called just before this Command runs the first time
void TimedShoot::Initialize() {
    Robot::shooter->setLeds(frc::Color::kGreen);
}

// Called repeatedly when this Command is scheduled to run
void TimedShoot::Execute() {
    //Robot::shooter->shooterLeft->Set(ShootSpeed);
    //Robot::shooter->shooterRight->Set(ShootSpeed * -1);
    Robot::shooter->shooterLeft->Set(ControlMode::Velocity, Robot::shooter->rpmToDeltaPer100ms(ShootSpeed));

}

// Called once after command times out
void TimedShoot::End() {
    Robot::shooter->shooterLeft->Set(ControlMode::PercentOutput, 0.0);
    Robot::shooter->setLeds(frc::Color::kRed);


}

// Called when another command which requires one or more of the same
// subsystems is scheduled to run
void TimedShoot::Interrupted() {
    Robot::shooter->shooterLeft->Set(ControlMode::PercentOutput, 0.0);
    Robot::shooter->setLeds(frc::Color::kRed);
}
