This years code
2 posters
Page 1 of 1
This years code
William Surmak Sun Dec 02, 2012 6:51 pm
Programmers!
As the season has come to a close I would like to share the code that we used at our final competition in New York. Look over it, and try to use your new found knowledge from the tutorials you've all been reading (I hope) to understand bits and pieces of it. Unfortunately the forum doesn't support copy and paste indentation so its a little hard to read.
/******************************************************************************
*
* Sprockets 2012 Base Code
* Authors: Brendan van Ryn
* Stuart Sullivan
* Aliah McCalla
* Will Surmak
******************************************************************************/
// Include our own header file with our own macros
#include "NewRobot.h"
// Header file for WPI standard hardware/robot overhead.
#include "WPILib.h"
// Header file for math functions
#include <math.h>
//Header file for time functions
#include <time.h>
// Macro Functions
// Clears the Driver Station output display
#define CLEARMESSAGE DriverStationLCD::GetInstance()->Clear()
// Provides full free-form formatting, as per the usual printf(). Displays formatted text, a, on Driver Station on line, b.
#define DISPLAYPRINTF(b, a, ...) {DriverStationLCD::GetInstance()->Printf(DriverStationLCD::kUser_Line ## b, 1, a, ## __VA_ARGS__); DriverStationLCD::GetInstance()->UpdateLCD();}
/* Contains the macro definitions for testing on the new robot. */
#warning "New robot"
// Button macros
#define SWERVEMODEBUTTON rightStick->GetRawButton(1)
#define MONSTERMODEBUTTON leftStick->GetRawButton(1)
#define CARMODEBUTTON rightStick->GetRawButton(6)
#define LOCKMODEBUTTON rightStick->GetRawButton(5)
#define BRAKEMODEBUTTON leftStick->GetRawButton(2)
#define TANKTURNINGMODEBUTTON rightStick->GetRawButton(3)
#define REVERSEDRIVEBUTTON rightStick->GetRawButton(4)
#define STRAIGHTMODEBUTTON rightStick->GetRawButton(8)
#define PIVOTMODEBUTTON rightStick->GetRawButton(2)
#define MANUALALIGNENABLEBUTTON (leftStick->GetRawButton(8) && leftStick->GetRawButton(9))
#define MANUALALIGNLEFTREAR leftStick->GetRawButton(7)
#define MANUALALIGNRIGHTREAR leftStick->GetRawButton(10)
#define MANUALALIGNLEFTFRONT leftStick->GetRawButton(6)
#define MANUALALIGNRIGHTFRONT leftStick->GetRawButton(11)
#define BRIDGEMANIPULATEBUTTON rightStick->GetRawButton(77)
#define HIGHGEARBUTTON speedStick->GetRawButton(8) //arm lock
#define LOWGEARBUTTON speedStick->GetRawButton(9)// arm unlock
#define NEUTRALGEARBUTTON leftStick->GetRawButton(3)
#define ARMUNWINDBUTTON rightStick->GetRawButton(11)
#define PIVOTMODEBUTTON rightStick->GetRawButton(2)
#define SHOOTBUTTON (speedStick->GetRawButton(1) && (speedStick->GetRawButton(3) || speedStick->GetRawButton(4) || speedStick->GetRawButton(5)))
#define FIRSTFIREBUTTON speedStick->GetRawButton(4)
#define SECONDFIREBUTTON speedStick->GetRawButton(3)
#define THIRDFIREBUTTON speedStick->GetRawButton(5)
#define FOURTHFIREBUTTON speedStick->GetRawButton(88)
#define BRIDGEMANIPULATORRAISEBUTTON speedStick->GetRawButton(6)
#define BRIDGEMANIPULATORLOWERBUTTON speedStick->GetRawButton(7)
#define MAGAZINEENABLEBUTTON speedStick->GetRawButton(11)
#define MAGAZINEREVERSEBUTTON speedStick->GetRawButton(10)
#define COLLECTORENABLEBUTTON speedStick->GetRawButton(11)
#define COLLECTORREVERSEBUTTON speedStick->GetRawButton(10)
#define MYSTERYBUTTON speedStick->GetRawButton(2)
// Port numbers
// Motors are on PWM ports
#define FRONTMOTORPORT 3
#define REARMOTORPORT 4
#define FRONTSTEERINGMOTORPORT 2
#define REARSTEERINGMOTORPORT 1
#define BOTTOMSHOOTINGMOTORPORT 9
#define LATERALCOLLECTORPORT 10
#define BRIDGEMANIPULATORMOTORPORT 7
// The gear-shifting servos are on PWM ports
#define FRONTSERVOPORT 5
#define REARSERVOPORT 6
#define ARMENGAGESERVOPORT 8
// Joysticks are on USB ports
#define LEFTJOYSTICKPORT 4
#define RIGHTJOYSTICKPORT 1
#define SPEEDJOYSTICKPORT 3
// Encoders are on Digital IO ports
#define FRONTSTEERINGENCODERPORTA 3
#define FRONTSTEERINGENCODERPORTB 4
#define REARSTEERINGENCODERPORTA 1
#define REARSTEERINGENCODERPORTB 2
#define DRIVEENCODERPORTA 14
#define DRIVEENCODERPORTB 13
// Limit switches are digital inputs
#define ARMDOWNSWITCHPORT 7
#define ARMUPSWITCHPORT 8
// Light sensors are also digital inputs
#define ARMSTARTPOSITIONPORT 9
#define ARMHITPOSITIONPORT 10
// Analogue channels (analogue board input)
#define STEERINGGYROPORT 2
#define ANALOGCHANNELPORT 4
// The autonomous switches enable/disable digital inputs
#define AUTONOMOUSSWITCHPORTONE 5
#define AUTONOMOUSSWITCHPORTTWO 6
// Relays have their own ports
#define MAGAZINERELAYPORT 2
#define MAGAZINERELAY2PORT 5
#define TRANSVERSECOLLECTORPORT 1
// Constants
// Hardware constants
#define GYROSENSITIVITY 0.007f // Volts per degree per second
#define PULSESPER90FRONT 1350 // Number of encoder pulses in a 90-degree turn (front steering)
#define PULSESPER90REAR 600 // Number of encoder pulses in a 90-degree turn (rear steering)
#define STEERINGDEADBANDFRONT 40 // Number of encoder pulses of proximity needed for the front steering
#define STEERINGAPPROACHDISTANCEFRONT 100 // Number of encoder pulses of proximity after which the wheels begin to slow down
#define STEERINGDEADBANDREAR 10 // Number of encoder pulses of proximity needed for the rear steering
#define STEERINGAPPROACHDISTANCEREAR 60 // Number of encoder pulses of proximity after which the wheels begin to slow down
#define MAGAZINEALTERNATORLIMIT 2000 // The number of times the IsOpteratorControl() loop must run before the magazine alternator resets
#define MAGAZINEALTERNATORLIMITONE 1800 // The number of times the IsOperatorControl() loop must run before the magazine reverses
// Mask values for TrackDrive() function (a mask, because 1 OR 2 == 3)
#define FRONT_MOTOR 1 // Tells the TrackDrive() Function to run the front motor
#define REAR_MOTOR 2 // Tells the TrackDrive() Function to run the rear motor
#define BOTH_MOTORS 3 // Tells the TrackDrive() Function to run both motors
// Hardware threshold constants
#define JOYSTICKDEADBANDX 0.08f // Minimum detectable joystick deflection
#define JOYSTICKDEADBANDY 0.08f
#define GYRODEADBANDANGLE 3.0f // Allowable error for gyro angle when tracking
#define DRIVEAPPROACHDISTANCE 70.0f // Distance at which the drive motors begin to slow during brake mode
#define DRIVEDEADBAND 2.0f // Allowable error for brake mode encoder
// Other constants
#define Pi 3.1415926535897932384626433832795028841971f
#define SWERVEMODE 0 // Drive modes
#define MONSTERMODE 1
#define CARMODE 2
#define LOWGEARPOSITION 160.0f // Angles for the gear-shifting servos
#define HIGHGEARPOSITION 70.0f
#define NEUTRALGEARPOSITION 110.0f
#define ARMENGAGEPOSITION 70.0f
#define ARMDISENGAGEPOSITION 120.0f
#define MAGAZINEREVERSETIME 700 // Time for magazine to reverse after running
#define SERVOPAUSETIME 1000 // Time to wait for arm-engaging servo to shift
#define AUTONOMOUSDISTANCE 1900 // Maximum distance to travel in autonomous
#define MAGAZINEPAUSETIME 400 // Amount of time to wait during autonomous for rollers to spin up to speed
#define AUTOSHOOTPAUSETIME 13000 // Amount of time to wait during autonomous before moving (third mode only)
#define AUTOBRIDGEPAUSETIME 8000 //Amount of time to wait during autonomous before raising the bridge manipulator
#define AUTOBRIDGEPAUSETIMETWO 6500 //Amount of time to wait for the bridge manipulator to raise
#define TARGETENCODERVALUE 2500 //Distance to get to the bridge in autonomous
#define TARGETENCODERVALUETWO 5400 // Target distance to move forward towards the second bridge in both bridge autonomous
#define TARGETREVERSEENCODERVALUE 2250 //Target value to reverse to in both bridge autonomous
#define STRAFEDISTANCE 4650 // Distance to strafe in first autonomous mode
#define STRAFEDISTANCETWO 1050 //Target strafe distance in shoot then strafe autonomous mode
#define TARGETENCODERVALUETHREE 3900
#define SHOOTERAPPROACHSPEED 0.0f // Distance from target speed at which the rollers begin to ramp down
#define SHOOTERCONSTANTSPEED 0.1f // Minimum power value for the shooter (a maintenance value)
#define SHOOTERDEADBAND 0.0f // Distance from target speed at which the roller speed is considered to be within a good enough threshold
#define COLLECTORSPEED -1.00f // Speed of collector motors
#define ARMDOWNTIME 8000 // Time in autonomous mode to pause when the arm first lowers
// Shooter constants
#define FIRESPEEDONE 6.0f http://5.65f Pre-set firing speeds
#define FIRESPEEDTWO 10.0f // full power
#define FIRESPEEDTHREE 6.8f // Our key shot
#define FIRESPEEDFOUR 6.8f //Note this is the autonomous value
// Prototypes for functions (tells the compiler the format)
float Abs(float); // Returns the absolute value of a number
int AbsI(int); // Returns the absolute value of an integer
// Our main and only class. You could consider this an alteration of "int main(void)" if it makes it easier.
class SimpleTracker : public SimpleRobot
{
// Global variables accessible to entire program
Joystick *leftStick; // Left driver joystick
Joystick *rightStick; // Right driver joystick
Joystick *speedStick; // Co-driver joystick
Victor *frontMotor; // Front drive motor
Victor *rearMotor; // Rear drive motor
Victor *bridgeManipulatorMotor; //Motor to raise and lower the bridge manipulator
Jaguar *frontSteer; // Front steering motor
Jaguar *rearSteer; // Rear steering motor
Jaguar *lateralCollector; // Lateral ball-collector rollers
Jaguar *bottomShooter; // Bottom shooting roller
Encoder *frontSteeringEncoder; // Front steering encoder
Encoder *rearSteeringEncoder; // Rear steering encoder
Encoder *driveEncoder; // Drive wheel encoder
Gyro *steeringGyro; // Steering gyro
Relay *magazineBelt; // Magazine conveyor
Relay *magazineBelt2; // Second magazine conveyor motor
Relay *transverseCollector; // Transverse collector belts
DigitalInput *armDownSwitch; // The limit switch that indicates the bridge manipulator is down
DigitalInput *armUpSwitch; // The limit switch that indicates that the bridge manipulator is up
DigitalInput *armStartSensor; // The light sensor that detects the primed position for the bridge manipulator
DigitalInput *armHitSensor; // The light sensor that detects when the bridge manipulator hits the bridge
DigitalInput *autonomousSwitchOne; // The autonomous switch that determines our autonomous mode
DigitalInput *autonomousSwitchTwo; // Another autonomous switch that determines our autonomous mode
AnalogChannel *rollerVoltage; // Voltage reading from shooting rollers (for speed)
Servo *frontDriveServo; // The servo to shift gears for the front gear box
Servo *rearDriveServo; // The servo to shift gears for the rear gear box
Servo *armEngageServo; // The servo to engage the bridge manipulator arm
/* "Constructor". This code is run when the robot is first turned on. */
public:
SimpleTracker(void)
{
// Assign object pointers here.
frontMotor = new Victor(FRONTMOTORPORT); // Driving Motors
rearMotor = new Victor(REARMOTORPORT);
frontSteer = new Jaguar(FRONTSTEERINGMOTORPORT); // Steering Motors
rearSteer = new Jaguar(REARSTEERINGMOTORPORT);
bottomShooter = new Jaguar(BOTTOMSHOOTINGMOTORPORT); // Other motors
lateralCollector = new Jaguar(LATERALCOLLECTORPORT);
bridgeManipulatorMotor = new Victor(BRIDGEMANIPULATORMOTORPORT);//Bridge manipulator motor
leftStick = new Joystick(LEFTJOYSTICKPORT); // Joysticks
rightStick = new Joystick(RIGHTJOYSTICKPORT);
speedStick = new Joystick(SPEEDJOYSTICKPORT);
frontSteeringEncoder = new Encoder(FRONTSTEERINGENCODERPORTA, FRONTSTEERINGENCODERPORTB); // Encoders
rearSteeringEncoder = new Encoder(REARSTEERINGENCODERPORTA, REARSTEERINGENCODERPORTB);
driveEncoder = new Encoder(DRIVEENCODERPORTA, DRIVEENCODERPORTB);
steeringGyro = new Gyro(1, STEERINGGYROPORT); // Analogue devices
rollerVoltage = new AnalogChannel(ANALOGCHANNELPORT);
magazineBelt = new Relay(MAGAZINERELAYPORT); // Relays
magazineBelt2 = new Relay(MAGAZINERELAY2PORT);
transverseCollector = new Relay(TRANSVERSECOLLECTORPORT);
armStartSensor = new DigitalInput(ARMSTARTPOSITIONPORT); // Light sensors
armHitSensor = new DigitalInput(ARMHITPOSITIONPORT);
autonomousSwitchOne = new DigitalInput(AUTONOMOUSSWITCHPORTONE); // Switches
autonomousSwitchTwo = new DigitalInput(AUTONOMOUSSWITCHPORTTWO);
armDownSwitch = new DigitalInput(ARMDOWNSWITCHPORT);
armUpSwitch = new DigitalInput(ARMUPSWITCHPORT);
frontDriveServo = new Servo(FRONTSERVOPORT); // Servos
rearDriveServo = new Servo(REARSERVOPORT);
armEngageServo = new Servo(ARMENGAGESERVOPORT);
// Initialize encoders
frontSteeringEncoder->Reset();
frontSteeringEncoder->Start();
rearSteeringEncoder->Reset();
rearSteeringEncoder->Start();
driveEncoder->Reset();
driveEncoder->Start();
// Initialize gyro
steeringGyro->Reset();
steeringGyro->SetSensitivity(GYROSENSITIVITY);
}
/* Code to run while in tele-op, or driver-controlled mode. */
void OperatorControl(void)
{
// Variables
float leftx, rightx, lefty, righty; // The x- and y-positions of the two driver joysticks
float speedz; // The position of the co-driver throttle
float x, y; // The average x- and y-positions between the two driver joysticks
float swerveSpeed; // The drive motor speed when in swerve-drive mode
float swerveAngle; // The angle to turn the wheels in swerve-drive mode
float swerveAngleFront, swerveAngleRear; // The individual angles for the front and back sides, for use in different steering modes
int steeringMode = SWERVEMODE; // Store the current mode of steering
int currentButtonState = 0, previousButtonState = 0; // Store the current and previous states of the manual alignment buttons
int driveWheelEncoderValue; // Distance travelled by robot
float frontGearSetting = LOWGEARPOSITION; // Store the angle setting for the servos
float rearGearSetting = LOWGEARPOSITION;
float armPosition = LOWGEARPOSITION;
int armStage = 0; // Current stage of bridge manipulator
int magazineReverseCount = 0; // Allow the magazine to reverse for a bit before shooting
int servoPauseCount = 0; // Give the servos time to shift
int magazineAlternator = 0; // Alternate the direction of the magazine when firing to provide spacing for the balls
int preFire = 0; // Are we trying to use a pre-set firing speed?
int brakeValue = -1; // Encoder value to maintain when in brake mode
int armOverride = 0; // Override flag for manual winding of bridge manipulator
double shooterSpeed = 0.0; // Voltage from motor to read roller speed
int direction = 1; // Joystick direction
int reverseToggleCurrent = 0; // Previous state of reversing button
int reverseTogglePrevious = 0; // Current state of reversing button
// Reset the drive and shooter encoders, and the timer
driveEncoder->Reset();
driveEncoder->Start();
// This code is run repeatedly so long as the robot is in teleop mode.
while(IsOperatorControl())
{
/* Tell the operating system that our program hasn't frozen */
GetWatchdog().Feed();
/* Message displays */
CLEARMESSAGE;
DISPLAYPRINTF(1, "Shooter Speed: %0.3f", shooterSpeed); // The speed of the shooting roller
DISPLAYPRINTF(2, "Distance: %d", driveWheelEncoderValue); // Value of drive wheel encoder
// Get joystick positions ( x's will always be the same direction)
leftx = -1 * leftStick->GetRawAxis(1);
lefty = direction * leftStick->GetRawAxis(2);
rightx = -1 * rightStick->GetRawAxis(1);
righty = direction * rightStick->GetRawAxis(2);
speedz = (speedStick->GetRawAxis(3)- 1.0f) / 2.0f * -8.0f; // Scale from 0 to 8
// Get voltage from analog channel
shooterSpeed = rollerVoltage->GetVoltage();
if(shooterSpeed < 0.0f) shooterSpeed = 0.0f; // Positive voltages only
// Prevent back-rolling
if(speedz < 0.0f) speedz = 0.0f;
// Run the servo counter
if(servoPauseCount > 0)
servoPauseCount--;
// Run the magazine alternator up to a pre-determined value, then reset
if(magazineAlternator > MAGAZINEALTERNATORLIMIT) magazineAlternator = 0;
else magazineAlternator++;
// Check drive mode buttons
if(SWERVEMODEBUTTON) steeringMode = SWERVEMODE;
else if(MONSTERMODEBUTTON) steeringMode = MONSTERMODE;
else if(CARMODEBUTTON) steeringMode = CARMODE;
//Check if the bridge manipulator needs to be raised or lowered
if(BRIDGEMANIPULATORRAISEBUTTON) bridgeManipulatorMotor->Set(0.7f);
else if(BRIDGEMANIPULATORLOWERBUTTON) bridgeManipulatorMotor->Set(-0.7f);
else bridgeManipulatorMotor->Set(0.0f);
// Check shooter buttons
if(FIRSTFIREBUTTON) SetRollerSpeed(FIRESPEEDONE, shooterSpeed), preFire = 1;
else if(SECONDFIREBUTTON) SetRollerSpeed(FIRESPEEDTWO, shooterSpeed), preFire = 1;
else if(THIRDFIREBUTTON) SetRollerSpeed(FIRESPEEDTHREE, shooterSpeed), preFire = 1;
else if(FOURTHFIREBUTTON) SetRollerSpeed(FIRESPEEDFOUR, shooterSpeed), preFire = 1;
else preFire = 0;
// Check gear shifting buttons
if(HIGHGEARBUTTON) frontGearSetting = rearGearSetting = HIGHGEARPOSITION, armPosition = ARMDISENGAGEPOSITION, armOverride = 0;
else if(LOWGEARBUTTON) frontGearSetting = rearGearSetting = LOWGEARPOSITION, armPosition = ARMDISENGAGEPOSITION, armOverride = 0;
else if(NEUTRALGEARBUTTON) frontGearSetting = LOWGEARPOSITION, rearGearSetting = NEUTRALGEARPOSITION, armPosition = ARMDISENGAGEPOSITION, armOverride = 0;
else if(BRIDGEMANIPULATEBUTTON && armPosition != ARMENGAGEPOSITION) frontGearSetting = LOWGEARPOSITION, rearGearSetting = NEUTRALGEARPOSITION, armPosition = ARMENGAGEPOSITION, servoPauseCount = SERVOPAUSETIME, armStage = 0, armOverride = 0;
else if(ARMUNWINDBUTTON) frontGearSetting = LOWGEARPOSITION, rearGearSetting = NEUTRALGEARPOSITION, armPosition = ARMENGAGEPOSITION, armOverride = 1;
else if(armOverride && !ARMUNWINDBUTTON) frontGearSetting = rearGearSetting = LOWGEARPOSITION, armPosition = ARMDISENGAGEPOSITION, armOverride = 0;
// Move servos to desired position
frontDriveServo->SetAngle(frontGearSetting);
rearDriveServo->SetAngle(rearGearSetting);
armEngageServo->SetAngle(armPosition);
// Get the drive wheel encoder value
driveWheelEncoderValue = driveEncoder->Get();
// If the brake button isn't being pressed, store a sentinel value as the brake value for reference
if(!BRAKEMODEBUTTON) brakeValue = -1;
// Check magazine buttons
if(MAGAZINEENABLEBUTTON || SHOOTBUTTON)
{
// Move forward for a set number of counts, then reverse for the rest in each cycle of the alternator
if(magazineAlternator < MAGAZINEALTERNATORLIMITONE) magazineBelt->Set(Relay::kForward), magazineBelt2->Set(Relay::kForward);
else magazineBelt->Set(Relay::kReverse), magazineBelt2->Set(Relay::kReverse);
// Prepare to revers when the button is released
magazineReverseCount = MAGAZINEREVERSETIME;
}
else if(MAGAZINEREVERSEBUTTON) magazineBelt->Set(Relay::kReverse), magazineBelt2->Set(Relay::kReverse);
else if(magazineReverseCount > 0) magazineBelt->Set(Relay::kReverse), magazineBelt2->Set(Relay::kReverse), magazineReverseCount--;
else magazineBelt->Set(Relay::kOff), magazineBelt2->Set(Relay::kOff);
// Check collector buttons (preFire prevents speed settings from affecting the back rolling of the shooter)
if(COLLECTORENABLEBUTTON) lateralCollector->Set(COLLECTORSPEED), transverseCollector->Set(Relay::kReverse), bottomShooter->Set(-0.45), preFire = 1;
else if(COLLECTORREVERSEBUTTON) lateralCollector->Set(-COLLECTORSPEED), transverseCollector->Set(Relay::kForward);
else
{
lateralCollector->Set(0.0f);
transverseCollector->Set(Relay::kOff);
if(lateralCollector->Get() > 0.1f) preFire = 0;
}
// Store the states of the toggle buttons from the previous iteration
previousButtonState = currentButtonState;
reverseTogglePrevious = reverseToggleCurrent;
// Check drive-reverse button
if(REVERSEDRIVEBUTTON) reverseToggleCurrent = 1;
else reverseToggleCurrent = 0;
// Check manual-alignment button
if(MANUALALIGNENABLEBUTTON) currentButtonState = 1;
else currentButtonState = 0;
// If the total y-deflection for each joystick is below the threshold, we shouldn't count it at all, allowing perfect strafing
if(Abs(lefty) < JOYSTICKDEADBANDY) lefty = 0.0f;
if(Abs(righty) < JOYSTICKDEADBANDY) righty = 0.0f;
//Averaging joystick positions
x = (leftx + rightx) / 2.0f;
y = (lefty + righty) / 2.0f;
// Making the joysticks cureved rather than linear
if(x > 0) x = x * x;
else if(x < 0) x = (-x * x);
if(y > 0) y = y * y;
else if(y < 0) y = (-y * y);
// Drive modes
// If the manual-alignment buttons are pressed
if(MANUALALIGNENABLEBUTTON)
{
// Check alignment buttons
if(MANUALALIGNLEFTREAR) rearSteer->Set(-0.8f);
else if(MANUALALIGNRIGHTREAR) rearSteer->Set(0.8f);
else rearSteer->Set(0);
if(MANUALALIGNLEFTFRONT) frontSteer->Set(-0.8f);
else if(MANUALALIGNRIGHTFRONT)frontSteer->Set(0.8f);
else frontSteer->Set(0);
}
// If the lock button is pressed
else if(LOCKMODEBUTTON)
{
// Lock the wheels to ninety degrees and drive tank-style
TrackFrontSteering(PULSESPER90FRONT);
TrackRearSteering(PULSESPER90REAR);
frontMotor->Set(y);
rearMotor->Set(y);
}
// If the brake buttons is pressed
else if(BRAKEMODEBUTTON)
{
// Store value to hold
if(brakeValue == -1) brakeValue = driveWheelEncoderValue;
// Keep wheels straight
TrackFrontSteering(0);
TrackRearSteering(0);
// Maintain position
TrackDrive(brakeValue, BOTH_MOTORS);
}
// If the tank turning button is pressed
else if(TANKTURNINGMODEBUTTON)
{
// Turn the wheels in opposite directions
TrackFrontSteering(PULSESPER90FRONT);
TrackRearSteering(-PULSESPER90REAR);
// Drive each end tank-style to rotate
frontMotor->Set(lefty);
rearMotor->Set(-righty);
}
// If the pivot mode button is pressed
else if(PIVOTMODEBUTTON)
{
// Keep the front wheels straight and turn the back wheels
TrackFrontSteering(0);
TrackRearSteering(PULSESPER90REAR);
// Drive the back to pivot
frontMotor->Set(0);
rearMotor->Set(rightx);
// Allow manual firing if no automatic firing is in use
if(!preFire) SetRollerSpeed(speedz, shooterSpeed);
}
//Keep the wheels straight so we can only move forward and backward
else if(STRAIGHTMODEBUTTON)
{
TrackFrontSteering(0);
TrackRearSteering(0);
frontMotor->Set(lefty);
rearMotor->Set(righty);
}
/* Otherwise, we drive normally. If the joysticks are not pushed left or right any significant amount,
* drive tank-style */
else if(Abs(x) < JOYSTICKDEADBANDX)
{
// Set the drive motors according to their respective joysticks
frontMotor->Set(lefty);
// Only move rear motor if we aren't in bride-manipulate mode
if(armPosition != ARMENGAGEPOSITION || servoPauseCount > 0 || armOverride) rearMotor->Set(righty);
// If not pre-set firing speed was selected, allow manual firing
if(!preFire) SetRollerSpeed(speedz, shooterSpeed);
// Keep the wheels straight
TrackFrontSteering(0);
TrackRearSteering(0);
}
else
{
// Determine our speed and angle of movement (force angle to be in first quadrant)
swerveSpeed = sqrt(x*x + y*y); // a^2 + b^2 = c^2
swerveAngle = -Abs(atan(x/y)); // x/y = tan(angle), therefore angle = atan(x/y); negative because of motor direction
swerveAngle = swerveAngle * 180 / Pi; // 1 radian = Pi/180 radians, so reverse
swerveAngle = swerveAngle / 90.0f; // Convert from degrees to a percentage of 90 degrees
// Set roller speeds manually if no pre-set speed has been selected
if(!preFire) SetRollerSpeed(speedz, shooterSpeed);
// Determine the quadrant of the joystick and convert the speed or angle appropriately
// We assume we're in quadrant one if none of these are true
if(x < 0 && y > 0) swerveAngle = -swerveAngle; // Quadrant two, reverse angle
else if(x < 0 && y <= 0) swerveSpeed = -swerveSpeed; // Quadrant three, reverse drive direction
else if(x >= 0 && y <= 0) // Quadrant four, reverse both
{
swerveSpeed = -swerveSpeed;
swerveAngle = -swerveAngle;
}
// If we are in monster mode and travelling faster than 80% max speed, we switch to car automatically (max speed is sqrt(2))
if(Abs(swerveSpeed) > 1.2f && steeringMode == MONSTERMODE) steeringMode = CARMODE;
else if(Abs(swerveSpeed < 1.2f && steeringMode == CARMODE)) steeringMode = MONSTERMODE;
// In monster, we reverse either the front or back steering depending on which direction we're travelling
if(steeringMode == MONSTERMODE)
{
// Multiplying by 0.75f makes the steering less sensitive as it has a smaller range
if(swerveSpeed <= 0) swerveAngleFront = -0.75f * swerveAngle, swerveAngleRear = 0.75f * swerveAngle;
else swerveAngleFront = 0.75f * swerveAngle, swerveAngleRear = -0.75f * swerveAngle;
}
// In car, we move only the front or only the back wheels, depending on which direction we're travelling
else if(steeringMode == CARMODE)
{
// Multiplying by 0.5f gives us 45 degrees at most of steering
if(swerveSpeed <= 0) swerveAngleFront = -0.5f * swerveAngle, swerveAngleRear = 0.0f;
else swerveAngleRear = -0.5f * swerveAngle, swerveAngleFront = 0.0f;
}
else
{
// Otherwise, in swerve, both sides get the same angle (negative because of the motor polarity)
swerveAngleFront = -swerveAngle;
swerveAngleRear = -swerveAngle;
}
// Convert front and rear angle percentages into pulses
swerveAngleFront *= PULSESPER90FRONT;
swerveAngleRear *= PULSESPER90REAR;
// Turn steering to desired angle
TrackFrontSteering((int)swerveAngleFront);
TrackRearSteering((int)swerveAngleRear);
// Drive at the calculated speed
frontMotor->Set(swerveSpeed);
// Only move rear wheels if bride manipulate mode is not engaged
if(armPosition != ARMENGAGEPOSITION || servoPauseCount > 0 || armOverride) rearMotor->Set(swerveSpeed);
}
// If the manual alignment buttons been released having previously being pressed, reset the encoders after aligning
if(previousButtonState == 1 && currentButtonState == 0)
{
frontSteeringEncoder->Reset();
frontSteeringEncoder->Start();
rearSteeringEncoder->Reset();
rearSteeringEncoder->Start();
frontSteer->Set(0);
rearSteer->Set(0);
}
// If the drive reverse-button was pressed and has been released, reverse the direction of the joysticks (for driving backwards)
if(reverseTogglePrevious == 1 && reverseToggleCurrent == 0) direction *= -1;
}
} // End of teleop
/* Code to run while in autonomous, or fully-automated mode. */
void Autonomous(void)
{
// Variables
int firstSwitch, secondSwitch; // Store the states of the autonomous switches
// Store the values of the switches for the autonomous modes
firstSwitch = autonomousSwitchOne->Get();
secondSwitch = autonomousSwitchTwo->Get();
// Check the states of switches and call the correct autonomous mode
if(firstSwitch && secondSwitch) AutonomousOne(); // Middle position is shoot then strafe
else if(firstSwitch && !secondSwitch) AutonomousTwo(); // To the right is shoot only
else if(secondSwitch && !firstSwitch) AutonomousThree(); // To the left is shoot then tip bridge
else
{
// Display an error if the switch setting is invalid (maybe the switch is unplugged or something)
CLEARMESSAGE;
DISPLAYPRINTF(1, "No autonomous switch");
}
} // End of Autonomous
/* Move the front wheels to the angle passed to this function in "targetPosition" */
int TrackFrontSteering(int targetPosition)
{
int frontSteeringEncoderValue; // Current wheel position
int remainingSteeringDistance; // Remaining distance to target position
float speed; // The speed to set the steering motor to
// Get the current position of the wheels from the encoder and calculate the distance to the target position
frontSteeringEncoderValue = frontSteeringEncoder->Get();
remainingSteeringDistance = AbsI(frontSteeringEncoderValue - targetPosition);
/* If the steering is within limits, we simply stop. Otherwise, we move either left or right. The division works thus:
* if the wheel is further away from the target than the approach distance, speed will be greater than one; otherwise,
* speed will be some percentage, linearly ramping the speed as it approaches zero. */
if(remainingSteeringDistance < STEERINGDEADBANDFRONT) speed = 0.0f;
else if(frontSteeringEncoderValue < targetPosition) speed = (float)remainingSteeringDistance / (float)STEERINGAPPROACHDISTANCEFRONT;
else speed = (float)-remainingSteeringDistance / (float)STEERINGAPPROACHDISTANCEFRONT;
// If we end up with an aforementioned speed greater than one (100%), we clamp it down to exactly 100%
if(speed > 1.0f) speed = 1.0f;
else if(speed < -1.0f) speed = -1.0f;
// Turn the motor at the calculated speed
frontSteer->Set(speed);
/* If the motor is hardly moving, the wheels are where they need to be, so return 0. Otherwise, set the motor speed and
* return 1 to indicate that it is still moving. */
if(speed == 0.0f) return 0;
else return 1;
}
/* This function is the same as the above function, but for the rear wheels, instead. */
int TrackRearSteering(int targetPosition)
{
int rearSteeringEncoderValue;
int remainingSteeringDistance;
float speed;
rearSteeringEncoderValue = rearSteeringEncoder->Get();
remainingSteeringDistance = AbsI(rearSteeringEncoderValue - targetPosition);
if(remainingSteeringDistance < STEERINGDEADBANDREAR) speed = 0.0f;
else if(rearSteeringEncoderValue < targetPosition) speed = (float)remainingSteeringDistance / (float)STEERINGAPPROACHDISTANCEREAR;
else speed = (float)-remainingSteeringDistance / (float)STEERINGAPPROACHDISTANCEREAR;
if(speed > 1.0f) speed = 1.0f;
else if(speed < -1.0f) speed = -1.0f;
rearSteer->Set(speed);
if(speed == 0.0f) return 0;
else return 1;
}
// Maintains a specific speed for the shooting roller. Returns true when roller is at that speed (similar to the steering)
int SetRollerSpeed(float targetSpeed, float currentSpeed)
{
float powerSetting;
// Calculate a power setting based on distance from target speed
powerSetting = Abs(targetSpeed - currentSpeed); // / SHOOTERAPPROACHSPEED + SHOOTERCONSTANTSPEED;
if(powerSetting > 1.0f) powerSetting = 1.0f;
// We can only speed up or stop. Reversing would strip the gears
if(currentSpeed < targetSpeed) bottomShooter->Set(powerSetting);
else bottomShooter->Set(0.0f);
return (Abs(targetSpeed - currentSpeed) < SHOOTERDEADBAND);
}
// This function attempts to maintain a specific encoder value on the drive block and is also similar to the steering
float TrackDrive(int targetPosition, char motorMask)
{
float distance; // Store the distance of the drive wheels from their target position
int currentPosition; // Current encoder position
// Get the current encoder position
currentPosition = driveEncoder->Get();
// Calculate the absolute distance and scale to a power setting
distance = (float)AbsI(targetPosition - currentPosition);
distance /= DRIVEAPPROACHDISTANCE;
// Move forward or backward at a proportional pace until within a deadband
if(currentPosition < targetPosition - DRIVEDEADBAND) distance *= 1.0f;
else if(currentPosition > targetPosition + DRIVEDEADBAND) distance *= -1.0f;
else distance = 0.0f;
// Move only the motors we want (either just the front, just the back, or both)
if(motorMask == FRONT_MOTOR || motorMask == BOTH_MOTORS) frontMotor->Set(distance);
if(motorMask == REAR_MOTOR || motorMask == BOTH_MOTORS) rearMotor->Set(distance);
// Return motor speed
return distance;
}
// Code for our first possible autonomous mode
void AutonomousOne(void) //middle
{
// Variables
int rollerPause = MAGAZINEPAUSETIME; // The time to wait for the rollers to spin up before enabling the magazine
int shootPause = AUTOSHOOTPAUSETIME; // The time to wait for the balls to finish shooting in autonomous
float shooterSpeed = 0.0f; // Current voltage reading
int encoderValue = 0; // Value from drive encoder
// Reset the drive encoder and gyro
steeringGyro->Reset();
driveEncoder->Reset();
driveEncoder->Start();
while(IsAutonomous())
{
/* Tell the system that the code hasn't frozen. */
GetWatchdog().Feed();
// Message displays
CLEARMESSAGE;
DISPLAYPRINTF(1, "Shoot then strafe"); // Display which autonomous mode is being used
// Get current encoder value and shooter speed
encoderValue = AbsI(driveEncoder->Get());
shooterSpeed = rollerVoltage->GetVoltage();
// Set servos
frontDriveServo->SetAngle(LOWGEARPOSITION);
rearDriveServo->SetAngle(LOWGEARPOSITION);
// Wait for the rollers to spin up before engaging the magazine belts to fire
if(rollerPause > 0)
{
// Spin up the rollers to the preset speed for the key shot
SetRollerSpeed(FIRESPEEDFOUR, shooterSpeed);
rollerPause--;
// Keep the wheels straight
TrackFrontSteering(0);
TrackRearSteering(0);
}
// Fire the ball for a period of time
else if(shootPause > 0)
{
// Fire
magazineBelt->Set(Relay::kForward);
magazineBelt2->Set(Relay::kForward);
shootPause--;
// Spin up the rollers to the preset speed for the key shot
SetRollerSpeed(FIRESPEEDTWO, shooterSpeed);
// Keep the wheels straight
TrackFrontSteering(0);
TrackRearSteering(0);
}
// Strafe after shooting for a certain distance
else if(encoderValue < STRAFEDISTANCETWO)
{
TrackFrontSteering((int)(-PULSESPER90FRONT * 0.78));
TrackRearSteering((int)(-PULSESPER90REAR * 0.78));
frontMotor->Set(0.75f);
rearMotor->Set(0.75f);
}
// Move forward until we hit the bridge
else if(encoderValue < TARGETENCODERVALUETHREE)
{
// Stop rollers
SetRollerSpeed(0.0f, shooterSpeed);
// Get the drive encoder value
encoderValue = AbsI(driveEncoder->Get());
// Keep the front wheels straight and drive forward slowly
TrackFrontSteering(0);
TrackRearSteering(0);
// Drive forward
frontMotor->Set(0.3f);
rearMotor->Set(0.3f);
// If our current encoder value is less then that of our target / 1.5 we lower the bridge manipulator
if(encoderValue < TARGETENCODERVALUETHREE / 1.5)
{
bridgeManipulatorMotor->Set(-0.8f);
}
// If not we raise it until it locks
else if(encoderValue == TARGETENCODERVALUETHREE || encoderValue > TARGETENCODERVALUETHREE / 1.5)
{
bridgeManipulatorMotor->Set(0.8f);
//spin up the collecters to collect the tipped balls
transverseCollector->Set(Relay::kReverse);
lateralCollector->Set(COLLECTORSPEED);
}
else bridgeManipulatorMotor->Set(0.0f);
}
// Stop
else
{
TrackFrontSteering(0);
TrackRearSteering(0);
frontMotor->Set(0.0f);
rearMotor->Set(0.0f);
bridgeManipulatorMotor->Set(0.0f);
transverseCollector->Set(Relay::kReverse);
lateralCollector->Set(COLLECTORSPEED);
}
}
}
void AutonomousTwo(void)
{
// Variables
int rollerPause = MAGAZINEPAUSETIME; // The time to wait for the rollers to spin up before enabling the magazine
int shootPause = AUTOSHOOTPAUSETIME; // The time to wait for the balls to finish shooting in autonomous
float shooterSpeed = 0.0f; // Current voltage reading
int encoderValue = 0; // Value from drive encoder
int targetEncoderValue = TARGETENCODERVALUE; //target value we need to get to
float rearSetting = LOWGEARPOSITION, frontSetting = LOWGEARPOSITION, armSetting = LOWGEARPOSITION; // Servo posiions
int movePause = 25000;
int flag = 1;
int flag2 = 1;
// Reset the drive encoder and gyro
driveEncoder->Reset();
driveEncoder->Start();
while(IsAutonomous())
{
/* Tell the system that the code hasn't frozen. */
GetWatchdog().Feed();
// Display messages
CLEARMESSAGE;
DISPLAYPRINTF(1, " tip both bridges"); // Display which autonomous mode is being used
DISPLAYPRINTF(3, "Encoder: %d", encoderValue); // The value of the encoder
// Get encoder value and roller speed
encoderValue = AbsI(driveEncoder->Get());
shooterSpeed = rollerVoltage->GetVoltage();
// Set servos
frontDriveServo->SetAngle(frontSetting);
rearDriveServo->SetAngle(rearSetting);
// Move forward until we hit the bridge
if(encoderValue < targetEncoderValue && flag == 1)
{
// Stop rollers
SetRollerSpeed(0.0f, shooterSpeed);
// Get the drive encoder value
encoderValue = AbsI(driveEncoder->Get());
// Keep the front wheels straight and drive forward slowly
TrackFrontSteering(0);
TrackRearSteering(0);
// Drive forward
frontMotor->Set(0.7f);
rearMotor->Set(0.7f);
// If our current encoder value is less then that of our target / 1.5 we lower the bridge manipulator
if(encoderValue < targetEncoderValue / 1.5)
{
bridgeManipulatorMotor->Set(-0.8f);
}
// If not we raise it until it locks
else if(encoderValue == targetEncoderValue || encoderValue > targetEncoderValue / 1.5)
{
bridgeManipulatorMotor->Set(0.8f);
//spin up the collecters to collect the tipped balls
//transverseCollector->Set(Relay::kReverse);=
//lateralCollector->Set(COLLECTORSPEED);
}
else
{
bridgeManipulatorMotor->Set(0.0f);
//spin up the collecters to collect the tipped balls
//transverseCollector->Set(Relay::kReverse);
//lateralCollector->Set(COLLECTORSPEED);
}
}
else if(movePause > 0)
{
frontMotor->Set(0.0f);
rearMotor->Set(0.0f);
//transverseCollector->Set(Relay::kReverse);
//lateralCollector->Set(COLLECTORSPEED);
movePause--;
}
else if(encoderValue > targetEncoderValue && flag == 1)
{
flag = 0;
}
else if(encoderValue > TARGETREVERSEENCODERVALUE && flag == 0 && flag2 == 1)
{
encoderValue = AbsI(driveEncoder->Get());
TrackFrontSteering(0);
TrackRearSteering(0);
frontMotor->Set(-0.3f);
rearMotor->Set(-0.3f);
}
else if(encoderValue < TARGETREVERSEENCODERVALUE && flag2 == 1)
{
flag2 = 0;
}
else if(encoderValue < STRAFEDISTANCE)
{
TrackFrontSteering((int)(-PULSESPER90FRONT)); //TrackFrontSteering((int)(-PULSESPER90FRONT * 0.78));
TrackRearSteering((int)(-PULSESPER90REAR)); //TrackRearSteering((int)(-PULSESPER90REAR * 0.78));
frontMotor->Set(0.75f);
rearMotor->Set(0.75f);
}
// Move forward until we hit the bridge
else if(encoderValue < TARGETENCODERVALUETWO)
{
// Get the drive encoder value
encoderValue = AbsI(driveEncoder->Get());
// Keep the front wheels straight and drive forward slowly
TrackFrontSteering(0);
TrackRearSteering(0);
// Drive forward
frontMotor->Set(0.4f);
rearMotor->Set(0.4f);
}
else
{
//stop the bridge manipulator
bridgeManipulatorMotor->Set(0.0f);
//stop the motors
frontMotor->Set(0.0f);
rearMotor->Set(0.0f);
}
}
}
// Third of the possible autonomous modes (to the left)
void AutonomousThree(void)
{
// Variables
int rollerPause = MAGAZINEPAUSETIME; // The time to wait for the rollers to spin up before enabling the magazine
int shootPause = AUTOSHOOTPAUSETIME; // The time to wait for the balls to finish shooting in autonomous
float shooterSpeed = 0.0f; // Current voltage reading
int encoderValue = 0; // Value from drive encoder
int targetEncoderValue = TARGETENCODERVALUE; //target value we need to get to
float rearSetting = LOWGEARPOSITION, frontSetting = LOWGEARPOSITION, armSetting = LOWGEARPOSITION; // Servo posiions
int movePause = 25000;
int flag = 1;
int flag2 = 1;
// Reset the drive encoder and gyro
driveEncoder->Reset();
driveEncoder->Start();
while(IsAutonomous())
{
/* Tell the system that the code hasn't frozen. */
GetWatchdog().Feed();
// Display messages
CLEARMESSAGE;
DISPLAYPRINTF(1, "Shoot then tip both bridges"); // Display which autonomous mode is being used
DISPLAYPRINTF(3, "Encoder: %d", encoderValue); // The value of the encoder
// Get encoder value and roller speed
encoderValue = AbsI(driveEncoder->Get());
shooterSpeed = rollerVoltage->GetVoltage();
// Set servos
frontDriveServo->SetAngle(frontSetting);
rearDriveServo->SetAngle(rearSetting);
// Wait for the rollers to spin up before engaging the magazine belts to fire
if(rollerPause > 0res )
{
// Spin up the rollers to the preset speed for the key shot
SetRollerSpeed(FIRESPEEDFOUR, shooterSpeed);
rollerPause--;
// Keep wheels straight
TrackFrontSteering(0);
TrackRearSteering(0);
}
// Fire the ball
else if(shootPause > 0)
{
// Fire
magazineBelt->Set(Relay::kForward);
magazineBelt2->Set(Relay::kForward);
shootPause--;
// Spin up the rollers to the preset speed for the key shot
SetRollerSpeed(FIRESPEEDFOUR, shooterSpeed);
// Keep wheels straight
TrackFrontSteering(0);
TrackRearSteering(0);
}
// Move forward until we hit the bridge
else if(encoderValue < targetEncoderValue && flag == 1)
{
// Stop rollers
SetRollerSpeed(0.0f, shooterSpeed);
// Get the drive encoder value
encoderValue = AbsI(driveEncoder->Get());
// Keep the front wheels straight and drive forward slowly
TrackFrontSteering(0);
TrackRearSteering(0);
// Drive forward
frontMotor->Set(0.5f);
rearMotor->Set(0.5f);
// If our current encoder value is less then that of our target / 1.5 we lower the bridge manipulator
if(encoderValue < targetEncoderValue / 1.5)
{
bridgeManipulatorMotor->Set(-0.8f);
}
// If not we raise it until it locks
else if(encoderValue == targetEncoderValue || encoderValue > targetEncoderValue / 1.5)
{
bridgeManipulatorMotor->Set(0.8f);
//spin up the collecters to collect the tipped balls
transverseCollector->Set(Relay::kReverse);
lateralCollector->Set(COLLECTORSPEED);
}
else
{
bridgeManipulatorMotor->Set(0.0f);
//spin up the collecters to collect the tipped balls
transverseCollector->Set(Relay::kReverse);
lateralCollector->Set(COLLECTORSPEED);
}
}
else if(movePause > 0)
{
frontMotor->Set(0.0f);
rearMotor->Set(0.0f);
transverseCollector->Set(Relay::kReverse);
lateralCollector->Set(COLLECTORSPEED);
movePause--;
}
else if(encoderValue > targetEncoderValue && flag == 1)
{
flag = 0;
}
else if(encoderValue > TARGETREVERSEENCODERVALUE && flag == 0 && flag2 == 1)
{
encoderValue = AbsI(driveEncoder->Get());
TrackFrontSteering(0);
TrackRearSteering(0);
frontMotor->Set(-0.3f);
rearMotor->Set(-0.3f);
}
else if(encoderValue < TARGETREVERSEENCODERVALUE && flag2 == 1)
{
flag2 = 0;
}
else if(encoderValue < STRAFEDISTANCE)
{
TrackFrontSteering((int)(-PULSESPER90FRONT)); //TrackFrontSteering((int)(-PULSESPER90FRONT * 0.78));
TrackRearSteering((int)(-PULSESPER90REAR)); //TrackRearSteering((int)(-PULSESPER90REAR * 0.78));
frontMotor->Set(0.75f);
rearMotor->Set(0.75f);
}
// Move forward until we hit the bridge
else if(encoderValue < TARGETENCODERVALUETWO)
{
// Get the drive encoder value
encoderValue = AbsI(driveEncoder->Get());
// Keep the front wheels straight and drive forward slowly
TrackFrontSteering(0);
TrackRearSteering(0);
// Drive forward
frontMotor->Set(0.4f);
rearMotor->Set(0.4f);
}
else
{
//stop the bridge manipulator
bridgeManipulatorMotor->Set(0.0f);
//stop the motors
frontMotor->Set(0.0f);
rearMotor->Set(0.0f);
}
}
}
}; /* This is an exception to our semi-colon rule, but its one of very few */
// Custom functions
float Abs(float number)
{
// Return the absolute magnitude of the given number (make it positive)
if(number < 0.0f) number = -number;
return number;
}
int AbsI(int number)
{
// Return the absolute magnitude of the given integer (make it positive)
if(number < 0) number = -number;
return number;
}
/* The entry point is FRC_UserProgram_StartupLibraryInit. Tell the linker where our code is. */
START_ROBOT_CLASS(SimpleTracker);
As the season has come to a close I would like to share the code that we used at our final competition in New York. Look over it, and try to use your new found knowledge from the tutorials you've all been reading (I hope) to understand bits and pieces of it. Unfortunately the forum doesn't support copy and paste indentation so its a little hard to read.
/******************************************************************************
*
* Sprockets 2012 Base Code
* Authors: Brendan van Ryn
* Stuart Sullivan
* Aliah McCalla
* Will Surmak
******************************************************************************/
// Include our own header file with our own macros
#include "NewRobot.h"
// Header file for WPI standard hardware/robot overhead.
#include "WPILib.h"
// Header file for math functions
#include <math.h>
//Header file for time functions
#include <time.h>
// Macro Functions
// Clears the Driver Station output display
#define CLEARMESSAGE DriverStationLCD::GetInstance()->Clear()
// Provides full free-form formatting, as per the usual printf(). Displays formatted text, a, on Driver Station on line, b.
#define DISPLAYPRINTF(b, a, ...) {DriverStationLCD::GetInstance()->Printf(DriverStationLCD::kUser_Line ## b, 1, a, ## __VA_ARGS__); DriverStationLCD::GetInstance()->UpdateLCD();}
/* Contains the macro definitions for testing on the new robot. */
#warning "New robot"
// Button macros
#define SWERVEMODEBUTTON rightStick->GetRawButton(1)
#define MONSTERMODEBUTTON leftStick->GetRawButton(1)
#define CARMODEBUTTON rightStick->GetRawButton(6)
#define LOCKMODEBUTTON rightStick->GetRawButton(5)
#define BRAKEMODEBUTTON leftStick->GetRawButton(2)
#define TANKTURNINGMODEBUTTON rightStick->GetRawButton(3)
#define REVERSEDRIVEBUTTON rightStick->GetRawButton(4)
#define STRAIGHTMODEBUTTON rightStick->GetRawButton(8)
#define PIVOTMODEBUTTON rightStick->GetRawButton(2)
#define MANUALALIGNENABLEBUTTON (leftStick->GetRawButton(8) && leftStick->GetRawButton(9))
#define MANUALALIGNLEFTREAR leftStick->GetRawButton(7)
#define MANUALALIGNRIGHTREAR leftStick->GetRawButton(10)
#define MANUALALIGNLEFTFRONT leftStick->GetRawButton(6)
#define MANUALALIGNRIGHTFRONT leftStick->GetRawButton(11)
#define BRIDGEMANIPULATEBUTTON rightStick->GetRawButton(77)
#define HIGHGEARBUTTON speedStick->GetRawButton(8) //arm lock
#define LOWGEARBUTTON speedStick->GetRawButton(9)// arm unlock
#define NEUTRALGEARBUTTON leftStick->GetRawButton(3)
#define ARMUNWINDBUTTON rightStick->GetRawButton(11)
#define PIVOTMODEBUTTON rightStick->GetRawButton(2)
#define SHOOTBUTTON (speedStick->GetRawButton(1) && (speedStick->GetRawButton(3) || speedStick->GetRawButton(4) || speedStick->GetRawButton(5)))
#define FIRSTFIREBUTTON speedStick->GetRawButton(4)
#define SECONDFIREBUTTON speedStick->GetRawButton(3)
#define THIRDFIREBUTTON speedStick->GetRawButton(5)
#define FOURTHFIREBUTTON speedStick->GetRawButton(88)
#define BRIDGEMANIPULATORRAISEBUTTON speedStick->GetRawButton(6)
#define BRIDGEMANIPULATORLOWERBUTTON speedStick->GetRawButton(7)
#define MAGAZINEENABLEBUTTON speedStick->GetRawButton(11)
#define MAGAZINEREVERSEBUTTON speedStick->GetRawButton(10)
#define COLLECTORENABLEBUTTON speedStick->GetRawButton(11)
#define COLLECTORREVERSEBUTTON speedStick->GetRawButton(10)
#define MYSTERYBUTTON speedStick->GetRawButton(2)
// Port numbers
// Motors are on PWM ports
#define FRONTMOTORPORT 3
#define REARMOTORPORT 4
#define FRONTSTEERINGMOTORPORT 2
#define REARSTEERINGMOTORPORT 1
#define BOTTOMSHOOTINGMOTORPORT 9
#define LATERALCOLLECTORPORT 10
#define BRIDGEMANIPULATORMOTORPORT 7
// The gear-shifting servos are on PWM ports
#define FRONTSERVOPORT 5
#define REARSERVOPORT 6
#define ARMENGAGESERVOPORT 8
// Joysticks are on USB ports
#define LEFTJOYSTICKPORT 4
#define RIGHTJOYSTICKPORT 1
#define SPEEDJOYSTICKPORT 3
// Encoders are on Digital IO ports
#define FRONTSTEERINGENCODERPORTA 3
#define FRONTSTEERINGENCODERPORTB 4
#define REARSTEERINGENCODERPORTA 1
#define REARSTEERINGENCODERPORTB 2
#define DRIVEENCODERPORTA 14
#define DRIVEENCODERPORTB 13
// Limit switches are digital inputs
#define ARMDOWNSWITCHPORT 7
#define ARMUPSWITCHPORT 8
// Light sensors are also digital inputs
#define ARMSTARTPOSITIONPORT 9
#define ARMHITPOSITIONPORT 10
// Analogue channels (analogue board input)
#define STEERINGGYROPORT 2
#define ANALOGCHANNELPORT 4
// The autonomous switches enable/disable digital inputs
#define AUTONOMOUSSWITCHPORTONE 5
#define AUTONOMOUSSWITCHPORTTWO 6
// Relays have their own ports
#define MAGAZINERELAYPORT 2
#define MAGAZINERELAY2PORT 5
#define TRANSVERSECOLLECTORPORT 1
// Constants
// Hardware constants
#define GYROSENSITIVITY 0.007f // Volts per degree per second
#define PULSESPER90FRONT 1350 // Number of encoder pulses in a 90-degree turn (front steering)
#define PULSESPER90REAR 600 // Number of encoder pulses in a 90-degree turn (rear steering)
#define STEERINGDEADBANDFRONT 40 // Number of encoder pulses of proximity needed for the front steering
#define STEERINGAPPROACHDISTANCEFRONT 100 // Number of encoder pulses of proximity after which the wheels begin to slow down
#define STEERINGDEADBANDREAR 10 // Number of encoder pulses of proximity needed for the rear steering
#define STEERINGAPPROACHDISTANCEREAR 60 // Number of encoder pulses of proximity after which the wheels begin to slow down
#define MAGAZINEALTERNATORLIMIT 2000 // The number of times the IsOpteratorControl() loop must run before the magazine alternator resets
#define MAGAZINEALTERNATORLIMITONE 1800 // The number of times the IsOperatorControl() loop must run before the magazine reverses
// Mask values for TrackDrive() function (a mask, because 1 OR 2 == 3)
#define FRONT_MOTOR 1 // Tells the TrackDrive() Function to run the front motor
#define REAR_MOTOR 2 // Tells the TrackDrive() Function to run the rear motor
#define BOTH_MOTORS 3 // Tells the TrackDrive() Function to run both motors
// Hardware threshold constants
#define JOYSTICKDEADBANDX 0.08f // Minimum detectable joystick deflection
#define JOYSTICKDEADBANDY 0.08f
#define GYRODEADBANDANGLE 3.0f // Allowable error for gyro angle when tracking
#define DRIVEAPPROACHDISTANCE 70.0f // Distance at which the drive motors begin to slow during brake mode
#define DRIVEDEADBAND 2.0f // Allowable error for brake mode encoder
// Other constants
#define Pi 3.1415926535897932384626433832795028841971f
#define SWERVEMODE 0 // Drive modes
#define MONSTERMODE 1
#define CARMODE 2
#define LOWGEARPOSITION 160.0f // Angles for the gear-shifting servos
#define HIGHGEARPOSITION 70.0f
#define NEUTRALGEARPOSITION 110.0f
#define ARMENGAGEPOSITION 70.0f
#define ARMDISENGAGEPOSITION 120.0f
#define MAGAZINEREVERSETIME 700 // Time for magazine to reverse after running
#define SERVOPAUSETIME 1000 // Time to wait for arm-engaging servo to shift
#define AUTONOMOUSDISTANCE 1900 // Maximum distance to travel in autonomous
#define MAGAZINEPAUSETIME 400 // Amount of time to wait during autonomous for rollers to spin up to speed
#define AUTOSHOOTPAUSETIME 13000 // Amount of time to wait during autonomous before moving (third mode only)
#define AUTOBRIDGEPAUSETIME 8000 //Amount of time to wait during autonomous before raising the bridge manipulator
#define AUTOBRIDGEPAUSETIMETWO 6500 //Amount of time to wait for the bridge manipulator to raise
#define TARGETENCODERVALUE 2500 //Distance to get to the bridge in autonomous
#define TARGETENCODERVALUETWO 5400 // Target distance to move forward towards the second bridge in both bridge autonomous
#define TARGETREVERSEENCODERVALUE 2250 //Target value to reverse to in both bridge autonomous
#define STRAFEDISTANCE 4650 // Distance to strafe in first autonomous mode
#define STRAFEDISTANCETWO 1050 //Target strafe distance in shoot then strafe autonomous mode
#define TARGETENCODERVALUETHREE 3900
#define SHOOTERAPPROACHSPEED 0.0f // Distance from target speed at which the rollers begin to ramp down
#define SHOOTERCONSTANTSPEED 0.1f // Minimum power value for the shooter (a maintenance value)
#define SHOOTERDEADBAND 0.0f // Distance from target speed at which the roller speed is considered to be within a good enough threshold
#define COLLECTORSPEED -1.00f // Speed of collector motors
#define ARMDOWNTIME 8000 // Time in autonomous mode to pause when the arm first lowers
// Shooter constants
#define FIRESPEEDONE 6.0f http://5.65f Pre-set firing speeds
#define FIRESPEEDTWO 10.0f // full power
#define FIRESPEEDTHREE 6.8f // Our key shot
#define FIRESPEEDFOUR 6.8f //Note this is the autonomous value
// Prototypes for functions (tells the compiler the format)
float Abs(float); // Returns the absolute value of a number
int AbsI(int); // Returns the absolute value of an integer
// Our main and only class. You could consider this an alteration of "int main(void)" if it makes it easier.
class SimpleTracker : public SimpleRobot
{
// Global variables accessible to entire program
Joystick *leftStick; // Left driver joystick
Joystick *rightStick; // Right driver joystick
Joystick *speedStick; // Co-driver joystick
Victor *frontMotor; // Front drive motor
Victor *rearMotor; // Rear drive motor
Victor *bridgeManipulatorMotor; //Motor to raise and lower the bridge manipulator
Jaguar *frontSteer; // Front steering motor
Jaguar *rearSteer; // Rear steering motor
Jaguar *lateralCollector; // Lateral ball-collector rollers
Jaguar *bottomShooter; // Bottom shooting roller
Encoder *frontSteeringEncoder; // Front steering encoder
Encoder *rearSteeringEncoder; // Rear steering encoder
Encoder *driveEncoder; // Drive wheel encoder
Gyro *steeringGyro; // Steering gyro
Relay *magazineBelt; // Magazine conveyor
Relay *magazineBelt2; // Second magazine conveyor motor
Relay *transverseCollector; // Transverse collector belts
DigitalInput *armDownSwitch; // The limit switch that indicates the bridge manipulator is down
DigitalInput *armUpSwitch; // The limit switch that indicates that the bridge manipulator is up
DigitalInput *armStartSensor; // The light sensor that detects the primed position for the bridge manipulator
DigitalInput *armHitSensor; // The light sensor that detects when the bridge manipulator hits the bridge
DigitalInput *autonomousSwitchOne; // The autonomous switch that determines our autonomous mode
DigitalInput *autonomousSwitchTwo; // Another autonomous switch that determines our autonomous mode
AnalogChannel *rollerVoltage; // Voltage reading from shooting rollers (for speed)
Servo *frontDriveServo; // The servo to shift gears for the front gear box
Servo *rearDriveServo; // The servo to shift gears for the rear gear box
Servo *armEngageServo; // The servo to engage the bridge manipulator arm
/* "Constructor". This code is run when the robot is first turned on. */
public:
SimpleTracker(void)
{
// Assign object pointers here.
frontMotor = new Victor(FRONTMOTORPORT); // Driving Motors
rearMotor = new Victor(REARMOTORPORT);
frontSteer = new Jaguar(FRONTSTEERINGMOTORPORT); // Steering Motors
rearSteer = new Jaguar(REARSTEERINGMOTORPORT);
bottomShooter = new Jaguar(BOTTOMSHOOTINGMOTORPORT); // Other motors
lateralCollector = new Jaguar(LATERALCOLLECTORPORT);
bridgeManipulatorMotor = new Victor(BRIDGEMANIPULATORMOTORPORT);//Bridge manipulator motor
leftStick = new Joystick(LEFTJOYSTICKPORT); // Joysticks
rightStick = new Joystick(RIGHTJOYSTICKPORT);
speedStick = new Joystick(SPEEDJOYSTICKPORT);
frontSteeringEncoder = new Encoder(FRONTSTEERINGENCODERPORTA, FRONTSTEERINGENCODERPORTB); // Encoders
rearSteeringEncoder = new Encoder(REARSTEERINGENCODERPORTA, REARSTEERINGENCODERPORTB);
driveEncoder = new Encoder(DRIVEENCODERPORTA, DRIVEENCODERPORTB);
steeringGyro = new Gyro(1, STEERINGGYROPORT); // Analogue devices
rollerVoltage = new AnalogChannel(ANALOGCHANNELPORT);
magazineBelt = new Relay(MAGAZINERELAYPORT); // Relays
magazineBelt2 = new Relay(MAGAZINERELAY2PORT);
transverseCollector = new Relay(TRANSVERSECOLLECTORPORT);
armStartSensor = new DigitalInput(ARMSTARTPOSITIONPORT); // Light sensors
armHitSensor = new DigitalInput(ARMHITPOSITIONPORT);
autonomousSwitchOne = new DigitalInput(AUTONOMOUSSWITCHPORTONE); // Switches
autonomousSwitchTwo = new DigitalInput(AUTONOMOUSSWITCHPORTTWO);
armDownSwitch = new DigitalInput(ARMDOWNSWITCHPORT);
armUpSwitch = new DigitalInput(ARMUPSWITCHPORT);
frontDriveServo = new Servo(FRONTSERVOPORT); // Servos
rearDriveServo = new Servo(REARSERVOPORT);
armEngageServo = new Servo(ARMENGAGESERVOPORT);
// Initialize encoders
frontSteeringEncoder->Reset();
frontSteeringEncoder->Start();
rearSteeringEncoder->Reset();
rearSteeringEncoder->Start();
driveEncoder->Reset();
driveEncoder->Start();
// Initialize gyro
steeringGyro->Reset();
steeringGyro->SetSensitivity(GYROSENSITIVITY);
}
/* Code to run while in tele-op, or driver-controlled mode. */
void OperatorControl(void)
{
// Variables
float leftx, rightx, lefty, righty; // The x- and y-positions of the two driver joysticks
float speedz; // The position of the co-driver throttle
float x, y; // The average x- and y-positions between the two driver joysticks
float swerveSpeed; // The drive motor speed when in swerve-drive mode
float swerveAngle; // The angle to turn the wheels in swerve-drive mode
float swerveAngleFront, swerveAngleRear; // The individual angles for the front and back sides, for use in different steering modes
int steeringMode = SWERVEMODE; // Store the current mode of steering
int currentButtonState = 0, previousButtonState = 0; // Store the current and previous states of the manual alignment buttons
int driveWheelEncoderValue; // Distance travelled by robot
float frontGearSetting = LOWGEARPOSITION; // Store the angle setting for the servos
float rearGearSetting = LOWGEARPOSITION;
float armPosition = LOWGEARPOSITION;
int armStage = 0; // Current stage of bridge manipulator
int magazineReverseCount = 0; // Allow the magazine to reverse for a bit before shooting
int servoPauseCount = 0; // Give the servos time to shift
int magazineAlternator = 0; // Alternate the direction of the magazine when firing to provide spacing for the balls
int preFire = 0; // Are we trying to use a pre-set firing speed?
int brakeValue = -1; // Encoder value to maintain when in brake mode
int armOverride = 0; // Override flag for manual winding of bridge manipulator
double shooterSpeed = 0.0; // Voltage from motor to read roller speed
int direction = 1; // Joystick direction
int reverseToggleCurrent = 0; // Previous state of reversing button
int reverseTogglePrevious = 0; // Current state of reversing button
// Reset the drive and shooter encoders, and the timer
driveEncoder->Reset();
driveEncoder->Start();
// This code is run repeatedly so long as the robot is in teleop mode.
while(IsOperatorControl())
{
/* Tell the operating system that our program hasn't frozen */
GetWatchdog().Feed();
/* Message displays */
CLEARMESSAGE;
DISPLAYPRINTF(1, "Shooter Speed: %0.3f", shooterSpeed); // The speed of the shooting roller
DISPLAYPRINTF(2, "Distance: %d", driveWheelEncoderValue); // Value of drive wheel encoder
// Get joystick positions ( x's will always be the same direction)
leftx = -1 * leftStick->GetRawAxis(1);
lefty = direction * leftStick->GetRawAxis(2);
rightx = -1 * rightStick->GetRawAxis(1);
righty = direction * rightStick->GetRawAxis(2);
speedz = (speedStick->GetRawAxis(3)- 1.0f) / 2.0f * -8.0f; // Scale from 0 to 8
// Get voltage from analog channel
shooterSpeed = rollerVoltage->GetVoltage();
if(shooterSpeed < 0.0f) shooterSpeed = 0.0f; // Positive voltages only
// Prevent back-rolling
if(speedz < 0.0f) speedz = 0.0f;
// Run the servo counter
if(servoPauseCount > 0)
servoPauseCount--;
// Run the magazine alternator up to a pre-determined value, then reset
if(magazineAlternator > MAGAZINEALTERNATORLIMIT) magazineAlternator = 0;
else magazineAlternator++;
// Check drive mode buttons
if(SWERVEMODEBUTTON) steeringMode = SWERVEMODE;
else if(MONSTERMODEBUTTON) steeringMode = MONSTERMODE;
else if(CARMODEBUTTON) steeringMode = CARMODE;
//Check if the bridge manipulator needs to be raised or lowered
if(BRIDGEMANIPULATORRAISEBUTTON) bridgeManipulatorMotor->Set(0.7f);
else if(BRIDGEMANIPULATORLOWERBUTTON) bridgeManipulatorMotor->Set(-0.7f);
else bridgeManipulatorMotor->Set(0.0f);
// Check shooter buttons
if(FIRSTFIREBUTTON) SetRollerSpeed(FIRESPEEDONE, shooterSpeed), preFire = 1;
else if(SECONDFIREBUTTON) SetRollerSpeed(FIRESPEEDTWO, shooterSpeed), preFire = 1;
else if(THIRDFIREBUTTON) SetRollerSpeed(FIRESPEEDTHREE, shooterSpeed), preFire = 1;
else if(FOURTHFIREBUTTON) SetRollerSpeed(FIRESPEEDFOUR, shooterSpeed), preFire = 1;
else preFire = 0;
// Check gear shifting buttons
if(HIGHGEARBUTTON) frontGearSetting = rearGearSetting = HIGHGEARPOSITION, armPosition = ARMDISENGAGEPOSITION, armOverride = 0;
else if(LOWGEARBUTTON) frontGearSetting = rearGearSetting = LOWGEARPOSITION, armPosition = ARMDISENGAGEPOSITION, armOverride = 0;
else if(NEUTRALGEARBUTTON) frontGearSetting = LOWGEARPOSITION, rearGearSetting = NEUTRALGEARPOSITION, armPosition = ARMDISENGAGEPOSITION, armOverride = 0;
else if(BRIDGEMANIPULATEBUTTON && armPosition != ARMENGAGEPOSITION) frontGearSetting = LOWGEARPOSITION, rearGearSetting = NEUTRALGEARPOSITION, armPosition = ARMENGAGEPOSITION, servoPauseCount = SERVOPAUSETIME, armStage = 0, armOverride = 0;
else if(ARMUNWINDBUTTON) frontGearSetting = LOWGEARPOSITION, rearGearSetting = NEUTRALGEARPOSITION, armPosition = ARMENGAGEPOSITION, armOverride = 1;
else if(armOverride && !ARMUNWINDBUTTON) frontGearSetting = rearGearSetting = LOWGEARPOSITION, armPosition = ARMDISENGAGEPOSITION, armOverride = 0;
// Move servos to desired position
frontDriveServo->SetAngle(frontGearSetting);
rearDriveServo->SetAngle(rearGearSetting);
armEngageServo->SetAngle(armPosition);
// Get the drive wheel encoder value
driveWheelEncoderValue = driveEncoder->Get();
// If the brake button isn't being pressed, store a sentinel value as the brake value for reference
if(!BRAKEMODEBUTTON) brakeValue = -1;
// Check magazine buttons
if(MAGAZINEENABLEBUTTON || SHOOTBUTTON)
{
// Move forward for a set number of counts, then reverse for the rest in each cycle of the alternator
if(magazineAlternator < MAGAZINEALTERNATORLIMITONE) magazineBelt->Set(Relay::kForward), magazineBelt2->Set(Relay::kForward);
else magazineBelt->Set(Relay::kReverse), magazineBelt2->Set(Relay::kReverse);
// Prepare to revers when the button is released
magazineReverseCount = MAGAZINEREVERSETIME;
}
else if(MAGAZINEREVERSEBUTTON) magazineBelt->Set(Relay::kReverse), magazineBelt2->Set(Relay::kReverse);
else if(magazineReverseCount > 0) magazineBelt->Set(Relay::kReverse), magazineBelt2->Set(Relay::kReverse), magazineReverseCount--;
else magazineBelt->Set(Relay::kOff), magazineBelt2->Set(Relay::kOff);
// Check collector buttons (preFire prevents speed settings from affecting the back rolling of the shooter)
if(COLLECTORENABLEBUTTON) lateralCollector->Set(COLLECTORSPEED), transverseCollector->Set(Relay::kReverse), bottomShooter->Set(-0.45), preFire = 1;
else if(COLLECTORREVERSEBUTTON) lateralCollector->Set(-COLLECTORSPEED), transverseCollector->Set(Relay::kForward);
else
{
lateralCollector->Set(0.0f);
transverseCollector->Set(Relay::kOff);
if(lateralCollector->Get() > 0.1f) preFire = 0;
}
// Store the states of the toggle buttons from the previous iteration
previousButtonState = currentButtonState;
reverseTogglePrevious = reverseToggleCurrent;
// Check drive-reverse button
if(REVERSEDRIVEBUTTON) reverseToggleCurrent = 1;
else reverseToggleCurrent = 0;
// Check manual-alignment button
if(MANUALALIGNENABLEBUTTON) currentButtonState = 1;
else currentButtonState = 0;
// If the total y-deflection for each joystick is below the threshold, we shouldn't count it at all, allowing perfect strafing
if(Abs(lefty) < JOYSTICKDEADBANDY) lefty = 0.0f;
if(Abs(righty) < JOYSTICKDEADBANDY) righty = 0.0f;
//Averaging joystick positions
x = (leftx + rightx) / 2.0f;
y = (lefty + righty) / 2.0f;
// Making the joysticks cureved rather than linear
if(x > 0) x = x * x;
else if(x < 0) x = (-x * x);
if(y > 0) y = y * y;
else if(y < 0) y = (-y * y);
// Drive modes
// If the manual-alignment buttons are pressed
if(MANUALALIGNENABLEBUTTON)
{
// Check alignment buttons
if(MANUALALIGNLEFTREAR) rearSteer->Set(-0.8f);
else if(MANUALALIGNRIGHTREAR) rearSteer->Set(0.8f);
else rearSteer->Set(0);
if(MANUALALIGNLEFTFRONT) frontSteer->Set(-0.8f);
else if(MANUALALIGNRIGHTFRONT)frontSteer->Set(0.8f);
else frontSteer->Set(0);
}
// If the lock button is pressed
else if(LOCKMODEBUTTON)
{
// Lock the wheels to ninety degrees and drive tank-style
TrackFrontSteering(PULSESPER90FRONT);
TrackRearSteering(PULSESPER90REAR);
frontMotor->Set(y);
rearMotor->Set(y);
}
// If the brake buttons is pressed
else if(BRAKEMODEBUTTON)
{
// Store value to hold
if(brakeValue == -1) brakeValue = driveWheelEncoderValue;
// Keep wheels straight
TrackFrontSteering(0);
TrackRearSteering(0);
// Maintain position
TrackDrive(brakeValue, BOTH_MOTORS);
}
// If the tank turning button is pressed
else if(TANKTURNINGMODEBUTTON)
{
// Turn the wheels in opposite directions
TrackFrontSteering(PULSESPER90FRONT);
TrackRearSteering(-PULSESPER90REAR);
// Drive each end tank-style to rotate
frontMotor->Set(lefty);
rearMotor->Set(-righty);
}
// If the pivot mode button is pressed
else if(PIVOTMODEBUTTON)
{
// Keep the front wheels straight and turn the back wheels
TrackFrontSteering(0);
TrackRearSteering(PULSESPER90REAR);
// Drive the back to pivot
frontMotor->Set(0);
rearMotor->Set(rightx);
// Allow manual firing if no automatic firing is in use
if(!preFire) SetRollerSpeed(speedz, shooterSpeed);
}
//Keep the wheels straight so we can only move forward and backward
else if(STRAIGHTMODEBUTTON)
{
TrackFrontSteering(0);
TrackRearSteering(0);
frontMotor->Set(lefty);
rearMotor->Set(righty);
}
/* Otherwise, we drive normally. If the joysticks are not pushed left or right any significant amount,
* drive tank-style */
else if(Abs(x) < JOYSTICKDEADBANDX)
{
// Set the drive motors according to their respective joysticks
frontMotor->Set(lefty);
// Only move rear motor if we aren't in bride-manipulate mode
if(armPosition != ARMENGAGEPOSITION || servoPauseCount > 0 || armOverride) rearMotor->Set(righty);
// If not pre-set firing speed was selected, allow manual firing
if(!preFire) SetRollerSpeed(speedz, shooterSpeed);
// Keep the wheels straight
TrackFrontSteering(0);
TrackRearSteering(0);
}
else
{
// Determine our speed and angle of movement (force angle to be in first quadrant)
swerveSpeed = sqrt(x*x + y*y); // a^2 + b^2 = c^2
swerveAngle = -Abs(atan(x/y)); // x/y = tan(angle), therefore angle = atan(x/y); negative because of motor direction
swerveAngle = swerveAngle * 180 / Pi; // 1 radian = Pi/180 radians, so reverse
swerveAngle = swerveAngle / 90.0f; // Convert from degrees to a percentage of 90 degrees
// Set roller speeds manually if no pre-set speed has been selected
if(!preFire) SetRollerSpeed(speedz, shooterSpeed);
// Determine the quadrant of the joystick and convert the speed or angle appropriately
// We assume we're in quadrant one if none of these are true
if(x < 0 && y > 0) swerveAngle = -swerveAngle; // Quadrant two, reverse angle
else if(x < 0 && y <= 0) swerveSpeed = -swerveSpeed; // Quadrant three, reverse drive direction
else if(x >= 0 && y <= 0) // Quadrant four, reverse both
{
swerveSpeed = -swerveSpeed;
swerveAngle = -swerveAngle;
}
// If we are in monster mode and travelling faster than 80% max speed, we switch to car automatically (max speed is sqrt(2))
if(Abs(swerveSpeed) > 1.2f && steeringMode == MONSTERMODE) steeringMode = CARMODE;
else if(Abs(swerveSpeed < 1.2f && steeringMode == CARMODE)) steeringMode = MONSTERMODE;
// In monster, we reverse either the front or back steering depending on which direction we're travelling
if(steeringMode == MONSTERMODE)
{
// Multiplying by 0.75f makes the steering less sensitive as it has a smaller range
if(swerveSpeed <= 0) swerveAngleFront = -0.75f * swerveAngle, swerveAngleRear = 0.75f * swerveAngle;
else swerveAngleFront = 0.75f * swerveAngle, swerveAngleRear = -0.75f * swerveAngle;
}
// In car, we move only the front or only the back wheels, depending on which direction we're travelling
else if(steeringMode == CARMODE)
{
// Multiplying by 0.5f gives us 45 degrees at most of steering
if(swerveSpeed <= 0) swerveAngleFront = -0.5f * swerveAngle, swerveAngleRear = 0.0f;
else swerveAngleRear = -0.5f * swerveAngle, swerveAngleFront = 0.0f;
}
else
{
// Otherwise, in swerve, both sides get the same angle (negative because of the motor polarity)
swerveAngleFront = -swerveAngle;
swerveAngleRear = -swerveAngle;
}
// Convert front and rear angle percentages into pulses
swerveAngleFront *= PULSESPER90FRONT;
swerveAngleRear *= PULSESPER90REAR;
// Turn steering to desired angle
TrackFrontSteering((int)swerveAngleFront);
TrackRearSteering((int)swerveAngleRear);
// Drive at the calculated speed
frontMotor->Set(swerveSpeed);
// Only move rear wheels if bride manipulate mode is not engaged
if(armPosition != ARMENGAGEPOSITION || servoPauseCount > 0 || armOverride) rearMotor->Set(swerveSpeed);
}
// If the manual alignment buttons been released having previously being pressed, reset the encoders after aligning
if(previousButtonState == 1 && currentButtonState == 0)
{
frontSteeringEncoder->Reset();
frontSteeringEncoder->Start();
rearSteeringEncoder->Reset();
rearSteeringEncoder->Start();
frontSteer->Set(0);
rearSteer->Set(0);
}
// If the drive reverse-button was pressed and has been released, reverse the direction of the joysticks (for driving backwards)
if(reverseTogglePrevious == 1 && reverseToggleCurrent == 0) direction *= -1;
}
} // End of teleop
/* Code to run while in autonomous, or fully-automated mode. */
void Autonomous(void)
{
// Variables
int firstSwitch, secondSwitch; // Store the states of the autonomous switches
// Store the values of the switches for the autonomous modes
firstSwitch = autonomousSwitchOne->Get();
secondSwitch = autonomousSwitchTwo->Get();
// Check the states of switches and call the correct autonomous mode
if(firstSwitch && secondSwitch) AutonomousOne(); // Middle position is shoot then strafe
else if(firstSwitch && !secondSwitch) AutonomousTwo(); // To the right is shoot only
else if(secondSwitch && !firstSwitch) AutonomousThree(); // To the left is shoot then tip bridge
else
{
// Display an error if the switch setting is invalid (maybe the switch is unplugged or something)
CLEARMESSAGE;
DISPLAYPRINTF(1, "No autonomous switch");
}
} // End of Autonomous
/* Move the front wheels to the angle passed to this function in "targetPosition" */
int TrackFrontSteering(int targetPosition)
{
int frontSteeringEncoderValue; // Current wheel position
int remainingSteeringDistance; // Remaining distance to target position
float speed; // The speed to set the steering motor to
// Get the current position of the wheels from the encoder and calculate the distance to the target position
frontSteeringEncoderValue = frontSteeringEncoder->Get();
remainingSteeringDistance = AbsI(frontSteeringEncoderValue - targetPosition);
/* If the steering is within limits, we simply stop. Otherwise, we move either left or right. The division works thus:
* if the wheel is further away from the target than the approach distance, speed will be greater than one; otherwise,
* speed will be some percentage, linearly ramping the speed as it approaches zero. */
if(remainingSteeringDistance < STEERINGDEADBANDFRONT) speed = 0.0f;
else if(frontSteeringEncoderValue < targetPosition) speed = (float)remainingSteeringDistance / (float)STEERINGAPPROACHDISTANCEFRONT;
else speed = (float)-remainingSteeringDistance / (float)STEERINGAPPROACHDISTANCEFRONT;
// If we end up with an aforementioned speed greater than one (100%), we clamp it down to exactly 100%
if(speed > 1.0f) speed = 1.0f;
else if(speed < -1.0f) speed = -1.0f;
// Turn the motor at the calculated speed
frontSteer->Set(speed);
/* If the motor is hardly moving, the wheels are where they need to be, so return 0. Otherwise, set the motor speed and
* return 1 to indicate that it is still moving. */
if(speed == 0.0f) return 0;
else return 1;
}
/* This function is the same as the above function, but for the rear wheels, instead. */
int TrackRearSteering(int targetPosition)
{
int rearSteeringEncoderValue;
int remainingSteeringDistance;
float speed;
rearSteeringEncoderValue = rearSteeringEncoder->Get();
remainingSteeringDistance = AbsI(rearSteeringEncoderValue - targetPosition);
if(remainingSteeringDistance < STEERINGDEADBANDREAR) speed = 0.0f;
else if(rearSteeringEncoderValue < targetPosition) speed = (float)remainingSteeringDistance / (float)STEERINGAPPROACHDISTANCEREAR;
else speed = (float)-remainingSteeringDistance / (float)STEERINGAPPROACHDISTANCEREAR;
if(speed > 1.0f) speed = 1.0f;
else if(speed < -1.0f) speed = -1.0f;
rearSteer->Set(speed);
if(speed == 0.0f) return 0;
else return 1;
}
// Maintains a specific speed for the shooting roller. Returns true when roller is at that speed (similar to the steering)
int SetRollerSpeed(float targetSpeed, float currentSpeed)
{
float powerSetting;
// Calculate a power setting based on distance from target speed
powerSetting = Abs(targetSpeed - currentSpeed); // / SHOOTERAPPROACHSPEED + SHOOTERCONSTANTSPEED;
if(powerSetting > 1.0f) powerSetting = 1.0f;
// We can only speed up or stop. Reversing would strip the gears
if(currentSpeed < targetSpeed) bottomShooter->Set(powerSetting);
else bottomShooter->Set(0.0f);
return (Abs(targetSpeed - currentSpeed) < SHOOTERDEADBAND);
}
// This function attempts to maintain a specific encoder value on the drive block and is also similar to the steering
float TrackDrive(int targetPosition, char motorMask)
{
float distance; // Store the distance of the drive wheels from their target position
int currentPosition; // Current encoder position
// Get the current encoder position
currentPosition = driveEncoder->Get();
// Calculate the absolute distance and scale to a power setting
distance = (float)AbsI(targetPosition - currentPosition);
distance /= DRIVEAPPROACHDISTANCE;
// Move forward or backward at a proportional pace until within a deadband
if(currentPosition < targetPosition - DRIVEDEADBAND) distance *= 1.0f;
else if(currentPosition > targetPosition + DRIVEDEADBAND) distance *= -1.0f;
else distance = 0.0f;
// Move only the motors we want (either just the front, just the back, or both)
if(motorMask == FRONT_MOTOR || motorMask == BOTH_MOTORS) frontMotor->Set(distance);
if(motorMask == REAR_MOTOR || motorMask == BOTH_MOTORS) rearMotor->Set(distance);
// Return motor speed
return distance;
}
// Code for our first possible autonomous mode
void AutonomousOne(void) //middle
{
// Variables
int rollerPause = MAGAZINEPAUSETIME; // The time to wait for the rollers to spin up before enabling the magazine
int shootPause = AUTOSHOOTPAUSETIME; // The time to wait for the balls to finish shooting in autonomous
float shooterSpeed = 0.0f; // Current voltage reading
int encoderValue = 0; // Value from drive encoder
// Reset the drive encoder and gyro
steeringGyro->Reset();
driveEncoder->Reset();
driveEncoder->Start();
while(IsAutonomous())
{
/* Tell the system that the code hasn't frozen. */
GetWatchdog().Feed();
// Message displays
CLEARMESSAGE;
DISPLAYPRINTF(1, "Shoot then strafe"); // Display which autonomous mode is being used
// Get current encoder value and shooter speed
encoderValue = AbsI(driveEncoder->Get());
shooterSpeed = rollerVoltage->GetVoltage();
// Set servos
frontDriveServo->SetAngle(LOWGEARPOSITION);
rearDriveServo->SetAngle(LOWGEARPOSITION);
// Wait for the rollers to spin up before engaging the magazine belts to fire
if(rollerPause > 0)
{
// Spin up the rollers to the preset speed for the key shot
SetRollerSpeed(FIRESPEEDFOUR, shooterSpeed);
rollerPause--;
// Keep the wheels straight
TrackFrontSteering(0);
TrackRearSteering(0);
}
// Fire the ball for a period of time
else if(shootPause > 0)
{
// Fire
magazineBelt->Set(Relay::kForward);
magazineBelt2->Set(Relay::kForward);
shootPause--;
// Spin up the rollers to the preset speed for the key shot
SetRollerSpeed(FIRESPEEDTWO, shooterSpeed);
// Keep the wheels straight
TrackFrontSteering(0);
TrackRearSteering(0);
}
// Strafe after shooting for a certain distance
else if(encoderValue < STRAFEDISTANCETWO)
{
TrackFrontSteering((int)(-PULSESPER90FRONT * 0.78));
TrackRearSteering((int)(-PULSESPER90REAR * 0.78));
frontMotor->Set(0.75f);
rearMotor->Set(0.75f);
}
// Move forward until we hit the bridge
else if(encoderValue < TARGETENCODERVALUETHREE)
{
// Stop rollers
SetRollerSpeed(0.0f, shooterSpeed);
// Get the drive encoder value
encoderValue = AbsI(driveEncoder->Get());
// Keep the front wheels straight and drive forward slowly
TrackFrontSteering(0);
TrackRearSteering(0);
// Drive forward
frontMotor->Set(0.3f);
rearMotor->Set(0.3f);
// If our current encoder value is less then that of our target / 1.5 we lower the bridge manipulator
if(encoderValue < TARGETENCODERVALUETHREE / 1.5)
{
bridgeManipulatorMotor->Set(-0.8f);
}
// If not we raise it until it locks
else if(encoderValue == TARGETENCODERVALUETHREE || encoderValue > TARGETENCODERVALUETHREE / 1.5)
{
bridgeManipulatorMotor->Set(0.8f);
//spin up the collecters to collect the tipped balls
transverseCollector->Set(Relay::kReverse);
lateralCollector->Set(COLLECTORSPEED);
}
else bridgeManipulatorMotor->Set(0.0f);
}
// Stop
else
{
TrackFrontSteering(0);
TrackRearSteering(0);
frontMotor->Set(0.0f);
rearMotor->Set(0.0f);
bridgeManipulatorMotor->Set(0.0f);
transverseCollector->Set(Relay::kReverse);
lateralCollector->Set(COLLECTORSPEED);
}
}
}
void AutonomousTwo(void)
{
// Variables
int rollerPause = MAGAZINEPAUSETIME; // The time to wait for the rollers to spin up before enabling the magazine
int shootPause = AUTOSHOOTPAUSETIME; // The time to wait for the balls to finish shooting in autonomous
float shooterSpeed = 0.0f; // Current voltage reading
int encoderValue = 0; // Value from drive encoder
int targetEncoderValue = TARGETENCODERVALUE; //target value we need to get to
float rearSetting = LOWGEARPOSITION, frontSetting = LOWGEARPOSITION, armSetting = LOWGEARPOSITION; // Servo posiions
int movePause = 25000;
int flag = 1;
int flag2 = 1;
// Reset the drive encoder and gyro
driveEncoder->Reset();
driveEncoder->Start();
while(IsAutonomous())
{
/* Tell the system that the code hasn't frozen. */
GetWatchdog().Feed();
// Display messages
CLEARMESSAGE;
DISPLAYPRINTF(1, " tip both bridges"); // Display which autonomous mode is being used
DISPLAYPRINTF(3, "Encoder: %d", encoderValue); // The value of the encoder
// Get encoder value and roller speed
encoderValue = AbsI(driveEncoder->Get());
shooterSpeed = rollerVoltage->GetVoltage();
// Set servos
frontDriveServo->SetAngle(frontSetting);
rearDriveServo->SetAngle(rearSetting);
// Move forward until we hit the bridge
if(encoderValue < targetEncoderValue && flag == 1)
{
// Stop rollers
SetRollerSpeed(0.0f, shooterSpeed);
// Get the drive encoder value
encoderValue = AbsI(driveEncoder->Get());
// Keep the front wheels straight and drive forward slowly
TrackFrontSteering(0);
TrackRearSteering(0);
// Drive forward
frontMotor->Set(0.7f);
rearMotor->Set(0.7f);
// If our current encoder value is less then that of our target / 1.5 we lower the bridge manipulator
if(encoderValue < targetEncoderValue / 1.5)
{
bridgeManipulatorMotor->Set(-0.8f);
}
// If not we raise it until it locks
else if(encoderValue == targetEncoderValue || encoderValue > targetEncoderValue / 1.5)
{
bridgeManipulatorMotor->Set(0.8f);
//spin up the collecters to collect the tipped balls
//transverseCollector->Set(Relay::kReverse);=
//lateralCollector->Set(COLLECTORSPEED);
}
else
{
bridgeManipulatorMotor->Set(0.0f);
//spin up the collecters to collect the tipped balls
//transverseCollector->Set(Relay::kReverse);
//lateralCollector->Set(COLLECTORSPEED);
}
}
else if(movePause > 0)
{
frontMotor->Set(0.0f);
rearMotor->Set(0.0f);
//transverseCollector->Set(Relay::kReverse);
//lateralCollector->Set(COLLECTORSPEED);
movePause--;
}
else if(encoderValue > targetEncoderValue && flag == 1)
{
flag = 0;
}
else if(encoderValue > TARGETREVERSEENCODERVALUE && flag == 0 && flag2 == 1)
{
encoderValue = AbsI(driveEncoder->Get());
TrackFrontSteering(0);
TrackRearSteering(0);
frontMotor->Set(-0.3f);
rearMotor->Set(-0.3f);
}
else if(encoderValue < TARGETREVERSEENCODERVALUE && flag2 == 1)
{
flag2 = 0;
}
else if(encoderValue < STRAFEDISTANCE)
{
TrackFrontSteering((int)(-PULSESPER90FRONT)); //TrackFrontSteering((int)(-PULSESPER90FRONT * 0.78));
TrackRearSteering((int)(-PULSESPER90REAR)); //TrackRearSteering((int)(-PULSESPER90REAR * 0.78));
frontMotor->Set(0.75f);
rearMotor->Set(0.75f);
}
// Move forward until we hit the bridge
else if(encoderValue < TARGETENCODERVALUETWO)
{
// Get the drive encoder value
encoderValue = AbsI(driveEncoder->Get());
// Keep the front wheels straight and drive forward slowly
TrackFrontSteering(0);
TrackRearSteering(0);
// Drive forward
frontMotor->Set(0.4f);
rearMotor->Set(0.4f);
}
else
{
//stop the bridge manipulator
bridgeManipulatorMotor->Set(0.0f);
//stop the motors
frontMotor->Set(0.0f);
rearMotor->Set(0.0f);
}
}
}
// Third of the possible autonomous modes (to the left)
void AutonomousThree(void)
{
// Variables
int rollerPause = MAGAZINEPAUSETIME; // The time to wait for the rollers to spin up before enabling the magazine
int shootPause = AUTOSHOOTPAUSETIME; // The time to wait for the balls to finish shooting in autonomous
float shooterSpeed = 0.0f; // Current voltage reading
int encoderValue = 0; // Value from drive encoder
int targetEncoderValue = TARGETENCODERVALUE; //target value we need to get to
float rearSetting = LOWGEARPOSITION, frontSetting = LOWGEARPOSITION, armSetting = LOWGEARPOSITION; // Servo posiions
int movePause = 25000;
int flag = 1;
int flag2 = 1;
// Reset the drive encoder and gyro
driveEncoder->Reset();
driveEncoder->Start();
while(IsAutonomous())
{
/* Tell the system that the code hasn't frozen. */
GetWatchdog().Feed();
// Display messages
CLEARMESSAGE;
DISPLAYPRINTF(1, "Shoot then tip both bridges"); // Display which autonomous mode is being used
DISPLAYPRINTF(3, "Encoder: %d", encoderValue); // The value of the encoder
// Get encoder value and roller speed
encoderValue = AbsI(driveEncoder->Get());
shooterSpeed = rollerVoltage->GetVoltage();
// Set servos
frontDriveServo->SetAngle(frontSetting);
rearDriveServo->SetAngle(rearSetting);
// Wait for the rollers to spin up before engaging the magazine belts to fire
if(rollerPause > 0res )
{
// Spin up the rollers to the preset speed for the key shot
SetRollerSpeed(FIRESPEEDFOUR, shooterSpeed);
rollerPause--;
// Keep wheels straight
TrackFrontSteering(0);
TrackRearSteering(0);
}
// Fire the ball
else if(shootPause > 0)
{
// Fire
magazineBelt->Set(Relay::kForward);
magazineBelt2->Set(Relay::kForward);
shootPause--;
// Spin up the rollers to the preset speed for the key shot
SetRollerSpeed(FIRESPEEDFOUR, shooterSpeed);
// Keep wheels straight
TrackFrontSteering(0);
TrackRearSteering(0);
}
// Move forward until we hit the bridge
else if(encoderValue < targetEncoderValue && flag == 1)
{
// Stop rollers
SetRollerSpeed(0.0f, shooterSpeed);
// Get the drive encoder value
encoderValue = AbsI(driveEncoder->Get());
// Keep the front wheels straight and drive forward slowly
TrackFrontSteering(0);
TrackRearSteering(0);
// Drive forward
frontMotor->Set(0.5f);
rearMotor->Set(0.5f);
// If our current encoder value is less then that of our target / 1.5 we lower the bridge manipulator
if(encoderValue < targetEncoderValue / 1.5)
{
bridgeManipulatorMotor->Set(-0.8f);
}
// If not we raise it until it locks
else if(encoderValue == targetEncoderValue || encoderValue > targetEncoderValue / 1.5)
{
bridgeManipulatorMotor->Set(0.8f);
//spin up the collecters to collect the tipped balls
transverseCollector->Set(Relay::kReverse);
lateralCollector->Set(COLLECTORSPEED);
}
else
{
bridgeManipulatorMotor->Set(0.0f);
//spin up the collecters to collect the tipped balls
transverseCollector->Set(Relay::kReverse);
lateralCollector->Set(COLLECTORSPEED);
}
}
else if(movePause > 0)
{
frontMotor->Set(0.0f);
rearMotor->Set(0.0f);
transverseCollector->Set(Relay::kReverse);
lateralCollector->Set(COLLECTORSPEED);
movePause--;
}
else if(encoderValue > targetEncoderValue && flag == 1)
{
flag = 0;
}
else if(encoderValue > TARGETREVERSEENCODERVALUE && flag == 0 && flag2 == 1)
{
encoderValue = AbsI(driveEncoder->Get());
TrackFrontSteering(0);
TrackRearSteering(0);
frontMotor->Set(-0.3f);
rearMotor->Set(-0.3f);
}
else if(encoderValue < TARGETREVERSEENCODERVALUE && flag2 == 1)
{
flag2 = 0;
}
else if(encoderValue < STRAFEDISTANCE)
{
TrackFrontSteering((int)(-PULSESPER90FRONT)); //TrackFrontSteering((int)(-PULSESPER90FRONT * 0.78));
TrackRearSteering((int)(-PULSESPER90REAR)); //TrackRearSteering((int)(-PULSESPER90REAR * 0.78));
frontMotor->Set(0.75f);
rearMotor->Set(0.75f);
}
// Move forward until we hit the bridge
else if(encoderValue < TARGETENCODERVALUETWO)
{
// Get the drive encoder value
encoderValue = AbsI(driveEncoder->Get());
// Keep the front wheels straight and drive forward slowly
TrackFrontSteering(0);
TrackRearSteering(0);
// Drive forward
frontMotor->Set(0.4f);
rearMotor->Set(0.4f);
}
else
{
//stop the bridge manipulator
bridgeManipulatorMotor->Set(0.0f);
//stop the motors
frontMotor->Set(0.0f);
rearMotor->Set(0.0f);
}
}
}
}; /* This is an exception to our semi-colon rule, but its one of very few */
// Custom functions
float Abs(float number)
{
// Return the absolute magnitude of the given number (make it positive)
if(number < 0.0f) number = -number;
return number;
}
int AbsI(int number)
{
// Return the absolute magnitude of the given integer (make it positive)
if(number < 0) number = -number;
return number;
}
/* The entry point is FRC_UserProgram_StartupLibraryInit. Tell the linker where our code is. */
START_ROBOT_CLASS(SimpleTracker);
William Surmak- Posts : 104
Join date : 2011-10-12
code
David Smith Mon Dec 03, 2012 11:06 pm
Now that it has been posted it is obvious why the camera did not work.
Line 127 should have a and all would work A OK!!!!!!!!!!
Line 127 should have a and all would work A OK!!!!!!!!!!
David Smith- Posts : 145
Join date : 2010-10-13
Similar topics» This Year's Code
» Current Camera Code
» Drive code
» Code Samples
» Interesting camera code with tracking
» Current Camera Code
» Drive code
» Code Samples
» Interesting camera code with tracking
Page 1 of 1
Permissions in this forum:
You cannot reply to topics in this forum|
|
|