diff --git a/.gitignore b/.gitignore index 8943d2e..0852c56 100644 --- a/.gitignore +++ b/.gitignore @@ -3,3 +3,7 @@ DriveBase/.settings DriveBase/sysProps.xml .metadata .settings/* +Robot2016/.settings +Robot2016/sysProps.xml +FRCUserProgram +/RemoteSystemsTempFiles/ diff --git a/DriveBase/src/Robot.cpp b/DriveBase/src/Robot.cpp index 8b4fca8..6daa5f7 100644 --- a/DriveBase/src/Robot.cpp +++ b/DriveBase/src/Robot.cpp @@ -1,6 +1,14 @@ #include "WPILib.h" //#include "TankDrive.h" #include "Shooter.h" +#include +#include +#include +#include +#include +#include +#include +#include /** @@ -9,6 +17,7 @@ * controlled motor. */ + #ifndef BUTTON_LAYOUT #define BUTTON_LAYOUT @@ -25,6 +34,7 @@ class Robot: public IterativeRobot { CANTalon shooter1; CANTalon shooter2; CANTalon ramp; + TankDrive drive; // Counter ramp_lift; RobotDrive drive; Shooter shooter; @@ -34,6 +44,12 @@ class Robot: public IterativeRobot { // The talon only receives control packets every 10ms. //double kUpdatePeriod = 0.010; + void LogData() + { + static PowerDistributionPanel pdp; + static DriverStation* ds = + } + public: Robot() : r1_drive(1), // right wheel 1 diff --git a/FRCUserProgram b/FRCUserProgram deleted file mode 100755 index bcba85b..0000000 Binary files a/FRCUserProgram and /dev/null differ diff --git a/Makefile b/Makefile deleted file mode 100644 index 4584c5a..0000000 --- a/Makefile +++ /dev/null @@ -1,32 +0,0 @@ -LIBS=wpi -override CFLAGS +=-l$(LIBS) -std=c++14 -TEAM=1786 -SSH_OPTIONS=-q -o UserKnownHostsFile=/dev/null -o StrictHostKeyChecking=no -SSH_SSHPASS=$(shell command -v sshpass >/dev/null 2>&1 && echo -n "sshpass -p ''") - -all: deploy - -build: FRCUserProgram - -FRCUserProgram: robot.cpp - @echo "Building FRCUserProgram" - arm-frc-linux-gnueabi-g++ robot.cpp -o FRCUserProgram $(CFLAGS) - -clean: - rm FRCUserProgram - -deploy: build - @echo "Copying FRCUserProgram" - @ssh $(SSH_OPTIONS) lvuser@10.17.86.20 'rm -f /home/lvuser/FRCUserProgram' - @scp $(SSH_OPTIONS) -o "LogLevel QUIET" FRCUserProgram lvuser@10.17.86.20:/home/lvuser/FRCUserProgram - @echo "Restarting FRCUserProgram" - @$(SSH_SSHPASS) ssh $(SSH_OPTIONS) admin@10.17.86.20 '. /etc/profile.d/natinst-path.sh; /usr/local/frc/bin/frcKillRobot.sh -t -r' - -restart: FRCUserProgram - @echo "Restarting FRCUserProgram" - @$(SSH_SSHPASS) ssh $(SSH_OPTIONS) admin@10.17.86.20 '. /etc/profile.d/natinst-path.sh; /usr/local/frc/bin/frcKillRobot.sh -t -r' - -stop: - @echo "Restarting FRCUserProgram" - @$(SSH_SSHPASS) ssh $(SSH_OPTIONS) admin@10.17.86.20 '. /etc/profile.d/natinst-path.sh; /usr/local/frc/bin/frcKillRobot.sh -t'y - diff --git a/RemoteSystemsTempFiles/.project b/RemoteSystemsTempFiles/.project deleted file mode 100644 index 5447a64..0000000 --- a/RemoteSystemsTempFiles/.project +++ /dev/null @@ -1,12 +0,0 @@ - - - RemoteSystemsTempFiles - - - - - - - org.eclipse.rse.ui.remoteSystemsTempNature - - diff --git a/Robot2016/.settings/org.eclipse.cdt.managedbuilder.core.prefs b/Robot2016/.settings/org.eclipse.cdt.managedbuilder.core.prefs deleted file mode 100644 index a3c3365..0000000 --- a/Robot2016/.settings/org.eclipse.cdt.managedbuilder.core.prefs +++ /dev/null @@ -1,13 +0,0 @@ -eclipse.preferences.version=1 -environment/buildEnvironmentInclude/cdt.managedbuild.config.gnu.cross.exe.debug.1104744751/CPATH/delimiter=; -environment/buildEnvironmentInclude/cdt.managedbuild.config.gnu.cross.exe.debug.1104744751/CPATH/operation=remove -environment/buildEnvironmentInclude/cdt.managedbuild.config.gnu.cross.exe.debug.1104744751/CPLUS_INCLUDE_PATH/delimiter=; -environment/buildEnvironmentInclude/cdt.managedbuild.config.gnu.cross.exe.debug.1104744751/CPLUS_INCLUDE_PATH/operation=remove -environment/buildEnvironmentInclude/cdt.managedbuild.config.gnu.cross.exe.debug.1104744751/C_INCLUDE_PATH/delimiter=; -environment/buildEnvironmentInclude/cdt.managedbuild.config.gnu.cross.exe.debug.1104744751/C_INCLUDE_PATH/operation=remove -environment/buildEnvironmentInclude/cdt.managedbuild.config.gnu.cross.exe.debug.1104744751/append=true -environment/buildEnvironmentInclude/cdt.managedbuild.config.gnu.cross.exe.debug.1104744751/appendContributed=true -environment/buildEnvironmentLibrary/cdt.managedbuild.config.gnu.cross.exe.debug.1104744751/LIBRARY_PATH/delimiter=; -environment/buildEnvironmentLibrary/cdt.managedbuild.config.gnu.cross.exe.debug.1104744751/LIBRARY_PATH/operation=remove -environment/buildEnvironmentLibrary/cdt.managedbuild.config.gnu.cross.exe.debug.1104744751/append=true -environment/buildEnvironmentLibrary/cdt.managedbuild.config.gnu.cross.exe.debug.1104744751/appendContributed=true diff --git a/Robot2016/Makefile b/Robot2016/Makefile new file mode 100644 index 0000000..36efedf --- /dev/null +++ b/Robot2016/Makefile @@ -0,0 +1,35 @@ +LIBS=wpi +CXX=arm-frc-linux-gnueabi-g++ +override CPPFLAGS +=-std=c++14 +LDFLAGS=-l$(LIBS) +TEAM=1786 +SSH_OPTIONS=-o UserKnownHostsFile=/dev/null -o StrictHostKeyChecking=no +SSH_SSHPASS=$(shell command -v sshpass >/dev/null 2>&1 && echo -n "sshpass -p ''") +SSH_HOST=roborio-1786-frc.local + +all: deploy + +build: FRCUserProgram + +FRCUserProgram: src/Robot.cpp + @echo "Building FRCUserProgram" + $(CXX) $(CPPFLAGS) $< -o $@ $(LDFLAGS) + +clean: + rm FRCUserProgram + +deploy: build + @echo "Copying FRCUserProgram" + ssh $(SSH_OPTIONS) lvuser@$(SSH_HOST) 'rm -f /home/lvuser/FRCUserProgram' + scp $(SSH_OPTIONS) -o "LogLevel QUIET" FRCUserProgram lvuser@$(SSH_HOST):/home/lvuser/FRCUserProgram + @echo "Restarting FRCUserProgram" + $(SSH_SSHPASS) ssh $(SSH_OPTIONS) admin@$(SSH_HOST) '. /etc/profile.d/natinst-path.sh; /usr/local/frc/bin/frcKillRobot.sh -t -r' + +restart: FRCUserProgram + echo "Restarting FRCUserProgram" + $(SSH_SSHPASS) ssh $(SSH_OPTIONS) admin@$(SSH_HOST) '. /etc/profile.d/natinst-path.sh; /usr/local/frc/bin/frcKillRobot.sh -t -r' + +stop: + @echo "Restarting FRCUserProgram" + $(SSH_SSHPASS) ssh $(SSH_OPTIONS) admin@$(SSH_HOST) '. /etc/profile.d/natinst-path.sh; /usr/local/frc/bin/frcKillRobot.sh -t'y + diff --git a/Robot2016/src/Robot.cpp b/Robot2016/src/Robot.cpp index d620e6b..2adc847 100644 --- a/Robot2016/src/Robot.cpp +++ b/Robot2016/src/Robot.cpp @@ -1,5 +1,13 @@ #include "WPILib.h" #include "Shooter.h" +#include +#include +#include +#include +#include +#include +#include +#include #include #ifndef BUTTON_LAYOUT @@ -10,6 +18,15 @@ #define RAMP_RAISE 5 // Button 3 for Raising Ramp #define RAMP_LOWER 3 // Button 4 to lower ramp. #define UNJAM 11 +#define GIM_UP 7 +#define GIM_DOWN 9 +#define GIM_ZERO 10 +#define GIM_SHOOT 8 + +#define GIMB_DEFAULT 768 //Default position of the gimble (*1000) +#define GIMB_DELTA 2 //Amount to increment by + +#define ARMS_SCALE 0.75 #define DEADZONE_RADIUS 0.05 // Deadzone Radius prevents tiny twitches in the joystick's value from // affecting the robot. Use this for cleaning up drive train and shooter. @@ -22,8 +39,13 @@ #endif // BUTTON_LAYOUT +#ifndef LOG +#define LOG(X) logA << X; logB << X; logC << X //for writing data to the csv file +#endif + class Robot: public IterativeRobot { +private: TalonSRX left_drive, right_drive; CANTalon shooter1, shooter2, ramp, @@ -31,6 +53,152 @@ class Robot: public IterativeRobot RobotDrive drive; Shooter shooter; Joystick driver_stick, operator_stick; + Timer auto_clock; + Servo gimbs; + int gimba; // angle of the gimble as a value of from 0 to 1000 + + // instance variables + bool pickupRunning; // don't want to spam the Talon with set messages. Toggle the pickup when a button is pressed or released. + bool inverting; + bool ramping; + bool shooting; + bool unjamming; + bool arming; + bool arcade; + float shooter_power; + + enum auto_states + { + START, + DRIVING_FORWARD, + STOP + }; + + auto_states auto_status; + + LiveWindow *lw = LiveWindow::GetInstance(); + SendableChooser *chooser; + const std::string autoNameDefault = "1.7 Seconds (Slightly Longer)"; + const std::string autoNameShort = "1.5 Seconds (Short, good on the moat)"; + const std::string autoNameOne = "1 Seconds"; + const std::string autoNameDisable = "Disable Autonomous (set time to 0)"; + std::string autoSelected; + + void LogCSVData() + { + static PowerDistributionPanel pdp; // preparing to read from the pdp + static std::vector motors; + + static std::ofstream logA, logB, logC; + timeval tm; + + SmartDashboard::PutBoolean("log A", logA.is_open()); + SmartDashboard::PutBoolean("log B", logB.is_open()); + SmartDashboard::PutBoolean("log C", logC.is_open()); + + SmartDashboard::PutBoolean("Ramp Limit F", !ramp.IsFwdLimitSwitchClosed()); + SmartDashboard::PutBoolean("Ramp Limit R", !ramp.IsRevLimitSwitchClosed()); + SmartDashboard::PutBoolean("Arms Limit F", !arms.IsFwdLimitSwitchClosed()); + SmartDashboard::PutBoolean("Arms Limit R", !arms.IsRevLimitSwitchClosed()); + + SmartDashboard::PutNumber("angle of camera", gimba); + + if (!logA.is_open() && !logB.is_open() && !logC.is_open()) + { + std::fstream logNumFile; + int logNum; + logNumFile.open("/home/lvuser/logNum"); + logNumFile >> logNum; + logNum++; + logNumFile.seekp(0); + logNumFile << logNum; + // writing to /home/lvuser/logs/[unixtime].log + logA.open("/media/sda1/logs/log" + std::to_string(logNum) + ".csv"); + std::cerr << (logA.is_open() ? "Opened" : "Failed to open") << "log A." << std::endl; + logB.open("/media/sdb1/logs/log" + std::to_string(logNum) + ".csv"); + std::cerr << (logB.is_open() ? "Opened" : "Failed to open") << "log B." << std::endl; + logC.open("/home/lvuser/logs/log" + std::to_string(logNum) + ".csv"); + std::cerr << (logC.is_open() ? "Opened" : "Failed to open") << "log C." << std::endl; + + LOG("Time\tpdpInput voltage\tpdpTemperature\tpdpTotal Current\t"); + for (int ii = 0; ii < 16; ii++) + { + LOG("pdpChannel " << ii << " current\t"); + } + + LOG("tlaunch_spinny Bus Voltage\ttlaunch_spinny Output Current\ttlaunch_spinny Output Voltage\ttlaunch_spinny Temperature"); + motors.push_back(&ramp); + LOG("\tShooter1 Bus Voltage\tShooter1 Output Current\tShooter1 Output Voltage\tShooter1 Temperature"); + motors.push_back(&shooter1); + LOG("\tShooter2 Bus Voltage\tShooter2 Output Current\tShooter2 Output Voltage\tShooter2 Temperature"); + motors.push_back(&shooter2); + + LOG("\tJoystick X\tJoystick Y\tJoystick Twist"); + LOG("\tAlliance\tLocation\tMatch Time\tFMS Attached\tBrowned Out"); + LOG("\tTestStage"); + LOG(std::endl); + } + gettimeofday(&tm, NULL); + LOG(time(0) << '.' << std::setfill('0') << std::setw(3) << tm.tv_usec/1000); + // Some general information + LOG("\t" << pdp.GetVoltage()); + LOG("\t" << pdp.GetTemperature()); + LOG("\t" << pdp.GetTotalCurrent()); + // current on each channel + for (int ii = 0; ii < 16; ii++) + { + LOG("\t" << pdp.GetCurrent(ii)); + } + + //Talon Data + for(int ii = 0; ii < motors.size(); ii++) + { + LOG("\t" << motors[ii]->GetBusVoltage()); + LOG("\t" << motors[ii]->GetOutputVoltage()); + LOG("\t" << motors[ii]->GetOutputCurrent()); + LOG("\t" << motors[ii]->GetTemperature()); + } + //control data + LOG("\t" << driver_stick.GetX()); + LOG("\t" << driver_stick.GetY()); + LOG("\t" << driver_stick.GetTwist()); + + //DriverStation Data + LOG("\t" << DriverStation::GetInstance().GetAlliance()); + LOG("\t" << DriverStation::GetInstance().GetLocation()); + LOG("\t" << DriverStation::GetInstance().GetMatchTime()); + LOG("\t" << DriverStation::GetInstance().IsFMSAttached()); + LOG("\t" << DriverStation::GetInstance().IsSysBrownedOut()); + LOG(std::endl); + } + + /** + * Takes the gross raw throttle input from joystick and returns a + * value between 0.0-1.0 (no negative values) + */ + float SaneThrottle(float rawThrottle) + { + return ((1.0 - rawThrottle) / 2.0); + } + + void UpdateDrive() + { + float x = -driver_stick.GetX(); + float y = -driver_stick.GetY(); + if (x > 0) + { + float right = y * SaneThrottle(driver_stick.GetThrottle()); + float left = (1-x)*y * SaneThrottle(driver_stick.GetThrottle()); + drive.TankDrive(left, right); + } + else + { + float left = y * SaneThrottle(driver_stick.GetThrottle()); + float right = (1+x)*y * SaneThrottle(driver_stick.GetThrottle()); + drive.TankDrive(left, right); + } + } + public: Robot(): left_drive(0), // Left DriveTrain Talons plug into PWM channel 1 with a Y-splitter @@ -43,33 +211,21 @@ public: shooter( // initialize Shooter object. &shooter1, &shooter2, &ramp), driver_stick(0), // right stick (operator) - operator_stick(1) // left stick (driver) + operator_stick(1), // left stick (driver) + gimbs(3), + gimba(GIMB_DEFAULT) { } -private: - // instance variables - bool pickupRunning; // don't want to spam the Talon with set messages. Toggle the pickup when a button is pressed or released. - bool inverting; - bool ramping; - bool shooting; - bool unjamming; - bool arming; - bool arcade; - float shooter_power; - - LiveWindow *lw = LiveWindow::GetInstance(); - SendableChooser *chooser; - const std::string autoNameDefault = "Default"; - const std::string autoNameCustom = "My Auto"; - std::string autoSelected; - void RobotInit() { chooser = new SendableChooser(); + chooser->AddDefault(autoNameDefault, (void*)&autoNameDefault); - chooser->AddObject(autoNameCustom, (void*)&autoNameCustom); + chooser->AddObject(autoNameShort, (void*)&autoNameShort); + chooser->AddObject(autoNameOne, (void*)&autoNameOne); + chooser->AddObject(autoNameDisable, (void*)&autoNameDisable); SmartDashboard::PutData("Auto Modes", chooser); shooter1.Enable(); shooter2.Enable(); @@ -85,34 +241,59 @@ private: shooter_power = 0; arcade = false; + // Initialize the number so we can get it later + SmartDashboard::PutNumber("shooting angle", GIMB_DEFAULT); + } - - /** - * This autonomous (along with the chooser code above) shows how to select between different autonomous modes - * using the dashboard. The sendable chooser code works with the Java SmartDashboard. If you prefer the LabVIEW - * Dashboard, remove all of the chooser code and uncomment the GetString line to get the auto name from the text box - * below the Gyro - * - * You can add additional auto modes by adding additional comparisons to the if-else structure below with additional strings. - * If using the SendableChooser make sure to add them to the chooser code above as well. - */ void AutonomousInit() { autoSelected = *((std::string*)chooser->GetSelected()); //std::string autoSelected = SmartDashboard::GetString("Auto Selector", autoNameDefault); std::cout << "Auto selected: " << autoSelected << std::endl; - if(autoSelected == autoNameCustom){ - //Custom Auto goes here - } else { - //Default Auto goes here - } + auto_status = START; } void AutonomousPeriodic() { + const float drivePower = 1; + float driveTime = 1.7; + LogCSVData(); + if (autoSelected == autoNameDisable) driveTime = 0; + else if (autoSelected == autoNameShort) driveTime = 1.5; + else if (autoSelected == autoNameOne) driveTime = 1; + else driveTime = 1.7; + //Default Auto goes here + switch (auto_status) + { + case START: + { + auto_clock.Start(); + auto_status = DRIVING_FORWARD; + break; + } + case DRIVING_FORWARD: + { + if (auto_clock.Get() > driveTime) + { + drive.TankDrive(0.0, 0.0); + auto_status = STOP; + } + else + { + drive.TankDrive(drivePower, drivePower); + } + break; + } + case STOP: + { + std::cout << "All done!\n" ; + break; + } + } + gimbs.Set(gimba/1000.0); } void TeleopInit() @@ -122,9 +303,10 @@ private: void TeleopPeriodic() { + LogCSVData(); std::cout << "arm encoder position: " << arms.GetEncPosition() << std::endl; std::cout << "arm encoder velocity: " << arms.GetEncVel() << std::endl; - drive.ArcadeDrive(-driver_stick.GetY(), -driver_stick.GetX()*0.75); + if(driver_stick.GetRawButton(7)) { arcade = true; @@ -171,11 +353,9 @@ private: shooter.BoostRamp(); ramping = true; }*/ - else if(ramping - && !operator_stick.GetRawButton(RAMP_LOWER) - && !operator_stick.GetRawButton(RAMP_RAISE)) + else if(ramping && !operator_stick.GetRawButton(RAMP_RAISE) + && !operator_stick.GetRawButton(RAMP_LOWER)) { - std::cout << "Stopping Ramp."; shooter.StopRamp(); ramping = false; } @@ -196,6 +376,23 @@ private: shooter.PickUp(false); unjamming = false; } + + /* + * This is for controlling the gimbal. + */ + if(operator_stick.GetRawButton(GIM_UP)) + { + gimba += GIMB_DELTA; + } + else if(operator_stick.GetRawButton(GIM_DOWN)){ + gimba -= GIMB_DELTA; + } + else if(operator_stick.GetRawButton(GIM_SHOOT)) + { + gimba = SmartDashboard::GetNumber("shooting angle", GIMB_DEFAULT); + } + gimbs.Set(gimba/1000.0); + /* * Run the Shooter only while the THUMB button is held down on the operator stick. * the 'pickupRunning' boolean is there to prevent the shooter from calling PickUp @@ -213,7 +410,6 @@ private: shooter.PickUp(false); pickupRunning = false; } - /* * The 'inverting' variable is used to make sure that the drive train isn't getting * inverted every iteration of the TeleopPeriodic method while the button is held down. @@ -231,101 +427,170 @@ private: { inverting = false; } - - - /* - * Unlike the previous actions, this method does need to be called every iteration of - * TeleopPeriodic. This is because the logic running this operation needs to be checked - * Every time the method is called. This cannot be a loop in the Shoot method because - * that would lock the robot every time the trigger is hit. - */ - /*if(operator_stick.GetRawButton(TRIGGER)) + + if(operator_stick.GetRawButton(4)) { - shooting = true; - shooter.Shoot(); + arms.Set(-operator_stick.GetY() * ARMS_SCALE); } - else if(shooting && !operator_stick.GetRawButton(TRIGGER)) + else { - shooting = false; - shooter.StopShooter(); - }*/ - - if(!arming && driver_stick.GetRawButton(RAMP_RAISE)) - { - arming = true; - arms.Set(0.5); - } - else if(!arming && driver_stick.GetRawButton(RAMP_LOWER)) - { - arming = true; - arms.Set(-0.5); - } - else if(arming && !driver_stick.GetRawButton(RAMP_RAISE) && !driver_stick.GetRawButton(RAMP_LOWER)) - { - arming = false; arms.Set(0); } // This code will become obsolete after the Shooter logic is complete. float opThrottle = SaneThrottle(operator_stick.GetThrottle()); - if(!pickupRunning && !unjamming) + if(!pickupRunning && ( opThrottle > shooter_power + DEADZONE_RADIUS + || opThrottle < shooter_power - DEADZONE_RADIUS)) { shooter.SetPower(opThrottle); } } - /** - * Takes the gross raw throttle input from joystick and returns a - * value between 0.0-1.0 (no negative values) - */ - float SaneThrottle(float rawThrottle) - { - return ((1.0 - rawThrottle) / 2.0); - } - void TestPeriodic() { - lw->Run(); - } + const int driveTime = 5; //Drive for 5 seconds + const float drivePower = 0.5, armPower=.35, rampPower=.4; //Arbitrary powers to test at + static Timer t; + enum test_stages { + INIT, + ARMS_UP, + ARMS_DOWN, + RAMP_UP, + RAMP_DOWN, + DRIVE_FORWARD, + DRIVE_BACKWARD, + TURN_CW, + TURN_CCW, + END}; - void SimpleDrive() - { - float x = -driver_stick.GetX(); - float y = -driver_stick.GetY(); - float left = 0; - float right = 0; + static enum test_stages test_stage, old_test_stage; - if (x > 0) + if (old_test_stage != test_stage) { - right = y; - left = (1- x*TURN_FACTOR)*y ; + //Reset timer between stages + t.Stop(); + t.Reset(); + + //Wait for a button press between stages + if (operator_stick.GetRawButton(TRIGGER)) + old_test_stage = test_stage; } else { - left = y; - right = (1+x*TURN_FACTOR)*y; + if(!t.Get()) t.Start(); + + switch (test_stage) + { + case INIT: + { + break; + } + + case ARMS_UP: + { + if (arms.GetForwardLimitOK()) + arms.Set(1); + else + { + arms.Set(0); + test_stage = ARMS_DOWN; + } + break; + } + + case ARMS_DOWN: + { + if (arms.GetReverseLimitOK()) + arms.Set(-1); + else + { + arms.Set(0); + test_stage = RAMP_UP; + } + break; + } + + case RAMP_UP: + { + if (arms.GetForwardLimitOK()) + ramp.Set(1); + else + { + ramp.Set(0); + test_stage = RAMP_DOWN; + } + break; + } + + case RAMP_DOWN: + { + if (arms.GetReverseLimitOK()) + ramp.Set(-1); + else + { + ramp.Set(0); + test_stage = DRIVE_FORWARD; + } + break; + } + + case DRIVE_FORWARD: + { + if (t.Get() < driveTime) + drive.TankDrive(drivePower, drivePower); + else + { + drive.TankDrive(0.0, 0.0); + test_stage = DRIVE_BACKWARD; + } + break; + } + + case DRIVE_BACKWARD: + { + if (t.Get() < driveTime) + drive.TankDrive(-drivePower, -drivePower); + else + { + drive.TankDrive(0.0, 0.0); + test_stage = TURN_CW; + } + break; + } + + case TURN_CW: + { + if (t.Get() < driveTime) + drive.TankDrive(drivePower, -drivePower); + else + { + drive.TankDrive(0.0, 0.0); + test_stage = TURN_CCW; + } + break; + } + + case TURN_CCW: + { + if (t.Get() < driveTime) + drive.TankDrive(-drivePower, drivePower); + else + { + drive.TankDrive(0.0, 0.0); + test_stage = END; + } + break; + } + + case END: + { + break; + } + } } - drive.TankDrive(left, right); - } - - void UpdateDrive() - { - float x = -driver_stick.GetX(); - float y = -driver_stick.GetY(); - if (x > 0) - { - float right = y * SaneThrottle(driver_stick.GetThrottle()); - float left = (1-x)*y * SaneThrottle(driver_stick.GetThrottle()); - drive.TankDrive(left, right); - } - else - { - float left = y * SaneThrottle(driver_stick.GetThrottle()); - float right = (1+x)*y * SaneThrottle(driver_stick.GetThrottle()); - drive.TankDrive(left, right); - } + LogCSVData(); } }; diff --git a/Robot2016/src/Shooter.h b/Robot2016/src/Shooter.h index 82e4ace..8339baf 100644 --- a/Robot2016/src/Shooter.h +++ b/Robot2016/src/Shooter.h @@ -12,6 +12,7 @@ #define LAUNCH_POWER 1 #define SPINUP_TIME 1.5 // seconds. #define LAUNCH_TIME 0.5 +#define RAMP_POWER 0.3 class Shooter { @@ -64,7 +65,7 @@ public: void LowerRamp() { - ramp->Set(-0.5); + ramp->Set(-RAMP_POWER); if(ramp->Limits::kReverseLimit){ SmartDashboard::PutNumber("ramp", 2); //going to put a circlar dial to show where the ramp could be } else { @@ -74,7 +75,7 @@ public: void RaiseRamp() { - ramp->Set(0.5); + ramp->Set(RAMP_POWER); if(ramp->Limits::kForwardLimit){ SmartDashboard::PutNumber("ramp", 0); //going to put a circlar dial to show where the ramp could be } else { @@ -88,21 +89,18 @@ public: } void Shoot() { - // TODO: Shooter Logic should go as follows: - - /* - Assuming a ball is held in the shooter. When the trigger is pulled, - the launch-spinny should be STOPPED. Start a Timer object counting - When... too hard to write.. I emailed you a flow chart. - */ + if (shotClock.Get() > (SPINUP_TIME + 0.1)) + { + state = RESETTING; + } switch (state) { case READY: { state = SPINNINGUP; ramp->Set(-1); - launcher->Set(PICKUP_POWER); - pickup->Set(PICKUP_POWER); + launcher->Set(LAUNCH_POWER); + pickup->Set(LAUNCH_POWER); shotClock.Reset(); shotClock.Start(); break; @@ -116,13 +114,13 @@ public: shotClock.Start(); } else { - std::cout << "*Goku noises*\n"; + //std::cout << "*Goku noises*\n"; } break; } case LAUNCH: { - ramp->Set(1); + ramp->Set(LAUNCH_POWER); state = LAUNCHING; break; } @@ -155,8 +153,7 @@ public: void PickUp(bool state = true) { pickup->Set((float) (state * PICKUP_POWER)); - launcher->Set((float) (state * PICKUP_POWER * -0.75)); - //ramp->Set(-1.0*PICKUP_POWER); + launcher->Set((float) (state * PICKUP_POWER * -1)); //std::cout << "picking up!\n"; } @@ -165,7 +162,7 @@ public: */ void Unjam() { - pickup->Set(PICKUP_POWER * -0.75); + pickup->Set(-1 * PICKUP_POWER); } void ShootLow() @@ -179,6 +176,11 @@ public: launcher->Set(power); //std::cout << "setting shooter power" << std::endl; } + + int GetState() + { + return state; + } private: //RobotDrive *shooterDrive; diff --git a/robot.cpp b/robot.cpp deleted file mode 100644 index 9565107..0000000 --- a/robot.cpp +++ /dev/null @@ -1,41 +0,0 @@ -#include "WPILib.h" - -/** - * This is a demo program showing the use of the RobotDrive class. - * The SampleRobot class is the base of a robot application that will automatically call your - * Autonomous and OperatorControl methods at the right time as controlled by the switches on - * the driver station or the field controls. - * - * WARNING: While it may look like a good choice to use for your code if you're inexperienced, - * don't. Unless you know what you are doing, complex code will be much more difficult under - * this system. Use IterativeRobot or Command-Based instead if you're new. - */ -class Robot: public SampleRobot -{ - RobotDrive myRobot; // robot drive system - Joystick stick; // only joystick - -public: - Robot() : - myRobot(0, 1), // initialize the RobotDrive to use motor controllers on ports 0 and 1 - stick(0) - { - myRobot.SetExpiration(0.1); - } - - /** - * Runs the motors with arcade steering. - */ - void OperatorControl() - { - while (IsOperatorControl() && IsEnabled()) - { - myRobot.ArcadeDrive(stick); // drive with arcade style (use right stick) - Wait(0.005); // wait for a motor update time - } - } - -}; - -START_ROBOT_CLASS(Robot) -