598 lines
14 KiB
C++
598 lines
14 KiB
C++
#include "WPILib.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>
|
|
|
|
#ifndef BUTTON_LAYOUT
|
|
#define BUTTON_LAYOUT
|
|
|
|
#define TRIGGER 1 // Trigger button number
|
|
#define THUMB 2 // Thumb button number
|
|
#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.
|
|
// Also used for detecting changes in an axis' value.
|
|
|
|
#define TURN_FACTOR 1.5 // Left(x,y) = y*(1 + TF*x) : x < 0
|
|
// = y : x >= 0
|
|
// Right(x,y) = y : x < 0
|
|
// = y*(1 - TF*x) : x >= 0
|
|
|
|
#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,
|
|
arms;
|
|
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<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:
|
|
Robot():
|
|
left_drive(0), // Left DriveTrain Talons plug into PWM channel 1 with a Y-splitter
|
|
right_drive(1), // Right DriveTrain Talons plug // left wheel 2
|
|
shooter1(11), // shooter drive 1
|
|
shooter2(10), // shooter drive 2
|
|
ramp(12),
|
|
arms(13),
|
|
drive(&left_drive, &right_drive),
|
|
shooter( // initialize Shooter object.
|
|
&shooter1, &shooter2, &ramp),
|
|
driver_stick(0), // right stick (operator)
|
|
operator_stick(1), // left stick (driver)
|
|
gimbs(3),
|
|
gimba(GIMB_DEFAULT)
|
|
{
|
|
|
|
}
|
|
|
|
void RobotInit()
|
|
{
|
|
chooser = new SendableChooser();
|
|
|
|
chooser->AddDefault(autoNameDefault, (void*)&autoNameDefault);
|
|
chooser->AddObject(autoNameShort, (void*)&autoNameShort);
|
|
chooser->AddObject(autoNameOne, (void*)&autoNameOne);
|
|
chooser->AddObject(autoNameDisable, (void*)&autoNameDisable);
|
|
SmartDashboard::PutData("Auto Modes", chooser);
|
|
shooter1.Enable();
|
|
shooter2.Enable();
|
|
left_drive.SetInverted(true);
|
|
right_drive.SetInverted(true);
|
|
//ramp.SetInverted(true);
|
|
inverting = false;
|
|
pickupRunning = false;
|
|
ramping = false;
|
|
shooting = false;
|
|
unjamming = false;
|
|
arming = false;
|
|
shooter_power = 0;
|
|
arcade = false;
|
|
|
|
// Initialize the number so we can get it later
|
|
SmartDashboard::PutNumber("shooting angle", GIMB_DEFAULT);
|
|
|
|
}
|
|
|
|
void AutonomousInit()
|
|
{
|
|
autoSelected = *((std::string*)chooser->GetSelected());
|
|
//std::string autoSelected = SmartDashboard::GetString("Auto Selector", autoNameDefault);
|
|
std::cout << "Auto selected: " << autoSelected << std::endl;
|
|
|
|
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()
|
|
{
|
|
|
|
}
|
|
|
|
void TeleopPeriodic()
|
|
{
|
|
LogCSVData();
|
|
std::cout << "arm encoder position: " << arms.GetEncPosition() << std::endl;
|
|
std::cout << "arm encoder velocity: " << arms.GetEncVel() << std::endl;
|
|
|
|
if(driver_stick.GetRawButton(7))
|
|
{
|
|
arcade = true;
|
|
}
|
|
if(driver_stick.GetRawButton(8))
|
|
{
|
|
arcade = false;
|
|
}
|
|
if (arcade)
|
|
{
|
|
drive.ArcadeDrive(driver_stick);
|
|
}
|
|
else
|
|
{
|
|
if (driver_stick.GetRawButton(THUMB))
|
|
{
|
|
float left = driver_stick.GetTwist();
|
|
float right = -driver_stick.GetTwist();
|
|
drive.TankDrive(left, right);
|
|
}
|
|
else
|
|
{
|
|
UpdateDrive();
|
|
}
|
|
}
|
|
//bool rampDoing = false;
|
|
// This is shit code for testing. Replace it with real code.
|
|
if(!ramping && operator_stick.GetRawButton(RAMP_RAISE))
|
|
{
|
|
std::cout << "Raising Ramp.";
|
|
//ramp.Set(1);
|
|
shooter.RaiseRamp();
|
|
ramping =true;
|
|
}
|
|
else if(!ramping && operator_stick.GetRawButton(RAMP_LOWER))
|
|
{
|
|
std::cout << "Lowering Ramp.";
|
|
//ramp.Set(-1);
|
|
shooter.LowerRamp();
|
|
ramping = true;
|
|
}
|
|
/*else if(!ramping && operator_stick.GetRawButton(TRIGGER))
|
|
{
|
|
shooter.BoostRamp();
|
|
ramping = true;
|
|
}*/
|
|
else if(ramping && !operator_stick.GetRawButton(RAMP_RAISE)
|
|
&& !operator_stick.GetRawButton(RAMP_LOWER))
|
|
{
|
|
shooter.StopRamp();
|
|
ramping = false;
|
|
}
|
|
|
|
|
|
if(!unjamming && operator_stick.GetRawButton(UNJAM))
|
|
{
|
|
unjamming = true;
|
|
shooter.Unjam();
|
|
}
|
|
else if(!unjamming && operator_stick.GetRawButton(TRIGGER))
|
|
{
|
|
shooter.ShootLow();
|
|
unjamming = true;
|
|
}
|
|
else if(unjamming && !operator_stick.GetRawButton(UNJAM) && !operator_stick.GetRawButton(TRIGGER))
|
|
{
|
|
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
|
|
* every iteration of the TeleopPeriodic method (once every 10ms!)
|
|
* The pickup is disabled when the thumb button is released, but the code still
|
|
* has 'pickupRunning' as true.
|
|
*/
|
|
if(operator_stick.GetRawButton(THUMB) && !pickupRunning)
|
|
{
|
|
shooter.PickUp();
|
|
pickupRunning = true;
|
|
}
|
|
else if(!operator_stick.GetRawButton(THUMB) && pickupRunning)
|
|
{
|
|
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.
|
|
* This is important because the TeleopPeriodic method executes something like once every 10ms.
|
|
* Thus, this if-else if pair make the button a toggle.
|
|
*/
|
|
if(driver_stick.GetRawButton(TRIGGER) && !inverting)
|
|
{
|
|
std::cout << "Inverting Drive Train.";
|
|
left_drive.SetInverted(!left_drive.GetInverted());
|
|
right_drive.SetInverted(!right_drive.GetInverted());
|
|
inverting = true;
|
|
}
|
|
else if(!driver_stick.GetRawButton(TRIGGER))
|
|
{
|
|
inverting = false;
|
|
}
|
|
|
|
if(operator_stick.GetRawButton(4))
|
|
{
|
|
arms.Set(-operator_stick.GetY() * ARMS_SCALE);
|
|
}
|
|
else
|
|
{
|
|
arms.Set(0);
|
|
}
|
|
|
|
// This code will become obsolete after the Shooter logic is complete.
|
|
float opThrottle = SaneThrottle(operator_stick.GetThrottle());
|
|
|
|
if(!pickupRunning && ( opThrottle > shooter_power + DEADZONE_RADIUS
|
|
|| opThrottle < shooter_power - DEADZONE_RADIUS))
|
|
{
|
|
shooter.SetPower(opThrottle);
|
|
}
|
|
}
|
|
|
|
void TestPeriodic()
|
|
{
|
|
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};
|
|
|
|
static enum test_stages test_stage, old_test_stage;
|
|
|
|
if (old_test_stage != test_stage)
|
|
{
|
|
//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
|
|
{
|
|
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;
|
|
}
|
|
}
|
|
}
|
|
|
|
LogCSVData();
|
|
}
|
|
};
|
|
|
|
START_ROBOT_CLASS(Robot)
|