logging code might have been merged with master

This commit is contained in:
john sandstedt 2016-02-26 16:20:37 -05:00
commit fb8087f6f5
2 changed files with 261 additions and 82 deletions

View File

@ -8,14 +8,20 @@
#include <sys/time.h> #include <sys/time.h>
#include <vector> #include <vector>
#include <cmath> #include <cmath>
#include <math.h>
#ifndef BUTTON_LAYOUT #ifndef BUTTON_LAYOUT
#define BUTTON_LAYOUT #define BUTTON_LAYOUT
#define TRIGGER 1 // Trigger button number #define TRIGGER 1 // Trigger button number
#define THUMB 2 // Thumb button number #define THUMB 2 // Thumb button number
#define RAMP_RAISE 3 // Button 3 for Raising Ramp #define RAMP_RAISE 5 // Button 3 for Raising Ramp
#define RAMP_LOWER 4 // Button 4 to lower ramp. #define RAMP_LOWER 3 // Button 4 to lower ramp.
#define UNJAM 11
#define DEADZONE_RADIUS 0.01 // 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.
#endif // BUTTON_LAYOUT #endif // BUTTON_LAYOUT
@ -25,14 +31,15 @@
class Robot: public IterativeRobot class Robot: public IterativeRobot
{ {
Talon left_drive, right_drive; TalonSRX left_drive, right_drive;
CANTalon shooter1, shooter2, CANTalon shooter1, shooter2,
ramp; ramp,
arms;
RobotDrive drive; RobotDrive drive;
Shooter shooter; Shooter shooter;
Joystick driver_stick, operator_stick; Joystick driver_stick, operator_stick;
//DriverStation DriverStation::GetInstance(); //DriverStation DriverStation::GetInstance();
float power; //float power;
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
@ -40,13 +47,13 @@ public:
shooter1(11), // shooter drive 1 shooter1(11), // shooter drive 1
shooter2(10), // shooter drive 2 shooter2(10), // shooter drive 2
ramp(12), ramp(12),
arms(13),
drive(&left_drive, &right_drive), drive(&left_drive, &right_drive),
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)
{ {
//DriverStation::GetInstance().GetInstance();
} }
@ -54,6 +61,11 @@ private:
// instance variables // 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 pickupRunning; // don't want to spam the Talon with set messages. Toggle the pickup when a button is pressed or released.
bool inverting; bool inverting;
bool ramping;
bool shooting;
bool unjamming;
bool arming;
float shooter_power;
LiveWindow *lw = LiveWindow::GetInstance(); LiveWindow *lw = LiveWindow::GetInstance();
SendableChooser *chooser; SendableChooser *chooser;
@ -81,12 +93,18 @@ private:
chooser->AddDefault(autoNameDefault, (void*)&autoNameDefault); chooser->AddDefault(autoNameDefault, (void*)&autoNameDefault);
chooser->AddObject(autoNameCustom, (void*)&autoNameCustom); chooser->AddObject(autoNameCustom, (void*)&autoNameCustom);
SmartDashboard::PutData("Auto Modes", chooser); SmartDashboard::PutData("Auto Modes", chooser);
ramp.Enable();
shooter1.Enable(); shooter1.Enable();
shooter2.Enable(); shooter2.Enable();
left_drive.SetInverted(true); left_drive.SetInverted(true);
right_drive.SetInverted(true); right_drive.SetInverted(true);
//ramp.SetInverted(true);
inverting = false; inverting = false;
pickupRunning = false;
ramping = false;
shooting = false;
unjamming = false;
arming = false;
shooter_power = 0;
} }
@ -102,9 +120,6 @@ private:
*/ */
void AutonomousInit() void AutonomousInit()
{ {
shooter.CalibrateRamp();
shooter.SetRamp(Shoot);
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;
@ -128,44 +143,89 @@ private:
void TeleopInit() void TeleopInit()
{ {
shooter.CalibrateRamp();
shooter.SetRamp(Shoot); // start in the full up position!
power = 0;
} }
void TeleopPeriodic() void TeleopPeriodic()
{ {
LogCSVData(); LogCSVData();
std::cout << "Ramp position: "<< ramp.GetEncPosition() << std::endl; std::cout << "arm encoder position: " << arms.GetEncPosition() << std::endl;
drive.ArcadeDrive(&driver_stick, true); std::cout << "arm encoder velocity: " << arms.GetEncVel() << std::endl;
drive.ArcadeDrive(-driver_stick.GetY(), -driver_stick.GetX()*0.75);
//bool rampDoing = false;
// This is shit code for testing. Replace it with real code. // This is shit code for testing. Replace it with real code.
if(operator_stick.GetRawButton(RAMP_RAISE)) if(!ramping && operator_stick.GetRawButton(RAMP_RAISE))
{ {
ramp.Set(1); std::cout << "Raising Ramp.";
//ramp.Set(1);
shooter.RaiseRamp();
ramping =true;
} }
else if(operator_stick.GetRawButton(RAMP_LOWER)) else if(!ramping && operator_stick.GetRawButton(RAMP_LOWER))
{ {
ramp.Set(-1); std::cout << "Lowering Ramp.";
//ramp.Set(-1);
shooter.LowerRamp();
ramping = true;
} }
else /*else if(!ramping && operator_stick.GetRawButton(TRIGGER))
{ {
ramp.Set(0); shooter.BoostRamp();
ramping = true;
}*/
else if(ramping
&& !operator_stick.GetRawButton(RAMP_LOWER)
&& !operator_stick.GetRawButton(RAMP_RAISE))
{
std::cout << "Stopping Ramp.";
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;
}
/*
* 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) if(operator_stick.GetRawButton(THUMB) && !pickupRunning)
{ {
shooter.PickUp(); shooter.PickUp();
pickupRunning = true; pickupRunning = true;
} }
else if(pickupRunning) else if(!operator_stick.GetRawButton(THUMB) && pickupRunning)
{ {
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
* 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(THUMB) && !inverting) if(driver_stick.GetRawButton(THUMB) && !inverting)
{ {
std::cout << "Inverting Drive Train.";
left_drive.SetInverted(!left_drive.GetInverted()); left_drive.SetInverted(!left_drive.GetInverted());
right_drive.SetInverted(!right_drive.GetInverted()); right_drive.SetInverted(!right_drive.GetInverted());
inverting = true; inverting = true;
@ -174,14 +234,59 @@ private:
{ {
inverting = false; inverting = false;
} }
if(((1.0 - operator_stick.GetThrottle()) / 2.0) > power + 0.005||((1.0 - operator_stick.GetThrottle()) / 2.0) < power -0.005)
/*
* Unlike the previous actions, this method does need to be called every itteration 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))
{ {
power = (1.0 - operator_stick.GetThrottle()) / 2.0; shooting = true;
shooter.SetPower(power); shooter.Shoot();
}
else if(shooting && !operator_stick.GetRawButton(TRIGGER))
{
shooting = false;
shooter.StopShooter();
}*/
if(!arming && driver_stick.GetRawButton(RAMP_RAISE))
{
arming = true;
arms.Set(1);
}
else if(!arming && driver_stick.GetRawButton(RAMP_LOWER))
{
arming = true;
arms.Set(-1);
}
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 && ( 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() void TestPeriodic()
{ {

View File

@ -8,89 +8,156 @@
#ifndef SRC_SHOOTER_H_ #ifndef SRC_SHOOTER_H_
#define SRC_SHOOTER_H_ #define SRC_SHOOTER_H_
#define PICKUP_POWER 0.8 #define PICKUP_POWER 0.75
#define RAMP_LOWER_DURATION 2 //Rotations. #define LAUNCH_POWER 1
#define SPINUP_TIME 1.5 // seconds.
#define LAUNCH_TIME 0.5
class Shooter
/** {
* You can use the values assigned to each of these values as the number
* of rotations to run the motor down from the "Shoot" position.
*
* This might not be the best way to do it, and also requires that we
* figure out how to read the Hall Effect signal from the motor.
*/
enum RampState {
Shoot = 0, // 0 rotations
Half = (RAMP_LOWER_DURATION / 2), // 1 rotation?
Down = RAMP_LOWER_DURATION, // 2 rotations?
Uncalibrated = -1,
Transitioning = -2
};
class Shooter {
public: public:
/** /**
* Shooter talons and ramp talon. * Shooter talons and launch-spinny talon.
* s2 is also for the pickup-mechanism and can be controlled independently. * s2 is also for the pickup-mechanism and can be controlled independently.
* *
*/ */
Shooter(CANTalon *s1, CANTalon *s2, CANTalon *r) { enum ShooterState
{
READY,
ON_FIRE,
SPINNINGUP,
LAUNCH,
LAUNCHING,
RESETTING
};
Shooter(CANTalon *s1, CANTalon *s2, CANTalon *r)
{
// shooterDrive = new RobotDrive(s1, s2); // shooterDrive = new RobotDrive(s1, s2);
launcher = s1; launcher = s1;
pickup = s2; pickup = s2;
ramp = r; ramp = r;
rampState = Uncalibrated; ready = true;
state = READY;
} }
/** /**
* Call this method on TeleopInit so that the ramp is properly * Call this method on TeleopInit so that the ramp is properly
* set at the beginning of the match. * set at the beginning of the match.
*/ */
RampState CalibrateRamp() {
// TODO:
// Raise ramp until limit switch is triggered,
// then lower the ramp to its lower limit.
return Down; virtual ~Shooter()
} {
virtual ~Shooter() {
delete launcher; delete launcher;
delete pickup; delete pickup;
delete ramp; delete ramp;
} }
void PickUp(bool state = true) { void StopShooter()
pickup->Set((float) (state * PICKUP_POWER)); {
std::cout << "picking up!\n"; ready = true;
ramp->Set(0);
launcher->Set(0);
pickup->Set(0);
} }
void SetRamp(RampState state) { void LowerRamp()
// TODO: {
// Move the Ramp to the set position. ramp->Set(-0.5);
if(ramp->Limits::kReverseLimit){
SmartDashboard::PutNumber("ramp", 2); //going to put a circlar dial to show where the ramp could be
} else {
SmartDashboard::PutNumber("ramp", 1);
}
}
void RaiseRamp()
{
ramp->Set(0.5);
if(ramp->Limits::kForwardLimit){
SmartDashboard::PutNumber("ramp", 0); //going to put a circlar dial to show where the ramp could be
} else {
SmartDashboard::PutNumber("ramp", 1);
}
}
void StopRamp()
{
ramp->Set(0);
}
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.
*/
switch (state) switch (state)
{ {
case Shoot: case READY:
{ {
if (ramp->GetForwardLimitOK()) state = SPINNINGUP;
ramp->Set(-1);
launcher->Set(PICKUP_POWER);
pickup->Set(PICKUP_POWER);
shotClock.Reset();
shotClock.Start();
break;
}
case SPINNINGUP:
{
if (shotClock.Get() > SPINUP_TIME)
{ {
ramp->Set(1); state = LAUNCH;
shotClock.Reset();
shotClock.Start();
} else } else
{ {
ramp->Set(0); std::cout << "*Goku noises*\n";
} }
break;
} }
case Half: case LAUNCH:
{ {
//yeah, put something here when you get the encoder working. ramp->Set(1);
std::cout << "Hey! Didja install an encoder? Because this is placeholder that Aidan would have removed if you had installed one and told him about it.\n"; state = LAUNCHING;
break;
} }
case Down: case LAUNCHING:
{ {
//see half. if (shotClock.Get() > LAUNCH_TIME)
std::cout << "Hey! Didja install an encoder? Because this is placeholder that Aidan would have removed if you had installed one and told him about it.\n"; {
state = RESETTING;
}
break;
}
case RESETTING:
{
ramp->Set(0);
launcher->Set(0);
pickup->Set(0);
state = READY;
break;
}
case ON_FIRE:
{
std::cout << "Something is wrong with the launch sequence.\n";
break;
} }
} }
}
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);
//std::cout << "picking up!\n";
} }
/** /**
@ -98,24 +165,31 @@ public:
*/ */
void Unjam() void Unjam()
{ {
pickup->Set(-1 * PICKUP_POWER); pickup->Set(PICKUP_POWER * -0.75);
} }
void SetPower(float power) { void ShootLow()
{
pickup->Set(-1);
}
void SetPower(float power)
{
pickup->Set(power); pickup->Set(power);
launcher->Set(power); launcher->Set(power);
std::cout << "setting shooter power" << std::endl; //std::cout << "setting shooter power" << std::endl;
} }
private: private:
RobotDrive *shooterDrive; //RobotDrive *shooterDrive;
CANTalon *launcher; CANTalon *launcher;
CANTalon *pickup; CANTalon *pickup;
CANTalon *ramp; CANTalon *ramp;
ShooterState state;
RampState rampState;
RampState targetState; Timer shotClock;
RampState previousState; bool ready;
int fake_position;
}; };
#endif /* SRC_SHOOTER_H_ */ #endif /* SRC_SHOOTER_H_ */