adam/FRC2016-old
Archived
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge branch 'release'

Browse Source
This commit is contained in:
Adam Goldsmith 2016-03-15 21:30:38 -04:00
commit f1d7d74f8f
10 changed files with 449 additions and 225 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
.gitignore vendored
View File

@ -3,3 +3,7 @@ DriveBase/.settings
DriveBase/sysProps.xml DriveBase/sysProps.xml
.metadata .metadata
.settings/* .settings/*
Robot2016/.settings
Robot2016/sysProps.xml
FRCUserProgram
/RemoteSystemsTempFiles/

16
DriveBase/src/Robot.cpp
View File

@ -1,6 +1,14 @@
#include "WPILib.h" #include "WPILib.h"
//#include "TankDrive.h" //#include "TankDrive.h"
#include "Shooter.h" #include "Shooter.h"
#include <ctime>
#include <iostream>
#include <iomanip>
#include <fstream>
#include <string>
#include <sys/time.h>
#include <vector>
#include <cmath>
/** /**
@ -9,6 +17,7 @@
* controlled motor. * controlled motor.
*/ */
#ifndef BUTTON_LAYOUT #ifndef BUTTON_LAYOUT
#define BUTTON_LAYOUT #define BUTTON_LAYOUT
@ -25,6 +34,7 @@ class Robot: public IterativeRobot {
CANTalon shooter1; CANTalon shooter1;
CANTalon shooter2; CANTalon shooter2;
CANTalon ramp; CANTalon ramp;
TankDrive drive;
// Counter ramp_lift; // Counter ramp_lift;
RobotDrive drive; RobotDrive drive;
Shooter shooter; Shooter shooter;
@ -34,6 +44,12 @@ class Robot: public IterativeRobot {
// The talon only receives control packets every 10ms. // The talon only receives control packets every 10ms.
//double kUpdatePeriod = 0.010; //double kUpdatePeriod = 0.010;
void LogData()
{
static PowerDistributionPanel pdp;
static DriverStation* ds =
}
public: public:
Robot() : Robot() :
r1_drive(1), // right wheel 1 r1_drive(1), // right wheel 1

BIN
FRCUserProgram
View File

Binary file not shown.

32
Makefile
View File

@ -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

12
RemoteSystemsTempFiles/.project
View File

@ -1,12 +0,0 @@
<?xml version="1.0" encoding="UTF-8"?>
<projectDescription>
<name>RemoteSystemsTempFiles</name>
<comment></comment>
<projects>
</projects>
<buildSpec>
</buildSpec>
<natures>
<nature>org.eclipse.rse.ui.remoteSystemsTempNature</nature>
</natures>
</projectDescription>

13
Robot2016/.settings/org.eclipse.cdt.managedbuilder.core.prefs
View File

@ -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

35
Robot2016/Makefile Normal file
View File

@ -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

487
Robot2016/src/Robot.cpp
View File

@ -1,5 +1,13 @@
#include "WPILib.h" #include "WPILib.h"
#include "Shooter.h" #include "Shooter.h"
#include <ctime>
#include <iostream>
#include <iomanip>
#include <fstream>
#include <string>
#include <sys/time.h>
#include <vector>
#include <cmath>
#include <math.h> #include <math.h>
#ifndef BUTTON_LAYOUT #ifndef BUTTON_LAYOUT
@ -10,6 +18,15 @@
#define RAMP_RAISE 5 // Button 3 for Raising Ramp #define RAMP_RAISE 5 // Button 3 for Raising Ramp
#define RAMP_LOWER 3 // Button 4 to lower ramp. #define RAMP_LOWER 3 // Button 4 to lower ramp.
#define UNJAM 11 #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 #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. // affecting the robot. Use this for cleaning up drive train and shooter.
@ -22,8 +39,13 @@
#endif // BUTTON_LAYOUT #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 class Robot: public IterativeRobot
{ {
private:
TalonSRX left_drive, right_drive; TalonSRX left_drive, right_drive;
CANTalon shooter1, shooter2, CANTalon shooter1, shooter2,
ramp, ramp,
@ -31,6 +53,152 @@ class Robot: public IterativeRobot
RobotDrive drive; RobotDrive drive;
Shooter shooter; Shooter shooter;
Joystick driver_stick, operator_stick; 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<CANTalon*> 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: public:
Robot(): Robot():
left_drive(0), // Left DriveTrain Talons plug into PWM channel 1 with a Y-splitter left_drive(0), // Left DriveTrain Talons plug into PWM channel 1 with a Y-splitter
@ -43,33 +211,21 @@ public:
shooter( // initialize Shooter object. shooter( // initialize Shooter object.
&shooter1, &shooter2, &ramp), &shooter1, &shooter2, &ramp),
driver_stick(0), // right stick (operator) 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() void RobotInit()
{ {
chooser = new SendableChooser(); chooser = new SendableChooser();
chooser->AddDefault(autoNameDefault, (void*)&autoNameDefault); 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); SmartDashboard::PutData("Auto Modes", chooser);
shooter1.Enable(); shooter1.Enable();
shooter2.Enable(); shooter2.Enable();
@ -85,34 +241,59 @@ private:
shooter_power = 0; shooter_power = 0;
arcade = false; 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() void AutonomousInit()
{ {
autoSelected = *((std::string*)chooser->GetSelected()); autoSelected = *((std::string*)chooser->GetSelected());
//std::string autoSelected = SmartDashboard::GetString("Auto Selector", autoNameDefault); //std::string autoSelected = SmartDashboard::GetString("Auto Selector", autoNameDefault);
std::cout << "Auto selected: " << autoSelected << std::endl; std::cout << "Auto selected: " << autoSelected << std::endl;
if(autoSelected == autoNameCustom){ auto_status = START;
//Custom Auto goes here
} else {
//Default Auto goes here
}
} }
void AutonomousPeriodic() 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() void TeleopInit()
@ -122,9 +303,10 @@ private:
void TeleopPeriodic() void TeleopPeriodic()
{ {
LogCSVData();
std::cout << "arm encoder position: " << arms.GetEncPosition() << std::endl; std::cout << "arm encoder position: " << arms.GetEncPosition() << std::endl;
std::cout << "arm encoder velocity: " << arms.GetEncVel() << 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)) if(driver_stick.GetRawButton(7))
{ {
arcade = true; arcade = true;
@ -171,11 +353,9 @@ private:
shooter.BoostRamp(); shooter.BoostRamp();
ramping = true; ramping = true;
}*/ }*/
else if(ramping else if(ramping && !operator_stick.GetRawButton(RAMP_RAISE)
&& !operator_stick.GetRawButton(RAMP_LOWER) && !operator_stick.GetRawButton(RAMP_LOWER))
&& !operator_stick.GetRawButton(RAMP_RAISE))
{ {
std::cout << "Stopping Ramp.";
shooter.StopRamp(); shooter.StopRamp();
ramping = false; ramping = false;
} }
@ -196,6 +376,23 @@ private:
shooter.PickUp(false); shooter.PickUp(false);
unjamming = 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. * 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 * the 'pickupRunning' boolean is there to prevent the shooter from calling PickUp
@ -213,7 +410,6 @@ private:
shooter.PickUp(false); shooter.PickUp(false);
pickupRunning = false; pickupRunning = false;
} }
/* /*
* The 'inverting' variable is used to make sure that the drive train isn't getting * 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. * inverted every iteration of the TeleopPeriodic method while the button is held down.
@ -231,101 +427,170 @@ private:
{ {
inverting = false; inverting = false;
} }
if(operator_stick.GetRawButton(4))
/*
* 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))
{ {
shooting = true; arms.Set(-operator_stick.GetY() * ARMS_SCALE);
shooter.Shoot();
} }
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); arms.Set(0);
} }
// This code will become obsolete after the Shooter logic is complete. // This code will become obsolete after the Shooter logic is complete.
float opThrottle = SaneThrottle(operator_stick.GetThrottle()); float opThrottle = SaneThrottle(operator_stick.GetThrottle());
if(!pickupRunning && !unjamming) if(!pickupRunning && ( opThrottle > shooter_power + DEADZONE_RADIUS
|| opThrottle < shooter_power - DEADZONE_RADIUS))
{ {
shooter.SetPower(opThrottle); 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() 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() static enum test_stages test_stage, old_test_stage;
{
float x = -driver_stick.GetX();
float y = -driver_stick.GetY();
float left = 0;
float right = 0;
if (x > 0) if (old_test_stage != test_stage)
{ {
right = y; //Reset timer between stages
left = (1- x*TURN_FACTOR)*y ; t.Stop();
t.Reset();
//Wait for a button press between stages
if (operator_stick.GetRawButton(TRIGGER))
old_test_stage = test_stage;
} }
else else
{ {
left = y; if(!t.Get()) t.Start();
right = (1+x*TURN_FACTOR)*y;
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); LogCSVData();
}
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);
}
} }
}; };

34
Robot2016/src/Shooter.h
View File

@ -12,6 +12,7 @@
#define LAUNCH_POWER 1 #define LAUNCH_POWER 1
#define SPINUP_TIME 1.5 // seconds. #define SPINUP_TIME 1.5 // seconds.
#define LAUNCH_TIME 0.5 #define LAUNCH_TIME 0.5
#define RAMP_POWER 0.3
class Shooter class Shooter
{ {
@ -64,7 +65,7 @@ public:
void LowerRamp() void LowerRamp()
{ {
ramp->Set(-0.5); ramp->Set(-RAMP_POWER);
if(ramp->Limits::kReverseLimit){ if(ramp->Limits::kReverseLimit){
SmartDashboard::PutNumber("ramp", 2); //going to put a circlar dial to show where the ramp could be SmartDashboard::PutNumber("ramp", 2); //going to put a circlar dial to show where the ramp could be
} else { } else {
@ -74,7 +75,7 @@ public:
void RaiseRamp() void RaiseRamp()
{ {
ramp->Set(0.5); ramp->Set(RAMP_POWER);
if(ramp->Limits::kForwardLimit){ if(ramp->Limits::kForwardLimit){
SmartDashboard::PutNumber("ramp", 0); //going to put a circlar dial to show where the ramp could be SmartDashboard::PutNumber("ramp", 0); //going to put a circlar dial to show where the ramp could be
} else { } else {
@ -88,21 +89,18 @@ public:
} }
void Shoot() void Shoot()
{ {
// TODO: Shooter Logic should go as follows: if (shotClock.Get() > (SPINUP_TIME + 0.1))
{
/* state = RESETTING;
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.
*/
switch (state) switch (state)
{ {
case READY: case READY:
{ {
state = SPINNINGUP; state = SPINNINGUP;
ramp->Set(-1); ramp->Set(-1);
launcher->Set(PICKUP_POWER); launcher->Set(LAUNCH_POWER);
pickup->Set(PICKUP_POWER); pickup->Set(LAUNCH_POWER);
shotClock.Reset(); shotClock.Reset();
shotClock.Start(); shotClock.Start();
break; break;
@ -116,13 +114,13 @@ public:
shotClock.Start(); shotClock.Start();
} else } else
{ {
std::cout << "*Goku noises*\n"; //std::cout << "*Goku noises*\n";
} }
break; break;
} }
case LAUNCH: case LAUNCH:
{ {
ramp->Set(1); ramp->Set(LAUNCH_POWER);
state = LAUNCHING; state = LAUNCHING;
break; break;
} }
@ -155,8 +153,7 @@ public:
void PickUp(bool state = true) void PickUp(bool state = true)
{ {
pickup->Set((float) (state * PICKUP_POWER)); pickup->Set((float) (state * PICKUP_POWER));
launcher->Set((float) (state * PICKUP_POWER * -0.75)); launcher->Set((float) (state * PICKUP_POWER * -1));
//ramp->Set(-1.0*PICKUP_POWER);
//std::cout << "picking up!\n"; //std::cout << "picking up!\n";
} }
@ -165,7 +162,7 @@ public:
*/ */
void Unjam() void Unjam()
{ {
pickup->Set(PICKUP_POWER * -0.75); pickup->Set(-1 * PICKUP_POWER);
} }
void ShootLow() void ShootLow()
@ -179,6 +176,11 @@ public:
launcher->Set(power); launcher->Set(power);
//std::cout << "setting shooter power" << std::endl; //std::cout << "setting shooter power" << std::endl;
} }
int GetState()
{
return state;
}
private: private:
//RobotDrive *shooterDrive; //RobotDrive *shooterDrive;

41
robot.cpp
View File

@ -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)