#include "WPILib.h" // Include all the necessary functions and classes for the robot
#define CUTOFF 3
#define CUTOFFR 1
#define CUTOFFM 1
#define CUTOFFF 1
#define TIMES 20
/*Information about hardware-
* This code assumes the following connections:
* Driver Station:
* USB 1: AIRFLO controller used for the mecanum drive
* USB 2: AIRFLO controller used for the scoring system
*
* Robot:
* General:
* Uses a mecanum drive powered by 4 CIM motors and jaguars
* Has a lift system driven by the AndyMark gearmotor in the 2012 kit of parts and a jaguar
* Has 2 fisher price motors from the 2012 kit of parts in the front of the feeder controlled by a spike relay
* Has 2 throttle control motors from the 2012 kit of parts on the rear of the feeder controlled by spike relays
* Has 3 pairs of IR recievers on the rail, with a emmiter on the feeder
* Has 3 pairs of IR recievers/emmiters on the feeder
*
* Digital Sidecar 1:
* PWM 1: Connected to front left drive motor
* PWM 2: Connected to rear left drive motor
* PWM 3: Connected to front right drive motor
* PWM 4: Connected to rear right drive motor
* PWM 5: Connected to lift motor via jaguar
* Relay 1: Connected to rear feeder spike
* Relay 2: Connected to middle feeder spike
* Relay 3: Connected to front feeder spikes (with a PWM splitter)
* Analog 1: Connected to the IR reciever at bottom of the rail system
* Analog 2: Connected to the IR reciever at bridge height of the rail system
* Analog 3: Connected to the IR reciever at the top of the rail system
* Analog 4: Connected to the IR reciever for the rear of the feeder system
* Analog 5: Connected to the IR reciever for the middle of the feeder system
* Analog 6: Connected to the IR reciever for the front of the feeder system
* Analog 8: Connected to the IR emmitter for the rail system
*/
/*Behavior of the robot-
* Gamepad 1:
* Mecanum drive:
* Pressing top triggers slows overall speed to .2
* Pressing lower triggers slows overall speed to .5
* Pressing button 11 toggles arcade and tank style of control
* When arcade style on:
* Left joypad drives x-y of robot
* Right joypad left-right drives the rotation
* When tank style on:
* Pushing the left joypad forward/backward will move the left set of wheels as such, likewise for the right
* Pushing the sticks right (The average is taken) causes overall strafe.
*
* Gamepad 2:
* Lift system:
* When left joypad let up on:
* Button 1: Go to bottom
* Button 2, 3: Go to bridge level
* Button 4: Go to lower hoop
* When left joypad released:
* Flick up to go up, down to go down
*
* Ball intake/output:
* Pressing the dpad forward/backwards will intake/output a ball
*/
/*Explanation of the code-
*Initiation-
* Declares variables
*MecanumDefaultCode(void)
* Sets the variables to correspond to components
* Sets axes of the gamepad
* Initializes some variables
*
*void AutonomousInit(void)
* Sets the servo's state
* Sends axis camera feed to DS
*
*void TeleopPeriodic(void)
* Driving-
* Sets throttle variable to 1, .5, or .2 depending on what shoulder buttons are pressed
* Grabs the axes of the gamepad, multiplys them by the throttle, and sends them to the MecanumDrive_Cartesian command of the drivebase
* Rail system-
* Determines which bottons (1-4) are pressed, sets a variable to it (doesn't change if none is)
* Determines which switch is pressed and changes a vairable to it (doesnt change if none is)
* Goes up if the desired height is greater than the actual height, down if lesser, and none if equal.
* Ball feeding-
* Finds out state of dpad
* if forward, expel the balls
* else if rearward, intake a ball if the position isn't blocked
* else (if none), do nothing
*/
class MecanumDefaultCode : public IterativeRobot // Makes an IterativeRobot (as opposed to a SimpleC or Simple one) named MecanumDefaultCode
{
// Declare variables
RobotDrive *robotDrive; // mecanum drivebase
Jaguar *liftMotor; // lift motor; will be controlled by a jaguar
Relay *rFeeders, *mFeeders, *fFeeders; // feeder system motors, controlled by spikes
Joystick *drivePad, *actionPad; // gamepad and joystick
AnalogChannel *railSwitch1, *railSwitch2, *railSwitch3, *railSwitch4; // switches on the rail system
AnalogChannel *rIRreciever, *mIRreciever, *fIRreciever; // the IR recievers
SmartDashboard *smartDash; // the smart dashboard on the driverstation
AxisCamera *camera; // the axis camera
float throttle; // throttle variable
int height, desiredHeight, counter, counter2; // lift system variables: actual height, wanted height
bool threeAxisDrive; // Wheter to use a three axis drive or tank drive for the mecanums
public:
MecanumDefaultCode(void)
{
robotDrive = new RobotDrive(1, 2, 3, 4); // Defines a robot drive system using mecanum drive on PWMS 1, 2, 3, and 4
liftMotor = new Jaguar(5); // Defines a new jaguar ar PWM 5
rFeeders = new Relay(1); // Defines a new relay at relay port 1
mFeeders = new Relay(2); // Defines a new relay at relay port 2
fFeeders = new Relay(3); // Defines a new relay at relay port 3
drivePad = new Joystick(1); // Define joystick being used at USB port #1 on the Drivers Station
actionPad = new Joystick(2); // Define joystick being used at USB port #2 on the Drivers Station
railSwitch1 = new AnalogChannel(1); // Define input at analog #1
railSwitch2 = new AnalogChannel(2); // Define input at analog #2
railSwitch3 = new AnalogChannel(3); // Define input at analog #3
rIRreciever = new AnalogChannel(5); // Define input at analog #5
mIRreciever = new AnalogChannel(6); // Define input at analog #6
fIRreciever = new AnalogChannel(7); // Define input at analog #7
smartDash=SmartDashboard::GetInstance(); // Declare the smart dashboard on the driverstation
drivePad->SetAxisChannel(Joystick::kXAxis, 1); // X is on Axis 1 for the gamepad
drivePad->SetAxisChannel(Joystick::kYAxis, 2); // Y is on Axis 2 for the gamepad
drivePad->SetAxisChannel(Joystick::kTwistAxis, 4); // Twist is on Axis 4 for the gamepad
height=1; // Initialize the height at the ground
desiredHeight=1; // Initialize the desired height at the ground, so that the robot doesn't try to move it at first
counter=0; // Initialize the counter so it isnt a random value
counter2=0;
threeAxisDrive=true; // Initalizes to three axis drive is used
}
void AutonomousInit(void) // Called once when teleop is enabled
{
AxisCamera &camera = AxisCamera::GetInstance(); // Send the axis camera feed to the Driverstation
}
void TeleopPeriodic(void) // Called once for each new packet from the driver station while teleop is enabled
{
counter++; // adds to counter
if(counter=TIMES){ // if counter equals TIMES
smartDash->Log(desiredHeight, "Desired Height"); // Logs out the desired height with label Desired Height
smartDash->Log(height, "Height"); // Logs out the height with label Height
smartDash->Log(threeAxisDrive, "Three axis drive?");
smartDash->Log(!railSwitch1->GetAverageVoltage()>CUTOFF, "At base?"); // Logs out if the IR Reciver at the base is tripped
smartDash->Log(!railSwitch2->GetAverageVoltage()>CUTOFF, "At bridge?"); // Logs out if the IR Reciver at the bridge is tripped
smartDash->Log(!railSwitch3->GetAverageVoltage()>CUTOFF, "At top?"); // Logs out if the IR Reciver at the top is tripped
smartDash->Log(!fIRreciever->GetAverageVoltage()>CUTOFFR, "Ball in rear?"); // Logs out if the IR Reciver in the feeder rear is tripped
smartDash->Log(!mIRreciever->GetAverageVoltage()>CUTOFFM, "Ball in middle?"); // Logs out if the IR Reciver at the feeder middle is tripped
smartDash->Log(!rIRreciever->GetAverageVoltage()>CUTOFFF, "Ball in front?"); // Logs out if the IR Reciver at the feeder front is tripped
smartDash->Log(railSwitch1->GetAverageVoltage(), "no. 1 voltage"); // Logs out the voltage of the lowest switch
smartDash->Log(railSwitch2->GetAverageVoltage(), "no. 2 voltage"); // Logs out the voltage of the feeder front
smartDash->Log(railSwitch3->GetAverageVoltage(), "no. 3 voltage"); // Logs out the voltage of the feeder front
smartDash->Log(rIRreciever->GetAverageVoltage(), "no. 4 voltage"); // Logs out the voltage of the feeder rear
smartDash->Log(mIRreciever->GetAverageVoltage(), "no. 5 voltage"); // Logs out the voltage of the feeder middle
smartDash->Log(fIRreciever->GetAverageVoltage(), "no. 6 voltage"); // Logs out the voltage of the feeder front
counter=0; // Resets counter
}
if(drivePad->GetRawButton(5)||drivePad->GetRawButton(6)) // if any top triggers on drive pad are pulled
throttle=.2; // set throttle to .2
else if(drivePad->GetRawButton(7)||drivePad->GetRawButton(8)) // if any of the lower triggers are pulled
throttle=.5; // set throttle to .5
else // if none are pulled (previous did not execute)
throttle=1; // set throttle to full
// Setting throttles
robotDrive->MecanumDrive_Cartesian(drivePad->GetX()*throttle, drivePad->GetY()*throttle, drivePad->GetTwist()*throttle);
if(!actionPad->GetRawButton(12)){ // if the left joypad on the action pad is not depressed
if(railSwitch1->GetAverageVoltage()>CUTOFF) // if rail switch 1 is receiving
height=1; // set height to 1
else if(railSwitch2->GetAverageVoltage()>CUTOFF) // else if rail switch 2 is receiving
height=2; // set height to 2
else if(railSwitch3->GetAverageVoltage()>CUTOFF) // else if rail switch 3 is receiving
height=3; // set height to 3
//says what the height is on the rail system based on the switches; doesnt modify if none is
if(actionPad->GetRawButton(1)) // if button 1 on action pad pressed
desiredHeight=1; // set the desired height to 1
else if(actionPad->GetRawButton(2)) // else if button 2 on action pad pressed
desiredHeight=2; // set the desired height to 2
else if(actionPad->GetRawButton(3)) // else if button 3 on action pad pressed
desiredHeight=2; // set the desired height to 3
else if(actionPad->GetRawButton(4)) // else if button 4 on action pad pressed
desiredHeight=3; // set the desired height to 4
//says what the desired height is based on what button on the gamepad is pressed, doesnt modify if none is
if(height>desiredHeight) // if greater than the desired height
liftMotor->SetSpeed(-1); // set speed of lift motor to full reverse (drop it)
if(height<desiredHeight) // if lesser than the desired height
liftMotor->SetSpeed(1); // set speed of lift motor to full forward (raise it)
if(height==desiredHeight) // otherwise
liftMotor->SetSpeed(0); // stop the lift motor
}
// Drives the rail system from button inputs
else if(actionPad->GetRawButton(12)) // if the left joypad on the action pad is depressed
liftMotor->SetSpeed(actionPad->GetRawAxis(2)); // drives the lift motor from the joystick axis
if(actionPad->GetRawAxis(6)<-.5){ // if dpad of action controller pushed forwards
rFeeders->Set(Relay::kForward); // set front relay forward
mFeeders->Set(Relay::kForward); // set middle relay forward
fFeeders->Set(Relay::kForward); // set rear relay forward
}
else if(actionPad->GetRawAxis(6)>.5){ // if dpad of action controller pushed downwards
// if(rIRreciever->GetAverageVoltage()>CUTOFFR) // if rear IR reciever recieving
rFeeders->Set(Relay::kReverse); // set rear relays backwards
// if(mIRreciever->GetAverageVoltage()>CUTOFFM) // if rear IR reciever recieving
mFeeders->Set(Relay::kReverse); // set rear relays backwards
// if(fIRreciever->GetAverageVoltage()>CUTOFFF) // if front IR reciever recieving
fFeeders->Set(Relay::kReverse); // set front relays backwards
}
else{ // if no dpads pushed forward or backwards
rFeeders->Set(Relay::kOff); // set rear relays off
mFeeders->Set(Relay::kOff); // set middle relays off
fFeeders->Set(Relay::kOff); // set front relays off
}
// Set the feeder motors to in, out or stopped
}
};
START_ROBOT_CLASS(MecanumDefaultCode);
