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

logging code might have been merged with master

Browse Source
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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

157
Robot2016/src/Robot.cpp
View File

@ -8,14 +8,20 @@
#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 3 // Button 3 for Raising Ramp
#define RAMP_LOWER 4 // Button 4 to lower ramp.
#define RAMP_RAISE 5 // Button 3 for Raising 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
@ -25,14 +31,15 @@
class Robot: public IterativeRobot
{
Talon left_drive, right_drive;
TalonSRX left_drive, right_drive;
CANTalon shooter1, shooter2,
ramp;
ramp,
arms;
RobotDrive drive;
Shooter shooter;
Joystick driver_stick, operator_stick;
//DriverStation DriverStation::GetInstance();
float power;
//float power;
public:
Robot():
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
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)
operator_stick(1) // left stick (driver)
{
//DriverStation::GetInstance().GetInstance();
}
@ -54,6 +61,11 @@ 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;
float shooter_power;
LiveWindow *lw = LiveWindow::GetInstance();
SendableChooser *chooser;
@ -81,12 +93,18 @@ private:
chooser->AddDefault(autoNameDefault, (void*)&autoNameDefault);
chooser->AddObject(autoNameCustom, (void*)&autoNameCustom);
SmartDashboard::PutData("Auto Modes", chooser);
ramp.Enable();
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;
}
@ -102,9 +120,6 @@ private:
*/
void AutonomousInit()
{
shooter.CalibrateRamp();
shooter.SetRamp(Shoot);
autoSelected = *((std::string*)chooser->GetSelected());
//std::string autoSelected = SmartDashboard::GetString("Auto Selector", autoNameDefault);
std::cout << "Auto selected: " << autoSelected << std::endl;
@ -128,44 +143,89 @@ private:
void TeleopInit()
{
shooter.CalibrateRamp();
shooter.SetRamp(Shoot); // start in the full up position!
power = 0;
}
void TeleopPeriodic()
{
LogCSVData();
std::cout << "Ramp position: "<< ramp.GetEncPosition() << std::endl;
drive.ArcadeDrive(&driver_stick, true);
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);
//bool rampDoing = false;
// 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)
{
shooter.PickUp();
pickupRunning = true;
}
else if(pickupRunning)
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(THUMB) && !inverting)
{
std::cout << "Inverting Drive Train.";
left_drive.SetInverted(!left_drive.GetInverted());
right_drive.SetInverted(!right_drive.GetInverted());
inverting = true;
@ -175,13 +235,58 @@ private:
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;
shooter.SetPower(power);
shooting = true;
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()
{

180
Robot2016/src/Shooter.h
View File

@ -8,89 +8,156 @@
#ifndef SRC_SHOOTER_H_
#define SRC_SHOOTER_H_
#define PICKUP_POWER 0.8
#define RAMP_LOWER_DURATION 2 //Rotations.
#define PICKUP_POWER 0.75
#define LAUNCH_POWER 1
#define SPINUP_TIME 1.5 // seconds.
#define LAUNCH_TIME 0.5
/**
* 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 {
class Shooter
{
public:
/**
* Shooter talons and ramp talon.
* Shooter talons and launch-spinny talon.
* 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);
launcher = s1;
pickup = s2;
ramp = r;
rampState = Uncalibrated;
ready = true;
state = READY;
}
/**
* Call this method on TeleopInit so that the ramp is properly
* 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 pickup;
delete ramp;
}
void PickUp(bool state = true) {
pickup->Set((float) (state * PICKUP_POWER));
std::cout << "picking up!\n";
void StopShooter()
{
ready = true;
ramp->Set(0);
launcher->Set(0);
pickup->Set(0);
}
void SetRamp(RampState state) {
// TODO:
// Move the Ramp to the set position.
void LowerRamp()
{
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)
{
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
{
ramp->Set(0);
std::cout << "*Goku noises*\n";
}
break;
}
case Half:
case LAUNCH:
{
//yeah, put something here when you get the encoder working.
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";
ramp->Set(1);
state = LAUNCHING;
break;
}
case Down:
case LAUNCHING:
{
//see half.
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";
if (shotClock.Get() > LAUNCH_TIME)
{
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()
{
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);
launcher->Set(power);
std::cout << "setting shooter power" << std::endl;
//std::cout << "setting shooter power" << std::endl;
}
private:
RobotDrive *shooterDrive;
//RobotDrive *shooterDrive;
CANTalon *launcher;
CANTalon *pickup;
CANTalon *ramp;
ShooterState state;
RampState rampState;
RampState targetState;
RampState previousState;
Timer shotClock;
bool ready;
int fake_position;
};
#endif /* SRC_SHOOTER_H_ */