Code Samples

Go down

Code Samples

Post  Brendan van Ryn on Sun Jan 22, 2012 9:35 pm

I created this board to post the robot code below, but feel free to use it for other code sample if you should feel it necessary.

Code:
/******************************************************************************
 *
 *    Sprockets 2012 Base Code
 *    Authors:    Brendan van Ryn
 *             Stuart Sullivan
 *             Seikun Kambashi
 *             Terry Shi
 *    Last Updated:   13/01/2012
 *
 ******************************************************************************/

// Remember to add the steering alignment (manual)

// Header file for WPI standard hardware/robot overhead.
#include "WPILib.h"

// Include the libraries for the camera
#include "vision\AxisCamera.h"

// Header file for math functions
#include <math.h>

// Macro Functions
// Clears the Driver Station output display
#define CLEARMESSAGE DriverStationLCD::GetInstance()->Clear()

// Prints an integer, a, on the Driver Station on line, b.
#define DISPLAYINTEGER(b, a) DriverStationLCD::GetInstance()->Printf(DriverStationLCD::kUser_Line ## b, 1, "%d", a); DriverStationLCD::GetInstance()->UpdateLCD()

// Prints a string, a, on the Driver Station on line, b.
#define DISPLAYSTRING(b, a) DriverStationLCD::GetInstance()->Printf(DriverStationLCD::kUser_Line ## b, 1, a); DriverStationLCD::GetInstance()->UpdateLCD()

// Button macros
#define SWERVEMODEBUTTON rightStick->GetRawButton(1)
#define MONSTERMODEBUTTON leftStick->GetRawButton(2)
#define MANUALALIGNENABLEBUTTON (rightStick->GetRawButton(8) && rightStick->GetRawButton(9))
#define MANUALALIGNLEFTREAR rightStick->GetRawButton(7)
#define MANUALALIGNRIGHTREAR rightStick->GetRawButton(10)
#define MANUALALIGNLEFTFRONT rightStick->GetRawButton(6)
#define MANUALALIGNRIGHTFRONT rightStick->GetRawButton(11)

// Port numbers
// Motors are on PWM ports
#define LFMOTORPORT 1
#define LRMOTORPORT 2
#define RFMOTORPORT 3
#define RRMOTORPORT 4
#define FRONTSTEERINGMOTORPORT 6
#define REARSTEERINGMOTORPORT 5

// Joysticks are on USB ports
#define LEFTJOYSTICKPORT 1
#define RIGHTJOYSTICKPORT 2

// Encoders are on Digital IO ports
#define FRONTSTEERINGENCODERPORTA 14
#define FRONTSTEERINGENCODERPORTB 13
#define REARSTEERINGENCODERPORTA 12
#define REARSTEERINGENCODERPORTB 11

// The gyro is on an analog channel
#define STEERINGGYROPORT 1

// Constants
// Hardware constants
#define GYROSENSITIVITY 0.1f // Volts per degree per second
#define PULSESPER90 1500 // Number of encoder pulses in a 90-degree turn
#define STEERINGDEADBAND 20 // Number of encoder pulses of proximity needed for the steering (lower numbers are more accurate, but can cause hunting)
#define STEERINGAPPROACHDISTANCE 200 // Number of encoder pulses of proximity after which the wheels begin to slow down

// Threshold constants
#define JOYSTICKDEADBANDX 0.0525f
#define JOYSTICKDEADBANDY 0.100f

// Other constants
#define Pi 3.1415926535897932384626433832795028841971f
#define SWERVEMODE 0
#define MONSTERMODE 1
#define CARMODE 2


// 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 this year, only) class. Don't worry about the word
"class" as we won't do much Object-Oriented stuff, but in the layout
provided to use for the robot code, we must have this one class. The
code for the robot goes here, along with all variables we need. It has
always been called "SimpleTracker" for whatever reason". */
class SimpleTracker : public SimpleRobot // Just ignore this, essentially. Treat it as "int main(void)" for simplicity.
{
    // "Global" variables are declared here.
   Joystick *leftStick; // Left driver joystick
   Joystick *rightStick; // Right driver joystick
    Jaguar *lfDrive; // Front-left drive motor
    Jaguar *lrDrive; // Rear-left drive motor
    Jaguar *rfDrive; // Front-right drive motor
    Jaguar *rrDrive; // Rear-right drive motor
    Victor *frontSteer; // Front steering motor
    Victor *rearSteer; // Rear steering motor
    Encoder *frontSteeringEncoder; // Front steering encoder
    Encoder *rearSteeringEncoder; // Rear steering encoder
    Gyro *steeringGyro; // Steering gyro
   
    /* This "public" means that these variables are available outside of
    this class...but we only have one, so it essentially does nothing.
    "SimpleTracker(void)" is called a constructor. Again, don't worry about
    the details. This is where our object-based variables (hardware components)
    are initialized. This means we assign them a particular port. */ 
    public:
   SimpleTracker(void)
   {
        // Assign object pointers here.     
        lfDrive = new Jaguar(LFMOTORPORT); // Driving Motors
        lrDrive = new Jaguar(LRMOTORPORT);
        rfDrive = new Jaguar(RFMOTORPORT);
        rrDrive = new Jaguar(RRMOTORPORT);
       
        frontSteer = new Victor(FRONTSTEERINGMOTORPORT); // Steering Motors
        rearSteer = new Victor(REARSTEERINGMOTORPORT);
       
        leftStick = new Joystick(LEFTJOYSTICKPORT); // Joysticks
        rightStick = new Joystick(RIGHTJOYSTICKPORT);
       
        frontSteeringEncoder = new Encoder(FRONTSTEERINGENCODERPORTA, FRONTSTEERINGENCODERPORTB); // Encoders
        rearSteeringEncoder = new Encoder(REARSTEERINGENCODERPORTA, REARSTEERINGENCODERPORTB);
       
        steeringGyro = new Gyro(1, STEERINGGYROPORT); // Analogue devices
       
        // Initialize encoders
        frontSteeringEncoder->Reset();
        frontSteeringEncoder->Start();
        rearSteeringEncoder->Reset();
        rearSteeringEncoder->Start();
       
        // Initialize gyro
        steeringGyro->Reset();
       
        // Enable camera
        Wait(8.0);
        AxisCamera &camera = AxisCamera::GetInstance();
   }
   
    /* 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 x, y; // The average x- and y-positions between the two 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; // Allow different angles for the front and back sides, for use in different steering modes
      int waitForSteering; // Should the drive motors wait for the wheels to finish turning?
      int frontFinished, rearFinished; // Store whether each driving side has finished turning
      int steeringMode = SWERVEMODE; // Store the current mode of steering (0 = strafe, 1 = monster, 2 = car)
      int currentButtonState = 0, previousButtonState = 0; // Store the current and previous states of the manual alignment buttons
      
      // This code is run repeatedly so long as the robot is in teleop mode.
      while (IsOperatorControl())
      {
         /* This is a fancy, convoluted way of telling the operating-system
         software that the code hasn't frozen. */
         GetWatchdog().Feed();
         
         /* Teleop code here. */
         // Check buttons
         if(SWERVEMODEBUTTON) steeringMode = SWERVEMODE;
         else if(MONSTERMODEBUTTON) steeringMode = MONSTERMODE;
         
         // Get joystick positions
         leftx = leftStick->GetRawAxis(1);
         lefty = leftStick->GetRawAxis(2);
         rightx = rightStick->GetRawAxis(1);
         righty = rightStick->GetRawAxis(2);
         
         // If the total y-deflection for each joystick is below the threshold, we shouldn't count it at all
         if(Abs(lefty) < JOYSTICKDEADBANDY) lefty = 0.0f;
         if(Abs(righty) < JOYSTICKDEADBANDY) righty = 0.0f;
                  
         // Average the joystick positions
         x = (leftx + rightx) / 2.0f;
         y = (lefty + righty) / 2.0f;
         
         // Store the currentButtonState from the previous iteration
         previousButtonState = currentButtonState;
         
         // If the manual-alignment buttons are pressed
         if(MANUALALIGNENABLEBUTTON)
         {
            currentButtonState = 1;
            if(MANUALALIGNLEFTREAR) rearSteer->Set(-0.15f);
            else if(MANUALALIGNRIGHTREAR) rearSteer->Set(0.15f);
            else if(MANUALALIGNLEFTFRONT) frontSteer->Set(-0.15f);
            else if(MANUALALIGNRIGHTFRONT)frontSteer->Set(0.15f);
         }
         
         // Otherwise, if the deflection left or right is less than a certain amount, we simply drive tank-style
         // Abs simply ensures that x is positive so that we don't need two separate if-statements
         else if(Abs(x) < JOYSTICKDEADBANDX)
         {
            // Tank drive
            // Set the drive motors according to their respective joysticks
            lfDrive->Set(lefty);
            lrDrive->Set(lefty);
            rfDrive->Set(righty);
            rrDrive->Set(righty);
            frontSteer->Set(0);
            rearSteer->Set(0);
            currentButtonState = 0;
         }
         
         else
         {
            // Swerve drive
            // Determine the total deflection of the joysticks to use as the speed setting
            swerveSpeed = sqrt(x*x + y*y); // a^2 + b^2 = c^2
            swerveAngle = atan(y/x); // y/x = tan(theta)
            swerveAngle = swerveAngle * 180 / Pi; // 1 degree = 180/Pi radians
            swerveAngle = swerveAngle / 90.0f * PULSESPER90; // Convert from degrees to encoder pulses
            
            // Determine the quadrant of the joystick and convert the speed or angle appropriately
            // Quadrant one is assumed, except for the following conditions
            if(x < 0 && y >= 0) swerveAngle = -swerveAngle; // Quadrant two
            else if(x < 0 && y < 0) swerveSpeed = -swerveSpeed; // Quadrant three
            else if(x >= 0 && y < 0)
            {
               swerveSpeed = -swerveSpeed;
               swerveAngle = -swerveAngle;
            }
            
            /* If the y-component is zero, we should try to move straight left or right. Therefore, we
             * need to wait for the wheels to finish turning before allowing the drive motors to move. */
            
            if(Abs(y) < JOYSTICKDEADBANDY) waitForSteering = 1;
            else waitForSteering = 0;
            
            // If we are in monster mode and travelling faster than 80% max speed, we switch to car automatically
            if(Abs(swerveSpeed) > 0.8f && steeringMode == MONSTERMODE) steeringMode = CARMODE;
         
            // Put steer-angle and drive-direction changes here for the other steering modes (monster, car, auto-monster, etc.)
            if(steeringMode == MONSTERMODE) swerveAngleFront = 0.75f * swerveAngle, swerveAngleRear = -0.75f * swerveAngle;
            else if(steeringMode == CARMODE)
            {
               if(swerveSpeed >= 0) swerveAngleFront = 0.5f * swerveAngle, swerveAngleRear = 0.0f;
               else swerveAngleRear = -0.5f * swerveAngle, swerveAngleFront = 0.0f;
            }
            else swerveAngleFront = swerveAngleRear = swerveAngle;
            
            /* Call pre-written functions that will use the encoder to get the wheels to gradually approach the desired
             * position. This function will return zero when the steering is finished.*/
            frontFinished = TrackFrontSteering((int)swerveAngleFront);
            rearFinished = TrackRearSteering((int)swerveAngleRear);
            
            // If the steering has finished, set the drive motors to move
            if((frontFinished == 0 && rearFinished == 0) || !waitForSteering)
            {
               lfDrive->Set(swerveSpeed);
               lrDrive->Set(swerveSpeed);
               rfDrive->Set(swerveSpeed);
               rrDrive->Set(swerveSpeed);
            }
            
            currentButtonState = 0;
         }
         
         // If the manual alignment buttons have JUST been released, reset the encoders again
         if(previousButtonState == 1 && currentButtonState == 0)
         {
            frontSteeringEncoder->Reset();
            frontSteeringEncoder->Start();
            rearSteeringEncoder->Reset();
            rearSteeringEncoder->Start();
         }
      }
   }

    /* Code to run while in autonomous, or fully-automated mode. */
   void Autonomous(void)
   {
      // Variables
      float steeringAngle; // Current angle from home as read by the gyro
      
      // This code is run repeatedly so long as the robot is in autonomous mode (lasting 15 seconds).
      while (IsAutonomous())
      {
            /* Tell the system that the code hasn't frozen. */
         GetWatchdog().Feed();
         
         /* Autonomous code here. */
         //Get the gyro angle
         steeringAngle = steeringGyro->GetAngle();
         
         // Display the angle on the Driver Station
         CLEARMESSAGE;
         DISPLAYINTEGER(1, (int)(steeringAngle * 100.0f)); // Multiply by 100 to get two decimal points and convert to an integer [Line 1]
         DISPLAYINTEGER(2, (int)(steeringAngle * 180.0f / Pi * 10.0f)); // Convert from radians to degrees (just in case) and make an integer [Line 2]
      }
   }
   
   /* This function handles the complicated problem of quickly, but accurately, moving the
    * front wheels to their correct position, as passed 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 < STEERINGDEADBAND) speed = 0.0f;
      else if(frontSteeringEncoderValue < targetPosition) speed = (float)remainingSteeringDistance / (float)STEERINGAPPROACHDISTANCE;
      else speed = (float)-remainingSteeringDistance / (float)STEERINGAPPROACHDISTANCE;
      
      // 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 < STEERINGDEADBAND) speed = 0.0f;
      else if(rearSteeringEncoderValue < targetPosition) speed = (float)remainingSteeringDistance / (float)STEERINGAPPROACHDISTANCE;
      else speed = (float)-remainingSteeringDistance / (float)STEERINGAPPROACHDISTANCE;
            
      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;
   }
}; /* Semicolons are not used after functions, but classes are treated
more like datatypes, so they DO have a semicolon after them. To think some
people say that object-oriented makes sense :/ */

// 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. This macro tells
the compiler to use the SimpleTracker class. There are a series of
"under-the-hood" operations so that the program starts in this class
instead of searching for int main(void). */
START_ROBOT_CLASS(SimpleTracker);

Brendan van Ryn

Posts : 95
Join date : 2010-09-30
Age : 24

Back to top Go down

Code Update

Post  Brendan van Ryn on Thu Feb 02, 2012 12:12 am

Code:
/******************************************************************************
 *
 *    Sprockets 2012 Base Code
 *    Authors:    Brendan van Ryn
 *             Stuart Sullivan
 *             Seikun Kambashi
 *             Terry Shi
 *    Last Updated:   13/01/2012
 *
 ******************************************************************************/

// Header file for WPI standard hardware/robot overhead.
#include "WPILib.h"

// Include the libraries for the camera
#include "vision/AxisCamera.h"

// Header file for math functions
#include <math.h>

// Macro Functions
// Clears the Driver Station output display
#define CLEARMESSAGE DriverStationLCD::GetInstance()->Clear()

// Prints an integer, a, on the Driver Station on line, b.
#define DISPLAYINTEGER(b, a) DriverStationLCD::GetInstance()->Printf(DriverStationLCD::kUser_Line ## b, 1, "%d", a); DriverStationLCD::GetInstance()->UpdateLCD()

// Prints a string, a, on the Driver Station on line, b.
#define DISPLAYSTRING(b, a) DriverStationLCD::GetInstance()->Printf(DriverStationLCD::kUser_Line ## b, 1, a); DriverStationLCD::GetInstance()->UpdateLCD()

// Button macros
#define SWERVEMODEBUTTON rightStick->GetRawButton(1)
#define MONSTERMODEBUTTON leftStick->GetRawButton(1)
#define CARMODEBUTTON leftStick->GetRawButton(2)
#define MANUALALIGNENABLEBUTTON (rightStick->GetRawButton(8) && rightStick->GetRawButton(9))
#define MANUALALIGNLEFTREAR rightStick->GetRawButton(7)
#define MANUALALIGNRIGHTREAR rightStick->GetRawButton(10)
#define MANUALALIGNLEFTFRONT rightStick->GetRawButton(6)
#define MANUALALIGNRIGHTFRONT rightStick->GetRawButton(11)

// Port numbers
// Motors are on PWM ports
#define LFMOTORPORT 1
#define LRMOTORPORT 2
#define RFMOTORPORT 3
#define RRMOTORPORT 4
#define FRONTSTEERINGMOTORPORT 6
#define REARSTEERINGMOTORPORT 5

// Joysticks are on USB ports
#define LEFTJOYSTICKPORT 4
#define RIGHTJOYSTICKPORT 1

// Encoders are on Digital IO ports
#define FRONTSTEERINGENCODERPORTA 14
#define FRONTSTEERINGENCODERPORTB 13
#define REARSTEERINGENCODERPORTA 12
#define REARSTEERINGENCODERPORTB 11

// The gyro is on an analog channel
#define STEERINGGYROPORT 2

// Constants
// Hardware constants
#define GYROSENSITIVITY 0.007f // Volts per degree per second
#define PULSESPER90 900 // Number of encoder pulses in a 90-degree turn
#define STEERINGDEADBAND 25 // Number of encoder pulses of proximity needed for the steering (lower numbers are more accurate, but can cause hunting)
#define STEERINGAPPROACHDISTANCE 60 // Number of encoder pulses of proximity after which the wheels begin to slow down

// Threshold constants
#define JOYSTICKDEADBANDX 0.100f
#define JOYSTICKDEADBANDY 0.100f

// Camera HSL Image Thresholds
#define HUEMAX 255
#define HUEMIN 0
#define SATURATIONMAX 255
#define SATURATIONMIN 0
#define LUMINANCEMAX 255
#define LUMINANCEMIN 0

// Other constants
#define Pi 3.1415926535897932384626433832795028841971f
#define SWERVEMODE 0
#define MONSTERMODE 1
#define CARMODE 2


// 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 this year, only) class. Don't worry about the word
"class" as we won't do much Object-Oriented stuff, but in the layout
provided to use for the robot code, we must have this one class. The
code for the robot goes here, along with all variables we need. It has
always been called "SimpleTracker" for whatever reason". */
class SimpleTracker : public SimpleRobot // Just ignore this, essentially. Treat it as "int main(void)" for simplicity.
{
    // "Global" variables are declared here.
   Joystick *leftStick; // Left driver joystick
   Joystick *rightStick; // Right driver joystick
    Jaguar *lfDrive; // Front-left drive motor
    Jaguar *lrDrive; // Rear-left drive motor
    Jaguar *rfDrive; // Front-right drive motor
    Jaguar *rrDrive; // Rear-right drive motor
    Victor *frontSteer; // Front steering motor
    Victor *rearSteer; // Rear steering motor
    Encoder *frontSteeringEncoder; // Front steering encoder
    Encoder *rearSteeringEncoder; // Rear steering encoder
    Gyro *steeringGyro; // Steering gyro
    HSLImage *cameraImage; // Hue/Saturation/Luminence image from camera
    BinaryImage *binaryImage; // The Object used to store and edit the image
    vector<ParticleAnalysisReport> *results; // An Object that stores the particles of the image
   
    /* This "public" means that these variables are available outside of
    this class...but we only have one, so it essentially does nothing.
    "SimpleTracker(void)" is called a constructor. Again, don't worry about
    the details. This is where our object-based variables (hardware components)
    are initialized. This means we assign them a particular port. */ 
    public:
   SimpleTracker(void)
   {
        // Assign object pointers here.     
        lfDrive = new Jaguar(LFMOTORPORT); // Driving Motors
        lrDrive = new Jaguar(LRMOTORPORT);
        rfDrive = new Jaguar(RFMOTORPORT);
        rrDrive = new Jaguar(RRMOTORPORT);
       
        frontSteer = new Victor(FRONTSTEERINGMOTORPORT); // Steering Motors
        rearSteer = new Victor(REARSTEERINGMOTORPORT);
       
        leftStick = new Joystick(LEFTJOYSTICKPORT); // Joysticks
        rightStick = new Joystick(RIGHTJOYSTICKPORT);
       
        frontSteeringEncoder = new Encoder(FRONTSTEERINGENCODERPORTA, FRONTSTEERINGENCODERPORTB); // Encoders
        rearSteeringEncoder = new Encoder(REARSTEERINGENCODERPORTA, REARSTEERINGENCODERPORTB);
       
        steeringGyro = new Gyro(1, STEERINGGYROPORT); // Analogue devices
       
        // Initialize encoders
        frontSteeringEncoder->Reset();
        frontSteeringEncoder->Start();
        rearSteeringEncoder->Reset();
        rearSteeringEncoder->Start();
       
        // Initialize gyro
        steeringGyro->Reset();
        //steeringGyro->SetSensitivity(GYROSENSITIVITY); // Value taken from example, if necessary
       
        // Enable camera
        AxisCamera &camera = AxisCamera::GetInstance();
        Wait(4.0);
       
        // Set the camera's preferences
        camera.WriteResolution(camera.kResolution_320x240);
        camera.WriteCompression(10);
        camera.WriteBrightness(30);
       
        // Save the image as a BinaryImage Object
        cameraImage = camera.GetImage();
        binaryImage = cameraImage->ThresholdHSL(HUEMIN, HUEMAX, SATURATIONMIN, SATURATIONMAX, LUMINANCEMIN, LUMINANCEMAX); // ImaqThreshold error: -1074396154
       
        // Perform the Convex Hull operation
        imaqConvexHull(binaryImage->GetImaqImage(), binaryImage->GetImaqImage(), 1);
       
        // Then save the newly processed image and put it in an array
        results = binaryImage->GetOrderedParticleAnalysisReports();
       
        // Checks if anything is in results
        if(results)
        {
           // Loop Through the Particle Array **Please move this variable declaration outside of the loop
           for(unsigned int x = 0; x < results->size();x++)
           {
              // Checks the
              if(results->at(x).particleArea>1000)
              {
                 results->at(x).center_mass_x_normalized;
                 results->at(x).center_mass_y_normalized;
              }
           }
        }
   }
   
    /* 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 x, y; // The average x- and y-positions between the two 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; // Allow different angles for the front and back sides, for use in different steering modes
      int waitForSteering; // Should the drive motors wait for the wheels to finish turning?
      int frontFinished, rearFinished; // Store whether each driving side has finished turning
      int steeringMode = SWERVEMODE; // Store the current mode of steering (0 = strafe, 1 = monster, 2 = car)
      int currentButtonState = 0, previousButtonState = 0; // Store the current and previous states of the manual alignment buttons
      
      // This code is run repeatedly so long as the robot is in teleop mode.
      while (IsOperatorControl())
      {
         /* This is a fancy, convoluted way of telling the operating-system
         software that the code hasn't frozen. */
         GetWatchdog().Feed();
         
         /* For debugging the encoder */
         CLEARMESSAGE;
         DISPLAYINTEGER(1, (int)(frontSteer->Get() * 100.0f));
         DISPLAYINTEGER(2, (int)(rearSteer->Get() * 100.0f));
         
         /* Teleop code here. */
         // Check buttons
         if(SWERVEMODEBUTTON) steeringMode = SWERVEMODE;
         else if(MONSTERMODEBUTTON) steeringMode = MONSTERMODE;
         else if(CARMODEBUTTON) steeringMode = CARMODE;
         
         // Get joystick positions
         leftx = leftStick->GetRawAxis(1);
         lefty = leftStick->GetRawAxis(2);
         rightx = rightStick->GetRawAxis(1);
         righty = rightStick->GetRawAxis(2);
         
         // If the total y-deflection for each joystick is below the threshold, we shouldn't count it at all
         if(Abs(lefty) < JOYSTICKDEADBANDY) lefty = 0.0f;
         if(Abs(righty) < JOYSTICKDEADBANDY) righty = 0.0f;
         
                  
         // Average the joystick positions
         x = (leftx + rightx) / 2.0f;
         y = (lefty + righty) / 2.0f;
         
                  
         // Store the currentButtonState from the previous iteration
         previousButtonState = currentButtonState;
         
         // If the manual-alignment buttons are pressed
         if(MANUALALIGNENABLEBUTTON)
         {
            currentButtonState = 1;
            if(MANUALALIGNLEFTREAR) rearSteer->Set(-0.35f);
            else if(MANUALALIGNRIGHTREAR) rearSteer->Set(0.35f);
            else rearSteer->Set(0);
               
            if(MANUALALIGNLEFTFRONT) frontSteer->Set(0.35f);
            else if(MANUALALIGNRIGHTFRONT)frontSteer->Set(-0.35f);
            else frontSteer->Set(0);
         }
         
         // Otherwise, if the deflection left or right is less than a certain amount, we simply drive tank-style
         // Abs simply ensures that x is positive so that we don't need two separate if-statements
         else if(Abs(x) < JOYSTICKDEADBANDX)
         {
            // Tank drive
            // Set the drive motors according to their respective joysticks
            lfDrive->Set(lefty);
            lrDrive->Set(lefty);
            rfDrive->Set(righty);
            rrDrive->Set(righty);
            currentButtonState = 0;
            
            TrackFrontSteering(0);
            TrackRearSteering(0);
         }
         
         else
         {
            // Swerve drive
            // Determine the total deflection of the joysticks to use as the speed setting
            swerveSpeed = sqrt(x*x + y*y); // a^2 + b^2 = c^2
            swerveAngle = -Abs(atan(x/y)); // x/y = tan(theta)
            swerveAngle = swerveAngle * 180 / Pi; // 1 degree = 180/Pi radians
            swerveAngle = swerveAngle / 90.0f * PULSESPER90; // Convert from degrees to encoder pulses
            
            /* If the y-component is zero, we should try to move straight left or right. Therefore, we
             * need to wait for the wheels to finish turning before allowing the drive motors to move. The
             * angle, for some reason, also needs to be inverted when moving straight left or right. */
            if(Abs(y) < JOYSTICKDEADBANDY)
            {
               waitForSteering = 1;
               if(x < 0.0f) swerveAngle = PULSESPER90, swerveSpeed = -swerveSpeed;
               else if(x > 0.0f) swerveAngle = -PULSESPER90;
            }
            else waitForSteering = 0;
                        
            // Determine the quadrant of the joystick and convert the speed or angle appropriately
            // Quadrant one is assumed, except for the following conditions
            if(x < 0 && y >= 0) swerveAngle = -swerveAngle; // Quadrant two
            else if(x < 0 && y < 0) swerveSpeed = -swerveSpeed; // Quadrant three
            else if(x >= 0 && y < 0) // Quadrant Four
            {
               swerveSpeed = -swerveSpeed;
               swerveAngle = -swerveAngle;               
            }
            
            // If we are in monster mode and travelling faster than 80% max speed, we switch to car automatically
            if(Abs(swerveSpeed) > 0.8f && steeringMode == MONSTERMODE) steeringMode = CARMODE;
         
            // Put steer-angle and drive-direction changes here for the other steering modes (monster, car, auto-monster, etc.)
            if(steeringMode == MONSTERMODE) swerveAngleFront = swerveAngleRear = -0.75f * swerveAngle;
            else if(steeringMode == CARMODE)
            {
               if(swerveSpeed <= 0) swerveAngleFront = -0.5f * swerveAngle, swerveAngleRear = 0.0f;
               else swerveAngleRear = 0.5f * swerveAngle, swerveAngleFront = 0.0f;
            }
            
            else
            {
               swerveAngleFront = -swerveAngle;
               swerveAngleRear = swerveAngle;
            }
            
            /* Call pre-written functions that will use the encoder to get the wheels to gradually approach the desired
             * position. This function will return zero when the steering is finished.*/
            frontFinished = TrackFrontSteering((int)swerveAngleFront);
            rearFinished = TrackRearSteering((int)swerveAngleRear);
            
            // If the steering has finished, set the drive motors to move
            if((frontFinished == 0 && rearFinished == 0) || !waitForSteering)
            {
               lfDrive->Set(swerveSpeed);
               lrDrive->Set(swerveSpeed);
               rfDrive->Set(swerveSpeed);
               rrDrive->Set(swerveSpeed);
            }
            
            currentButtonState = 0;
         }
         
         // If the manual alignment buttons have JUST been released, reset the encoders again
         if(previousButtonState == 1 && currentButtonState == 0)
         {
            frontSteeringEncoder->Reset();
            frontSteeringEncoder->Start();
            rearSteeringEncoder->Reset();
            rearSteeringEncoder->Start();
            frontSteer->Set(0);
            rearSteer->Set(0);
         }
      }
   }

   /* Code to run while in autonomous, or fully-automated mode. */
   void Autonomous(void)
   {
      // Variables
      float steeringAngle; // Current angle from home as read by the gyro
      
      // This code is run repeatedly so long as the robot is in autonomous mode (lasting 15 seconds).
      while (IsAutonomous())
      {
         /* Tell the system that the code hasn't frozen. */
         GetWatchdog().Feed();
         
         /* Autonomous code here. */
         //Get the gyro angle
         steeringAngle = steeringGyro->GetAngle();
         
         // Display the angle on the Driver Station
         CLEARMESSAGE;
         DISPLAYINTEGER(1, (int)steeringAngle);
      }
   }

   /* This function handles the complicated problem of quickly, but accurately, moving the
    * front wheels to their correct position, as passed 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 < STEERINGDEADBAND) speed = 0.0f;
      else if(frontSteeringEncoderValue < targetPosition) speed = (float)remainingSteeringDistance / (float)STEERINGAPPROACHDISTANCE;
      else speed = (float)-remainingSteeringDistance / (float)STEERINGAPPROACHDISTANCE;
      
      // 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 < STEERINGDEADBAND) speed = 0.0f;
      else if(rearSteeringEncoderValue < targetPosition) speed = (float)remainingSteeringDistance / (float)STEERINGAPPROACHDISTANCE;
      else speed = (float)-remainingSteeringDistance / (float)STEERINGAPPROACHDISTANCE;
            
      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;
   }
   
}; /* Semicolons are not used after functions, but classes are treated
more like datatypes, so they DO have a semicolon after them. To think some
people say that object-oriented makes sense :/ */

// 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. This macro tells
the compiler to use the SimpleTracker class. There are a series of
"under-the-hood" operations so that the program starts in this class
instead of searching for int main(void). */
START_ROBOT_CLASS(SimpleTracker);

Brendan van Ryn

Posts : 95
Join date : 2010-09-30
Age : 24

Back to top Go down

Common Syntax

Post  Brendan van Ryn on Sat Feb 04, 2012 12:31 am

Here are a few common things you guys will come across, in the hopes that it'll make things less confusing for you. I happened to have some spare time, so I threw this reference together as an aid to anyone that gets confused:

Code:

// Declaring a new object (very top of "class")
objectType *objectName;


// Assigning the object's port (instantiating; the first subsection of the "class")
objectName = new objectType(PORTNUMBER);
    /* OR */
objectName = new objectType(SLOTNUMBER, PORTNUMBER);
    /* OR, for things like encoders */
objectName = new objectType(PORTNUMBER1, PORTNUMBER2);
  /* Different slots? */
objectName = new objectType(SLOTNUMBER1, PORTNUMBER1, SLOTNUMBER2, PORTNUMBER2);


// Using the object
objectName->actionToPerform(arguments);
  /* Examples */
encoderValue = encoder->Get();
lfMotor->Set(0.5f);
motorSpeed = lfMotor->Get();
gyroAngle = gyro->GetAngle();
gyro->SetSensitivity(0.007f);
  /* Typing the arrow and waiting should show a list of possible functions and properties, if you aren't sure what you want. */
  /* The camera is a bit different; it is a reference instead of a point (something stupid only C++ uses...) */
cameraImage = camera.GetImage();
camera.reset();
  /* Essentially, there is a dot instead in the case of the camera, and only in the case of the camera. */


/* Displaying things on the Driver Station. These should be grouped together, with only one group per subsection (teleop, autonomous, etc...) */
CLEARMESSAGE;
DISPLAYINTEGER(lineNumber, valueToDisplay);
DISPLAYSTRING(lineNumber, "Text to show");
  /* Examples */
CLEARMESSAGE;
DISPLAYINTEGER(1, frontSteeringEncoder->Get());
DISPLAYINTEGER(2, motorSpeed);
DISPLAYSTRING(3, "This is a test");


// Declaring variables (always at the top of the given subsection, given function, or the "class"
type variableName [= value] // Value is optional
  /* Examples */
int i;
float j = 7.0f;
int q = 1, var, test = 4; // "var" is not given a value. When not initialized, the value of a variable is unknown

I hope this stuff helps a bit, and if I get some positive feedback, I might post more. If there are any other code examples you'd like to see, post it under the Questions board, and I'll try to get it done quickly Wink

Brendan van Ryn

Posts : 95
Join date : 2010-09-30
Age : 24

Back to top Go down

New Cod as of February 4th

Post  Brendan van Ryn on Sun Feb 05, 2012 12:04 am

Here's the full version of code, for those of you following along (as you should be Wink)

Code:
/******************************************************************************
 *
 *    Sprockets 2012 Base Code
 *    Authors:    Brendan van Ryn
 *             Stuart Sullivan
 *             Seikun Kambashi
 *             Terry Shi
 *    Last Updated:   13/01/2012
 *
 ******************************************************************************/

// Header file for WPI standard hardware/robot overhead.
#include "WPILib.h"

// Include the libraries for the camera
#include "vision/AxisCamera.h"

// Header file for math functions
#include <math.h>

// Macro Functions
// Clears the Driver Station output display
#define CLEARMESSAGE DriverStationLCD::GetInstance()->Clear()

// Prints an integer, a, on the Driver Station on line, b.
#define DISPLAYINTEGER(b, a) DriverStationLCD::GetInstance()->Printf(DriverStationLCD::kUser_Line ## b, 1, "%d", a);

DriverStationLCD::GetInstance()->UpdateLCD()

// Prints a string, a, on the Driver Station on line, b.
#define DISPLAYSTRING(b, a) DriverStationLCD::GetInstance()->Printf(DriverStationLCD::kUser_Line ## b, 1, a); DriverStationLCD::GetInstance()->UpdateLCD()

// Prints a float, a, on the Driver Station on line, b.
#define DISPLAYFLOAT(b, a) DriverStationLCD::GetInstance()->Printf(DriverStationLCD::kUser_Line ## b, 1, a); DriverStationLCD::GetInstance()->UpdateLCD()

// Button macros
#define SWERVEMODEBUTTON rightStick->GetRawButton(1)
#define MONSTERMODEBUTTON leftStick->GetRawButton(1)
#define CARMODEBUTTON leftStick->GetRawButton(2)
#define MANUALALIGNENABLEBUTTON (rightStick->GetRawButton(8) && rightStick->GetRawButton(9))
#define MANUALALIGNLEFTREAR rightStick->GetRawButton(7)
#define MANUALALIGNRIGHTREAR rightStick->GetRawButton(10)
#define MANUALALIGNLEFTFRONT rightStick->GetRawButton(6)
#define MANUALALIGNRIGHTFRONT rightStick->GetRawButton(11)
#define GETIMAGEBUTTON leftStick->GetRawButton(3)

// Port numbers
// Motors are on PWM ports
#define LFMOTORPORT 1
#define LRMOTORPORT 2
#define RFMOTORPORT 3
#define RRMOTORPORT 4
#define FRONTSTEERINGMOTORPORT 6
#define REARSTEERINGMOTORPORT 5

// Joysticks are on USB ports
#define LEFTJOYSTICKPORT 4
#define RIGHTJOYSTICKPORT 1

// Encoders are on Digital IO ports
#define FRONTSTEERINGENCODERPORTA 14
#define FRONTSTEERINGENCODERPORTB 13
#define REARSTEERINGENCODERPORTA 12
#define REARSTEERINGENCODERPORTB 11

// The gyro is on an analog channel
#define STEERINGGYROPORT 2

// Constants
// Hardware constants
#define GYROSENSITIVITY 0.007f // Volts per degree per second
#define PULSESPER90 900 // Number of encoder pulses in a 90-degree turn
#define STEERINGDEADBAND 25 // Number of encoder pulses of proximity needed for the steering (lower numbers are more accurate, but can cause hunting)
#define STEERINGAPPROACHDISTANCE 60 // Number of encoder pulses of proximity after which the wheels begin to slow down

// Hardware threshold constants
#define JOYSTICKDEADBANDX 0.100f
#define JOYSTICKDEADBANDY 0.100f

// Camera HSL Image Thresholds
#define HUEMAX 255
#define HUEMIN 0
#define SATURATIONMAX 15
#define SATURATIONMIN 0
#define LUMINANCEMAX 255
#define LUMINANCEMIN 160
#define MINIMUMAREA 220

// Other constants
#define Pi 3.1415926535897932384626433832795028841971f
#define SWERVEMODE 0
#define MONSTERMODE 1
#define CARMODE 2


// 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 this year, only) class. Don't worry about the word
"class" as we won't do much Object-Oriented stuff, but in the layout
provided to use for the robot code, we must have this one class. The
code for the robot goes here, along with all variables we need. It has
always been called "SimpleTracker" for whatever reason". */
class SimpleTracker : public SimpleRobot // Just ignore this, essentially. Treat it as "int main(void)" for simplicity.
{
    // "Global" variables are declared here.
   Joystick *leftStick; // Left driver joystick
   Joystick *rightStick; // Right driver joystick
    Jaguar *lfDrive; // Front-left drive motor
    Jaguar *lrDrive; // Rear-left drive motor
    Jaguar *rfDrive; // Front-right drive motor
    Jaguar *rrDrive; // Rear-right drive motor
    Victor *frontSteer; // Front steering motor
    Victor *rearSteer; // Rear steering motor
    Encoder *frontSteeringEncoder; // Front steering encoder
    Encoder *rearSteeringEncoder; // Rear steering encoder
    Gyro *steeringGyro; // Steering gyro
    HSLImage *cameraImage; // Hue/Saturation/Luminence image from camera
    BinaryImage *binaryImage; // The Object used to store and edit the image
   
    /* This "public" means that these variables are available outside of
    this class...but we only have one, so it essentially does nothing.
    "SimpleTracker(void)" is called a constructor. Again, don't worry about
    the details. This is where our object-based variables (hardware components)
    are initialized. This means we assign them a particular port. */ 
    public:
   SimpleTracker(void)
   {
        // Assign object pointers here.     
        lfDrive = new Jaguar(LFMOTORPORT); // Driving Motors
        lrDrive = new Jaguar(LRMOTORPORT);
        rfDrive = new Jaguar(RFMOTORPORT);
        rrDrive = new Jaguar(RRMOTORPORT);
       
        frontSteer = new Victor(FRONTSTEERINGMOTORPORT); // Steering Motors
        rearSteer = new Victor(REARSTEERINGMOTORPORT);
       
        leftStick = new Joystick(LEFTJOYSTICKPORT); // Joysticks
        rightStick = new Joystick(RIGHTJOYSTICKPORT);
       
        frontSteeringEncoder = new Encoder(FRONTSTEERINGENCODERPORTA, FRONTSTEERINGENCODERPORTB); // Encoders
        rearSteeringEncoder = new Encoder(REARSTEERINGENCODERPORTA, REARSTEERINGENCODERPORTB);
       
        steeringGyro = new Gyro(1, STEERINGGYROPORT); // Analogue devices
       
        // Initialize encoders
        frontSteeringEncoder->Reset();
        frontSteeringEncoder->Start();
        rearSteeringEncoder->Reset();
        rearSteeringEncoder->Start();
       
        // Initialize gyro
        steeringGyro->Reset();
        //steeringGyro->SetSensitivity(GYROSENSITIVITY); // Value taken from example, if necessary
       
        // Enable camera and wait for a bit
        AxisCamera &camera = AxisCamera::GetInstance();
        Wait(4.0);
       
        // Set the camera's preferences
        camera.WriteResolution(camera.kResolution_320x240);
        camera.WriteCompression(0);
        camera.WriteBrightness(45);
        camera.WriteColorLevel(256);
   }
   
    /* 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 x, y; // The average x- and y-positions between the two 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; // Allow different angles for the front and back sides, for use in different steering modes
      int waitForSteering; // Should the drive motors wait for the wheels to finish turning?
      int frontFinished, rearFinished; // Store whether each driving side has finished turning
      int steeringMode = SWERVEMODE; // Store the current mode of steering (0 = strafe, 1 = monster, 2 = car)
      int currentButtonState = 0, previousButtonState = 0; // Store the current and previous states of the manual alignment buttons
      int targetX = 0; // Store the number of distinct sections on the refined camera image
      
      // This code is run repeatedly so long as the robot is in teleop mode.
      while (IsOperatorControl())
      {
         /* This is a fancy, convoluted way of telling the operating-system
         software that the code hasn't frozen. */
         GetWatchdog().Feed();
         
         /* For debugging the encoder */
         CLEARMESSAGE;
         DISPLAYINTEGER(1, (int)(frontSteer->Get() * 100.0f));
         DISPLAYINTEGER(2, (int)(rearSteer->Get() * 100.0f));
         DISPLAYINTEGER(3, targetX);
         
         // Call code to acquire and process camera image
         if(GETIMAGEBUTTON) targetX = GetProcessedImage();
         
         /* Teleop code here. */
         // Check buttons
         if(SWERVEMODEBUTTON) steeringMode = SWERVEMODE;
         else if(MONSTERMODEBUTTON) steeringMode = MONSTERMODE;
         else if(CARMODEBUTTON) steeringMode = CARMODE;
         
         // Get joystick positions
         leftx = leftStick->GetRawAxis(1);
         lefty = leftStick->GetRawAxis(2);
         rightx = rightStick->GetRawAxis(1);
         righty = rightStick->GetRawAxis(2);
         
         // If the total y-deflection for each joystick is below the threshold, we shouldn't count it at all
         if(Abs(lefty) < JOYSTICKDEADBANDY) lefty = 0.0f;
         if(Abs(righty) < JOYSTICKDEADBANDY) righty = 0.0f;
         
                  
         // Average the joystick positions
         x = (leftx + rightx) / 2.0f;
         y = (lefty + righty) / 2.0f;
         
                  
         // Store the currentButtonState from the previous iteration
         previousButtonState = currentButtonState;
         
         // If the manual-alignment buttons are pressed
         if(MANUALALIGNENABLEBUTTON)
         {
            currentButtonState = 1;
            if(MANUALALIGNLEFTREAR) rearSteer->Set(-0.35f);
            else if(MANUALALIGNRIGHTREAR) rearSteer->Set(0.35f);
            else rearSteer->Set(0);
               
            if(MANUALALIGNLEFTFRONT) frontSteer->Set(0.35f);
            else if(MANUALALIGNRIGHTFRONT)frontSteer->Set(-0.35f);
            else frontSteer->Set(0);
         }
         
         // Otherwise, if the deflection left or right is less than a certain amount, we simply drive tank-style
         // Abs simply ensures that x is positive so that we don't need two separate if-statements
         else if(Abs(x) < JOYSTICKDEADBANDX)
         {
            // Tank drive
            // Set the drive motors according to their respective joysticks
            lfDrive->Set(lefty);
            lrDrive->Set(lefty);
            rfDrive->Set(righty);
            rrDrive->Set(righty);
            currentButtonState = 0;
            
            TrackFrontSteering(0);
            TrackRearSteering(0);
         }
         
         else
         {
            // Swerve drive
            // Determine the total deflection of the joysticks to use as the speed setting
            swerveSpeed = sqrt(x*x + y*y); // a^2 + b^2 = c^2
            swerveAngle = -Abs(atan(x/y)); // x/y = tan(theta)
            swerveAngle = swerveAngle * 180 / Pi; // 1 degree = 180/Pi radians
            swerveAngle = swerveAngle / 90.0f * PULSESPER90; // Convert from degrees to encoder pulses
            
            /* If the y-component is zero, we should try to move straight left or right. Therefore, we
             * need to wait for the wheels to finish turning before allowing the drive motors to move. The
             * angle, for some reason, also needs to be inverted when moving straight left or right. */
            if(Abs(y) < JOYSTICKDEADBANDY)
            {
               waitForSteering = 1;
               if(x < 0.0f) swerveAngle = PULSESPER90, swerveSpeed = -swerveSpeed;
               else if(x > 0.0f) swerveAngle = -PULSESPER90;
            }
            else waitForSteering = 0;
                        
            // Determine the quadrant of the joystick and convert the speed or angle appropriately
            // Quadrant one is assumed, except for the following conditions
            if(x < 0 && y >= 0) swerveAngle = -swerveAngle; // Quadrant two
            else if(x < 0 && y < 0) swerveSpeed = -swerveSpeed; // Quadrant three
            else if(x >= 0 && y < 0) // Quadrant Four
            {
               swerveSpeed = -swerveSpeed;
               swerveAngle = -swerveAngle;               
            }
            
            // If we are in monster mode and travelling faster than 80% max speed, we switch to car automatically
            if(Abs(swerveSpeed) > 0.8f && steeringMode == MONSTERMODE) steeringMode = CARMODE;
         
            // Put steer-angle and drive-direction changes here for the other steering modes (monster, car, auto-monster, etc.)
            if(steeringMode == MONSTERMODE) swerveAngleFront = swerveAngleRear = -0.75f * swerveAngle;
            else if(steeringMode == CARMODE)
            {
               if(swerveSpeed <= 0) swerveAngleFront = -0.5f * swerveAngle, swerveAngleRear = 0.0f;
               else swerveAngleRear = 0.5f * swerveAngle, swerveAngleFront = 0.0f;
            }
            
            else
            {
               swerveAngleFront = -swerveAngle;
               swerveAngleRear = swerveAngle;
            }
            
            /* Call pre-written functions that will use the encoder to get the wheels to gradually approach the desired
             * position. This function will return zero when the steering is finished.*/
            frontFinished = TrackFrontSteering((int)swerveAngleFront);
            rearFinished = TrackRearSteering((int)swerveAngleRear);
            
            // If the steering has finished, set the drive motors to move
            if((frontFinished == 0 && rearFinished == 0) || !waitForSteering)
            {
               lfDrive->Set(swerveSpeed);
               lrDrive->Set(swerveSpeed);
               rfDrive->Set(swerveSpeed);
               rrDrive->Set(swerveSpeed);
            }
            
            currentButtonState = 0;
         }
         
         // If the manual alignment buttons have JUST been released, reset the encoders again
         if(previousButtonState == 1 && currentButtonState == 0)
         {
            frontSteeringEncoder->Reset();
            frontSteeringEncoder->Start();
            rearSteeringEncoder->Reset();
            rearSteeringEncoder->Start();
            frontSteer->Set(0);
            rearSteer->Set(0);
         }
      }
   }

   /* Code to run while in autonomous, or fully-automated mode. */
   void Autonomous(void)
   {
      // Variables
      float steeringAngle; // Current angle from home as read by the gyro
      
      // This code is run repeatedly so long as the robot is in autonomous mode (lasting 15 seconds).
      while (IsAutonomous())
      {
         /* Tell the system that the code hasn't frozen. */
         GetWatchdog().Feed();
         
         /* Autonomous code here. */
         //Get the gyro angle
         steeringAngle = steeringGyro->GetAngle();
         
         // Display the angle on the Driver Station
         CLEARMESSAGE;
         DISPLAYINTEGER(1, (int)steeringAngle);
         
         /* Using gyro angle, adjust tank steering to drive straight */
         if(steeringAngle < 0)
         {
            lrDrive->Set(0.8);
            lfDrive->Set(0.8);
            rrDrive->Set(0.0);
            rfDrive->Set(0.0);
         }
         
         else if(steeringAngle > 0)
         {
            rrDrive->Set(0.8);
            rfDrive->Set(0.8);
             lrDrive->Set(0.0);
             lfDrive->Set(0.0);
         }
         
         else
         {
            rrDrive->Set(0.4);
            rfDrive->Set(0.4);
            lrDrive->Set(0.4);
            lfDrive->Set(0.4);
         }
      }
   }
   
   /* This function handles the complicated problem of quickly, but accurately, moving the
    * front wheels to their correct position, as passed 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 < STEERINGDEADBAND) speed = 0.0f;
      else if(frontSteeringEncoderValue < targetPosition) speed = (float)remainingSteeringDistance / (float)STEERINGAPPROACHDISTANCE;
      else speed = (float)-remainingSteeringDistance / (float)STEERINGAPPROACHDISTANCE;
      
      // 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 < STEERINGDEADBAND) speed = 0.0f;
      else if(rearSteeringEncoderValue < targetPosition) speed = (float)remainingSteeringDistance / (float)STEERINGAPPROACHDISTANCE;
      else speed = (float)-remainingSteeringDistance / (float)STEERINGAPPROACHDISTANCE;
            
      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;
   }
   
   /* Take the camera image and process it, returning the x-position of the largest particle. This should represent the
   horizontal centre of the most in-focus target. Comparing this value to the image width (320) should allow us to
   centre the image by rotating the robot. If the targets are out of view, -1 is returned. */
   int GetProcessedImage()
   {
      register int i;
      int numberOfParticles, largestParticle;
      double maxArea = 0, width, height, maxWidth = 0, maxHeight = 0, horizontalPosition;
      
      // Save the image as a BinaryImage Object
      AxisCamera &camera = AxisCamera::GetInstance();
      cameraImage = camera.GetImage();
      binaryImage = cameraImage->ThresholdHSL(HUEMIN, HUEMAX, SATURATIONMIN, SATURATIONMAX, LUMINANCEMIN, LUMINANCEMAX);
      
      // Perform the Convex Hull operation
      imaqConvexHull(binaryImage->GetImaqImage(), binaryImage->GetImaqImage(), 1);
      
      // Count the number of distinct sections remaining
      imaqCountParticles(binaryImage->GetImaqImage(), 2, &numberOfParticles);
            
       // Save image to the cRIO's memory
       cameraImage->Write("camIm4.jpeg");
       binaryImage->Write("binIm4.bmp");
      
       // For each particle found
       for(i = 0; i < numberOfParticles; i++)
       {
          // Get the width and height of the given particle
          imaqMeasureParticle(binaryImage->GetImaqImage(), i, 0, IMAQ_MT_BOUNDING_RECT_WIDTH, &width);
          imaqMeasureParticle(binaryImage->GetImaqImage(), i, 0, IMAQ_MT_BOUNDING_RECT_HEIGHT, &height);
          
          // If the area of this particle is the largest area so far
          if(width * height > maxArea)
          {
             // Store the dimensions and area of the particle, along with the index of said particle
             maxWidth = width;
             maxHeight = height;
             maxArea = width * height;
             largestParticle = i;
          }
       }
      
       // If the largest particle is outside of the threshold, return -1 to indicate that the targets aren't visible
       if(maxArea < MINIMUMAREA) return -1;
      
       // Find the x-position of the centre of the largest particle, assumed to be the nearest target
       imaqMeasureParticle(binaryImage->GetImaqImage(), largestParticle, 0, IMAQ_MT_CENTER_OF_MASS_X, &horizontalPosition);
      
       // Return this x-position
       return (int)horizontalPosition;
   }
}; /* Semicolons are not used after functions, but classes are treated
more like datatypes, so they DO have a semicolon after them. To think some
people say that object-oriented makes sense :/ */

// 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. This macro tells
the compiler to use the SimpleTracker class. There are a series of
"under-the-hood" operations so that the program starts in this class
instead of searching for int main(void). */
START_ROBOT_CLASS(SimpleTracker);

Brendan van Ryn

Posts : 95
Join date : 2010-09-30
Age : 24

Back to top Go down

GetProcessImage

Post  Brendan van Ryn on Sun Feb 05, 2012 12:22 am

Here is the particular image processing function written today. It either returns the horizontal position of the centre of the nearest target (backboard), or it returns -1 if no backboards are visible:

Code:

/* Take the camera image and process it, returning the x-position of the largest particle. This should represent the
   horizontal centre of the most in-focus target. Comparing this value to the image width (320) should allow us to
   centre the image by rotating the robot. If the targets are out of view, -1 is returned. */
   int GetProcessedImage()
   {
      register int i;
      int numberOfParticles, largestParticle;
      double maxArea = 0, width, height, maxWidth = 0, maxHeight = 0, horizontalPosition;
      
      // Save the image as a BinaryImage Object
      AxisCamera &camera = AxisCamera::GetInstance();
      cameraImage = camera.GetImage();
      binaryImage = cameraImage->ThresholdHSL(HUEMIN, HUEMAX, SATURATIONMIN, SATURATIONMAX, LUMINANCEMIN, LUMINANCEMAX);
      
      // Perform the Convex Hull operation
      imaqConvexHull(binaryImage->GetImaqImage(), binaryImage->GetImaqImage(), 1);
      
      // Count the number of distinct sections remaining
      imaqCountParticles(binaryImage->GetImaqImage(), 2, &numberOfParticles);
            
       // Save image to the cRIO's memory
       cameraImage->Write("camIm4.jpeg");
       binaryImage->Write("binIm4.bmp");
      
       // For each particle found
       for(i = 0; i < numberOfParticles; i++)
       {
          // Get the width and height of the given particle
          imaqMeasureParticle(binaryImage->GetImaqImage(), i, 0, IMAQ_MT_BOUNDING_RECT_WIDTH, &width);
          imaqMeasureParticle(binaryImage->GetImaqImage(), i, 0, IMAQ_MT_BOUNDING_RECT_HEIGHT, &height);
          
          // If the area of this particle is the largest area so far
          if(width * height > maxArea)
          {
             // Store the dimensions and area of the particle, along with the index of said particle
             maxWidth = width;
             maxHeight = height;
             maxArea = width * height;
             largestParticle = i;
          }
       }
      
       // If the largest particle is outside of the threshold, return -1 to indicate that the targets aren't visible
       if(maxArea < MINIMUMAREA) return -1;
      
       // Find the x-position of the centre of the largest particle, assumed to be the nearest target
       imaqMeasureParticle(binaryImage->GetImaqImage(), largestParticle, 0, IMAQ_MT_CENTER_OF_MASS_X, &horizontalPosition);
      
       // Return this x-position
       return (int)horizontalPosition;
   }

First, we acquire a reference to the axis camera--we don't create a "new" instance, because only one can exist, so an instance of it is always already kicking around. Then, we acquire the current image from the camera and filter out the pixels that don't fall within the threshold values we have selected. This resultant image is stored in "binaryImage". In this image, an outline of a rectangle should exist where the reflective tape is. The "ConvexHull" operation fills in this rectangle and other particles on the screen to get a smaller number of larger polygons instead of a larger number of small pixel clusters. Then, we find out how many distinct particles exist (a particle being a group of contiguous, touching pixels). It is at this point that we save the files to the disk. However, saving to the disk using the "->Write()" function is only for debugging, as it allows us to check the new image created through the above processing. Doing this too many times repeatedly will cause the cRIO to crash, so we put the function on a button in teleop, temporarily. When the autonomous is made, this write command should be commented out.

From this point, we use a "for" loop to check the width and height of each particle in the image (because we already know how many there are). Each particle has a number assigned to it to identify it from the others. We multiply the width and height of each particle to calculate its area, then compare it to a running variable that tracks the largest area encountered so far. In this way, after the loop is finished, we will know the width, height, area, and identification number of the largest particle on the screen--which, presumably, should be the target backboard. However, if none of the targets are actually within the field of view of the camera, the largest particle will actually be just the largest piece of noise in the image. Therefore, we check the maximum area found against a minimum value we extrapolated from experiments. If it is too small, we return -1. Otherwise, we find the x-coordinate of the target's centre and return that to the calling procedure.

I hope that all made sense Smile Feel free to ask questions Wink

Brendan van Ryn

Posts : 95
Join date : 2010-09-30
Age : 24

Back to top Go down

Optimization

Post  Brendan van Ryn on Sun Feb 05, 2012 11:57 pm

So, with this new version, just a few changes have been made. Stuart, Aliah, and Will put in some code for Autonomous to rotate the robot in order to centre the target in the camera's field of view, but they ran into problems. So, I've made some efficiency and debugging changes that I hope will resolve these issues, though nothing has been tested. I also implemented a pivoting design over attempting tank-drive correction (which won't work with the old robot). Finally, some comments and new macros for port numbers have been placed that represent the prospective layout of the hardware on the new robot. Please take a look, and ask questions if necessary. The new robot should be up and ready to test sometime tomorrow, so if you'd like to help out or take a look, please come Wink

Code:
/******************************************************************************
 *
 *    Sprockets 2012 Base Code
 *    Authors:    Brendan van Ryn
 *             Stuart Sullivan
 *             Seikun Kambashi
 *             Terry Shi
 *    Last Updated:   13/01/2012
 *
 ******************************************************************************/

// Header file for WPI standard hardware/robot overhead.
#include "WPILib.h"

// Include the libraries for the camera
#include "vision/AxisCamera.h"

// Header file for math functions
#include <math.h>

// Macro Functions
// Clears the Driver Station output display
#define CLEARMESSAGE DriverStationLCD::GetInstance()->Clear()

// Prints an integer, a, on the Driver Station on line, b.
#define DISPLAYINTEGER(b, a) DriverStationLCD::GetInstance()->Printf(DriverStationLCD::kUser_Line ## b, 1, "%d", a); DriverStationLCD::GetInstance()->UpdateLCD()

// Prints a string, a, on the Driver Station on line, b.
#define DISPLAYSTRING(b, a) DriverStationLCD::GetInstance()->Printf(DriverStationLCD::kUser_Line ## b, 1, a); DriverStationLCD::GetInstance()->UpdateLCD()

// Prints a float, a, on the Driver Station on line, b.
#define DISPLAYFLOAT(b, a) DriverStationLCD::GetInstance()->Printf(DriverStationLCD::kUser_Line ## b, 1, a); DriverStationLCD::GetInstance()->UpdateLCD()

// Button macros
#define SWERVEMODEBUTTON rightStick->GetRawButton(1)
#define MONSTERMODEBUTTON leftStick->GetRawButton(1)
#define CARMODEBUTTON leftStick->GetRawButton(2)
#define MANUALALIGNENABLEBUTTON (rightStick->GetRawButton(8) && rightStick->GetRawButton(9))
#define MANUALALIGNLEFTREAR rightStick->GetRawButton(7)
#define MANUALALIGNRIGHTREAR rightStick->GetRawButton(10)
#define MANUALALIGNLEFTFRONT rightStick->GetRawButton(6)
#define MANUALALIGNRIGHTFRONT rightStick->GetRawButton(11)
#define GETIMAGEBUTTON leftStick->GetRawButton(3)

// Port numbers
// Motors are on PWM ports
#define LFMOTORPORT 1
#define LRMOTORPORT 2
#define RFMOTORPORT 3
#define RRMOTORPORT 4
#define FRONTSTEERINGMOTORPORT 5 //3
#define REARSTEERINGMOTORPORT 6 //4

// The gear-shifting servos are digital inputs
#define FRONTSERVOPORT 7
#define REARSERVOPORT 8

// Joysticks are on USB ports
#define LEFTJOYSTICKPORT 4
#define RIGHTJOYSTICKPORT 1

// Encoders are on Digital IO ports
#define FRONTSTEERINGENCODERPORTA 14 // 3
#define FRONTSTEERINGENCODERPORTB 13 // 4
#define REARSTEERINGENCODERPORTA 12 // 1
#define REARSTEERINGENCODERPORTB 11 // 2

// The gyro is on an analog channel
#define STEERINGGYROPORT 2

// Constants
// Hardware constants
#define GYROSENSITIVITY 0.007f // Volts per degree per second
#define PULSESPER90 900 // Number of encoder pulses in a 90-degree turn
#define STEERINGDEADBAND 25 // Number of encoder pulses of proximity needed for the steering (lower numbers are more accurate, but can cause hunting)
#define STEERINGAPPROACHDISTANCE 60 // Number of encoder pulses of proximity after which the wheels begin to slow down
#define CAMERAFREQUENCY 1000 // The number of times the IsOperatorControl() or IsAutonomous() loops must run before updating the camera image

// Hardware threshold constants
#define JOYSTICKDEADBANDX 0.100f
#define JOYSTICKDEADBANDY 0.100f

// Camera HSL Image Thresholds
#define HUEMAX 255
#define HUEMIN 0
#define SATURATIONMAX 15
#define SATURATIONMIN 0
#define LUMINANCEMAX 255
#define LUMINANCEMIN 160
#define MINIMUMAREA 220

// The deadband for the image tracking
#define IMAGEDEADBAND 5

// Other constants
#define Pi 3.1415926535897932384626433832795028841971f
#define SWERVEMODE 0
#define MONSTERMODE 1
#define CARMODE 2


// 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 this year, only) class. Don't worry about the word
"class" as we won't do much Object-Oriented stuff, but in the layout
provided to use for the robot code, we must have this one class. The
code for the robot goes here, along with all variables we need. It has
always been called "SimpleTracker" for whatever reason". */
class SimpleTracker : public SimpleRobot // Just ignore this, essentially. Treat it as "int main(void)" for simplicity.
{
    // "Global" variables are declared here.
   Joystick *leftStick; // Left driver joystick
   Joystick *rightStick; // Right driver joystick
    Jaguar *lfDrive; // Front-left drive motor
    Jaguar *lrDrive; // Rear-left drive motor
    Jaguar *rfDrive; // Front-right drive motor
    Jaguar *rrDrive; // Rear-right drive motor
    Victor *frontSteer; // Front steering motor
    Victor *rearSteer; // Rear steering motor
    Encoder *frontSteeringEncoder; // Front steering encoder
    Encoder *rearSteeringEncoder; // Rear steering encoder
    Gyro *steeringGyro; // Steering gyro
    HSLImage *cameraImage; // Hue/Saturation/Luminence image from camera
    BinaryImage *binaryImage; // The Object used to store and edit the image
   
    /* This "public" means that these variables are available outside of
    this class...but we only have one, so it essentially does nothing.
    "SimpleTracker(void)" is called a constructor. Again, don't worry about
    the details. This is where our object-based variables (hardware components)
    are initialized. This means we assign them a particular port. */ 
    public:
   SimpleTracker(void)
   {
        // Assign object pointers here.     
        lfDrive = new Jaguar(LFMOTORPORT); // Driving Motors
        lrDrive = new Jaguar(LRMOTORPORT);
        rfDrive = new Jaguar(RFMOTORPORT);
        rrDrive = new Jaguar(RRMOTORPORT);
       
        frontSteer = new Victor(FRONTSTEERINGMOTORPORT); // Steering Motors
        rearSteer = new Victor(REARSTEERINGMOTORPORT);
       
        leftStick = new Joystick(LEFTJOYSTICKPORT); // Joysticks
        rightStick = new Joystick(RIGHTJOYSTICKPORT);
       
        frontSteeringEncoder = new Encoder(FRONTSTEERINGENCODERPORTA, FRONTSTEERINGENCODERPORTB); // Encoders
        rearSteeringEncoder = new Encoder(REARSTEERINGENCODERPORTA, REARSTEERINGENCODERPORTB);
       
        steeringGyro = new Gyro(1, STEERINGGYROPORT); // Analogue devices
       
        // Initialize encoders
        frontSteeringEncoder->Reset();
        frontSteeringEncoder->Start();
        rearSteeringEncoder->Reset();
        rearSteeringEncoder->Start();
       
        // Initialize gyro
        steeringGyro->Reset();
        //steeringGyro->SetSensitivity(GYROSENSITIVITY); // Value taken from example, if necessary
       
        // Enable camera and wait for a bit
        AxisCamera &camera = AxisCamera::GetInstance();
        Wait(4.0);
       
        // Set the camera's preferences
        camera.WriteResolution(camera.kResolution_320x240);
        camera.WriteCompression(0);
        camera.WriteBrightness(45);
        camera.WriteColorLevel(256);
   }
   
    /* 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 x, y; // The average x- and y-positions between the two 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; // Allow different angles for the front and back sides, for use in different steering modes
      int waitForSteering; // Should the drive motors wait for the wheels to finish turning?
      int frontFinished, rearFinished; // Store whether each driving side has finished turning
      int steeringMode = SWERVEMODE; // Store the current mode of steering (0 = strafe, 1 = monster, 2 = car)
      int currentButtonState = 0, previousButtonState = 0; // Store the current and previous states of the manual alignment buttons
      int targetX = 0; // Store the number of distinct sections on the refined camera image
      
      // Acquire reference to axis camera
      AxisCamera &camera = AxisCamera::GetInstance();
      
      // This code is run repeatedly so long as the robot is in teleop mode.
      while (IsOperatorControl())
      {
         /* This is a fancy, convoluted way of telling the operating-system
         software that the code hasn't frozen. */
         GetWatchdog().Feed();
         
         /* For debugging the encoder */
         CLEARMESSAGE;
         DISPLAYINTEGER(1, (int)(frontSteer->Get() * 100.0f));
         DISPLAYINTEGER(2, (int)(rearSteer->Get() * 100.0f));
         DISPLAYINTEGER(3, targetX);
         
         // Call code to acquire and process camera image
         if(GETIMAGEBUTTON) targetX = GetProcessedImage(camera);
         
         /* Teleop code here. */
         // Check buttons
         if(SWERVEMODEBUTTON) steeringMode = SWERVEMODE;
         else if(MONSTERMODEBUTTON) steeringMode = MONSTERMODE;
         else if(CARMODEBUTTON) steeringMode = CARMODE;
         
         // Get joystick positions
         leftx = leftStick->GetRawAxis(1);
         lefty = leftStick->GetRawAxis(2);
         rightx = rightStick->GetRawAxis(1);
         righty = rightStick->GetRawAxis(2);
         
         // If the total y-deflection for each joystick is below the threshold, we shouldn't count it at all
         if(Abs(lefty) < JOYSTICKDEADBANDY) lefty = 0.0f;
         if(Abs(righty) < JOYSTICKDEADBANDY) righty = 0.0f;
         
                  
         // Average the joystick positions
         x = (leftx + rightx) / 2.0f;
         y = (lefty + righty) / 2.0f;
         
                  
         // Store the currentButtonState from the previous iteration
         previousButtonState = currentButtonState;
         
         // If the manual-alignment buttons are pressed
         if(MANUALALIGNENABLEBUTTON)
         {
            currentButtonState = 1;
            if(MANUALALIGNLEFTREAR) rearSteer->Set(-0.35f);
            else if(MANUALALIGNRIGHTREAR) rearSteer->Set(0.35f);
            else rearSteer->Set(0);
               
            if(MANUALALIGNLEFTFRONT) frontSteer->Set(0.35f);
            else if(MANUALALIGNRIGHTFRONT)frontSteer->Set(-0.35f);
            else frontSteer->Set(0);
         }
         
         // Otherwise, if the deflection left or right is less than a certain amount, we simply drive tank-style
         // Abs simply ensures that x is positive so that we don't need two separate if-statements
         else if(Abs(x) < JOYSTICKDEADBANDX)
         {
            // Tank drive
            // Set the drive motors according to their respective joysticks
            lfDrive->Set(lefty);
            lrDrive->Set(lefty);
            rfDrive->Set(righty);
            rrDrive->Set(righty);
            currentButtonState = 0;
            
            TrackFrontSteering(0);
            TrackRearSteering(0);
         }
         
         else
         {
            // Swerve drive
            // Determine the total deflection of the joysticks to use as the speed setting
            swerveSpeed = sqrt(x*x + y*y); // a^2 + b^2 = c^2
            swerveAngle = -Abs(atan(x/y)); // x/y = tan(theta)
            swerveAngle = swerveAngle * 180 / Pi; // 1 degree = 180/Pi radians
            swerveAngle = swerveAngle / 90.0f * PULSESPER90; // Convert from degrees to encoder pulses
            
            /* If the y-component is zero, we should try to move straight left or right. Therefore, we
             * need to wait for the wheels to finish turning before allowing the drive motors to move. The
             * angle, for some reason, also needs to be inverted when moving straight left or right. */
            if(Abs(y) < JOYSTICKDEADBANDY)
            {
               waitForSteering = 1;
               if(x < 0.0f) swerveAngle = PULSESPER90, swerveSpeed = -swerveSpeed;
               else if(x > 0.0f) swerveAngle = -PULSESPER90;
            }
            else waitForSteering = 0;
                        
            // Determine the quadrant of the joystick and convert the speed or angle appropriately
            // Quadrant one is assumed, except for the following conditions
            if(x < 0 && y >= 0) swerveAngle = -swerveAngle; // Quadrant two
            else if(x < 0 && y < 0) swerveSpeed = -swerveSpeed; // Quadrant three
            else if(x >= 0 && y < 0) // Quadrant Four
            {
               swerveSpeed = -swerveSpeed;
               swerveAngle = -swerveAngle;               
            }
            
            // If we are in monster mode and travelling faster than 80% max speed, we switch to car automatically
            if(Abs(swerveSpeed) > 0.8f && steeringMode == MONSTERMODE) steeringMode = CARMODE;
         
            // Put steer-angle and drive-direction changes here for the other steering modes (monster, car, auto-monster, etc.)
            if(steeringMode == MONSTERMODE) swerveAngleFront = swerveAngleRear = -0.75f * swerveAngle;
            else if(steeringMode == CARMODE)
            {
               if(swerveSpeed <= 0) swerveAngleFront = -0.5f * swerveAngle, swerveAngleRear = 0.0f;
               else swerveAngleRear = 0.5f * swerveAngle, swerveAngleFront = 0.0f;
            }
            
            else
            {
               swerveAngleFront = -swerveAngle;
               swerveAngleRear = swerveAngle;
            }
            
            /* Call pre-written functions that will use the encoder to get the wheels to gradually approach the desired
             * position. This function will return zero when the steering is finished.*/
            frontFinished = TrackFrontSteering((int)swerveAngleFront);
            rearFinished = TrackRearSteering((int)swerveAngleRear);
            
            // If the steering has finished, set the drive motors to move
            if((frontFinished == 0 && rearFinished == 0) || !waitForSteering)
            {
               lfDrive->Set(swerveSpeed);
               lrDrive->Set(swerveSpeed);
               rfDrive->Set(swerveSpeed);
               rrDrive->Set(swerveSpeed);
            }
            
            currentButtonState = 0;
         }
         
         // If the manual alignment buttons have JUST been released, reset the encoders again
         if(previousButtonState == 1 && currentButtonState == 0)
         {
            frontSteeringEncoder->Reset();
            frontSteeringEncoder->Start();
            rearSteeringEncoder->Reset();
            rearSteeringEncoder->Start();
            frontSteer->Set(0);
            rearSteer->Set(0);
         }
      }
   }

   /* Code to run while in autonomous, or fully-automated mode. */
   void Autonomous(void)
   {
      // Variables
      float steeringAngle; // Current angle from home as read by the gyro
      int imageCenter; // The center point of the clearest target
      int dividerCount = 0; // Count the number of times the loop has been executed
      
      // Acquire reference to axis camera
      AxisCamera &camera = AxisCamera::GetInstance();
      
      // This code is run repeatedly so long as the robot is in autonomous mode (lasting 15 seconds).
      while (IsAutonomous())
      {
         /* Tell the system that the code hasn't frozen. */
         GetWatchdog().Feed();
         
         /* Autonomous code here. */
         //Get the gyro angle
         steeringAngle = steeringGyro->GetAngle();
         
         // Update the center position of the target once every few times through this loop
         if(dividerCount >= CAMERAFREQUENCY)
         {
            imageCenter = GetProcessedImage(camera);
            dividerCount = 0;
         }
         
         else dividerCount++;
         
         // Display the center value
         CLEARMESSAGE;
         DISPLAYINTEGER(1, imageCenter);
         
         // Turn the front wheels left and straighten the rear wheels for pivoting
         TrackFrontSteering(PULSESPER90);
         TrackRearSteering(0);
         
         // If the centre is -1, the target is not in view
         if(imageCenter == -1) { DISPLAYSTRING(2, "No target."); }
         
         // Otherwise, if the center is too far right, turn the robot right
         else if(imageCenter + IMAGEDEADBAND < 160)
         {
            DISPLAYSTRING(2, "Right");
            lfDrive->Set(0.4);
            rfDrive->Set(0.4);
         }
         
         // If the center is too far left, turn the robot left
         else if(imageCenter - IMAGEDEADBAND > 160)
         {
            DISPLAYSTRING(2, "Left");
            lfDrive->Set(-0.4);
            rfDrive->Set(-0.4);
         }
         
         // If the centre is close enough to the middle of the screen, stop
         else
         {
            DISPLAYSTRING(2, "Stop");
            lfDrive->Set(0.0);
            rfDrive->Set(0.0);
         }
      }
   }
   
   /* This function handles the complicated problem of quickly, but accurately, moving the
    * front wheels to their correct position, as passed 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 < STEERINGDEADBAND) speed = 0.0f;
      else if(frontSteeringEncoderValue < targetPosition) speed = (float)remainingSteeringDistance / (float)STEERINGAPPROACHDISTANCE;
      else speed = (float)-remainingSteeringDistance / (float)STEERINGAPPROACHDISTANCE;
      
      // 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 < STEERINGDEADBAND) speed = 0.0f;
      else if(rearSteeringEncoderValue < targetPosition) speed = (float)remainingSteeringDistance / (float)STEERINGAPPROACHDISTANCE;
      else speed = (float)-remainingSteeringDistance / (float)STEERINGAPPROACHDISTANCE;
            
      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;
   }
   
   /* Take the camera image and process it, returning the x-position of the largest particle. This should represent the
   horizontal centre of the most in-focus target. Comparing this value to the image width (320) should allow us to
   centre the image by rotating the robot. If the targets are out of view, -1 is returned. */
   int GetProcessedImage(AxisCamera &camera)
   {
      register int i;
      int numberOfParticles, largestParticle;
      double maxArea = 0, width, height, maxWidth = 0, maxHeight = 0, horizontalPosition;
      
      // Save the image as a BinaryImage Object
      cameraImage = camera.GetImage();
      binaryImage = cameraImage->ThresholdHSL(HUEMIN, HUEMAX, SATURATIONMIN, SATURATIONMAX, LUMINANCEMIN, LUMINANCEMAX);
      
      // Perform the Convex Hull operation
      imaqConvexHull(binaryImage->GetImaqImage(), binaryImage->GetImaqImage(), 1);
      
      // Count the number of distinct sections remaining
      imaqCountParticles(binaryImage->GetImaqImage(), 2, &numberOfParticles);
            
       // Save image to the cRIO's memory (enable for debugging purposes only)
       //cameraImage->Write("camIm4.jpeg");
       //binaryImage->Write("binIm4.bmp");
      
       // For each particle found
       for(i = 0; i < numberOfParticles; i++)
       {
          // Get the width and height of the given particle
          imaqMeasureParticle(binaryImage->GetImaqImage(), i, 0, IMAQ_MT_BOUNDING_RECT_WIDTH, &width);
          imaqMeasureParticle(binaryImage->GetImaqImage(), i, 0, IMAQ_MT_BOUNDING_RECT_HEIGHT, &height);
          
          // If the area of this particle is the largest area so far
          if(width * height > maxArea)
          {
             // Store the dimensions and area of the particle, along with the index of said particle
             maxWidth = width;
             maxHeight = height;
             maxArea = width * height;
             largestParticle = i;
          }
       }
      
       // If the largest particle is outside of the threshold, return -1 to indicate that the targets aren't visible
       if(maxArea < MINIMUMAREA) return -1;
      
       // Find the x-position of the centre of the largest particle, assumed to be the nearest target
       imaqMeasureParticle(binaryImage->GetImaqImage(), largestParticle, 0, IMAQ_MT_CENTER_OF_MASS_X, &horizontalPosition);
      
       // Return this x-position
       return (int)horizontalPosition;
   }
}; /* Semicolons are not used after functions, but classes are treated
more like datatypes, so they DO have a semicolon after them. To think some
people say that object-oriented makes sense :/ */

// 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. This macro tells
the compiler to use the SimpleTracker class. There are a series of
"under-the-hood" operations so that the program starts in this class
instead of searching for int main(void). */
START_ROBOT_CLASS(SimpleTracker);

Brendan van Ryn

Posts : 95
Join date : 2010-09-30
Age : 24

Back to top Go down

Robot Configuration

Post  Brendan van Ryn on Wed Feb 08, 2012 8:19 pm

First off, here is the code from today. I change the autonomous driving, because it was written for tank; that will not work during testing. Instead, the front wheels pivot to 90 degrees and we use the front drive motors to rotate. Also, the camera and gyro should work simultaneously now. I don't know if the thresholds, frequency, or deadbands work well enough yet for the targeting to be speedy or accurate, but it should work. I also set the gyro sensitivity back to 0.007 volts/degree/second, as it yields a more accurate reading (negative for left and positive for right, by the way). For the next little while, I am going to be doing a lot of code stuff myself because things have gotten messy, out of hand, and broken in some places. I must stress the importance of keeping the whole programming team up-to-date on ANY code changes made. Make a list if you must, but I want to know each and every line or macro that was changed and why--post it here on the forum. As for what will be done in the next two days leading up to my shift on Saturday, I don't know (and I will be late to my shift due to a doctor's appointment, but I'll stay longer if possible).

Anyway, I've posted the current goals elsewhere, but what I want to focus on here is an extremely important and potentially confusing change. Take a look:
Code:
/******************************************************************************
 *
 *    Sprockets 2012 Base Code
 *    Authors:    Brendan van Ryn
 *             Stuart Sullivan
 *             Terry Shi
 *             Aliah McCalla
 *             Will Surmak
 *    Last Updated:   13/01/2012
 *
 ******************************************************************************/

/* Robot configuration macros. Do not change. */
#define NEW 1
#define OLD 2

/* Robot configuration */
#define ROBOT OLD // Set this to either "OLD" or "NEW", without the quotation marks, to configure for the given robot

/* Set the correct macros based on which robot is being used. Do not change this. */
#if !defined ROBOT
#warning "Robot version not specified. Assuming new configuration..."
#include "NewRobot.h"
#elif ROBOT == NEW
#include "NewRobot.h"
#elif ROBOT == OLD
#include "OldRobot.h"
#else
#warning "Robot configuration must be either 'OLD' or 'NEW'. Assuming new..."
#include "NewRobot.h"
#endif

// Header file for WPI standard hardware/robot overhead.
#include "WPILib.h"

// Include the libraries for the camera
#include "vision/AxisCamera.h"

// Header file for math functions
#include <math.h>

// Macro Functions
// Clears the Driver Station output display
#define CLEARMESSAGE DriverStationLCD::GetInstance()->Clear()

// Prints an integer, a, on the Driver Station on line, b.
#define DISPLAYINTEGER(b, a) DriverStationLCD::GetInstance()->Printf(DriverStationLCD::kUser_Line ## b, 1, "%d", a); DriverStationLCD::GetInstance()->UpdateLCD()

// Prints a string, a, on the Driver Station on line, b.
#define DISPLAYSTRING(b, a) DriverStationLCD::GetInstance()->Printf(DriverStationLCD::kUser_Line ## b, 1, a); DriverStationLCD::GetInstance()->UpdateLCD()

// Prints a float, a, on the Driver Station on line, b.
#define DISPLAYFLOAT(b, a) DriverStationLCD::GetInstance()->Printf(DriverStationLCD::kUser_Line ## b, 1, "%0.2f", a); DriverStationLCD::GetInstance()->UpdateLCD()


// 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 this year, only) class. Don't worry about the word
"class" as we won't do much Object-Oriented stuff, but in the layout
provided to use for the robot code, we must have this one class. The
code for the robot goes here, along with all variables we need. It has
always been called "SimpleTracker" for whatever reason". */
class SimpleTracker : public SimpleRobot // Just ignore this, essentially. Treat it as "int main(void)" for simplicity.
{
    // "Global" variables are declared here.
   Joystick *leftStick; // Left driver joystick
   Joystick *rightStick; // Right driver joystick
    Jaguar *lfDrive; // Front-left drive motor
    Jaguar *lrDrive; // Rear-left drive motor
    Jaguar *rfDrive; // Front-right drive motor
    Jaguar *rrDrive; // Rear-right drive motor
    Victor *frontSteer; // Front steering motor
    Victor *rearSteer; // Rear steering motor
    Encoder *frontSteeringEncoder; // Front steering encoder
    Encoder *rearSteeringEncoder; // Rear steering encoder
    Gyro *steeringGyro; // Steering gyro
    HSLImage *cameraImage; // Hue/Saturation/Luminence image from camera
    BinaryImage *binaryImage; // The Object used to store and edit the image
   
    /* This "public" means that these variables are available outside of
    this class...but we only have one, so it essentially does nothing.
    "SimpleTracker(void)" is called a constructor. Again, don't worry about
    the details. This is where our object-based variables (hardware components)
    are initialized. This means we assign them a particular port. */ 
    public:
   SimpleTracker(void)
   {
        // Assign object pointers here.     
        lfDrive = new Jaguar(LFMOTORPORT); // Driving Motors
        lrDrive = new Jaguar(LRMOTORPORT);
        rfDrive = new Jaguar(RFMOTORPORT);
        rrDrive = new Jaguar(RRMOTORPORT);
       
        frontSteer = new Victor(FRONTSTEERINGMOTORPORT); // Steering Motors
        rearSteer = new Victor(REARSTEERINGMOTORPORT);
       
        leftStick = new Joystick(LEFTJOYSTICKPORT); // Joysticks
        rightStick = new Joystick(RIGHTJOYSTICKPORT);
       
        frontSteeringEncoder = new Encoder(FRONTSTEERINGENCODERPORTA, FRONTSTEERINGENCODERPORTB); // Encoders
        rearSteeringEncoder = new Encoder(REARSTEERINGENCODERPORTA, REARSTEERINGENCODERPORTB);
       
        steeringGyro = new Gyro(1, STEERINGGYROPORT); // Analogue devices
       
        // Initialize encoders
        frontSteeringEncoder->Reset();
        frontSteeringEncoder->Start();
        rearSteeringEncoder->Reset();
        rearSteeringEncoder->Start();
       
        // Initialize gyro
        steeringGyro->Reset();
        steeringGyro->SetSensitivity(GYROSENSITIVITY); // Changes the sensitivity of the gyro
       
        // Enable camera and wait for a bit
        AxisCamera &camera = AxisCamera::GetInstance();
        Wait(4.0);
       
        // Set the camera's preferences
        camera.WriteResolution(camera.kResolution_160x120);
        camera.WriteCompression(0);
        camera.WriteBrightness(50);
        camera.WriteColorLevel(256);
   }
   
    /* 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 x, y; // The average x- and y-positions between the two 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; // Allow different angles for the front and back sides, for use in different steering modes
      int waitForSteering; // Should the drive motors wait for the wheels to finish turning?
      int frontFinished, rearFinished; // Store whether each driving side has finished turning
      int steeringMode = SWERVEMODE; // Store the current mode of steering (0 = strafe, 1 = monster, 2 = car)
      int currentButtonState = 0, previousButtonState = 0; // Store the current and previous states of the manual alignment buttons
      int targetX = 0; // Store the number of distinct sections on the refined camera image
      
      // Acquire reference to axis camera
      AxisCamera &camera = AxisCamera::GetInstance();
      
      // This code is run repeatedly so long as the robot is in teleop mode.
      while (IsOperatorControl())
      {
         /* This is a fancy, convoluted way of telling the operating-system
         software that the code hasn't frozen. */
         GetWatchdog().Feed();
         
         /* For debugging the encoder */
         CLEARMESSAGE;
         DISPLAYFLOAT(4, steeringGyro->GetAngle());
         DISPLAYINTEGER(1, (int)(frontSteeringEncoder->Get()));
         DISPLAYINTEGER(2, (int)(rearSteeringEncoder->Get()));
         DISPLAYINTEGER(3, targetX);
            
         // Call code to acquire and process camera image
         if(GETIMAGEBUTTON) targetX = GetProcessedImage(camera);
         
         /* Teleop code here. */
         // Check buttons
         if(SWERVEMODEBUTTON) steeringMode = SWERVEMODE;
         else if(MONSTERMODEBUTTON) steeringMode = MONSTERMODE;
         else if(CARMODEBUTTON) steeringMode = CARMODE;
         
         // Get joystick positions
         leftx = leftStick->GetRawAxis(1);
         lefty = leftStick->GetRawAxis(2);
         rightx = rightStick->GetRawAxis(1);
         righty = rightStick->GetRawAxis(2);
         
         // If the total y-deflection for each joystick is below the threshold, we shouldn't count it at all
         if(Abs(lefty) < JOYSTICKDEADBANDY) lefty = 0.0f;
         if(Abs(righty) < JOYSTICKDEADBANDY) righty = 0.0f;
         
                  
         // Average the joystick positions
         x = (leftx + rightx) / 2.0f;
         y = (lefty + righty) / 2.0f;
         
                  
         // Store the currentButtonState from the previous iteration
         previousButtonState = currentButtonState;
         
         // If the manual-alignment buttons are pressed
         if(MANUALALIGNENABLEBUTTON)
         {
            currentButtonState = 1;
            if(MANUALALIGNLEFTREAR) rearSteer->Set(-0.35f);
            else if(MANUALALIGNRIGHTREAR) rearSteer->Set(0.35f);
            else rearSteer->Set(0);
               
            if(MANUALALIGNLEFTFRONT) frontSteer->Set(0.35f);
            else if(MANUALALIGNRIGHTFRONT)frontSteer->Set(-0.35f);
            else frontSteer->Set(0);
         }
         
         
         // Otherwise, if the deflection left or right is less than a certain amount, we simply drive tank-style
         // Abs simply ensures that x is positive so that we don't need two separate if-statements
         else if(Abs(x) < JOYSTICKDEADBANDX)
         {
            // Tank drive
            // Set the drive motors according to their respective joysticks
            lfDrive->Set(lefty);
            lrDrive->Set(lefty);
            rfDrive->Set(righty);
            rrDrive->Set(righty);
            currentButtonState = 0;
            
            TrackFrontSteering(0);
            TrackRearSteering(0);
         }
         
         else
         {
            // Swerve drive
            // Determine the total deflection of the joysticks to use as the speed setting
            swerveSpeed = sqrt(x*x + y*y); // a^2 + b^2 = c^2
            swerveAngle = -Abs(atan(x/y)); // x/y = tan(theta)
            swerveAngle = swerveAngle * 180 / Pi; // 1 degree = 180/Pi radians
            swerveAngle = swerveAngle / 90.0f * PULSESPER90; // Convert from degrees to encoder pulses
            
            /* If the y-component is zero, we should try to move straight left or right. Therefore, we
             * need to wait for the wheels to finish turning before allowing the drive motors to move. The
             * angle, for some reason, also needs to be inverted when moving straight left or right. */
            if(Abs(y) < JOYSTICKDEADBANDY)
            {
               waitForSteering = 1;
               if(x < 0.0f) swerveAngle = PULSESPER90, swerveSpeed = -swerveSpeed;
               else if(x > 0.0f) swerveAngle = -PULSESPER90;
            }
            else waitForSteering = 0;
                        
            // Determine the quadrant of the joystick and convert the speed or angle appropriately
            // Quadrant one is assumed, except for the following conditions
            if(x < 0 && y >= 0) swerveAngle = -swerveAngle; // Quadrant two
            else if(x < 0 && y < 0) swerveSpeed = -swerveSpeed; // Quadrant three
            else if(x >= 0 && y < 0) // Quadrant Four
            {
               swerveSpeed = -swerveSpeed;
               swerveAngle = -swerveAngle;               
            }
            
            // If we are in monster mode and travelling faster than 80% max speed, we switch to car automatically
            if(Abs(swerveSpeed) > 0.8f && steeringMode == MONSTERMODE) steeringMode = CARMODE;
         
            // Put steer-angle and drive-direction changes here for the other steering modes (monster, car, auto-monster, etc.)
            if(steeringMode == MONSTERMODE) swerveAngleFront = swerveAngleRear = -0.75f * swerveAngle;
            else if(steeringMode == CARMODE)
            {
               if(swerveSpeed <= 0) swerveAngleFront = -0.5f * swerveAngle, swerveAngleRear = 0.0f;
               else swerveAngleRear = 0.5f * swerveAngle, swerveAngleFront = 0.0f;
            }
            
            else
            {
               swerveAngleFront = -swerveAngle;
               swerveAngleRear = swerveAngle;
            }
            
            /* Call pre-written functions that will use the encoder to get the wheels to gradually approach the desired
             * position. This function will return zero when the steering is finished.*/
            frontFinished = TrackFrontSteering((int)swerveAngleFront);
            rearFinished = TrackRearSteering((int)swerveAngleRear);
            
            // If the steering has finished, set the drive motors to move
            if((frontFinished == 0 && rearFinished == 0) || !waitForSteering)
            {
               lfDrive->Set(swerveSpeed);
               lrDrive->Set(swerveSpeed);
               rfDrive->Set(swerveSpeed);
               rrDrive->Set(swerveSpeed);
            }
            
            currentButtonState = 0;
         }
         
         // If the manual alignment buttons have JUST been released, reset the encoders again
         if(previousButtonState == 1 && currentButtonState == 0)
         {
            frontSteeringEncoder->Reset();
            frontSteeringEncoder->Start();
            rearSteeringEncoder->Reset();
            rearSteeringEncoder->Start();
            frontSteer->Set(0);
            rearSteer->Set(0);
         }
      }
   }
   
   /* Code to run while in autonomous, or fully-automated mode. */
   void Autonomous(void)
   {
      // Variables
      float steeringAngle; // Current angle from home as read by the gyro
      int imageCenter; // The center point of the clearest target
      int dividerCount = 0; // Count the number of times the loop has been executed
      
      // Acquire reference to axis camera
      AxisCamera &camera = AxisCamera::GetInstance();
      
      // This code is run repeatedly so long as the robot is in autonomous mode (lasting 15 seconds).
      while (IsAutonomous())
      {
         /* Tell the system that the code hasn't frozen. */
         GetWatchdog().Feed();
         
         /* Autonomous code here. */
         //Get the gyro angle
         steeringAngle = steeringGyro->GetAngle();
         
         // Update the center position of the target once every few times through this loop
         if(dividerCount >= CAMERAFREQUENCY)
         {
            imageCenter = GetProcessedImage(camera);
            dividerCount = 0;
         }
         
         else dividerCount++;
         
         // Display the center value
         CLEARMESSAGE;
         DISPLAYINTEGER(1, imageCenter);
         DISPLAYINTEGER(3, (int)(steeringAngle * 10.0f));
         
         // Turn the front wheels left and straighten the rear wheels for pivoting
         TrackFrontSteering(PULSESPER90);
         TrackRearSteering(0);
         
         // If the centre is -1, the target is not in view
         if(imageCenter == -1)
         {
            DISPLAYSTRING(2, "No target.");
            
            // Use the gyro to correct instead
            // If the angle is greater than zero, we need to turn left
            if(steeringAngle - GYRODEADBANDANGLE > 0.0f)
            {
               lfDrive->Set(0.4);
               rfDrive->Set(0.4);
            }
            
            // If the angle is less than zero, we need to turn right
            else if(steeringAngle + GYRODEADBANDANGLE < 0.0f)
            {
               lfDrive->Set(-0.4);
               rfDrive->Set(-0.4);
            }
            
            // Otherwise, we are within our threshold and stop
            else
            {
               lfDrive->Set(0);
               rfDrive->Set(0);
            }
         }
         
         // Otherwise, if the center is too far right, turn the robot right
         else if(imageCenter + IMAGEDEADBAND < 160)
         {
            DISPLAYSTRING(2, "Right");
            lfDrive->Set(0.4);
            rfDrive->Set(0.4);
         }
         
         // If the center is too far left, turn the robot left
         else if(imageCenter - IMAGEDEADBAND > 160)
         {
            DISPLAYSTRING(2, "Left");
            lfDrive->Set(-0.4);
            rfDrive->Set(-0.4);
         }
         
         // If the centre is close enough to the middle of the screen, stop
         else
         {
            DISPLAYSTRING(2, "Stop");
            lfDrive->Set(0.0);
            rfDrive->Set(0.0);                         
         }
      }
   }
   
   /* This function handles the complicated problem of quickly, but accurately, moving the
    * front wheels to their correct position, as passed 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 < STEERINGDEADBAND) speed = 0.0f;
      else if(frontSteeringEncoderValue < targetPosition) speed = (float)remainingSteeringDistance / (float)STEERINGAPPROACHDISTANCE;
      else speed = (float)-remainingSteeringDistance / (float)STEERINGAPPROACHDISTANCE;
      
      // 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 < STEERINGDEADBAND) speed = 0.0f;
      else if(rearSteeringEncoderValue < targetPosition) speed = (float)remainingSteeringDistance / (float)STEERINGAPPROACHDISTANCE;
      else speed = (float)-remainingSteeringDistance / (float)STEERINGAPPROACHDISTANCE;
            
      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;
   }
   
   /* Take the camera image and process it, returning the x-position of the largest particle. This should represent the
   horizontal centre of the most in-focus target. Comparing this value to the image width (320) should allow us to
   centre the image by rotating the robot. If the targets are out of view, -1 is returned. */
   int GetProcessedImage(AxisCamera &camera)
   {
      register int i;
      int numberOfParticles, largestParticle;
      double maxArea = 0, width, height, maxWidth = 0, maxHeight = 0, horizontalPosition;
      
      // Save the image as a BinaryImage Object
      cameraImage = camera.GetImage();
      binaryImage = cameraImage->ThresholdHSL(HUEMIN, HUEMAX, SATURATIONMIN, SATURATIONMAX, LUMINANCEMIN, LUMINANCEMAX);
      
      // Perform the Convex Hull operation
      imaqConvexHull(binaryImage->GetImaqImage(), binaryImage->GetImaqImage(), 1);
      
      // Count the number of distinct sections remaining
      imaqCountParticles(binaryImage->GetImaqImage(), 2, &numberOfParticles);
            
       // Save image to the cRIO's memory (enable for debugging purposes only)
       //cameraImage->Write("camIm4.jpeg");
       //binaryImage->Write("binIm4.bmp");
      
       // For each particle found
       for(i = 0; i < numberOfParticles; i++)
       {
          // Get the width and height of the given particle
          imaqMeasureParticle(binaryImage->GetImaqImage(), i, 0, IMAQ_MT_BOUNDING_RECT_WIDTH, &width);
          imaqMeasureParticle(binaryImage->GetImaqImage(), i, 0, IMAQ_MT_BOUNDING_RECT_HEIGHT, &height);
          
          // If the area of this particle is the largest area so far
          if(width * height > maxArea)
          {
             // Store the dimensions and area of the particle, along with the index of said particle
             maxWidth = width;
             maxHeight = height;
             maxArea = width * height;
             largestParticle = i;
          }
       }
      
       // If the largest particle is outside of the threshold, return -1 to indicate that the targets aren't visible
       if(maxArea < MINIMUMAREA) return -1;
      
       // Find the x-position of the centre of the largest particle, assumed to be the nearest target
       imaqMeasureParticle(binaryImage->GetImaqImage(), largestParticle, 0, IMAQ_MT_CENTER_OF_MASS_X, &horizontalPosition);
      
       // Return this x-position
       return (int)horizontalPosition;
   }
}; /* Semicolons are not used after functions, but classes are treated
more like datatypes, so they DO have a semicolon after them. To think some
people say that object-oriented makes sense :/ */

// 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. This macro tells
the compiler to use the SimpleTracker class. There are a series of
"under-the-hood" operations so that the program starts in this class
instead of searching for int main(void). */
START_ROBOT_CLASS(SimpleTracker);

As you can see, many of the macros have been removed. They have been copied into a pair of header files, titled "NewRobot.h" and "OldRobot.h". They can be found by looking under the project tree on the left panel in WindRiver, but they should open into tabs along the top automatically as long as no one closes them. These contain the same essential set of macros, but with the values changed depending on which robot is intended to be used. Therefore, by keeping these files separate and accurate, there will be no confusion as to which value is associated with which robot, and it will not be possible to forget to change a value when moving from one to the other. The only thing that needs to be done is that the line near the top that says
"#define ROBOT NEW" must be changed to "#define ROBOT OLD", and vice-versa. Simply specify the robot you desire to use.

This means that macro value changes are made WITHIN these files. Make sure that these files stay synchronized--a new macro added to one file MUST be added to the other. All of the same macros MUST be there, but they can have different values. On top of this, changes to these files must ALSO be posted on this board, and these files must be copied, at the end of each day, into the two other text files under "Robotics 2012\Code\" AND backups must be made in the "2012 Backups" folder as well. They will be named as follows: "Backup#New - Macros.txt" and "Backup#Old - Macros.txt". New is for the "NewRobot.h" header file, and Old is for the "OldRobot.h" header file. Replace the "#" with the appropriate number, and replace "Macros" with a list of all of the values that were changed.

Anyway, I believe that is all. Check this code over, but don't worry about the preprocessor magic used to get these separate files working Wink

Brendan van Ryn

Posts : 95
Join date : 2010-09-30
Age : 24

Back to top Go down

Re: Code Samples

Post  Brendan van Ryn on Sun Feb 12, 2012 6:49 pm

Current version of the robot code (with many untested changes and features):
Code:
/******************************************************************************
 *
 *    Sprockets 2012 Base Code
 *    Authors:    Brendan van Ryn
 *             Stuart Sullivan
 *             Terry Shi
 *             Aliah McCalla
 *             Will Surmak
 *              Jacob 't Mannetje
 *    Last Updated:   13/01/2012
 *
 ******************************************************************************/

/* Robot configuration macros. Do not change. */
#define NEW 1
#define OLD 2

/* Robot configuration */
#define ROBOT NEW // Set this to either "OLD" or "NEW", without the quotation marks, to configure for the given robot

/* Set the correct macros based on which robot is being used. Do not change this. */
#if !defined ROBOT
#warning "Robot version not specified. Assuming new configuration..."
#include "NewRobot.h"
#elif ROBOT == NEW
#include "NewRobot.h"
#elif ROBOT == OLD
#include "OldRobot.h"
#else
#warning "Robot configuration must be either 'OLD' or 'NEW'. Assuming new..."
#include "NewRobot.h"
#endif

// Header file for WPI standard hardware/robot overhead.
#include "WPILib.h"

// Include the libraries for the camera
#include "vision/AxisCamera.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()

// Prints an integer, a, on the Driver Station on line, b.
#define DISPLAYINTEGER(b, a) DriverStationLCD::GetInstance()->Printf(DriverStationLCD::kUser_Line ## b, 1, "%d", a); DriverStationLCD::GetInstance()->UpdateLCD()

// Prints a string, a, on the Driver Station on line, b.
#define DISPLAYSTRING(b, a) DriverStationLCD::GetInstance()->Printf(DriverStationLCD::kUser_Line ## b, 1, a); DriverStationLCD::GetInstance()->UpdateLCD()

// Prints a float, a, on the Driver Station on line, b.
#define DISPLAYFLOAT(b, a) DriverStationLCD::GetInstance()->Printf(DriverStationLCD::kUser_Line ## b, 1, "%0.2f", a); DriverStationLCD::GetInstance()->UpdateLCD()

// 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
double WidthToDistance(double); // Returns the distance from the camera depending on the width


/* Our main (and this year, only) class. Don't worry about the word
"class" as we won't do much Object-Oriented stuff, but in the layout
provided to use for the robot code, we must have this one class. The
code for the robot goes here, along with all variables we need. It has
always been called "SimpleTracker" for whatever reason". */
class SimpleTracker : public SimpleRobot // Just ignore this, essentially. Treat it as "int main(void)" for simplicity.
{
    // "Global" variables are declared here.
   Joystick *leftStick; // Left driver joystick
   Joystick *rightStick; // Right driver joystick
   Joystick *speedStick; // Third driver joystick
    Jaguar *lfDrive; // Front-left drive motor
    Jaguar *lrDrive; // Rear-left drive motor
    Jaguar *rfDrive; // Front-right drive motor
    Jaguar *rrDrive; // Rear-right drive motor
    Victor *frontSteer; // Front steering motor
    Victor *rearSteer; // Rear steering motor
    Victor *topShooter; // Top shooting roller
    Victor *bottomShooter; // Bottom shooting roller
    Encoder *frontSteeringEncoder; // Front steering encoder
    Encoder *rearSteeringEncoder; // Rear steering encoder
    Gyro *steeringGyro; // Steering gyro
    HSLImage *cameraImage; // Hue/Saturation/Luminence image from camera
    BinaryImage *binaryImage; // The object used to store and edit the filtered image
    Image *imaqImage; // Stores the image in a format that the built-in imaq functions can use
    Relay *magazineBelt; // Magazine conveyor
    Relay *lateralCollector; // Lateral collector belts
    Relay *transverseCollector; // Transverse collector belts
    DigitalInput *topRollerEncoder; // Light sensor for top launching roller
    DigitalInput *bottomRollerEncoder; // Light sensor for bottom launching roller
    Servo *frontDriveServo; // The servo to shift gears for the front gear box
    Servo *rearDriveServo; // THe servo to shift gears for the rear gear box
    double maxTargetWidth; // Width of clearest target in view
    double maxTargetHeight; // Height of clearest target in view
   
    /* This "public" means that these variables are available outside of
    this class...but we only have one, so it essentially does nothing.
    "SimpleTracker(void)" is called a constructor. Again, don't worry about
    the details. This is where our object-based variables (hardware components)
    are initialized. This means we assign them a particular port. */ 
    public:
   SimpleTracker(void)
   {
        // Assign object pointers here.     
        lfDrive = new Jaguar(LFMOTORPORT); // Driving Motors
        lrDrive = new Jaguar(LRMOTORPORT);
        rfDrive = new Jaguar(RFMOTORPORT);
        rrDrive = new Jaguar(RRMOTORPORT);
       
        frontSteer = new Victor(FRONTSTEERINGMOTORPORT); // Steering Motors
        rearSteer = new Victor(REARSTEERINGMOTORPORT);
       
        topShooter = new Victor(TOPSHOOTINGMOTORPORT); // Shooter motors
        bottomShooter = new Victor(BOTTOMSHOOTINGMOTORPORT);
       
        leftStick = new Joystick(LEFTJOYSTICKPORT); // Joysticks
        rightStick = new Joystick(RIGHTJOYSTICKPORT);
        speedStick = new Joystick(SPEEDJOYSTICKPORT);
       
        frontSteeringEncoder = new Encoder(FRONTSTEERINGENCODERPORTA, FRONTSTEERINGENCODERPORTB); // Encoders
        rearSteeringEncoder = new Encoder(REARSTEERINGENCODERPORTA, REARSTEERINGENCODERPORTB);
       
        steeringGyro = new Gyro(1, STEERINGGYROPORT); // Analogue devices
       
        magazineBelt = new Relay(MAGAZINERELAYPORT); // Relays
        lateralCollector = new Relay(LATERALCOLLECTORPORT);
        transverseCollector = new Relay(TRANSVERSECOLLECTORPORT);
       
        topRollerEncoder = new DigitalInput(TOPROLLERENCODERPORT); // Shooting encoder light sensors
        bottomRollerEncoder = new DigitalInput(BOTTOMROLLERENCODERPORT);
       
        frontDriveServo = new Servo(FRONTSERVOPORT);
        rearDriveServo = new Servo(REARSERVOPORT);

        // Initialize encoders
        frontSteeringEncoder->Reset();
        frontSteeringEncoder->Start();
        rearSteeringEncoder->Reset();
        rearSteeringEncoder->Start();
       
        // Initialize gyro
        steeringGyro->Reset();
        steeringGyro->SetSensitivity(GYROSENSITIVITY); // Changes the sensitivity of the gyro
       
        // frontDriveServo->SetAngle(0.0);
       
        // Enable camera and wait for a bit
        AxisCamera &camera = AxisCamera::GetInstance();
        Wait(8.0);
       
        // Set the camera's preferences
        camera.WriteResolution(camera.kResolution_320x240); // Make sure this resolution matches the width and height macros
        camera.WriteCompression(0);
        camera.WriteBrightness(50);
        camera.WriteColorLevel(256);
   }
   
    /* 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 x, y; // The average x- and y-positions between the two 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; // Allow different angles for the front and back sides, for use in different steering modes
      int waitForSteering; // Should the drive motors wait for the wheels to finish turning?
      int frontFinished, rearFinished; // Store whether each driving side has finished turning
      int steeringMode = SWERVEMODE; // Store the current mode of steering (0 = strafe, 1 = monster, 2 = car)
      int currentButtonState = 0, previousButtonState = 0; // Store the current and previous states of the manual alignment buttons
      int targetX = 0; // Store the number of distinct sections on the refined camera image
      double distance = 0.0; // The temp variable to display distance, used to test the distance formula that is in progress
      int dividerCount = 0; // Count the number of times the loop has been executed
      float topShooterSpeed = 0.0f; // Speed of the top shooter
      float bottomShooterSpeed = 0.0f; // Speed of the bottom shooter
      int topShooterPosition, bottomShooterPosition; // Positions of the top and bottom shooting encoders
      
      // Acquire reference to axis camera
      AxisCamera &camera = AxisCamera::GetInstance();
      
      // This code is run repeatedly so long as the robot is in teleop mode.
      while (IsOperatorControl())
      {
         /* This is a fancy, convoluted way of telling the operating-system
         software that the code hasn't frozen. */
         GetWatchdog().Feed();
         
         /* Message displays */
         CLEARMESSAGE;
         DISPLAYINTEGER(1, frontSteeringEncoder->Get()); // Encoder value for top shooting roller
         DISPLAYINTEGER(2, rearSteeringEncoder->Get()); // Encoder value for bottom shooting roller
         DISPLAYINTEGER(3, targetX); // X-position of target (or -1)
         DISPLAYFLOAT(4, distance); // Distance to target, in feet
         DISPLAYFLOAT(5, topShooterSpeed);
         DISPLAYFLOAT(6, bottomShooterSpeed);
         
         // Call code to acquire and process camera image
         if(GETIMAGEBUTTON)
         {
            targetX = GetProcessedImage(camera);
            distance = WidthToDistance(maxTargetWidth);
         }
         
         // Update the encoder speed every few times through this loop
         if(dividerCount >= ENCODERFREQUENCY)
         {
            // Store the number of pulses that have passed in the given period of time
            topShooterSpeed = (float)topShooterPosition;
            bottomShooterSpeed = (float)bottomShooterPosition;
            
            // Reset encoders
            TopShooterEncoder(1);
            BottomShooterEncoder(1);
            
            dividerCount = 0;
         }
         
         else dividerCount++;
         
         /* Teleop code here. */
         // Check  drive mode buttons
         if(SWERVEMODEBUTTON) steeringMode = SWERVEMODE;
         else if(MONSTERMODEBUTTON) steeringMode = MONSTERMODE;
         else if(CARMODEBUTTON) steeringMode = CARMODE;
         
         // Check belt buttons
         if(MAGAZINEENABLEBUTTON) magazineBelt->Set(Relay::kForward);
         else magazineBelt->Set(Relay::kOff);
         if(LATERALCOLLECTORBUTTON) lateralCollector->Set(Relay::kReverse);
         else lateralCollector->Set(Relay::kOff);
         if(TRANSVERSECOLLECTORBUTTON) transverseCollector->Set(Relay::kReverse);
         else transverseCollector->Set(Relay::kOff);
         
         // Check shooter buttons
         if(SHOOTFULLBUTTON) topShooter->Set(0.3), bottomShooter->Set(0.3); //SetRollerSpeed(5.0f, topShooterSpeed, bottomShooterSpeed);
         else topShooter->Set(0), bottomShooter->Set(0);
         
         // These must be called each time in order for the encoder positions to update
         topShooterPosition = TopShooterEncoder(0);
         bottomShooterPosition = BottomShooterEncoder(0);
                  
         // Get joystick positions
         leftx = -leftStick->GetRawAxis(1);
         lefty = leftStick->GetRawAxis(2);
         rightx = -rightStick->GetRawAxis(1);
         righty = rightStick->GetRawAxis(2);
         
         // If the total y-deflection for each joystick is below the threshold, we shouldn't count it at all
         if(Abs(lefty) < JOYSTICKDEADBANDY) lefty = 0.0f;
         if(Abs(righty) < JOYSTICKDEADBANDY) righty = 0.0f;
         
                  
         // Average the joystick positions
         x = (leftx + rightx) / 2.0f;
         y = (lefty + righty) / 2.0f;
         
                  
         // Store the currentButtonState from the previous iteration
         previousButtonState = currentButtonState;
         
         // If the manual-alignment buttons are pressed
         if(MANUALALIGNENABLEBUTTON)
         {
            currentButtonState = 1;
            if(MANUALALIGNLEFTREAR) rearSteer->Set(0.75f);
            else if(MANUALALIGNRIGHTREAR) rearSteer->Set(-0.75f);
            else rearSteer->Set(0);
               
            if(MANUALALIGNLEFTFRONT) frontSteer->Set(-0.75f);
            else if(MANUALALIGNRIGHTFRONT)frontSteer->Set(0.75f);
            else frontSteer->Set(0);
         }
         
         // Otherwise, if the deflection left or right is less than a certain amount, we simply drive tank-style
         // Abs simply ensures that x is positive so that we don't need two separate if-statements
         else if(Abs(x) < JOYSTICKDEADBANDX)
         {
            // Tank drive
            // Set the drive motors according to their respective joysticks
            lfDrive->Set(lefty);
            lrDrive->Set(lefty);
            rfDrive->Set(righty);
            rrDrive->Set(righty);
            currentButtonState = 0;
            
            TrackFrontSteering(0);
            TrackRearSteering(0);
         }
         
         else
         {
            // Swerve drive
            // Determine the total deflection of the joysticks to use as the speed setting
            swerveSpeed = sqrt(x*x + y*y); // a^2 + b^2 = c^2
            swerveAngle = -Abs(atan(x/y)); // x/y = tan(theta)
            swerveAngle = swerveAngle * 180 / Pi; // 1 degree = 180/Pi radians
            swerveAngle = swerveAngle / 90.0f * PULSESPER90; // Convert from degrees to encoder pulses
            
            /* If the y-component is zero, we should try to move straight left or right. Therefore, we
             * need to wait for the wheels to finish turning before allowing the drive motors to move. The
             * angle, for some reason, also needs to be inverted when moving straight left or right. */
            if(Abs(y) < JOYSTICKDEADBANDY)
            {
               waitForSteering = 1;
               if(x < 0.0f) swerveAngle = PULSESPER90, swerveSpeed = -swerveSpeed;
               else if(x > 0.0f) swerveAngle = -PULSESPER90;
            }
            else waitForSteering = 0;
                        
            // Determine the quadrant of the joystick and convert the speed or angle appropriately
            // Quadrant one is assumed, except for the following conditions
            if(x < 0 && y >= 0) swerveAngle = -swerveAngle; // Quadrant two
            else if(x < 0 && y < 0) swerveSpeed = -swerveSpeed; // Quadrant three
            else if(x >= 0 && y < 0) // Quadrant Four
            {
               swerveSpeed = -swerveSpeed;
               swerveAngle = -swerveAngle;               
            }
            
            // If we are in monster mode and travelling faster than 80% max speed, we switch to car automatically
            if(Abs(swerveSpeed) > 0.8f && steeringMode == MONSTERMODE) steeringMode = CARMODE;
         
            // Put steer-angle and drive-direction changes here for the other steering modes (monster, car, auto-monster, etc.)
            if(steeringMode == MONSTERMODE) swerveAngleFront = swerveAngleRear = -0.75f * swerveAngle;
            else if(steeringMode == CARMODE)
            {
               if(swerveSpeed <= 0) swerveAngleFront = -0.5f * swerveAngle, swerveAngleRear = 0.0f;
               else swerveAngleRear = 0.5f * swerveAngle, swerveAngleFront = 0.0f;
            }
            
            else
            {
               swerveAngleFront = -swerveAngle;
               swerveAngleRear = swerveAngle;
            }
            
            /* Call pre-written functions that will use the encoder to get the wheels to gradually approach the desired
             * position. This function will return zero when the steering is finished.*/
            frontFinished = TrackFrontSteering((int)swerveAngleFront);
            rearFinished = TrackRearSteering((int)swerveAngleRear);
            
            // If the steering has finished, set the drive motors to move
            if((frontFinished == 0 && rearFinished == 0) || !waitForSteering)
            {
               lfDrive->Set(swerveSpeed);
               lrDrive->Set(swerveSpeed);
               rfDrive->Set(swerveSpeed);
               rrDrive->Set(swerveSpeed);
            }
            
            currentButtonState = 0;
         }
         
         // If the manual alignment buttons have JUST been released, reset the encoders again
         if(previousButtonState == 1 && currentButtonState == 0)
         {
            frontSteeringEncoder->Reset();
            frontSteeringEncoder->Start();
            rearSteeringEncoder->Reset();
            rearSteeringEncoder->Start();
            frontSteer->Set(0);
            rearSteer->Set(0);
         }
      }
   }
   
   /* Code to run while in autonomous, or fully-automated mode. */
   void Autonomous(void)
   {
      // Variables
      float gyroAngle; // Current angle from home as read by the gyro
      int imageCenter = 0; // The center point of the clearest target
      int dividerCount = 0; // Counts the number of times the loop has been executed
      int numImages = 0; // Counts the number of images that have been taken from the camera
      float correctionSpeed; // Stores the speed at which the drive motors should move to compensate the camera/gyro
      
      // Acquire reference to axis camera
      AxisCamera &camera = AxisCamera::GetInstance();
      
      // This code is run repeatedly so long as the robot is in autonomous mode (lasting 15 seconds).
      while (IsAutonomous())
      {
         /* Tell the system that the code hasn't frozen. */
         GetWatchdog().Feed();
         
         /* Autonomous code here. */
         //Get the gyro angle
         gyroAngle = steeringGyro->GetAngle();
         
         // Update the center position of the target once every few times through this loop
         if(dividerCount >= CAMERAFREQUENCY)
         {
            imageCenter = GetProcessedImage(camera);
            dividerCount = 0;
            numImages++;
         }
         
         else dividerCount++;
         
         // Message displays
         CLEARMESSAGE;
         DISPLAYINTEGER(1, imageCenter); // Centre position of target (or -1)
         DISPLAYINTEGER(3, (int)(gyroAngle * 10.0f)); // Gyro angle
         DISPLAYINTEGER(4, numImages); // Number of images from the camera taken so far
         
         // Turn the front wheels left and straighten the rear wheels for pivoting
         TrackFrontSteering(0);
         TrackRearSteering(PULSESPER90);
         
         // If the centre is -1, the target is not in view
         if(imageCenter == -1)
         {
            // Use the gyro to correct instead
            // If the angle is greater than zero, we need to turn left
            if(gyroAngle - GYRODEADBANDANGLE > 0.0f)
            {
               DISPLAYSTRING(2, "Gyro left");
               rrDrive->Set(0.3);
               lrDrive->Set(0.3);
            }
            
            // If the angle is less than zero, we need to turn right
            else if(gyroAngle + GYRODEADBANDANGLE < 0.0f)
            {
               DISPLAYSTRING(2, "Gyro right");
               rrDrive->Set(-0.3);
               lrDrive->Set(-0.3);
            }
            
            // Otherwise, we are within our threshold, so stop
            else
            {
               DISPLAYSTRING(2, "No target");
               rrDrive->Set(0);
               lrDrive->Set(0);
            }
         }
         
         else
         {
            // Calculate the speed at which to move the wheels
            correctionSpeed = Abs((float)imageCenter - IMAGEWIDTH / 2.0f) / (float)IMAGEAPPROACHDISTANCE * 0.4f;
            if(correctionSpeed > 0.4f) correctionSpeed = 0.4f;
            
            // Otherwise, if the center is too far right, turn the robot right
            if(imageCenter + IMAGEDEADBAND < (int)IMAGEWIDTH / 2)
            {
               DISPLAYSTRING(2, "Right");
               rrDrive->Set(correctionSpeed);
               lrDrive->Set(correctionSpeed);
            }
            
            // If the center is too far left, turn the robot left
            else if(imageCenter - IMAGEDEADBAND > (int)IMAGEWIDTH / 2)
            {
               DISPLAYSTRING(2, "Left");
               rrDrive->Set(-correctionSpeed);
               lrDrive->Set(-correctionSpeed);
            }
            
            // If the centre is close enough to the middle of the screen, stop
            else
            {
               DISPLAYSTRING(2, "Stop");
               rrDrive->Set(0.0);
               lrDrive->Set(0.0);                         
            }
         }
         
         /* Simple, straight-movement autonomous
         // Turn the rear wheels left to correct if we are off to the left
         // Otherwise, turn the wheels right to correct if we are off to the right
         // Otherwise, move straight
         if(gyroAngle + GYRODEADBAND < 0.0f) TrackRearSteering(PULSESPER90 / 9);
         else if(gyroAngle - GYRODEADBAND > 0.0f) TrackRearSteering(-PULSESPER90 / 9);
         else TrackRearSteering(0);
         
         // Keep the front wheels straight and drive forward slowly
         TrackFrontSteering(0);
         lfDrive->Set(0.3);
         lrDrive->Set(0.3);
         rfDrive->Set(0.3);
         rrDrive->Set(0.3);*/
      }
   }
   
   /* This function handles the complicated problem of quickly, but accurately, moving the
    * front wheels to their correct position, as passed 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 < STEERINGDEADBAND) speed = 0.0f;
      else if(frontSteeringEncoderValue < targetPosition) speed = (float)remainingSteeringDistance / (float)STEERINGAPPROACHDISTANCE;
      else speed = (float)-remainingSteeringDistance / (float)STEERINGAPPROACHDISTANCE;
      
      // 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 < STEERINGDEADBAND) speed = 0.0f;
      else if(rearSteeringEncoderValue < targetPosition) speed = (float)remainingSteeringDistance / (float)STEERINGAPPROACHDISTANCE;
      else speed = (float)-remainingSteeringDistance / (float)STEERINGAPPROACHDISTANCE;
      
      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;
   }
   
   /* Take the camera image and process it, returning the x-position of the largest particle. This should represent the
   horizontal centre of the most in-focus target. Comparing this value to the image width (320) should allow us to
   centre the image by rotating the robot. If the targets are out of view, -1 is returned. */
   int GetProcessedImage(AxisCamera &camera)
   {
      register int i;
      int numberOfParticles, largestParticle;
      double maxArea = 0, width, height, horizontalPosition;
      
      // Save the image as a BinaryImage Object
      cameraImage = camera.GetImage();
      binaryImage = cameraImage->ThresholdHSL(HUEMIN, HUEMAX, SATURATIONMIN, SATURATIONMAX, LUMINANCEMIN, LUMINANCEMAX);
      imaqImage = binaryImage->GetImaqImage();
      
      // Perform the Convex Hull operation
      imaqConvexHull(imaqImage, imaqImage, 1);
      
      // Count the number of distinct sections remaining
      imaqCountParticles(imaqImage, 2, &numberOfParticles);
            
       // Save image to the cRIO's memory (enable for debugging purposes only)
       //cameraImage->Write("camIm4.jpeg");
       //binaryImage->Write("binIm4.bmp");
      
       // For each particle found
       for(i = 0; i < numberOfParticles; i++)
       {
          // Get the width and height of the given particle
          imaqMeasureParticle(imaqImage, i, 0, IMAQ_MT_BOUNDING_RECT_WIDTH, &width);
          imaqMeasureParticle(imaqImage, i, 0, IMAQ_MT_BOUNDING_RECT_HEIGHT, &height);
          
          // If the area of this particle is the largest area so far
          if(width * height > maxArea)
          {
             // Store the dimensions and area of the particle, along with the index of said particle
             maxTargetWidth = width;
             maxTargetHeight = height;
             maxArea = width * height;
             largestParticle = i;
          }
       }
        
       // If the largest particle is outside of the threshold, return -1 to indicate that the targets aren't visible
       if(maxArea < MINIMUMAREA)
       {
          delete binaryImage;
          delete cameraImage;
          
          return -1;
       }
      
       // Find the x-position of the centre of the largest particle, assumed to be the nearest target
       imaqMeasureParticle(imaqImage, largestParticle, 0, IMAQ_MT_CENTER_OF_MASS_X, &horizontalPosition);
      
       // Return this x-position
       return (int)horizontalPosition;
   }
   
   /* This function handles all of the encoder processing for the top shooting roller. The parameter, if true
    * (any non-zero number), resets the pulse count to zero. Otherwise, it keeps counting from where it was. The
    * return value is the current pulse count of the encoder. */
   int TopShooterEncoder(int reset)
   {
      // "Static" means it will retain it's value from previous calls to the function
      static int previousState = 0; // Previous state of the lightSensor
      static int pulseCount = 0; // Number of pulses recorded so far
      int currentState;
      
      // Record one pulse each time the light sensor's state changes
      currentState = topRollerEncoder->Get();
      if(currentState != previousState) pulseCount++;
      previousState = currentState;
      
      // Reset if necessary, and return the current number of pulses
      if(reset != 0) pulseCount = 0;
      return pulseCount;
   }
   
   /* This function is identical to the above function, but for the other roller */
   int BottomShooterEncoder(int reset)
   {
      static int previousState = 0;
      static int pulseCount = 0;
      int currentState;
      
      currentState = bottomRollerEncoder->Get();
      if(currentState != previousState) pulseCount++;
      previousState = currentState;
      
      if(reset != 0) pulseCount = 0;
      return pulseCount;
   }
   
   // Maintains a specific speed for the shooting rollers. Returns true when rollers are at that speed ***
   int SetRollerSpeed(float targetSpeed, float topSpeed, float bottomSpeed)
   {
      static float topPower = 0, bottomPower = 0;
      int toReturn = 0;
      
      // If the top roller is too fast, slow it down; if it's too slow, speed it up
      // If it's where it needs to be, set it's flag to 1 for being finished
      if(topSpeed + SHOOTERDEADBAND < targetSpeed && topPower < 1.0f) topPower += 0.001f;
      else if(topSpeed - SHOOTERDEADBAND > targetSpeed && topPower > 0.0f) topPower -= 0.001f;
      else toReturn |= 1;
      
      // Same as above, but for bottom roller, and uses a different flag (the second bit instead of the first)
      if(bottomSpeed + SHOOTERDEADBAND < targetSpeed && bottomPower < 1.0f) bottomPower += 0.001f;
      else if(bottomSpeed - SHOOTERDEADBAND > targetSpeed && bottomPower > 0.0f) bottomPower -= 0.001f;
      else toReturn |= 2;
      
      // Set the motors to their determined speeds
      topShooter->Set(topPower);
      bottomShooter->Set(bottomPower);
      
      // Return a value that reflects which motors are at the desired speed, and which aren't.
      return toReturn;
   }
}; /* Semicolons are not used after functions, but classes are treated
more like datatypes, so they DO have a semicolon after them. To think some
people say that object-oriented makes sense :/ */

// 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;
}

// Converts the width of the rectangle to a distance in feet
double WidthToDistance(double x)
{
   double result; // The resut of the calculations
   
   result = GOALWIDTH * IMAGEWIDTH / 2 / x / 2;
   result = result / tan(HALFCAMERAVIEWINGANGLE * Pi / 180);
   result = result * LINEARDISTANCEK + CONSTANTDISTANCEK;
   
   return result;
}

/* The entry point is FRC_UserProgram_StartupLibraryInit. This macro tells
the compiler to use the SimpleTracker class. There are a series of
"under-the-hood" operations so that the program starts in this class
instead of searching for int main(void). */
START_ROBOT_CLASS(SimpleTracker);

Pertinent header file:
Code:
/* 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 leftStick->GetRawButton(2)
#define MANUALALIGNENABLEBUTTON (rightStick->GetRawButton(8) && rightStick->GetRawButton(9))
#define MANUALALIGNLEFTREAR rightStick->GetRawButton(7)
#define MANUALALIGNRIGHTREAR rightStick->GetRawButton(10)
#define MANUALALIGNLEFTFRONT rightStick->GetRawButton(6)
#define MANUALALIGNRIGHTFRONT rightStick->GetRawButton(11)
#define GETIMAGEBUTTON leftStick->GetRawButton(3)
#define MAGAZINEENABLEBUTTON rightStick->GetRawButton(5)
#define LATERALCOLLECTORBUTTON leftStick->GetRawButton(5)
#define TRANSVERSECOLLECTORBUTTON leftStick->GetRawButton(5)
#define SHOOTFULLBUTTON rightStick->GetRawButton(2)

// Port numbers
// Motors are on PWM ports
#define LFMOTORPORT 3
#define LRMOTORPORT 8
#define RFMOTORPORT 7
#define RRMOTORPORT 4
#define FRONTSTEERINGMOTORPORT 2
#define REARSTEERINGMOTORPORT 1
#define TOPSHOOTINGMOTORPORT 9
#define BOTTOMSHOOTINGMOTORPORT 10

// The gear-shifting servos are on PWM ports
#define FRONTSERVOPORT 5
#define REARSERVOPORT 6

// 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

// Light sensors for encoders are also digital inputs
#define TOPROLLERENCODERPORT 6
#define BOTTOMROLLERENCODERPORT 5

// The gyro is on an analog channel
#define STEERINGGYROPORT 2

// Relays have their own ports
#define MAGAZINERELAYPORT 2
#define LATERALCOLLECTORPORT 1
#define TRANSVERSECOLLECTORPORT 3

// Constants
// Hardware constants
#define GYROSENSITIVITY 0.007f // Volts per degree per second
#define PULSESPER90 1800 // Number of encoder pulses in a 90-degree turn
#define STEERINGDEADBAND 50 // Number of encoder pulses of proximity needed for the steering (lower numbers are more accurate, but can cause hunting)
#define STEERINGAPPROACHDISTANCE 120 // Number of encoder pulses of proximity after which the wheels begin to slow down
#define CAMERAFREQUENCY 3000 // The number of times the IsOperatorControl() or IsAutonomous() loops must run before updating the camera image
#define HALFCAMERAVIEWINGANGLE 23.5 // Half of the viewing angle of the camera
#define LINEARDISTANCEK 1.0 // The camera distance is multiplied by this constant
#define CONSTANTDISTANCEK 0.0 // The camera distance has this added to it
#define IMAGEWIDTH 320.0 // The width of the image from the camera
#define IMAGEHEIGHT 240.0 // The height of the image from the camera
#define ENCODERFREQUENCY 200 // The number of times the IsOperatorControl() loop must run before updating the encoder speed

// Hardware threshold constants
#define JOYSTICKDEADBANDX 0.100f
#define JOYSTICKDEADBANDY 0.100f
#define GYRODEADBANDANGLE 3.0f
#define SHOOTERDEADBAND 0.2f

// Camera HSL Image Thresholds
#define HUEMAX 255
#define HUEMIN 0
#define SATURATIONMAX 15
#define SATURATIONMIN 0
#define LUMINANCEMAX 255
#define LUMINANCEMIN 200 // 160
#define MINIMUMAREA 220

// The deadband for the the horizontal pivoting to target
#define IMAGEDEADBAND 10
#define IMAGEAPPROACHDISTANCE 60

// Other constants
#define Pi 3.1415926535897932384626433832795028841971f
#define SWERVEMODE 0
#define MONSTERMODE 1
#define CARMODE 2
#define GOALWIDTH 2.0

Brendan van Ryn

Posts : 95
Join date : 2010-09-30
Age : 24

Back to top Go down

Today's Progress

Post  StuartSullivan on Fri Feb 17, 2012 8:01 pm

The main changes have been to the autonomous but there are other miscellaneous changes.
- A new shooter button has been added to spin the rollers at a speed dependent on the distance from the clearest target mainly for testing purposes.
- The ball feeder system is now able to run in reverse on R4.
- The feeder and conveyor system are all on the same button, R5.
- The Autonomous Alignment, camera and gyro, has been tweaked.

Main code
Code:
 /******************************************************************************
 *
 *    Sprockets 2012 Base Code
 *    Authors:    Brendan van Ryn
 *             Stuart Sullivan
 *             Terry Shi
 *             Aliah McCalla
 *             Will Surmak
 *              Jacob 't Mannetje
 *    Last Updated:   15/02/2012
 *
 ******************************************************************************/

/* Robot configuration macros. Do not change. */
#define NEW 1
#define OLD 2

/* Robot configuration */
#define ROBOT NEW // Set this to either "OLD" or "NEW", without the quotation marks, to configure for the given robot

/* Set the correct macros based on which robot is being used. Do not change this. */
#if !defined ROBOT
#warning "Robot version not specified. Assuming new configuration..."
#include "NewRobot.h"
#elif ROBOT == NEW
#include "NewRobot.h"
#elif ROBOT == OLD
#include "OldRobot.h"
#else
#warning "Robot configuration must be either 'OLD' or 'NEW'. Assuming new..."
#include "NewRobot.h"
#endif

// Header file for WPI standard hardware/robot overhead.
#include "WPILib.h"

// Include the libraries for the camera
#include "vision/AxisCamera.h"

// Header file for math functions
#include <math.h>

// Macro Functions
// Clears the Driver Station output display
#define CLEARMESSAGE DriverStationLCD::GetInstance()->Clear()

// Prints an integer, a, on the Driver Station on line, b.
#define DISPLAYINTEGER(b, a) DriverStationLCD::GetInstance()->Printf(DriverStationLCD::kUser_Line ## b, 1, "%d", a); DriverStationLCD::GetInstance()->UpdateLCD()

// Prints a string, a, on the Driver Station on line, b.
#define DISPLAYSTRING(b, a) DriverStationLCD::GetInstance()->Printf(DriverStationLCD::kUser_Line ## b, 1, a); DriverStationLCD::GetInstance()->UpdateLCD()

// Prints a float, a, on the Driver Station on line, b.
#define DISPLAYFLOAT(b, a) DriverStationLCD::GetInstance()->Printf(DriverStationLCD::kUser_Line ## b, 1, "%0.2f", a); DriverStationLCD::GetInstance()->UpdateLCD()

// 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
double WidthToDistance(double); // Returns the distance from the camera depending on the width


/* Our main (and this year, only) class. Don't worry about the word
"class" as we won't do much Object-Oriented stuff, but in the layout
provided to use for the robot code, we must have this one class. The
code for the robot goes here, along with all variables we need. It has
always been called "SimpleTracker" for whatever reason". */
class SimpleTracker : public SimpleRobot // Just ignore this, essentially. Treat it as "int main(void)" for simplicity.
{
    // "Global" variables are declared here.
   Joystick *leftStick; // Left driver joystick
   Joystick *rightStick; // Right driver joystick
   Joystick *speedStick; // Third driver joystick
    Jaguar *lfDrive; // Front-left drive motor
    Jaguar *lrDrive; // Rear-left drive motor
    Jaguar *rfDrive; // Front-right drive motor
    Jaguar *rrDrive; // Rear-right drive motor
    Victor *frontSteer; // Front steering motor
    Victor *rearSteer; // Rear steering motor
    Victor *topShooter; // Top shooting roller
    Victor *bottomShooter; // Bottom shooting roller
    Encoder *frontSteeringEncoder; // Front steering encoder
    Encoder *rearSteeringEncoder; // Rear steering encoder
    Gyro *steeringGyro; // Steering gyro
    HSLImage *cameraImage; // Hue/Saturation/Luminence image from camera
    BinaryImage *binaryImage; // The object used to store and edit the filtered image
    Image *imaqImage; // Stores the image in a format that the built-in imaq functions can use
    Relay *magazineBelt; // Magazine conveyor
    Relay *lateralCollector; // Lateral collector belts
    Relay *transverseCollector; // Transverse collector belts
    DigitalInput *topRollerEncoder; // Light sensor for top launching roller
    DigitalInput *driveWheelEncoder; // Light sensor for drive wheel encoding
    Servo *frontDriveServo; // The servo to shift gears for the front gear box
    Servo *rearDriveServo; // THe servo to shift gears for the rear gear box
    double maxTargetWidth; // Width of clearest target in view
    double maxTargetHeight; // Height of clearest target in view
   
    /* This "public" means that these variables are available outside of
    this class...but we only have one, so it essentially does nothing.
    "SimpleTracker(void)" is called a constructor. Again, don't worry about
    the details. This is where our object-based variables (hardware components)
    are initialized. This means we assign them a particular port. */ 
    public:
   SimpleTracker(void)
   {
        // Assign object pointers here.     
        lfDrive = new Jaguar(LFMOTORPORT); // Driving Motors
        lrDrive = new Jaguar(LRMOTORPORT);
        rfDrive = new Jaguar(RFMOTORPORT);
        rrDrive = new Jaguar(RRMOTORPORT);
       
        frontSteer = new Victor(FRONTSTEERINGMOTORPORT); // Steering Motors
        rearSteer = new Victor(REARSTEERINGMOTORPORT);
       
        topShooter = new Victor(TOPSHOOTINGMOTORPORT); // Shooter motors
        bottomShooter = new Victor(BOTTOMSHOOTINGMOTORPORT);
       
        leftStick = new Joystick(LEFTJOYSTICKPORT); // Joysticks
        rightStick = new Joystick(RIGHTJOYSTICKPORT);
        speedStick = new Joystick(SPEEDJOYSTICKPORT);
       
        frontSteeringEncoder = new Encoder(FRONTSTEERINGENCODERPORTA, FRONTSTEERINGENCODERPORTB); // Encoders
        rearSteeringEncoder = new Encoder(REARSTEERINGENCODERPORTA, REARSTEERINGENCODERPORTB);
       
        steeringGyro = new Gyro(1, STEERINGGYROPORT); // Analogue devices
       
        magazineBelt = new Relay(MAGAZINERELAYPORT); // Relays
        lateralCollector = new Relay(LATERALCOLLECTORPORT);
        transverseCollector = new Relay(TRANSVERSECOLLECTORPORT);
       
        topRollerEncoder = new DigitalInput(TOPROLLERENCODERPORT); // Shooting encoder light sensor
       
        driveWheelEncoder = new DigitalInput(DRIVEWHEELENCODERPORT); // Driving encoder light sensor
       
        frontDriveServo = new Servo(FRONTSERVOPORT);
        rearDriveServo = new Servo(REARSERVOPORT);

        // Initialize encoders
        frontSteeringEncoder->Reset();
        frontSteeringEncoder->Start();
        rearSteeringEncoder->Reset();
        rearSteeringEncoder->Start();
       
        // Initialize gyro
        steeringGyro->Reset();
        steeringGyro->SetSensitivity(GYROSENSITIVITY); // Changes the sensitivity of the gyro
       
        // frontDriveServo->SetAngle(0.0);
       
        // Enable camera and wait for a bit
        AxisCamera &camera = AxisCamera::GetInstance();
        Wait(8.0);
       
        // Set the camera's preferences
        camera.WriteResolution(camera.kResolution_320x240); // Make sure this resolution matches the width and height macros
        http://camera.WriteColorLevel();
        http://camera.WriteWhiteBalance();
        camera.WriteCompression(0);
        camera.WriteBrightness(50);
        camera.WriteColorLevel(256);
   }
   
    /* 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 speedx = 0.0f, speedy = 0.0f; // The x- and y-positions of the third speed-control shift
      float x, y; // The average x- and y-positions between the two 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; // Allow different angles for the front and back sides, for use in different steering modes
      int waitForSteering; // Should the drive motors wait for the wheels to finish turning?
      int frontFinished, rearFinished; // Store whether each driving side has finished turning
      int steeringMode = SWERVEMODE; // Store the current mode of steering (0 = strafe, 1 = monster, 2 = car)
      int currentButtonState = 0, previousButtonState = 0; // Store the current and previous states of the manual alignment buttons
      int targetX = 0; // Store the number of distinct sections on the refined camera image
      double distance = 0.0; // The temp variable to display distance, used to test the distance formula that is in progress
      int dividerCount = 0; // Count the number of times the loop has been executed
      float topShooterSpeed = 0.0f; // Speed of the top shooter
      int topShooterPosition; // Position of the top and shooting encoder
      int driveWheelEncoderValue; // Distance travelled by robot
      float gearSetting = LOWGEARPOSITION; // Store the angle setting for the gear-shifting servos
      
      // Acquire reference to axis camera
      AxisCamera &camera = AxisCamera::GetInstance();
      
      // This code is run repeatedly so long as the robot is in teleop mode.
      while (IsOperatorControl())
      {
         /* This is a fancy, convoluted way of telling the operating-system
         software that the code hasn't frozen. */
         GetWatchdog().Feed();
         
         /* Message displays */
         CLEARMESSAGE;
         DISPLAYINTEGER(1, frontSteeringEncoder->Get()); // Encoder value for top shooting roller
         DISPLAYINTEGER(2, rearSteeringEncoder->Get()); // Encoder value for bottom shooting roller
         DISPLAYINTEGER(3, targetX); // X-position of target (or -1)
         DISPLAYFLOAT(4, distance); // Distance to target, in feet
         DISPLAYFLOAT(5, topShooterSpeed);
         DISPLAYINTEGER(6, driveWheelEncoderValue);
         
         // Call code to acquire and process camera image
         if(GETIMAGEBUTTON)
         {
            targetX = GetProcessedImage(camera);
            distance = WidthToDistance(maxTargetWidth);
         }
         
         // Update the encoder speed every few times through this loop
         if(dividerCount >= ENCODERFREQUENCY)
         {
            // Store the number of pulses that have passed in the given period of time
            topShooterSpeed = (float)topShooterPosition;
            
            // Reset encoders
            TopShooterEncoder(1);
            
            dividerCount = 0;
         }
         else dividerCount++;
         
         /* Teleop code here. */
         // Check  drive mode buttons
         if(SWERVEMODEBUTTON) steeringMode = SWERVEMODE;
         else if(MONSTERMODEBUTTON) steeringMode = MONSTERMODE;
         else if(CARMODEBUTTON) steeringMode = CARMODE;
         
         // Check belt buttons
         if(MAGAZINEENABLEBUTTON) magazineBelt->Set(Relay::kForward);
         else magazineBelt->Set(Relay::kOff);
         if(LATERALCOLLECTORBUTTON) lateralCollector->Set(Relay::kReverse);
         else if(LATERALREVERSEBUTTON) lateralCollector->Set(Relay::kForward);
         else lateralCollector->Set(Relay::kOff);
         if(TRANSVERSECOLLECTORBUTTON) transverseCollector->Set(Relay::kReverse);
         else if(TRANSVERSEREVERSEBUTTON) transverseCollector->Set(Relay::kForward);
         else transverseCollector->Set(Relay::kOff);
         
         // Check shooter buttons
         if(FIRSTFIREBUTTON) SetRollerSpeed(FIRESPEEDONE, topShooterSpeed);
         else if(SECONDFIREBUTTON) SetRollerSpeed(FIRESPEEDTWO, topShooterSpeed);
         else if(THIRDFIREBUTTON) SetRollerSpeed(FIRESPEEDTHREE, topShooterSpeed);
         else if(AUTOSHOOTBYDISTANCE)
         {
            //SetRollerSpeed((float)TOPROLLERSPEED*(distance/MAXRANGE), topShooterSpeed);
            distance = WidthToDistance(maxTargetWidth);
            topShooter->Set(SHOOTINGROLLERK * distance + SHOOTINGROLLERCONSTANT);
            bottomShooter->Set(-SHOOTINGROLLERK * distance + SHOOTINGROLLERCONSTANT);                     
         }
         else topShooter->Set(-speedy), bottomShooter->Set(speedy);
         
         
         // Check gear shifting buttons
         if(HIGHGEARBUTTON) gearSetting = HIGHGEARPOSITION;
         else if(LOWGEARBUTTON) gearSetting = LOWGEARPOSITION;
         
         
         // Switch to the desired gear
         frontDriveServo->SetAngle(gearSetting);
         rearDriveServo->SetAngle(gearSetting);
         
         // These must be called each time in order for the encoder positions to update
         topShooterPosition = TopShooterEncoder(0);
         driveWheelEncoderValue = DriveWheelEncoder(0);
                  
         // Get joystick positions
         leftx = STICKDIRECTIONX * leftStick->GetRawAxis(1);
         lefty = leftStick->GetRawAxis(2);
         rightx = STICKDIRECTIONX * rightStick->GetRawAxis(1);
         righty = rightStick->GetRawAxis(2);
         speedx = speedStick->GetRawAxis(1);
         speedy = speedStick->GetRawAxis(2);
         
         // If the total y-deflection for each joystick is below the threshold, we shouldn't count it at all
         if(Abs(lefty) < JOYSTICKDEADBANDY) lefty = 0.0f;
         if(Abs(righty) < JOYSTICKDEADBANDY) righty = 0.0f;
         if(speedy > 0) speedy = 0;
         
                  
         // Average the joystick positions
         x = (leftx + rightx) / 2.0f;
         y = (lefty + righty) / 2.0f;
         
                  
         // Store the currentButtonState from the previous iteration
         previousButtonState = currentButtonState;
         
         // If the manual-alignment buttons are pressed
         if(MANUALALIGNENABLEBUTTON)
         {
            currentButtonState = 1;
            if(MANUALALIGNLEFTREAR) rearSteer->Set(-0.6f);
            else if(MANUALALIGNRIGHTREAR) rearSteer->Set(0.6f);
            else rearSteer->Set(0);
               
            if(MANUALALIGNLEFTFRONT) frontSteer->Set(-0.7f);
            else if(MANUALALIGNRIGHTFRONT)frontSteer->Set(0.7f);
            else frontSteer->Set(0);
         }
         
         // Otherwise, if the deflection left or right is less than a certain amount, we simply drive tank-style
         // Abs simply ensures that x is positive so that we don't need two separate if-statements
         else if(Abs(x) < JOYSTICKDEADBANDX)
         {
            // Tank drive
            // Set the drive motors according to their respective joysticks
            lfDrive->Set(lefty);
            lrDrive->Set(lefty);
            rfDrive->Set(righty);
            rrDrive->Set(righty);
            currentButtonState = 0;
            
            TrackFrontSteering(0);
            TrackRearSteering(0);
         }
         
         else
         {
            // Swerve drive
            // Determine the total deflection of the joysticks to use as the speed setting
            swerveSpeed = sqrt(x*x + y*y); // a^2 + b^2 = c^2
            swerveAngle = -Abs(atan(x/y)); // x/y = tan(theta)
            swerveAngle = swerveAngle * 180 / Pi; // 1 degree = 180/Pi radians
            swerveAngle = swerveAngle / 90.0f * PULSESPER90; // Convert from degrees to encoder pulses
            
            /* If the y-component is zero, we should try to move straight left or right. Therefore, we
             * need to wait for the wheels to finish turning before allowing the drive motors to move. The
             * angle, for some reason, also needs to be inverted when moving straight left or right. */
            if(Abs(y) < JOYSTICKDEADBANDY)
            {
               waitForSteering = 1;
               if(x < 0.0f) swerveAngle = PULSESPER90, swerveSpeed = -swerveSpeed;
               else if(x > 0.0f) swerveAngle = -PULSESPER90;
            }
            else waitForSteering = 0;
                        
            // Determine the quadrant of the joystick and convert the speed or angle appropriately
            // Quadrant one is assumed, except for the following conditions
            if(x < 0 && y >= 0) swerveAngle = -swerveAngle; // Quadrant two
            else if(x < 0 && y < 0) swerveSpeed = -swerveSpeed; // Quadrant three
            else if(x >= 0 && y < 0) // Quadrant Four
            {
               swerveSpeed = -swerveSpeed;
               swerveAngle = -swerveAngle;               
            }
            
            // If we are in monster mode and travelling faster than 80% max speed, we switch to car automatically
            if(Abs(swerveSpeed) > 1.1f && steeringMode == MONSTERMODE) steeringMode = CARMODE;
         
            // Put steer-angle and drive-direction changes here for the other steering modes (monster, car, auto-monster, etc.)
            if(steeringMode == MONSTERMODE)
            {
               if(swerveSpeed <= 0) swerveAngleFront = -0.75f * swerveAngle, swerveAngleRear = 0.75f * swerveAngle;
               else swerveAngleFront = 0.75f * swerveAngle, swerveAngleRear = -0.75f * swerveAngle;
            }
            else if(steeringMode == CARMODE)
            {
               if(swerveSpeed <= 0) swerveAngleFront = -0.5f * swerveAngle, swerveAngleRear = 0.0f;
               else swerveAngleRear = -0.5f * swerveAngle, swerveAngleFront = 0.0f;
            }
            
            else
            {
               swerveAngleFront = -swerveAngle;
               swerveAngleRear = -swerveAngle;
            }
            
            /* Call pre-written functions that will use the encoder to get the wheels to gradually approach the desired
             * position. This function will return zero when the steering is finished.*/
            frontFinished = TrackFrontSteering((int)swerveAngleFront);
            rearFinished = TrackRearSteering((int)swerveAngleRear);
            
            // If the steering has finished, set the drive motors to move
            if((frontFinished == 0 && rearFinished == 0) || !waitForSteering)
            {
               lfDrive->Set(swerveSpeed);
               lrDrive->Set(swerveSpeed);
               rfDrive->Set(swerveSpeed);
               rrDrive->Set(swerveSpeed);
            }
            
            currentButtonState = 0;
         }
         
         // If the manual alignment buttons have JUST been released, reset the encoders again
         if(previousButtonState == 1 && currentButtonState == 0)
         {
            frontSteeringEncoder->Reset();
            frontSteeringEncoder->Start();
            rearSteeringEncoder->Reset();
            rearSteeringEncoder->Start();
            frontSteer->Set(0);
            rearSteer->Set(0);
         }
      }
   }
   
   /* Code to run while in autonomous, or fully-automated mode. */
   void Autonomous(void)
   {
      // Variables
      float gyroAngle; // Current angle from home as read by the gyro
      int imageCenter = 0; // The center point of the clearest target
      int dividerCount = 0; // Counts the number of times the loop has been executed
      float correctionSpeed; // Stores the speed at which the drive motors should move to compensate the camera/gyro
      float imageDistance = 0.0f; // Stores the calculated distance between the robot and the clearest target
      
      // Acquire reference to axis camera
      AxisCamera &camera = AxisCamera::GetInstance();
      
      // This code is run repeatedly so long as the robot is in autonomous mode (lasting 15 seconds).
      while (IsAutonomous())
      {
         /* Tell the system that the code hasn't frozen. */
         GetWatchdog().Feed();
         
         /* Autonomous code here. */
         // Get the gyro angle
         gyroAngle = steeringGyro->GetAngle();
         
         // Update the center position of the target once every few times through this loop
         if(dividerCount >= CAMERAFREQUENCY)
         {
            imageCenter = GetProcessedImage(camera);
            dividerCount = 0;
         }         
         else dividerCount++;
         
         // Store the distance to the target
         imageDistance = WidthToDistance(maxTargetWidth);
         
         // Message displays
         CLEARMESSAGE;
         DISPLAYINTEGER(1, imageCenter); // Centre position of target (or -1)
         DISPLAYFLOAT(3, gyroAngle);  // The float value of the gyro angle
         DISPLAYFLOAT(4, imageDistance); // Feet from target
         
         // Turn the front wheels left and straighten the rear wheels for pivoting
         TrackFrontSteering(-PULSESPER90 / 3);
         TrackRearSteering(PULSESPER90 / 3);
         
         // If the centre is -1, the target is not in view
         if(imageCenter == -1)
         {
            // Stop the shooting rollers
            topShooter->Set(0.0f);
            bottomShooter->Set(0.0f);
            
            if(Abs(gyroAngle) < GYROALIGNINGDEADBAND) correctionSpeed = Abs(gyroAngle)/GYROALIGNINGDEADBAND*0.3+0.05;
            else correctionSpeed = 0.3;
            
            // Use the gyro to correct instead
            // If the angle is greater than zero, we need to turn left
            if(gyroAngle - GYRODEADBANDANGLE > 0.0f)
            {
               DISPLAYSTRING(2, "Gyro left");
               lfDrive->Set(-correctionSpeed);
               rfDrive->Set(-correctionSpeed);
               rrDrive->Set(correctionSpeed);
               lrDrive->Set(correctionSpeed);
            }
            
            // If the angle is less than zero, we need to turn right
            else if(gyroAngle + GYRODEADBANDANGLE < 0.0f)
            {
               DISPLAYSTRING(2, "Gyro right");
               lfDrive->Set(correctionSpeed);
               rfDrive->Set(correctionSpeed);
               rrDrive->Set(-correctionSpeed);
               lrDrive->Set(-correctionSpeed);
            }
            
            // Otherwise, we are within our threshold, so stop
            else
            {
               DISPLAYSTRING(2, "Centered");
               lfDrive->Set(0.0);
               rfDrive->Set(0.0);
               rrDrive->Set(0.0);
               lrDrive->Set(0.0);
            }
         }
         
         else
         {
            // Set the firing speed proportionally to the image distance
            // topShooter->Set(SHOOTINGROLLERK * imageDistance + SHOOTINGROLLERCONSTANT);
            // bottomShooter->Set(-SHOOTINGROLLERK * imageDistance + SHOOTINGROLLERCONSTANT);
            
            // Calculate the speed at which to move the wheels
            correctionSpeed = Abs((float)imageCenter - IMAGEWIDTH / 2.0f) / (float)IMAGEAPPROACHDISTANCE * 0.6f;
            if(correctionSpeed > 0.6f) correctionSpeed = 0.6f;
            
            // Otherwise, if the center is too far left, turn the robot right
            if(imageCenter + IMAGEDEADBAND < (int)IMAGEWIDTH / 2)
            {
               DISPLAYSTRING(2, "Right");
               lfDrive->Set(correctionSpeed);
               rfDrive->Set(correctionSpeed);
               rrDrive->Set(-correctionSpeed);
               lrDrive->Set(-correctionSpeed);
            }
            
            // If the center is too far right, turn the robot left
            else if(imageCenter - IMAGEDEADBAND > (int)IMAGEWIDTH / 2)
            {
               DISPLAYSTRING(2, "Left");
               lfDrive->Set(-correctionSpeed);
               rfDrive->Set(-correctionSpeed);
               rrDrive->Set(correctionSpeed);
               lrDrive->Set(correctionSpeed);
            }
            
            // If the centre is close enough to the middle of the screen, stop
            else
            {
               DISPLAYSTRING(2, "Stop");
               lfDrive->Set(0.0);
               rfDrive->Set(0.0);
               rrDrive->Set(0.0);
               lrDrive->Set(0.0);                         
            }
         }
         
         /* Simple, straight-movement autonomous
         // Turn the rear wheels left to correct if we are off to the left
         // Otherwise, turn the wheels right to correct if we are off to the right
         // Otherwise, move straight
         if(gyroAngle + GYRODEADBANDANGLE < 0.0f) TrackRearSteering(PULSESPER90 / 9);
         else if(gyroAngle - GYRODEADBANDANGLE > 0.0f) TrackRearSteering(-PULSESPER90 / 9);
         else TrackRearSteering(0);
         
         // Keep the front wheels straight and drive forward slowly
         TrackFrontSteering(0);
         lfDrive->Set(-0.3);
         lrDrive->Set(-0.3);
         rfDrive->Set(-0.3);
         rrDrive->Set(-0.3);*/
      }
   }
   
   /* This function handles the complicated problem of quickly, but accurately, moving the
    * front wheels to their correct position, as passed 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 < STEERINGDEADBAND) speed = 0.0f;
      else if(frontSteeringEncoderValue < targetPosition) speed = (float)remainingSteeringDistance / (float)STEERINGAPPROACHDISTANCE;
      else speed = (float)-remainingSteeringDistance / (float)STEERINGAPPROACHDISTANCE;
      
      // 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 < STEERINGDEADBAND) speed = 0.0f;
      else if(rearSteeringEncoderValue < targetPosition) speed = (float)remainingSteeringDistance / (float)STEERINGAPPROACHDISTANCE;
      else speed = (float)-remainingSteeringDistance / (float)STEERINGAPPROACHDISTANCE;
      
      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;
   }
   
   /* Take the camera image and process it, returning the x-position of the largest particle. This should represent the
   horizontal centre of the most in-focus target. Comparing this value to the image width (320) should allow us to
   centre the image by rotating the robot. If the targets are out of view, -1 is returned. */
   int GetProcessedImage(AxisCamera &camera)
   {
      register int i;
      int numberOfParticles, largestParticle;
      double maxArea = 0, width, height, horizontalPosition;
      
      // Save the image as a BinaryImage Object
      cameraImage = camera.GetImage();
      //binaryImage = cameraImage->ThresholdHSLHUEMIN, HUEMAX, SATURATIONMIN, SATURATIONMAX, LUMINANCEMIN, LUMINANCEMAX);
      binaryImage = cameraImage->ThresholdRGB(REDMIN, REDMAX, GREENMIN, GREENMAX, BLUEMIN, BLUEMAX);
      imaqImage = binaryImage->GetImaqImage();
      
      // Perform the Convex Hull operation
      imaqConvexHull(imaqImage, imaqImage, 1);
      
      // Count the number of distinct sections remaining
      imaqCountParticles(imaqImage, 2, &numberOfParticles);
            
       // Save image to the cRIO's memory (enable for debugging purposes only)
       //cameraImage->Write("camIm4.jpeg");
       //binaryImage->Write("binIm4.bmp");
      
       // For each particle found
       for(i = 0; i < numberOfParticles; i++)
       {
          // Get the width and height of the given particle
          imaqMeasureParticle(imaqImage, i, 0, IMAQ_MT_BOUNDING_RECT_WIDTH, &width);
          imaqMeasureParticle(imaqImage, i, 0, IMAQ_MT_BOUNDING_RECT_HEIGHT, &height);
          
          // If the area of this particle is the largest area so far
          if(width * height > maxArea)
          {
             // Store the dimensions and area of the particle, along with the index of said particle
             maxTargetWidth = width;
             maxTargetHeight = height;
             maxArea = width * height;
             largestParticle = i;
          }
       }
      
       // Find the x-position of the centre of the largest particle, assumed to be the nearest target
       imaqMeasureParticle(imaqImage, largestParticle, 0, IMAQ_MT_CENTER_OF_MASS_X, &horizontalPosition);
      
       delete binaryImage;
       delete cameraImage;
      
       // If the largest particle is outside of the threshold, return -1 to indicate that the targets aren't visible
       if(maxArea < MINIMUMAREA) return -1;
      
       // Return this x-position
       return (int)horizontalPosition;
   }
   
   /* This function handles all of the encoder processing for the top shooting roller. The parameter, if true
    * (any non-zero number), resets the pulse count to zero. Otherwise, it keeps counting from where it was. The
    * return value is the current pulse count of the encoder. */
   int TopShooterEncoder(int reset)
   {
      // "Static" means it will retain it's value from previous calls to the function
      static int previousState = 0; // Previous state of the lightSensor
      static int pulseCount = 0; // Number of pulses recorded so far
      int currentState;
      
      // Record one pulse each time the light sensor's state changes
      currentState = topRollerEncoder->Get();
      if(currentState != previousState) pulseCount++;
      previousState = currentState;
      
      // Reset if necessary, and return the current number of pulses
      if(reset != 0) pulseCount = 0;
      return pulseCount;
   }
   
   // Same as the above, but for drive wheels
   int DriveWheelEncoder(int reset)
   {
      // "Static" means it will retain it's value from previous calls to the function
      static int previousState = 0; // Previous state of the lightSensor
      static int pulseCount = 0; // Number of pulses recorded so far
      int currentState;
         
      // Record one pulse each time the light sensor's state changes
      currentState = driveWheelEncoder->Get();
      if(currentState != previousState) pulseCount++;
      previousState = currentState;
         
      // Reset if necessary, and return the current number of pulses
      if(reset != 0) pulseCount = 0;
      return pulseCount;
   }
   
   // Maintains a specific speed for the shooting rollers. Returns true when rollers are at that speed ***
   int SetRollerSpeed(float targetSpeed, float topSpeed)
   {
      static float topPower = 0;
      int toReturn = 0;
      
      // If the top roller is too fast, slow it down; if it's too slow, speed it up
      // If it's where it needs to be, set it's flag to 1 for being finished
      if(topSpeed + SHOOTERDEADBAND < targetSpeed && topPower < 1.0f) topPower += FIRESPEEDCHANGE;
      else if(topSpeed - SHOOTERDEADBAND > targetSpeed && topPower > 0.0f) topPower -= FIRESPEEDCHANGE;
      else toReturn |= 1;
      
      // Set the motors to their determined speeds
      topShooter->Set(topPower);
      bottomShooter->Set(-topPower);
      
      // Return a value that reflects which motors are at the desired speed, and which aren't.
      return toReturn;
   }
}; /* Semicolons are not used after functions, but classes are treated
more like datatypes, so they DO have a semicolon after them. To think some
people say that object-oriented makes sense :/ */

// 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;
}

// Converts the width of the rectangle to a distance in feet
double WidthToDistance(double x)
{
   double result; // The resut of the calculations
   
   result = GOALWIDTH * IMAGEWIDTH / x / 2;
   result = result / tan(HALFCAMERAVIEWINGANGLE * Pi / 180);
   result = result * LINEARDISTANCEK + CONSTANTDISTANCEK;
   
   return result;
}

/* The entry point is FRC_UserProgram_StartupLibraryInit. This macro tells
the compiler to use the SimpleTracker class. There are a series of
"under-the-hood" operations so that the program starts in this class
instead of searching for int main(void). */
START_ROBOT_CLASS(SimpleTracker);

New Robot Header File
Code:

/* 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 leftStick->GetRawButton(2)
#define MANUALALIGNENABLEBUTTON (rightStick->GetRawButton(8) && rightStick->GetRawButton(9))
#define MANUALALIGNLEFTREAR rightStick->GetRawButton(7)
#define MANUALALIGNRIGHTREAR rightStick->GetRawButton(10)
#define MANUALALIGNLEFTFRONT rightStick->GetRawButton(6)
#define MANUALALIGNRIGHTFRONT rightStick->GetRawButton(11)
#define GETIMAGEBUTTON leftStick->GetRawButton(3)
#define MAGAZINEENABLEBUTTON rightStick->GetRawButton(5)
#define LATERALCOLLECTORBUTTON rightStick->GetRawButton(5)
#define TRANSVERSECOLLECTORBUTTON rightStick->GetRawButton(5)
#define FIRSTFIREBUTTON speedStick->GetRawButton(4)
#define SECONDFIREBUTTON speedStick->GetRawButton(3)
#define THIRDFIREBUTTON speedStick->GetRawButton(5)
#define HIGHGEARBUTTON leftStick->GetRawButton(6)
#define LOWGEARBUTTON leftStick->GetRawButton(7)
#define LATERALREVERSEBUTTON rightStick->GetRawButton(4)
#define TRANSVERSEREVERSEBUTTON rightStick->GetRawButton(4)
#define AUTOSHOOTBYDISTANCE speedStick->GetRawButton(1)

// Port numbers
// Motors are on PWM ports
#define LFMOTORPORT 3
#define LRMOTORPORT 8
#define RFMOTORPORT 7
#define RRMOTORPORT 4
#define FRONTSTEERINGMOTORPORT 2
#define REARSTEERINGMOTORPORT 1
#define TOPSHOOTINGMOTORPORT 9
#define BOTTOMSHOOTINGMOTORPORT 10

// The gear-shifting servos are on PWM ports
#define FRONTSERVOPORT 5
#define REARSERVOPORT 6

// 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

// Light sensors for encoders are also digital inputs
#define TOPROLLERENCODERPORT 6
#define DRIVEWHEELENCODERPORT 5

// The gyro is on an analog channel
#define STEERINGGYROPORT 2

// Relays have their own ports
#define MAGAZINERELAYPORT 2
#define LATERALCOLLECTORPORT 1
#define TRANSVERSECOLLECTORPORT 3

// Constants
// Hardware constants
#define GYROSENSITIVITY 0.007f // Volts per degree per second
#define PULSESPER90 1800 // Number of encoder pulses in a 90-degree turn
#define STEERINGDEADBAND 50 // Number of encoder pulses of proximity needed for the steering (lower numbers are more accurate, but can cause hunting)
#define STEERINGAPPROACHDISTANCE 80 // Number of encoder pulses of proximity after which the wheels begin to slow down
#define CAMERAFREQUENCY 2000 // The number of times the IsOperatorControl() or IsAutonomous() loops must run before updating the camera image
#define HALFCAMERAVIEWINGANGLE 23.5 // Half of the viewing angle of the camera
#define LINEARDISTANCEK 1.0 // The camera distance is multiplied by this constant
#define CONSTANTDISTANCEK 0.0 // The camera distance has this added to it
#define IMAGEWIDTH 320.0 // The width of the image from the camera
#define IMAGEHEIGHT 240.0 // The height of the image from the camera
#define ENCODERFREQUENCY 400 // The number of times the IsOperatorControl() loop must run before updating the encoder speed

// Hardware threshold constants
#define JOYSTICKDEADBANDX 0.100f
#define JOYSTICKDEADBANDY 0.100f
#define GYRODEADBANDANGLE 3.0f
#define SHOOTERDEADBAND 0.0f

// Camera HSL Image Thresholds
#define HUEMAX 255
#define HUEMIN 0
#define SATURATIONMAX 15
#define SATURATIONMIN 0
#define LUMINANCEMAX 255
#define LUMINANCEMIN 200 // 160
#define REDMIN 245
#define REDMAX 255
#define BLUEMIN 20
#define BLUEMAX 100
#define GREENMIN 20
#define GREENMAX 100
#define MINIMUMAREA 220

// The deadband for the the horizontal pivoting to target
#define IMAGEDEADBAND 10
#define IMAGEAPPROACHDISTANCE 60

// Approching deadband for aligning with the gyro
#define GYROALIGNINGDEADBAND 15

// Other constants
#define Pi 3.1415926535897932384626433832795028841971f
#define SWERVEMODE 0
#define MONSTERMODE 1
#define CARMODE 2
#define GOALWIDTH 2.0
#define STICKDIRECTIONX -1 // Use this to control whether or not the x-axis of the joysticks is inverted
#define FIRESPEEDONE 8.0f // Shooter firing speed pre-sets
#define FIRESPEEDTWO 12.0f
#define FIRESPEEDTHREE 16.0f
#define FIRESPEEDCHANGE 0.00001f
#define LOWGEARPOSITION 0.0f // Angles for the gear-shifting servos
#define HIGHGEARPOSITION 30.0f
#define SHOOTINGROLLERCONSTANT 0.05f // Minimum shooting speed
#define SHOOTINGROLLERK (1.0f / 70.0f) // Linear scaling constant for shooting speed versus distance
#define MAXRANGE 50
#define TOPROLLERSPEED 32

StuartSullivan

Posts : 12
Join date : 2010-10-09
Age : 24

Back to top Go down

Re: Code Samples

Post  Brendan van Ryn on Sun Feb 26, 2012 3:00 pm

An update hasn't been posted in a while, so here's the new version of the robot code. Those of you (hopefully that means "all") who have been reading my tutorials, take a look through it and see if you're starting to understand things more Wink

Code:


/******************************************************************************
 *
 *    Sprockets 2012 Base Code
 *    Authors:    Brendan van Ryn
 *             Stuart Sullivan
 *             Terry Shi
 *             Aliah McCalla
 *             Will Surmak
 *              Jacob 't Mannetje
 *    Last Updated:   15/02/2012
 *
 ******************************************************************************/

/* Robot configuration macros. Do not change. */
#define NEW 1
#define OLD 2

/* Robot configuration */
#define ROBOT NEW // Set this to either "OLD" or "NEW", without the quotation marks, to configure for the given robot

/* Set the correct macros based on which robot is being used. Do not change this. */
#if !defined ROBOT
#warning "Robot version not specified. Assuming new configuration..."
#include "NewRobot.h"
#elif ROBOT == NEW
#include "NewRobot.h"
#elif ROBOT == OLD
#include "OldRobot.h"
#else
#warning "Robot configuration must be either 'OLD' or 'NEW'. Assuming new..."
#include "NewRobot.h"
#endif

// Header file for WPI standard hardware/robot overhead.
#include "WPILib.h"

// Include the libraries for the camera
#include "vision/AxisCamera.h"

// Header file for math functions
#include <math.h>

// Macro Functions
// Clears the Driver Station output display
#define CLEARMESSAGE DriverStationLCD::GetInstance()->Clear()

// Prints an integer, a, on the Driver Station on line, b.
#define DISPLAYINTEGER(b, a) DriverStationLCD::GetInstance()->Printf(DriverStationLCD::kUser_Line ## b, 1, "%d", a); DriverStationLCD::GetInstance()->UpdateLCD()

// Prints a string, a, on the Driver Station on line, b.
#define DISPLAYSTRING(b, a) DriverStationLCD::GetInstance()->Printf(DriverStationLCD::kUser_Line ## b, 1, a); DriverStationLCD::GetInstance()->UpdateLCD()

// Prints a float, a, on the Driver Station on line, b.
#define DISPLAYFLOAT(b, a) DriverStationLCD::GetInstance()->Printf(DriverStationLCD::kUser_Line ## b, 1, "%0.2f", a); DriverStationLCD::GetInstance()->UpdateLCD()

// 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
double WidthToDistance(double); // Returns the distance from the camera depending on the width
void SquareSign(float*); // Squares the argument sent to it, preserving the sign


/* Our main (and this year, only) class. Don't worry about the word
"class" as we won't do much Object-Oriented stuff, but in the layout
provided to use for the robot code, we must have this one class. The
code for the robot goes here, along with all variables we need. It has
always been called "SimpleTracker" for whatever reason". */
class SimpleTracker : public SimpleRobot // Just ignore this, essentially. Treat it as "int main(void)" for simplicity.
{
    // "Global" variables are declared here.
   Joystick *leftStick; // Left driver joystick
   Joystick *rightStick; // Right driver joystick
   Joystick *speedStick; // Third driver joystick
    Jaguar *lfDrive; // Front-left drive motor
    Jaguar *lrDrive; // Rear-left drive motor
    Jaguar *rfDrive; // Front-right drive motor
    Jaguar *rrDrive; // Rear-right drive motor
    Victor *frontSteer; // Front steering motor
    Victor *rearSteer; // Rear steering motor
    Victor *topShooter; // Top shooting roller
    Victor *bottomShooter; // Bottom shooting roller
    Encoder *frontSteeringEncoder; // Front steering encoder
    Encoder *rearSteeringEncoder; // Rear steering encoder
    Encoder *driveEncoder; // Drive wheel encoder
    Gyro *steeringGyro; // Steering gyro
    HSLImage *cameraImage; // Hue/Saturation/Luminence image from camera
    BinaryImage *binaryImage; // The object used to store and edit the filtered image
    Image *imaqImage; // Stores the image in a format that the built-in imaq functions can use
    Relay *magazineBelt; // Magazine conveyor
    Relay *magazineBelt2; // Second Motor for the Magazine
    Relay *lateralCollector; // Lateral collector belts
    Relay *transverseCollector; // Transverse collector belts
    Relay *lateralCollector2; // The supplementry motor for the lateral collector belts
    DigitalInput *topRollerEncoder; // Light sensor for top launching roller
    DigitalInput *bottomRollerEncoder; // Lights sensor for bottom launching roller
    DigitalInput *driveWheelEncoder; // Light sensor for drive wheel encoding
    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
    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
    double maxTargetWidth; // Width of clearest target in view
    double maxTargetHeight; // Height of clearest target in view
   
    /* This "public" means that these variables are available outside of
    this class...but we only have one, so it essentially does nothing.
    "SimpleTracker(void)" is called a constructor. Again, don't worry about
    the details. This is where our object-based variables (hardware components)
    are initialized. This means we assign them a particular port. */ 
    public:
   SimpleTracker(void)
   {
        // Assign object pointers here.     
        lfDrive = new Jaguar(LFMOTORPORT); // Driving Motors
        lrDrive = new Jaguar(LRMOTORPORT);
        rfDrive = new Jaguar(RFMOTORPORT);
        rrDrive = new Jaguar(RRMOTORPORT);
       
        frontSteer = new Victor(FRONTSTEERINGMOTORPORT); // Steering Motors
        rearSteer = new Victor(REARSTEERINGMOTORPORT);
       
        topShooter = new Victor(TOPSHOOTINGMOTORPORT); // Shooter motors
        bottomShooter = new Victor(BOTTOMSHOOTINGMOTORPORT);
       
        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
       
        magazineBelt = new Relay(MAGAZINERELAYPORT); // Relays
        magazineBelt2 = new Relay(MAGAZINERELAY2PORT);
        lateralCollector = new Relay(LATERALCOLLECTORPORT);
        transverseCollector = new Relay(TRANSVERSECOLLECTORPORT);
        lateralCollector2 = new Relay(LATERALCOLLERTOR2PORT);
       
        topRollerEncoder = new DigitalInput(TOPROLLERENCODERPORT); // Shooting encoder light sensors
        bottomRollerEncoder = new DigitalInput(BOTTOMROLLERENCODERPORT);
        driveWheelEncoder = new DigitalInput(DRIVEWHEELENCODERPORT); // Driving encoder light sensor
       
        armDownSwitch = new DigitalInput(ARMDOWNSWITCHPORT); // The bridge manipulator limit switches
        armUpSwitch = new DigitalInput(ARMUPSWITCHPORT);
       
        frontDriveServo = new Servo(FRONTSERVOPORT); // Gear shifting servos
        rearDriveServo = new Servo(REARSERVOPORT);
        armEngageServo = new Servo(ARMENGAGESERVOPORT); // Servo to engage bridge manipulator

        // Initialize encoders
        frontSteeringEncoder->Reset();
        frontSteeringEncoder->Start();
        rearSteeringEncoder->Reset();
        rearSteeringEncoder->Start();
        driveEncoder->Reset();
        driveEncoder->Start();
       
        // Initialize gyro
        steeringGyro->Reset();
        steeringGyro->SetSensitivity(GYROSENSITIVITY); // Changes the sensitivity of the gyro
       
        // Enable camera and wait for a bit
        AxisCamera &camera = AxisCamera::GetInstance();
        Wait(8.0);
       
        // Set the camera's preferences
        camera.WriteResolution(camera.kResolution_320x240); // Make sure this resolution matches the width and height macros
        http://camera.WriteColorLevel();
        http://camera.WriteWhiteBalance();
        camera.WriteCompression(0);
        camera.WriteBrightness(50);
        camera.WriteColorLevel(256);
   }
   
    /* 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 speedx = 0.0f, speedy = 0.0f; // The x- and y-positions of the third speed-control shift
      float x, y; // The average x- and y-positions between the two 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; // Allow different angles for the front and back sides, for use in different steering modes
      int waitForSteering; // Should the drive motors wait for the wheels to finish turning?
      int frontFinished, rearFinished; // Store whether each driving side has finished turning
      int steeringMode = SWERVEMODE; // Store the current mode of steering (0 = strafe, 1 = monster, 2 = car)
      int currentButtonState = 0, previousButtonState = 0; // Store the current and previous states of the manual alignment buttons
      double distance = 0.0; // The temp variable to display distance, used to test the distance formula that is in progress
      int dividerCount = 0; // Count the number of times the loop has been executed
      float topShooterSpeed = 0.0f; // Speed of the top shooter
      float bottomShooterSpeed = 0.0f; // Speed of the bottom shooter
      int topShooterPosition; // Position of the top and bottom shooting encoders
      int bottomShooterPosition;
      int driveWheelEncoderValue; // Distance travelled by robot
      int targetFound; // Has the target been centred?
      int targetMode = 0; // Are we in targetting mode?
      float frontGearSetting = LOWGEARPOSITION; // Store the angle setting for the gear-shifting servos
      float rearGearSetting = LOWGEARPOSITION;
      float armPosition = LOWGEARPOSITION; // Stores the angle setting for the bridge-manipulator servo
      int armUnwind = 0; // Clean up for arm engagement
      int magazineReverseCount = 0; // Allow the magazine to reverse for a bit before shooting
      int servoPauseCount = 0; // Give the servos time to shift
      int armPauseCount = 0; // The amount of time that the arm will be set down after htting the switch
      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 fireReady = 0; // Are the rollers up to firing speed?
      float fireSpeed = 0.0f; // Firing speed for shooter based on camera distance
   
      // Acquire reference to axis camera
      AxisCamera &camera = AxisCamera::GetInstance();
      
      // Reset the drive and shooter encoders
      TopShooterEncoder(1);
      BottomShooterEncoder(1);
      driveEncoder->Reset();
      driveEncoder->Start();
      
      // This code is run repeatedly so long as the robot is in teleop mode.
      while (IsOperatorControl())
      {
         /* This is a fancy, convoluted way of telling the operating-system
         software that the code hasn't frozen. */
         GetWatchdog().Feed();
         
         /* Message displays */
         CLEARMESSAGE;
         DISPLAYFLOAT(1, distance); // Distance to target, in feet
         DISPLAYINTEGER(2, driveWheelEncoderValue); // Encoder value from drive wheel
         DISPLAYFLOAT(3, topShooterSpeed); // Display the top shooter speed reading
         DISPLAYFLOAT(4, bottomShooterSpeed); // Display the bottom shooter speed reading
         
         // Get joystick positions and square, preserving sign, to reduce sensitivity
         leftx = STICKDIRECTIONX * leftStick->GetRawAxis(1); SquareSign(&leftx);
         lefty = leftStick->GetRawAxis(2); SquareSign(&lefty);
         rightx = STICKDIRECTIONX * rightStick->GetRawAxis(1); SquareSign(&rightx);
         righty = rightStick->GetRawAxis(2); SquareSign(&righty);
         speedx = speedStick->GetRawAxis(1);
         speedy = speedStick->GetRawAxis(2);
         
         // Prevent back-rolling
         if(speedy > 0) speedy = 0.0f;
         
         // Update the encoder speed every few times through this loop
         if(dividerCount >= ENCODERFREQUENCY)
         {
            // Store the number of pulses that have passed in the given period of time
            topShooterSpeed = (float)topShooterPosition;
            bottomShooterSpeed = (float)bottomShooterPosition;
            
            // Reset encoders
            TopShooterEncoder(1);
            BottomShooterEncoder(1);
            dividerCount = 0;
            
            // Get the distance and display it
            targetFound = GetProcessedImage(camera);
            distance = WidthToDistance(maxTargetWidth);
         }
         else dividerCount++;
         
         //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++;
         
         /* Teleop code here. */
         // Check  drive mode buttons
         if(SWERVEMODEBUTTON) steeringMode = SWERVEMODE;
         else if(MONSTERMODEBUTTON) steeringMode = MONSTERMODE;
         else if(CARMODEBUTTON) steeringMode = CARMODE;
         
         // Check shooter buttons
         if(FIRSTFIREBUTTON) fireReady = SetRollerSpeed(FIRESPEEDONE, topShooterSpeed, bottomShooterSpeed), preFire = 1;
         else if(SECONDFIREBUTTON) fireReady = SetRollerSpeed(FIRESPEEDTWO, topShooterSpeed, bottomShooterSpeed), preFire = 1;
         else if(THIRDFIREBUTTON) fireReady = SetRollerSpeed(FIRESPEEDTHREE, topShooterSpeed, bottomShooterSpeed), preFire = 1;
         else preFire = 0, fireReady = 1;
         
         // Enter or exit targeting mode
         if(TARGETMODEENABLE) targetMode = 1, magazineReverseCount = MAGAZINEREVERSETIME;
         else if(TARGETMODEDISABLE) targetMode = 0;
         
         // Check gear shifting buttons
         if(HIGHGEARBUTTON && !targetMode) frontGearSetting = rearGearSetting = HIGHGEARPOSITION, armPosition = LOWGEARPOSITION;
         else if(LOWGEARBUTTON || targetMode) frontGearSetting = rearGearSetting = LOWGEARPOSITION, armPosition = LOWGEARPOSITION;
         else if(NEUTRALGEARBUTTON && !targetMode) frontGearSetting = LOWGEARPOSITION, rearGearSetting = NEUTRALGEARPOSITION, armPosition = LOWGEARPOSITION;
         else if(BRIDGEMANIPULATEBUTTON && !targetMode) frontGearSetting = LOWGEARPOSITION, rearGearSetting = NEUTRALGEARPOSITION, armPosition = ARMENGAGEPOSITION, servoPauseCount = SERVOPAUSETIME, armPauseCount = ARMPAUSETIME;
         
         // Move servos to desired position
         frontDriveServo->SetAngle(frontGearSetting);
         rearDriveServo->SetAngle(rearGearSetting);
         armEngageServo->SetAngle(armPosition);
                     
         // These must be called each time in order for the encoder positions to update
         topShooterPosition = TopShooterEncoder(0);
         bottomShooterPosition = BottomShooterEncoder(0);
         driveWheelEncoderValue = driveEncoder->Get();
         
         // Check belt buttons
         if(MAGAZINEENABLEBUTTON) magazineBelt->Set(Relay::kForward), magazineBelt2->Set(Relay::kForward), speedy = 0.7, magazineReverseCount = MAGAZINEREVERSETIME;
         else if(SOLOMAGAZINEBUTTON && fireReady)
         {
            if(magazineAlternator > MAGAZINEALTERNATORLIMIT / 8) magazineBelt->Set(Relay::kForward), magazineBelt2->Set(Relay::kForward);
            else magazineBelt->Set(Relay::kOff), magazineBelt2->Set(Relay::kOff);
         }
         // else if(magazineReverseCount > 0 && !targetMode) magazineBelt->Set(Relay::kReverse), magazineBelt2->Set(Relay::kReverse), magazineReverseCount--;
         else magazineBelt->Set(Relay::kOff), magazineBelt2->Set(Relay::kOff);
         if(LATERALCOLLECTORBUTTON) lateralCollector->Set(Relay::kReverse), lateralCollector2->Set(Relay::kReverse);
         else if(LATERALREVERSEBUTTON) magazineBelt->Set(Relay::kReverse), magazineBelt2->Set(Relay::kReverse), lateralCollector->Set(Relay::kForward), lateralCollector2->Set(Relay::kForward);
         else lateralCollector->Set(Relay::kOff), lateralCollector2->Set(Relay::kOff);
         if(TRANSVERSECOLLECTORBUTTON) transverseCollector->Set(Relay::kReverse);
         else if(TRANSVERSEREVERSEBUTTON) transverseCollector->Set(Relay::kForward);
         else transverseCollector->Set(Relay::kOff);
         
         // If the total y-deflection for each joystick is below the threshold, we shouldn't count it at all
         if(Abs(lefty) < JOYSTICKDEADBANDY) lefty = 0.0f;
         if(Abs(righty) < JOYSTICKDEADBANDY) righty = 0.0f;
            
         // Average the joystick positions
         x = (leftx + rightx) / 2.0f;
         y = (lefty + righty) / 2.0f;
                  
         // Store the currentButtonState from the previous iteration
         previousButtonState = currentButtonState;
         
         // Update the camera image
         targetFound = FindTarget(camera, 0);
         
         // If the manual-alignment buttons are pressed
         if(MANUALALIGNENABLEBUTTON)
         {
            currentButtonState = 1;
            if(MANUALALIGNLEFTREAR) rearSteer->Set(-0.6f);
            else if(MANUALALIGNRIGHTREAR) rearSteer->Set(0.6f);
            else rearSteer->Set(0);
               
            if(MANUALALIGNLEFTFRONT) frontSteer->Set(-0.7f);
            else if(MANUALALIGNRIGHTFRONT)frontSteer->Set(0.7f);
            else frontSteer->Set(0);
         }
         
         // Sets the speed dependent on the distance
         if(AUTOSHOOTBYDISTANCE) SetRollerSpeed(SHOOTINGROLLERK * distance + SHOOTINGROLLERCONSTANT, topShooterSpeed, bottomShooterSpeed);
         
         // If we are manipulating the bridge
         else if(armPosition == ARMENGAGEPOSITION && servoPauseCount == 0)
         {
            // Stop the front motors
            //rfDrive->Set(0);
            //lfDrive->Set(0);
            
            if(Abs(x) < JOYSTICKDEADBANDX) rfDrive->Set(lefty), lfDrive->Set(lefty);
               
            // Drive winch until switch is hit
            if(!armDownSwitch->Get() && !armUnwind) rrDrive->Set(0.4), lrDrive->Set(0.4);
            
            // Reverse winch until second switch is hit
            else
            {
               armUnwind = 1;
               
               // After switch is hit, exit arm manipulation mode
               if(armUpSwitch->Get())
               {
                  if(armPauseCount > 0) rrDrive->Set(0.05), lrDrive->Set(0.05), armPauseCount--;
                  else rrDrive->Set(-0.2), lrDrive->Set(-0.2);
               }
               else
               {
                  rearGearSetting = LOWGEARPOSITION;
                  frontGearSetting = LOWGEARPOSITION;
                  armPosition = LOWGEARPOSITION;
                  
                  armUnwind = 0;
               }
            }
         }
         
         // The Pivot mode seperate from the rest of the drive
         else if(PIVOTMODEBUTTON)
         {
            TrackFrontSteering(0);
            TrackRearSteering(PULSESPER90);
            rfDrive->Set(righty);
            rrDrive->Set(righty);
         }
         
         // If we are in targetting mode
         else if(targetMode)
         {
            // Track the target
            targetFound = FindTarget(camera, 1);
            
            // If it is visible, prepare to shoot
            if(targetFound)
            {
               // Get the distance to the target
               distance = WidthToDistance(maxTargetWidth);
               
               // Once the target is in view, set the roller speed based on distance
               fireSpeed = SHOOTINGROLLERK * distance + SHOOTINGROLLERCONSTANT;
               if(fireSpeed < 0.0f) fireSpeed = 0.0f;
               fireReady = SetRollerSpeed(fireSpeed, topShooterSpeed, bottomShooterSpeed);
                  
               // Reverse the magazine for a bit
               if(magazineReverseCount > 0)
               {
                  magazineBelt->Set(Relay::kReverse), magazineBelt2->Set(Relay::kReverse);
                  magazineReverseCount--;
               }
               
               // Then, move it forward for shooting
               else if(SHOOTBUTTON && fireReady) magazineBelt->Set(Relay::kForward), magazineBelt2->Set(Relay::kForward);
            }
            
            // Otherwise, wait
            else
            {
               distance = 0.0f;
               topShooter->Set(0.0f);
               bottomShooter->Set(0.0f);
               
               // Reverse the magazine for a bit
               if(magazineReverseCount > 0)
               {
                  magazineBelt->Set(Relay::kReverse), magazineBelt2->Set(Relay::kReverse);
                  magazineReverseCount--;
               }
               
               // Then, stop it
               else magazineBelt->Set(Relay::kOff), magazineBelt2->Set(Relay::kOff);
            }
         }
         
         // Otherwise, if the deflection left or right is less than a certain amount, we simply drive tank-style
         // Abs simply ensures that x is positive so that we don't need two separate if-statements
         else if(Abs(x) < JOYSTICKDEADBANDX)
         {
            // Tank drive
            // Set the drive motors according to their respective joysticks
            lfDrive->Set(lefty);
            lrDrive->Set(lefty);
            rfDrive->Set(righty);
            rrDrive->Set(righty);
            if(!preFire)
            {
               topShooter->Set(speedy); // Set roller speeds manually
               bottomShooter->Set(-speedy);
            }
            currentButtonState = 0;
            
            TrackFrontSteering(0);
            TrackRearSteering(0);
         }
         
         else
         {
            // Swerve drive
            // Determine the total deflection of the joysticks to use as the speed setting
            swerveSpeed = sqrt(x*x + y*y); // a^2 + b^2 = c^2
            swerveAngle = -Abs(atan(x/y)); // x/y = tan(theta)
            swerveAngle = swerveAngle * 180 / Pi; // 1 degree = 180/Pi radians
            swerveAngle = swerveAngle / 90.0f * PULSESPER90; // Convert from degrees to encoder pulses
            
            // Set roller speeds manually
            if(!preFire)
            {
               topShooter->Set(speedy);
               bottomShooter->Set(-speedy);
            }
            
            /* If the y-component is zero, we should try to move straight left or right. Therefore, we
             * need to wait for the wheels to finish turning before allowing the drive motors to move. The
             * angle, for some reason, also needs to be inverted when moving straight left or right. */
            if(Abs(y) < JOYSTICKDEADBANDY)
            {
               waitForSteering = 1;
               if(x < 0.0f) swerveAngle = PULSESPER90, swerveSpeed = -swerveSpeed;
               else if(x > 0.0f) swerveAngle = -PULSESPER90;
            }
            else waitForSteering = 0;
                        
            // Determine the quadrant of the joystick and convert the speed or angle appropriately
            // Quadrant one is assumed, except for the following conditions
            if(x < 0 && y >= 0) swerveAngle = -swerveAngle; // Quadrant two
            else if(x < 0 && y < 0) swerveSpeed = -swerveSpeed; // Quadrant three
            else if(x >= 0 && y < 0) // Quadrant Four
            {
               swerveSpeed = -swerveSpeed;
               swerveAngle = -swerveAngle;               
            }
            
            // If we are in monster mode and travelling faster than 80% max speed, we switch to car automatically
            if(Abs(swerveSpeed) > 1.2f && steeringMode == MONSTERMODE) steeringMode = CARMODE;
         
            // Put steer-angle and drive-direction changes here for the other steering modes (monster, car, auto-monster, etc.)
            if(steeringMode == MONSTERMODE)
            {
               if(swerveSpeed <= 0) swerveAngleFront = -0.75f * swerveAngle, swerveAngleRear = 0.75f * swerveAngle;
               else swerveAngleFront = 0.75f * swerveAngle, swerveAngleRear = -0.75f * swerveAngle;
            }
            else if(steeringMode == CARMODE)
            {
               if(swerveSpeed <= 0) swerveAngleFront = -0.5f * swerveAngle, swerveAngleRear = 0.0f;
               else swerveAngleRear = -0.5f * swerveAngle, swerveAngleFront = 0.0f;
            }
            
            else
            {
               swerveAngleFront = -swerveAngle;
               swerveAngleRear = -swerveAngle;
            }
            
            /* Call pre-written functions that will use the encoder to get the wheels to gradually approach the desired
             * position. This function will return zero when the steering is finished.*/
            frontFinished = TrackFrontSteering((int)swerveAngleFront);
            rearFinished = TrackRearSteering((int)swerveAngleRear);
            
            // If the steering has finished, set the drive motors to move
            if((frontFinished == 0 && rearFinished == 0) || !waitForSteering)
            {
               lfDrive->Set(swerveSpeed);
               lrDrive->Set(swerveSpeed);
               rfDrive->Set(swerveSpeed);
               rrDrive->Set(swerveSpeed);
            }
            
            currentButtonState = 0;
         }
         
         // If the manual alignment buttons have JUST been released, reset the encoders again
         if(previousButtonState == 1 && currentButtonState == 0)
         {
            frontSteeringEncoder->Reset();
            frontSteeringEncoder->Start();
            rearSteeringEncoder->Reset();
            rearSteeringEncoder->Start();
            frontSteer->Set(0);
            rearSteer->Set(0);
         }
      }
   } // End of teleop
   
   /* Code to run while in autonomous, or fully-automated mode. */
   void Autonomous(void)
   {
      // Variables
      float gyroAngle; // Current angle from home as read by the gyro
      int encoderValue; // Stores the encoder distance traveled by the robot
      int encoderDivider; // Track the number of times the autonomous mode loop runs
      float topShooterSpeed = 0.0f; // Speed of top shooter
      float bottomShooterSpeed = 0.0f; // Speed of bottom shooter
      int topShooterPosition = 0; // Encoder count for top shooter
      int bottomShooterPosition = 0; // Encoder count for bottom shooter
      int armUnwind = 0; // Clean up for arm engagement
      int armPauseCount = 500; // The amount of time that the arm will be set down after htting the switch
      
      // Acquire reference to axis camera
      //AxisCamera &camera = AxisCamera::GetInstance();
      
      // Reset the drive and shooter encoders
      TopShooterEncoder(1);
      BottomShooterEncoder(1);
      driveEncoder->Reset();
      driveEncoder->Start();
      
      // This code is run repeatedly so long as the robot is in autonomous mode (lasting 15 seconds).
      while (IsAutonomous())
      {
         /* Tell the system that the code hasn't frozen. */
         GetWatchdog().Feed();
         
         /* Autonomous code here. */
         // Get the gyro angle
         gyroAngle = steeringGyro->GetAngle();
         
         // Get the pulse count from the drive encoder
         encoderValue = driveEncoder->Get();
                  
         // Message displays
         CLEARMESSAGE;
         DISPLAYFLOAT(1, gyroAngle);  // The float value of the gyro angle
         DISPLAYINTEGER(2, encoderValue); // Current position of encoder
         DISPLAYINTEGER(4, armUnwind);
         
         // Get encoder values for shooters
         topShooterPosition = TopShooterEncoder(0);
         bottomShooterPosition = BottomShooterEncoder(0);
         
         /*/ Every ENCODEFREQUENCY times through the loop
         if(encoderDivider >= ENCODERFREQUENCY)
         {
            // Record the encoder positions as the speed for this period of time
            topShooterSpeed = (float)topShooterPosition;
            bottomShooterSpeed = (float)bottomShooterPosition;
            
            
            // Reset
            encoderDivider = 0;
            TopShooterEncoder(1);
            BottomShooterEncoder(1);
         }
         else encoderDivider++;*/
         
         // Turn the shooting rollers
         // SetRollerSpeed(FIRESPEEDONE, topShooterSpeed, bottomShooterSpeed);
         
         /* Simple, straight-movement autonomous */
         // Turn the rear wheels left to correct if we are off to the left
         // Otherwise, turn the wheels right to correct if we are off to the right
         // Otherwise, move straight
         if(gyroAngle + GYRODEADBANDANGLE < 0.0f) TrackRearSteering(PULSESPER90 / 8);
         else if(gyroAngle - GYRODEADBANDANGLE > 0.0f) TrackRearSteering(-PULSESPER90 / 8);
         else TrackRearSteering(0);
         
         // Keep the front wheels straight and drive forward slowly
         TrackFrontSteering(0);
                  
         // Until we have driven the required distance, move backwards
         if(encoderValue > AUTONOMOUSDISTANCE)
         {
            // Set gears to drive
            frontDriveServo->SetAngle(LOWGEARPOSITION);
            rearDriveServo->SetAngle(NEUTRALGEARPOSITION);
            armEngageServo->SetAngle(ARMENGAGEPOSITION);
                     
            lfDrive->Set(0.4);
            rfDrive->Set(0.4);
         }
            
         // Then, stop and shoot
         else
         {
            // Stop drive motors
            lfDrive->Set(0);
            rfDrive->Set(0);            

            if(armUnwind != 2)
            {      
               // Drive winch until switch is hit
               if(!armDownSwitch->Get() && armUnwind == 0) rrDrive->Set(0.4), lrDrive->Set(0.4);
               
               // Reverse winch until second switch is hit
               else
               {
                  // Store new state
                  armUnwind = 1;
                  
                  // Wait a bit, then reverse until the switch is hit
                  if(armUpSwitch->Get())
                  {
                     if(armPauseCount > 0) rrDrive->Set(0.05), lrDrive->Set(0.05), armPauseCount--;
                     else rrDrive->Set(-0.2), lrDrive->Set(-0.2);
                  }
                  
                  // Exit arm manipulation
                  else
                  {
                     armUnwind = 2;
                     frontDriveServo->SetAngle(LOWGEARPOSITION);
                     rearDriveServo->SetAngle(LOWGEARPOSITION);
                     armEngageServo->SetAngle(LOWGEARPOSITION);
                     lrDrive->Set(0), rrDrive->Set(0);
                  }
               }
            }
         }
      } // End of IsAutonomous
   } // End of Autonomous
   
   /* This function handles the complicated problem of quickly, but accurately, moving the
    * front wheels to their correct position, as passed 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 < STEERINGDEADBAND) speed = 0.0f;
      else if(frontSteeringEncoderValue < targetPosition) speed = (float)remainingSteeringDistance / (float)STEERINGAPPROACHDISTANCE;
      else speed = (float)-remainingSteeringDistance / (float)STEERINGAPPROACHDISTANCE;
      
      // 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 < STEERINGDEADBAND) speed = 0.0f;
      else if(rearSteeringEncoderValue < targetPosition) speed = (float)remainingSteeringDistance / (float)STEERINGAPPROACHDISTANCE;
      else speed = (float)-remainingSteeringDistance / (float)STEERINGAPPROACHDISTANCE;
      
      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;
   }
   
   /* Take the camera image and process it, returning the x-position of the largest particle. This should represent the
   horizontal centre of the most in-focus target. Comparing this value to the image width (320) should allow us to
   centre the image by rotating the robot. If the targets are out of view, -1 is returned. */
   int GetProcessedImage(AxisCamera &camera)
   {
      register int i;
      int numberOfParticles, largestParticle;
      double maxArea = 0, width, height, horizontalPosition;
      
      // Save the image as a BinaryImage Object
      cameraImage = camera.GetImage();
      //binaryImage = cameraImage->ThresholdHSLHUEMIN, HUEMAX, SATURATIONMIN, SATURATIONMAX, LUMINANCEMIN, LUMINANCEMAX);
      binaryImage = cameraImage->ThresholdRGB(REDMIN, REDMAX, GREENMIN, GREENMAX, BLUEMIN, BLUEMAX);
      imaqImage = binaryImage->GetImaqImage();
      
      // Perform the Convex Hull operation
      imaqConvexHull(imaqImage, imaqImage, 1);
      
      // Count the number of distinct sections remaining
      imaqCountParticles(imaqImage, 2, &numberOfParticles);
            
       // Save image to the cRIO's memory (enable for debugging purposes only)
       cameraImage->Write("camIm4.jpeg");
       binaryImage->Write("binIm4.bmp");
      
       // For each particle found
       for(i = 0; i < numberOfParticles; i++)
       {
          // Get the width and height of the given particle
          imaqMeasureParticle(imaqImage, i, 0, IMAQ_MT_BOUNDING_RECT_WIDTH, &width);
          imaqMeasureParticle(imaqImage, i, 0, IMAQ_MT_BOUNDING_RECT_HEIGHT, &height);
          
          // If the area of this particle is the largest area so far
          if(width * height > maxArea)
          {
             // Store the dimensions and area of the particle, along with the index of said particle
             maxTargetWidth = width;
             maxTargetHeight = height;
             maxArea = width * height;
             largestParticle = i;
          }
       }
      
       // Find the x-position of the centre of the largest particle, assumed to be the nearest target
       imaqMeasureParticle(imaqImage, largestParticle, 0, IMAQ_MT_CENTER_OF_MASS_X, &horizontalPosition);
      
       delete binaryImage;
       delete cameraImage;
      
       // If the largest particle is outside of the threshold, return -1 to indicate that the targets aren't visible
       if(maxArea < MINIMUMAREA) return -1;
      
       // Return this x-position
       return (int)horizontalPosition;
   }
   
   /* This function handles all of the encoder processing for the top shooting roller. The parameter, if true
    * (any non-zero number), resets the pulse count to zero. Otherwise, it keeps counting from where it was. The
    * return value is the current pulse count of the encoder. */
   int TopShooterEncoder(int reset)
   {
      // "Static" means it will retain it's value from previous calls to the function
      static int previousState = 0; // Previous state of the lightSensor
      static int pulseCount = 0; // Number of pulses recorded so far
      int currentState;
      
      // Record one pulse each time the light sensor's state changes
      currentState = topRollerEncoder->Get();
      if(currentState != previousState) pulseCount++;
      previousState = currentState;
      
      // Reset if necessary, and return the current number of pulses
      if(reset != 0) pulseCount = 0;
      return pulseCount;
   }
   
   // Same as the above, but for the bottom shooter
   int BottomShooterEncoder(int reset)
   {
      static int previousState = 0;
      static int pulseCount = 0;
      int currentState;
      
      currentState = bottomRollerEncoder->Get();
      if(currentState != previousState) pulseCount++;
      previousState = currentState;
      
      if(reset != 0) pulseCount = 0;
      return pulseCount;
   }
   
   // Maintains a specific speed for the shooting rollers. Returns true when rollers are at that speed
   int SetRollerSpeed(float targetSpeed, float topSpeed, float bottomSpeed)
   {
      static float topPower = 0.0f; // Store power setting for top motor
      static float bottomPower = 0.0f; // Store power setting for bottom motor
      float bottomTargetSpeed = targetSpeed;
      // If the roller is traveling at or faster than the desired speed, reduce power to almost none
      if(topSpeed >= targetSpeed) topPower = TOPROLLERSPEEDIDLE;
      else topPower = (targetSpeed - topSpeed) / ROLLERSPEEDAPPROACHDISTANCE + ROLLERSPEEDMINIMUM;
      
      if(targetSpeed == FIRESPEEDONE) bottomTargetSpeed = BOTTOMSPEEDONE;
      else if(targetSpeed == FIRESPEEDTWO) bottomTargetSpeed = BOTTOMSPEEDTWO;
      else if(targetSpeed == FIRESPEEDTHREE) bottomTargetSpeed = BOTTOMSPEEDTHREE;
      
      if(bottomSpeed >= bottomTargetSpeed) bottomPower = TOPROLLERSPEEDIDLE;
      else bottomPower = (bottomTargetSpeed - bottomSpeed) / ROLLERSPEEDAPPROACHDISTANCE + ROLLERSPEEDMINIMUM;
      // Set a constant power for the bottom roller, depending on distance from the target according to the camera
      //if(WidthToDistance(maxTargetWidth) < 5.0 || targetSpeed == FIRESPEEDONE) bottomPower = 0.22f;
      //else if(targetSpeed == FIRESPEEDTWO) bottomPower = 0.22f;
      //else bottomPower = 0.75f;
      
      // Clamp the power setting within the acceptable range
      if(topPower > 1.0f) topPower = 1.0f;
      if(bottomPower > 1.0f) bottomPower = 1.0f;
      // Set the motors to their determined speeds
      topShooter->Set(-topPower);
      bottomShooter->Set(bottomPower);
      
      // Display power settings
      DISPLAYFLOAT(5, topPower);
      DISPLAYFLOAT(6, bottomPower);
      
      // Return whether or not the motor has reached the desired speed
      return (Abs(topSpeed - targetSpeed) < SHOOTERSPEEDDEADBAND && Abs(bottomSpeed - bottomTargetSpeed) < SHOOTERSPEEDDEADBAND);
   }
   
   /* Track the target with the camera if in view, otherwise rotate to gryo angle zero. Return true if aimed at target
    * and false otherwise. */
   int FindTarget(AxisCamera &camera, int track)
   {
      static int divider = 0, imageCenter;
      float gyroAngle, correctionSpeed;
      
      // Get the angle from the gyro
      gyroAngle = steeringGyro->GetAngle();
      
      // Every few times through the loop, update the camera image information
      if(divider >= CAMERAFREQUENCY)
      {
         imageCenter = GetProcessedImage(camera);
         divider = 0;
      }
      else divider++;
      
      if(!track) return (imageCenter != -1);
      
      // Turn the wheels to allow rotation in an arc
      TrackFrontSteering(0);
      TrackRearSteering(PULSESPER90);
      
      // If no image is found, steer based on the gyro angle
      if(imageCenter == -1)
      {
         correctionSpeed = Abs(gyroAngle) / GYROAPPROACHDISTANCE * 0.3f + 0.1f;
         if(correctionSpeed > 0.4f) correctionSpeed = 0.4f;
         
         if(gyroAngle - GYRODEADBANDANGLE > 0.0f) correctionSpeed *= -1.0f;
         else if(gyroAngle + GYRODEADBANDANGLE >= 0.0f) correctionSpeed = 0.0f;
         
         rrDrive->Set(correctionSpeed);
         lrDrive->Set(correctionSpeed);
      }
      
      // Otherwise, move to centre the target from the camera image
      else
      {
         correctionSpeed = Abs((float)imageCenter - IMAGEWIDTH / 2.0f) / (float)IMAGEAPPROACHDISTANCE * 0.6f;
         if(correctionSpeed > 0.6f) correctionSpeed = 0.6f;
         
         // Offset the center of the image according to the offset of the camera from the robot's center
         imageCenter += IMAGEOFFSET;
         
         if(imageCenter - IMAGEDEADBAND > (int)IMAGEWIDTH / 2) correctionSpeed *= -1.0f;
         else if(imageCenter + IMAGEDEADBAND >= (int)IMAGEWIDTH / 2) correctionSpeed = 0.0f;
         
         rrDrive->Set(correctionSpeed);
         lrDrive->Set(correctionSpeed);
      }
      
      // Return true if the image is visible and false otherwise
      if(imageCenter != -1) return 1;
      else return 0;
   }
}; /* Semicolons are not used after functions, but classes are treated
more like datatypes, so they DO have a semicolon after them. To think some
people say that object-oriented makes sense :/ */

// 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;
}

// Converts the width of the rectangle to a distance in feet
double WidthToDistance(double x)
{
   double result; // The resut of the calculations
   
   result = GOALWIDTH * IMAGEWIDTH / x / 2;
   result = result / tan(HALFCAMERAVIEWINGANGLE * Pi / 180);
   result = result * LINEARDISTANCEK + CONSTANTDISTANCEK;
   
   return result;
}

// Square the argument sent to it, preserving the sign
void SquareSign(float *x)
{
   if(*x < 0) *x = -pow(*x, 2);
   else *x = pow(*x, 2);
}

/* The entry point is FRC_UserProgram_StartupLibraryInit. This macro tells
the compiler to use the SimpleTracker class. There are a series of
"under-the-hood" operations so that the program starts in this class
instead of searching for int main(void). */
START_ROBOT_CLASS(SimpleTracker);

The header file:

Code:


/* 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 leftStick->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 HIGHGEARBUTTON leftStick->GetRawButton(5)
#define LOWGEARBUTTON leftStick->GetRawButton(4)
#define NEUTRALGEARBUTTON leftStick->GetRawButton(3)

#define BRIDGEMANIPULATEBUTTON rightStick->GetRawButton(7)
#define PIVOTMODEBUTTON rightStick->GetRawButton(2)

#define AUTOSHOOTBYDISTANCE speedStick->GetRawButton(1)
#define SHOOTBUTTON speedStick->GetRawButton(3)
#define SOLOMAGAZINEBUTTON speedStick->GetRawButton(3)
#define FIRSTFIREBUTTON speedStick->GetRawButton(4)
#define SECONDFIREBUTTON speedStick->GetRawButton(5)
#define THIRDFIREBUTTON speedStick->GetRawButton(2)

#define TARGETMODEENABLE speedStick->GetRawButton(8)
#define TARGETMODEDISABLE speedStick->GetRawButton(9)

#define MAGAZINEENABLEBUTTON speedStick->GetRawButton(10)
#define LATERALCOLLECTORBUTTON speedStick->GetRawButton(10)
#define TRANSVERSECOLLECTORBUTTON speedStick->GetRawButton(10)
#define LATERALREVERSEBUTTON speedStick->GetRawButton(7)
#define TRANSVERSEREVERSEBUTTON speedStick->GetRawButton(7)

// Port numbers
// Motors are on PWM ports
#define LFMOTORPORT 3
#define LRMOTORPORT 8
#define RFMOTORPORT 7
#define RRMOTORPORT 4
#define FRONTSTEERINGMOTORPORT 2
#define REARSTEERINGMOTORPORT 1
#define TOPSHOOTINGMOTORPORT 10
#define BOTTOMSHOOTINGMOTORPORT 9

// 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 13
#define DRIVEENCODERPORTB 14

// Light sensors for encoders are also digital inputs
#define TOPROLLERENCODERPORT 6
#define BOTTOMROLLERENCODERPORT 9
#define DRIVEWHEELENCODERPORT 5

// Limit switches are digital inputs
#define ARMDOWNSWITCHPORT 7
#define ARMUPSWITCHPORT 8

// The gyro is on an analog channel
#define STEERINGGYROPORT 2

// Relays have their own ports
#define MAGAZINERELAYPORT 2
#define MAGAZINERELAY2PORT 5
#define LATERALCOLLECTORPORT 1
#define TRANSVERSECOLLECTORPORT 3
#define LATERALCOLLERTOR2PORT 4

// Constants
// Hardware constants
#define GYROSENSITIVITY 0.007f // Volts per degree per second
#define PULSESPER90 1800 // Number of encoder pulses in a 90-degree turn
#define STEERINGDEADBAND 50 // Number of encoder pulses of proximity needed for the steering (lower numbers are more accurate, but can cause hunting)
#define STEERINGAPPROACHDISTANCE 80 // Number of encoder pulses of proximity after which the wheels begin to slow down
#define CAMERAFREQUENCY 2000 // The number of times the Is-lied by this constant
#define HALFCAMERAVIEWINGANGLE 23.5 // Half of the viewing angle of the camera
#define LINEARDISTANCEK 1.0 // The camera distance is multiplied by this constant
#define CONSTANTDISTANCEK 0.0 // The camera distance has this added to it
#define IMAGEWIDTH 320.0 // The width of the image from the camera
#define IMAGEHEIGHT 240.0 // The height of the image from the camera
#define ENCODERFREQUENCY 300 // The number of times the IsOperatorControl() loop must run before updating the encoder speed
#define MAGAZINEALTERNATORLIMIT 800 // The number of time the IsOpteratorControl() loop must run before the magazine alternator resets

// Hardware threshold constants
#define JOYSTICKDEADBANDX 0.100f
#define JOYSTICKDEADBANDY 0.100f
#define GYRODEADBANDANGLE 3.0f
#define SHOOTERDEADBAND 0.0f
#define SERVODEADBAND 2.0f

// Camera HSL Image Thresholds
#define HUEMAX 255
#define HUEMIN 0
#define SATURATIONMAX 15
#define SATURATIONMIN 0
#define LUMINANCEMAX 255
#define LUMINANCEMIN 200 // 160
#define REDMIN 190
#define REDMAX 255
#define BLUEMIN 0
#define BLUEMAX 150
#define GREENMIN 0
#define GREENMAX 150
#define MINIMUMAREA 220

// The deadband for the the horizontal pivoting to target
#define IMAGEDEADBAND 10
#define IMAGEAPPROACHDISTANCE 60

// Approching deadband for aligning with the gyro
#define GYROAPPROACHDISTANCE 15

// Other constants
#define Pi 3.1415926535897932384626433832795028841971f
#define SWERVEMODE 0
#define MONSTERMODE 1
#define CARMODE 2
#define GOALWIDTH 2.0
#define STICKDIRECTIONX -1 // Use this to control whether or not the x-axis of the joysticks is inverted
#define FIRESPEEDONE 0.0f // Shooter firing speed pre-sets
#define BOTTOMSPEEDONE 30.0f
#define FIRESPEEDTWO 9.0f
#define BOTTOMSPEEDTWO 35.0f
#define FIRESPEEDTHREE 0.0f
#define BOTTOMSPEEDTHREE 30.0f
#define FIRESPEEDCHANGE 0.0001f
#define LOWGEARPOSITION 160.0f // Angles for the gear-shifting servos
#define HIGHGEARPOSITION 80.0f
#define NEUTRALGEARPOSITION 120.0f
#define ARMENGAGEPOSITION 60.0f
#define MAGAZINEREVERSETIME 400 // Time for magazine to reverse before shooting
#define SERVOPAUSETIME 800 // Time to wait for arm-engaging servo to shift
#define IMAGEOFFSET 0 // Counteract the offset of the camera from the robot's center
#define ARMPAUSETIME 10000 // The amount of time that the arm is going to be down
#define ROLLERRATIO (1.0f/1.75f) // The ratio of the bottom roller's speed to the top roller's speed (0.5 halves the bottom)
#define AUTONOMOUSDISTANCE -1850.0f // Distance to travel in autonomous

// Shooter constants
#define SHOOTINGROLLERCONSTANT -4.0f // Minimum shooting speed
#define SHOOTINGROLLERK 1.0922619047619f // Linear scaling constant for shooting speed versus distance
#define ROLLERSPEEDAPPROACHDISTANCE 30.0f // Macros for scaling the roller speeds for the automatic speed-monitoring function
#define ROLLERSPEEDMINIMUM 0.09f // Constant added to ramping value
#define TOPROLLERSPEEDIDLE 0.08f // Roller power when at or above desired speed
#define SHOOTERSPEEDDEADBAND 3 // The allowable error in the shooter speeds

Brendan van Ryn

Posts : 95
Join date : 2010-09-30
Age : 24

Back to top Go down

Re: Code Samples

Post  William Surmak on Mon Mar 05, 2012 8:26 pm

Would it be possible to put an up to date robot code in? I would like to use my knowledge from your tutorials to see if i can understand the code so far.
avatar
William Surmak

Posts : 104
Join date : 2011-10-12

Back to top Go down

Re: Code Samples

Post  Brendan van Ryn on Mon Mar 05, 2012 10:48 pm

Hey Will. This is as recent as it gets, although I made a few very small changes to the steering that aren't in this version. Also, many lines are commented out (they have "//" at the very beginning) to prevent hardware issues, and will need to be restored later. Otherwise, this is how it stands right now:

Code:


/******************************************************************************
 *
 *    Sprockets 2012 Base Code
 *    Authors:    Brendan van Ryn
 *             Stuart Sullivan
 *             Aliah McCalla
 *             Will Surmak
 *              Jacob 't Mannetje
 *             Caleb
 *    Last Updated:   15/02/2012
 *
 ******************************************************************************/



/* Robot configuration macros. Do not change. */
#define NEW 1
#define OLD 2

/* Robot configuration */
#define ROBOT NEW // Set this to either "OLD" or "NEW", without the quotation marks, to configure for the given robot

/* Set the correct macros based on which robot is being used. Do not change this. */
#if !defined ROBOT
#warning "Robot version not specified. Assuming new configuration..."
#include "NewRobot.h"
#elif ROBOT == NEW
#include "NewRobot.h"
#elif ROBOT == OLD
#include "OldRobot.h"
#else
#warning "Robot configuration must be either 'OLD' or 'NEW'. Assuming new..."
#include "NewRobot.h"
#endif

// Header file for WPI standard hardware/robot overhead.
#include "WPILib.h"

// Include the libraries for the camera
#include "vision/AxisCamera.h"

// Header file for math functions
#include <math.h>

// Macro Functions
// Clears the Driver Station output display
#define CLEARMESSAGE DriverStationLCD::GetInstance()->Clear()

// Prints an integer, a, on the Driver Station on line, b.
#define DISPLAYINTEGER(b, a) DriverStationLCD::GetInstance()->Printf(DriverStationLCD::kUser_Line ## b, 1, "%d", a); DriverStationLCD::GetInstance()->UpdateLCD()

// Prints a string, a, on the Driver Station on line, b.
#define DISPLAYSTRING(b, a) DriverStationLCD::GetInstance()->Printf(DriverStationLCD::kUser_Line ## b, 1, a); DriverStationLCD::GetInstance()->UpdateLCD()

// Prints a float, a, on the Driver Station on line, b.
#define DISPLAYFLOAT(b, a) DriverStationLCD::GetInstance()->Printf(DriverStationLCD::kUser_Line ## b, 1, "%0.2f", a); DriverStationLCD::GetInstance()->UpdateLCD()

// 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
double WidthToDistance(double); // Returns the distance from the camera depending on the width
void SquareSign(float*); // Squares the argument sent to it, preserving the sign


/* Our main (and this year, only) class. Don't worry about the word
"class" as we won't do much Object-Oriented stuff, but in the layout
provided to use for the robot code, we must have this one class. The
code for the robot goes here, along with all variables we need. It has
always been called "SimpleTracker" for whatever reason". */
class SimpleTracker : public SimpleRobot // Just ignore this, essentially. Treat it as "int main(void)" for simplicity.
{
    // "Global" variables are declared here.
   Joystick *leftStick; // Left driver joystick
   Joystick *rightStick; // Right driver joystick
   Joystick *speedStick; // Third driver joystick
    Jaguar *frontMotor; // Front-left drive motor
    Jaguar *rearMotor; // Rear-left drive motor
    Victor *frontSteer; // Front steering motor
    Victor *rearSteer; // Rear steering motor
    Jaguar *topShooter; // Top shooting roller
    Jaguar *bottomShooter; // Bottom shooting roller
    Encoder *frontSteeringEncoder; // Front steering encoder
    Encoder *rearSteeringEncoder; // Rear steering encoder
    Encoder *driveEncoder; // Drive wheel encoder
    Gyro *steeringGyro; // Steering gyro
    HSLImage *cameraImage; // Hue/Saturation/Luminence image from camera
    BinaryImage *binaryImage; // The object used to store and edit the filtered image
    Image *imaqImage; // Stores the image in a format that the built-in imaq functions can use
    Relay *magazineBelt; // Magazine conveyor
    Relay *magazineBelt2; // Second Motor for the Magazine
    Relay *lateralCollector; // Lateral collector belts
    Relay *transverseCollector; // Transverse collector belts
    Relay *lateralCollector2; // The supplementry motor for the lateral collector belts
    DigitalInput *driveWheelEncoder; // Light sensor for drive wheel encoding
    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
    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
    double maxTargetWidth; // Width of clearest target in view
    double maxTargetHeight; // Height of clearest target in view
   
    /* This "public" means that these variables are available outside of
    this class...but we only have one, so it essentially does nothing.
    "SimpleTracker(void)" is called a constructor. Again, don't worry about
    the details. This is where our object-based variables (hardware components)
    are initialized. This means we assign them a particular port. */ 
    public:
   SimpleTracker(void)
   {
        // Assign object pointers here.     
        frontMotor = new Jaguar(FRONTMOTORPORT); // Driving Motors
        rearMotor = new Jaguar(REARMOTORPORT);
//       
//        frontSteer = new Victor(FRONTSTEERINGMOTORPORT); // Steering Motors
//        rearSteer = new Victor(REARSTEERINGMOTORPORT);
       
        topShooter = new Jaguar(TOPSHOOTINGMOTORPORT); // Shooter motors
        bottomShooter = new Jaguar(BOTTOMSHOOTINGMOTORPORT);
 
        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
//       
//        magazineBelt = new Relay(MAGAZINERELAYPORT); // Relays
//        magazineBelt2 = new Relay(MAGAZINERELAY2PORT);
//        lateralCollector = new Relay(LATERALCOLLECTORPORT);
//        transverseCollector = new Relay(TRANSVERSECOLLECTORPORT);
//        lateralCollector2 = new Relay(LATERALCOLLERTOR2PORT);
//       
//        driveWheelEncoder = new DigitalInput(DRIVEWHEELENCODERPORT); // Driving encoder light sensor
        armStartSensor = new DigitalInput(ARMSTARTPOSITIONPORT); // Bridge manipulator light sensors
        armHitSensor = new DigitalInput(ARMHITPOSITIONPORT);
       
        armDownSwitch = new DigitalInput(ARMDOWNSWITCHPORT); // The bridge manipulator limit switches
        armUpSwitch = new DigitalInput(ARMUPSWITCHPORT);
       
        frontDriveServo = new Servo(FRONTSERVOPORT); // Gear shifting servos
        rearDriveServo = new Servo(REARSERVOPORT);
        armEngageServo = new Servo(ARMENGAGESERVOPORT); // Servo to engage bridge manipulator
       
        // Initialize encoders
        /*frontSteeringEncoder->Reset();
        frontSteeringEncoder->Start();
        rearSteeringEncoder->Reset();
        rearSteeringEncoder->Start();
        driveEncoder->Reset();
        driveEncoder->Start();*/
       
//        // Initialize gyro
//        steeringGyro->Reset();
//        steeringGyro->SetSensitivity(GYROSENSITIVITY); // Changes the sensitivity of the gyro
//               
        // Enable camera and wait for a bit
        AxisCamera &camera = AxisCamera::GetInstance();
        Wait(8.0);
       
        // Set the camera's preferences
        camera.WriteResolution(camera.kResolution_320x240); // Make sure this resolution matches the width and height macros
        camera.WriteCompression(0);
        camera.WriteBrightness(50);
        camera.WriteColorLevel(256);
   }
   
    /* 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 speedx = 0.0f, speedy = 0.0f; // The x- and y-positions of the third speed-control shift
      float x, y; // The average x- and y-positions between the two 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; // Allow different angles for the front and back sides, for use in different steering modes
      int waitForSteering; // Should the drive motors wait for the wheels to finish turning?
      int frontFinished, rearFinished; // Store whether each driving side has finished turning
      int steeringMode = SWERVEMODE; // Store the current mode of steering (0 = strafe, 1 = monster, 2 = car)
      int currentButtonState = 0, previousButtonState = 0; // Store the current and previous states of the manual alignment buttons
      double distance = 0.0; // The temp variable to display distance, used to test the distance formula that is in progress
      int dividerCount = 0; // Count the number of times the loop has been executed
      int driveWheelEncoderValue; // Distance travelled by robot
      int targetFound; // Has the target been centred?
      int targetMode = 0; // Are we in targetting mode?
      float frontGearSetting = LOWGEARPOSITION; // Store the angle setting for the gear-shifting servos
      float rearGearSetting = LOWGEARPOSITION;
      float armPosition = LOWGEARPOSITION; // Stores the angle setting for the bridge-manipulator servo
      int armStage = 0; // Clean up for arm engagement
      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 fireReady = 0; // Are the rollers up to firing speed?
      float fireSpeed = 0.0f; // Firing speed for shooter based on camera distance
   
      // Acquire reference to axis camera
      AxisCamera &camera = AxisCamera::GetInstance();
      
      // Reset the drive and shooter encoders
      /*
      driveEncoder->Reset();
      driveEncoder->Start();*/
      
      // This code is run repeatedly so long as the robot is in teleop mode.
      while (IsOperatorControl())
      {
         /* This is a fancy, convoluted way of telling the operating-system
         software that the code hasn't frozen. */
         GetWatchdog().Feed();
         
         /* Message displays */
         CLEARMESSAGE;
         DISPLAYFLOAT(1, distance); // Distance to target, in feet
         DISPLAYINTEGER(2, targetFound); // Encoder value from drive wheel
         DISPLAYFLOAT(3, topShooter->Get()); // Display the top shooter speed reading
         DISPLAYFLOAT(4, bottomShooter->Get()); // Display the bottom shooter speed reading
         DISPLAYINTEGER(5, targetFound - (int)IMAGEWIDTH / 2); // Display the position of the topmost target
         
         // Get joystick positions and square, preserving sign, to reduce sensitivity
         leftx = STICKDIRECTIONX * leftStick->GetRawAxis(1); SquareSign(&leftx);
         lefty = leftStick->GetRawAxis(2); SquareSign(&lefty);
         rightx = STICKDIRECTIONX * rightStick->GetRawAxis(1); SquareSign(&rightx);
         righty = rightStick->GetRawAxis(2); SquareSign(&righty);
         speedx = speedStick->GetRawAxis(1);
         speedy = speedStick->GetRawAxis(2);
         
         // Prevent back-rolling
         if(speedy > 0) speedy = 0.0f;
//         
         // Update the encoder speed every few times through this loop
         if(dividerCount >= ENCODERFREQUENCY)
         {
            // Get the distance and display it
            targetFound = GetProcessedImage(camera);
            distance = WidthToDistance(maxTargetWidth);
         }
         else dividerCount++;
//         
         // 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++;
//         
//         /* Teleop code here. */
//         // Check  drive mode buttons
//         if(SWERVEMODEBUTTON) steeringMode = SWERVEMODE;
//         else if(MONSTERMODEBUTTON) steeringMode = MONSTERMODE;
//         else if(CARMODEBUTTON) steeringMode = CARMODE;
//         
         // Check shooter buttons
         if(FIRSTFIREBUTTON) fireReady = SetRollerSpeed(FIRESPEEDONE), preFire = 1;
         else if(SECONDFIREBUTTON) fireReady = SetRollerSpeed(FIRESPEEDTWO), preFire = 1;
         else if(THIRDFIREBUTTON) fireReady = SetRollerSpeed(FIRESPEEDTHREE), preFire = 1;
         else preFire = 0, fireReady = 1;
//         
//         // Enter or exit targeting mode
//         if(TARGETMODEENABLE) targetMode = 1, magazineReverseCount = MAGAZINEREVERSETIME;
//         else if(TARGETMODEDISABLE) targetMode = 0;
         
//         // Reset the gyro reading
//         if(GYRORESETBUTTON) steeringGyro->Reset();
         
         // Check gear shifting buttons
         if(HIGHGEARBUTTON && !targetMode) frontGearSetting = rearGearSetting = HIGHGEARPOSITION, armPosition = LOWGEARPOSITION;
         else if(LOWGEARBUTTON || targetMode) frontGearSetting = rearGearSetting = LOWGEARPOSITION, armPosition = LOWGEARPOSITION;
         else if(NEUTRALGEARBUTTON && !targetMode) frontGearSetting = LOWGEARPOSITION, rearGearSetting = NEUTRALGEARPOSITION, armPosition = LOWGEARPOSITION;
         else if(BRIDGEMANIPULATEBUTTON && !targetMode && armPosition != ARMENGAGEPOSITION) frontGearSetting = LOWGEARPOSITION, rearGearSetting = NEUTRALGEARPOSITION, armPosition = ARMENGAGEPOSITION, servoPauseCount = SERVOPAUSETIME, armStage = 0;
         
         // Move servos to desired position
         frontDriveServo->SetAngle(frontGearSetting);
         rearDriveServo->SetAngle(rearGearSetting);
         armEngageServo->SetAngle(armPosition);
                     
//         // These must be called each time in order for the encoder positions to update
//         driveWheelEncoderValue = driveEncoder->Get();
//         
//         // Check belt buttons
//         if(MAGAZINEENABLEBUTTON) magazineBelt->Set(Relay::kForward), magazineBelt2->Set(Relay::kForward), speedy = 0.7, magazineReverseCount = MAGAZINEREVERSETIME;
//         else if(SOLOMAGAZINEBUTTON && fireReady)
//         {
//            if(magazineAlternator > MAGAZINEALTERNATORLIMIT / 8) magazineBelt->Set(Relay::kForward), magazineBelt2->Set(Relay::kForward);
//            else magazineBelt->Set(Relay::kOff), magazineBelt2->Set(Relay::kOff);
//         }
//         // else if(magazineReverseCount > 0 && !targetMode) magazineBelt->Set(Relay::kReverse), magazineBelt2->Set(Relay::kReverse), magazineReverseCount--;
//         else magazineBelt->Set(Relay::kOff), magazineBelt2->Set(Relay::kOff);
//         if(LATERALCOLLECTORBUTTON) lateralCollector->Set(Relay::kReverse), lateralCollector2->Set(Relay::kReverse);
//         else if(LATERALREVERSEBUTTON) magazineBelt->Set(Relay::kReverse), magazineBelt2->Set(Relay::kReverse), lateralCollector->Set(Relay::kForward), lateralCollector2->Set(Relay::kForward);
//         else lateralCollector->Set(Relay::kOff), lateralCollector2->Set(Relay::kOff);
//         if(TRANSVERSECOLLECTORBUTTON) transverseCollector->Set(Relay::kReverse);
//         else if(TRANSVERSEREVERSEBUTTON) transverseCollector->Set(Relay::kForward);
//         else transverseCollector->Set(Relay::kOff);
//         
         // If the total y-deflection for each joystick is below the threshold, we shouldn't count it at all
         if(Abs(lefty) < JOYSTICKDEADBANDY) lefty = 0.0f;
         if(Abs(righty) < JOYSTICKDEADBANDY) righty = 0.0f;
            
         // Average the joystick positions
         x = (leftx + rightx) / 2.0f;
         y = (lefty + righty) / 2.0f;
//                  
//         // Store the currentButtonState from the previous iteration
//         previousButtonState = currentButtonState;
//         
         // Update the camera image
//         targetFound = FindTarget(camera, 0);
//         
//         // If the manual-alignment buttons are pressed
//         if(MANUALALIGNENABLEBUTTON)
//         {
//            currentButtonState = 1;
//            if(MANUALALIGNLEFTREAR) rearSteer->Set(-0.6f);
//            else if(MANUALALIGNRIGHTREAR) rearSteer->Set(0.6f);
//            else rearSteer->Set(0);
//               
//            if(MANUALALIGNLEFTFRONT) frontSteer->Set(-0.7f);
//            else if(MANUALALIGNRIGHTFRONT)frontSteer->Set(0.7f);
//            else frontSteer->Set(0);
//         }
//         
         // Sets the speed dependent on the distance to top target
         if(AUTOSHOOTBYDISTANCE) SetRollerSpeed(LINEARDISTANCEKT * distance + CONSTANTDISTANCEKT);
//         
         // If we are manipulating the bridge
         /*else*/ if(armPosition == ARMENGAGEPOSITION && servoPauseCount == 0)
         {
            // Drive rear wheels off of joystick
            if(Abs(x) < JOYSTICKDEADBANDX) frontMotor->Set(lefty);
            
            if(!BRIDGEMANIPULATEBUTTON) armStage = 4;
            
            /// Drive winch until first light sensor is triggered (arm is in primed position)
            if(!armStartSensor->Get() && armStage == 0) rearMotor->Set(0.9f);
            else if(armStage == 0) armStage = 1;
            
            // Stop winch until the light sensor on the bridge dector is triggered (bridge is hit)
            if(!armHitSensor->Get() && armStage == 1) rearMotor->Set(0.0f);
            else if(armStage == 1) armStage = 2;
            
            // Continue to run winch until bottom switch is hit
            if(!armDownSwitch->Get() && armStage == 2) rearMotor->Set(0.9f);
            else if(armStage == 2) armStage = 3;
               
            // Wait for manipulator button to be released
            if(BRIDGEMANIPULATEBUTTON && armStage == 3) rearMotor->Set(0.0f);
            else if(armStage == 3) armStage = 4;
            
            // Return arm
            if(armUpSwitch->Get() && armStage == 4) rearMotor->Set(-0.6f);
            else if(armStage == 4) armStage = 5;
            
            // Disengage arm, set drive units to low gear, and stop winch motor; reset the armStage for next time
            if(armStage == 5)
            {
               rearGearSetting = LOWGEARPOSITION;
               frontGearSetting = LOWGEARPOSITION;
               armPosition = LOWGEARPOSITION;
               
               rearMotor->Set(0.0f);
            }
         }
         
         // The lock button sets the wheels straight sideways for aligning on the bridge
         else if(LOCKMODEBUTTON)
         {
            TrackFrontSteering(PULSESPER90);
            TrackRearSteering(PULSESPER90);
            frontMotor->Set(y);
            rearMotor->Set(y);
         }
            
         // The pivot mode separate from the rest of the drive
         else if(PIVOTMODEBUTTON)
         {
            TrackFrontSteering(0);
            TrackRearSteering(PULSESPER90);
            frontMotor->Set(0);
            rearMotor->Set(righty);
         }
         
         // If we are in targetting mode
         else if(targetMode)
         {
            // Track the target
            targetFound = FindTarget(camera, 1);
            
            // If it is visible, prepare to shoot
            if(targetFound)
            {
               // Get the distance to the target
               distance = WidthToDistance(maxTargetWidth);
               
               // Once the target is in view, set the roller speed based on distance
               if(distance > 9) fireSpeed = LINEARDISTANCEKT * distance + CONSTANTDISTANCEKT;
               else fireSpeed = LINEARDISTANCEKB * distance + CONSTANTDISTANCEKB;
               if(fireSpeed < 0.0f) fireSpeed = 0.0f;
               fireReady = SetRollerSpeed(fireSpeed);
                  
               // Reverse the magazine for a bit
               if(magazineReverseCount > 0)
               {
                  magazineBelt->Set(Relay::kReverse), magazineBelt2->Set(Relay::kReverse);
                  magazineReverseCount--;
               }
               
               // Then, move it forward for shooting
               else if(SHOOTBUTTON && fireReady) magazineBelt->Set(Relay::kForward), magazineBelt2->Set(Relay::kForward);
            }
            
            // Otherwise, wait
            else
            {
               distance = 0.0f;
               topShooter->Set(0.0f);
               bottomShooter->Set(0.0f);
               
               // Reverse the magazine for a bit
               if(magazineReverseCount > 0)
               {
                  magazineBelt->Set(Relay::kReverse), magazineBelt2->Set(Relay::kReverse);
                  magazineReverseCount--;
               }
               
               // Then, stop it
               else magazineBelt->Set(Relay::kOff), magazineBelt2->Set(Relay::kOff);
            }
         }
                  
         // Otherwise, if the deflection left or right is less than a certain amount, we simply drive tank-style
         // Abs simply ensures that x is positive so that we don't need two separate if-statements
         else if(Abs(x) < JOYSTICKDEADBANDX)
         {
            // Tank drive
            // Set the drive motors according to their respective joysticks
            frontMotor->Set(lefty);
            rearMotor->Set(righty);
            
            if(!preFire && !AUTOSHOOTBYDISTANCE)
            {
               topShooter->Set(speedy); // Set roller speeds manually
               bottomShooter->Set(-speedy);
            }
            currentButtonState = 0;
            
//            TrackFrontSteering(0);
//            TrackRearSteering(0);
         }
//         
//         else
//         {
//            // Swerve drive
//            // Determine the total deflection of the joysticks to use as the speed setting
//            swerveSpeed = sqrt(x*x + y*y); // a^2 + b^2 = c^2
//            swerveAngle = -Abs(atan(x/y)); // x/y = tan(theta)
//            swerveAngle = swerveAngle * 180 / Pi; // 1 degree = 180/Pi radians
//            swerveAngle = swerveAngle / 90.0f * PULSESPER90; // Convert from degrees to encoder pulses
//            
//            // Set roller speeds manually
//            if(!preFire)
//            {
//               topShooter->Set(speedy);
//               bottomShooter->Set(-speedy);
//            }
//            
//            /* If the y-component is zero, we should try to move straight left or right. Therefore, we
//             * need to wait for the wheels to finish turning before allowing the drive motors to move. The
//             * angle, for some reason, also needs to be inverted when moving straight left or right. */
//            if(Abs(y) < JOYSTICKDEADBANDY)
//            {
//               waitForSteering = 1;
//               if(x < 0.0f) swerveAngle = PULSESPER90, swerveSpeed = -swerveSpeed;
//               else if(x > 0.0f) swerveAngle = -PULSESPER90;
//            }
//            else waitForSteering = 0;
//                        
//            // Determine the quadrant of the joystick and convert the speed or angle appropriately
//            // Quadrant one is assumed, except for the following conditions
//            if(x < 0 && y >= 0) swerveAngle = -swerveAngle; // Quadrant two
//            else if(x < 0 && y < 0) swerveSpeed = -swerveSpeed; // Quadrant three
//            else if(x >= 0 && y < 0) // Quadrant Four
//            {
//               swerveSpeed = -swerveSpeed;
//               swerveAngle = -swerveAngle;               
//            }
//            
//            // If we are in monster mode and travelling faster than 80% max speed, we switch to car automatically
//            if(Abs(swerveSpeed) > 1.2f && steeringMode == MONSTERMODE) steeringMode = CARMODE;
//         
//            // Put steer-angle and drive-direction changes here for the other steering modes (monster, car, auto-monster, etc.)
//            if(steeringMode == MONSTERMODE)
//            {
//               if(swerveSpeed <= 0) swerveAngleFront = -0.75f * swerveAngle, swerveAngleRear = 0.75f * swerveAngle;
//               else swerveAngleFront = 0.75f * swerveAngle, swerveAngleRear = -0.75f * swerveAngle;
//            }
//            else if(steeringMode == CARMODE)
//            {
//               if(swerveSpeed <= 0) swerveAngleFront = -0.5f * swerveAngle, swerveAngleRear = 0.0f;
//               else swerveAngleRear = -0.5f * swerveAngle, swerveAngleFront = 0.0f;
//            }
//            
//            else
//            {
//               swerveAngleFront = -swerveAngle;
//               swerveAngleRear = -swerveAngle;
//            }
//            
//            /* Call pre-written functions that will use the encoder to get the wheels to gradually approach the desired
//             * position. This function will return zero when the steering is finished.*/
//            frontFinished = TrackFrontSteering((int)swerveAngleFront);
//            rearFinished = TrackRearSteering((int)swerveAngleRear);
//            
//            // If the steering has finished, set the drive motors to move
//            if((frontFinished == 0 && rearFinished == 0) || !waitForSteering)
//            {
//               frontMotor->Set(swerveSpeed);
//               rearMotor->Set(swerveSpeed);
//            }
//            
//            currentButtonState = 0;
//         }
//         
//         // If the manual alignment buttons have JUST been released, reset the encoders again
//         if(previousButtonState == 1 && currentButtonState == 0)
//         {
//            frontSteeringEncoder->Reset();
//            frontSteeringEncoder->Start();
//            rearSteeringEncoder->Reset();
//            rearSteeringEncoder->Start();
//            frontSteer->Set(0);
//            rearSteer->Set(0);
//         }
        }
   } // End of teleop
   
   /* Code to run while in autonomous, or fully-automated mode. */
   void Autonomous(void)
   {
      // Variables
      float gyroAngle = 0; // Current angle from home as read by the gyro
      int encoderValue = 0; // Stores the encoder distance traveled by the robot
      int armStage = 0; // Current stage for bridge manipulator
      int autoPauseCount = SERVOPAUSETIME; // The amount of time to wait for the servos to reach their target position
      int magazinePauseCount = MAGAZINEPAUSETIME; // The amount of time to wait for the rollers to reach speed
      
      // Acquire reference to axis camera
      AxisCamera &camera = AxisCamera::GetInstance();
      
      // Reset the drive encoder and gyro
//      driveEncoder->Reset();
//      driveEncoder->Start();
//      steeringGyro->Reset();
      
      // This code is run repeatedly so long as the robot is in autonomous mode (lasting 15 seconds).
      while (IsAutonomous())
      {
         /* Tell the system that the code hasn't frozen. */
         GetWatchdog().Feed();
         
         /* Autonomous code here. */
//         // Get the gyro angle
//         gyroAngle = steeringGyro->GetAngle();
//         
//         // Get the pulse count from the drive encoder
//         encoderValue = AbsI(driveEncoder->Get());
                  
         // Message displays
         CLEARMESSAGE;
         DISPLAYFLOAT(1, gyroAngle);  // The float value of the gyro angle
         DISPLAYINTEGER(2, encoderValue); // Current position of encoder
         DISPLAYINTEGER(3, armStage);
         
         /* Simple, straight-movement autonomous */
         // Turn the rear wheels left to correct if we are off to the left
         // Otherwise, turn the wheels right to correct if we are off to the right
         // Otherwise, move straight
//         if(gyroAngle + GYRODEADBANDANGLE < 0.0f) TrackRearSteering(PULSESPER90 / 8);
//         else if(gyroAngle - GYRODEADBANDANGLE > 0.0f) TrackRearSteering(-PULSESPER90 / 8);
//         else TrackRearSteering(0);
         
         // Keep the front wheels straight and drive forward slowly
//         TrackFrontSteering(0);
                  
         // Until we have hit the bridge, move forward
         if(armStage < 2)
         {
            // Set gears to front drive and arm engagement
            frontDriveServo->SetAngle(LOWGEARPOSITION);
            rearDriveServo->SetAngle(NEUTRALGEARPOSITION);
            armEngageServo->SetAngle(ARMENGAGEPOSITION);
            
            // Give the servos time to get into place
            if(autoPauseCount > 0)
            {
               autoPauseCount--;
               continue;
            }
            
            // Drive
            frontMotor->Set(0.4);
            
            /// Drive winch until first light sensor is triggered (arm is in primed position)
            if(!armStartSensor->Get() && armStage == 0) rearMotor->Set(0.6);
            else if(armStage == 0) armStage = 1;
                        
            // Stop winch until the light sensor on the bridge dector is triggered (bridge is hit) or we travel too far (probably missing)
            if((!armHitSensor->Get() || encoderValue > AUTONOMOUSDISTANCE) && armStage == 1) rearMotor->Set(0.0f);
            else if(armStage == 1) armStage = 2;
         }
         
         // We have hit the bridge now
         else if(armStage < 4)
         {
            // Set gears to front drive and arm engagement
            frontDriveServo->SetAngle(LOWGEARPOSITION);
            rearDriveServo->SetAngle(NEUTRALGEARPOSITION);
            armEngageServo->SetAngle(ARMENGAGEPOSITION);
            
            // Stop
            frontMotor->Set(0.0f);
            
            // Continue to run winch until bottom switch is hit
            if(!armDownSwitch->Get() && armStage == 2) rearMotor->Set(0.9f);
            else if(armStage == 2) armStage = 3;
            
            // Return arm to home position
            if(armUpSwitch->Get() && armStage == 3) rearMotor->Set(-0.6f);
            else if(armStage == 3) armStage = 4;
         }
         
         // Bridge tipping is complete
         else
         {
            // Return gears to low
            frontDriveServo->Set(LOWGEARPOSITION);
            rearDriveServo->Set(LOWGEARPOSITION);
            armEngageServo->Set(LOWGEARPOSITION);
            
            // Spin the rollers at the correct launching speed for the top target
            SetRollerSpeed(FINALAUTONOMOUSDISTANCE * LINEARDISTANCEKT + CONSTANTDISTANCEKT);
            
            // Wait for the rollers to get up to speed, the launch the balls
            if(magazinePauseCount > 0) magazinePauseCount--;
            else magazineBelt->Set(Relay::kForward), magazineBelt2->Set(Relay::kForward);
               
            // Stop winch and drive
            frontMotor->Set(0.0f);
            rearMotor->Set(0.0f);
            
            // Code to move elsewhere or shoot goes here
         }
      } // End of IsAutonomous
   } // End of Autonomous
   
   /* This function handles the complicated problem of quickly, but accurately, moving the
    * front wheels to their correct position, as passed 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 < STEERINGDEADBAND) speed = 0.0f;
      else if(frontSteeringEncoderValue < targetPosition) speed = (float)remainingSteeringDistance / (float)STEERINGAPPROACHDISTANCE;
      else speed = (float)-remainingSteeringDistance / (float)STEERINGAPPROACHDISTANCE;
      
      // 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 < STEERINGDEADBAND) speed = 0.0f;
      else if(rearSteeringEncoderValue < targetPosition) speed = (float)remainingSteeringDistance / (float)STEERINGAPPROACHDISTANCE;
      else speed = (float)-remainingSteeringDistance / (float)STEERINGAPPROACHDISTANCE;
      
      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;
   }
   
   /* Take the camera image and process it, returning the x-position of the largest particle. This should represent the
   horizontal centre of the most in-focus target. Comparing this value to the image width (320) should allow us to
   centre the image by rotating the robot. If the targets are out of view, -1 is returned. */
   int GetProcessedImage(AxisCamera &camera)
   {
      register int i, j, k;
      int numberOfParticles, highestParticle, indices[4];
      double maxHeight = -1, width, height, horizontalPosition, verticalPosition;
      double targetAreas[4];
      
      // Set the target areas to a sentinel value to detect empty positions
      for(i = 0; i < 4; i++) targetAreas[i] = -1.0f;
      
      // Save the image as a BinaryImage Object
      cameraImage = camera.GetImage();
      //binaryImage = cameraImage->ThresholdHSLHUEMIN, HUEMAX, SATURATIONMIN, SATURATIONMAX, LUMINANCEMIN, LUMINANCEMAX);
      binaryImage = cameraImage->ThresholdRGB(REDMIN, REDMAX, GREENMIN, GREENMAX, BLUEMIN, BLUEMAX);
      imaqImage = binaryImage->GetImaqImage();
      
      // Perform the Convex Hull operation
      imaqConvexHull(imaqImage, imaqImage, 1);
      
      // Count the number of distinct sections remaining
      imaqCountParticles(imaqImage, 2, &numberOfParticles);
            
       // Save image to the cRIO's memory (enable for debugging purposes only)
       //cameraImage->Write("camIm4.jpeg");
       //binaryImage->Write("binIm4.bmp");
      
       // For each particle found
       for(i = 0; i < numberOfParticles; i++)
       {
          // Get the width and height of the given particle
          imaqMeasureParticle(imaqImage, i, 0, IMAQ_MT_BOUNDING_RECT_WIDTH, &width);
          imaqMeasureParticle(imaqImage, i, 0, IMAQ_MT_BOUNDING_RECT_HEIGHT, &height);
          
          // Search through array of the four largest particles
          // Add the current one to the array if it overcomes any already there
          // Also store the index of each
          for(j = 0; j < 4; j++)
          {
             // If we find an empty spot, just fill it
             if(targetAreas[j] == -1.0f)
             {
                targetAreas[j] = width * height;
                indices[j] = i;
                break;
             }
             
             // If the current particle exceeds one already in the array
             if(width * height > targetAreas[j])
             {
                // Move all other entries back one spot
                for(k = j; k < 3; k++)
                {
                   targetAreas[k+1] = targetAreas[k];
                   indices[k+1] = indices[k];
                }
                
                // Store the current one
                targetAreas[j] = width * height;
                indices[j] = i;
                break;
             }
          }
       }
      
       // Search through the recorded target candidates
       for(i = 0; i < 4; i++)
       {
          // Skip any empty spots
          if(targetAreas[i] == -1) continue;
          
          // Find the specific target's vertical position
          imaqMeasureParticle(imaqImage, indices[i], 0, IMAQ_MT_CENTER_OF_MASS_Y, &verticalPosition);
          
          // If it's current position is greater than the largest recorded so far, store it as the highest
          if(verticalPosition > maxHeight)
          {
             maxHeight = verticalPosition;
             highestParticle = indices[i];
          }
       }
      
       // Find the x-position of the centre of the top target, with a few assumptions made
       imaqMeasureParticle(imaqImage, highestParticle, 0, IMAQ_MT_CENTER_OF_MASS_X, &horizontalPosition);
       imaqMeasureParticle(imaqImage, highestParticle, 0, IMAQ_MT_BOUNDING_RECT_WIDTH, &maxTargetWidth);
      
       // Free memory
       delete binaryImage;
       delete cameraImage;
      
       // If the largest particle is outside of the threshold, return -1 to indicate that the targets aren't visible
       if(targetAreas[highestParticle] < MINIMUMAREA) return -1;
      
       // Return this x-position
       return (int)horizontalPosition;
   }
   
   // Maintains a specific speed for the shooting rollers. Returns true when rollers are at that speed
   int SetRollerSpeed(float targetSpeed)
   {
      /* Insert time delay */
      
      topShooter->Set(-targetSpeed * ROLLERRATIO);
      bottomShooter->Set(targetSpeed);
      
      return 1;
   }
   
   /* Track the target with the camera if in view, otherwise rotate to gryo angle zero. Return true if aimed at target
    * and false otherwise. */
   int FindTarget(AxisCamera &camera, int track)
   {
      static int divider = 0, imageCenter;
      float gyroAngle, correctionSpeed;
      
//      // Get the angle from the gyro
//      gyroAngle = steeringGyro->GetAngle();
      
      // Every few times through the loop, update the camera image information
      if(divider >= CAMERAFREQUENCY)
      {
         imageCenter = GetProcessedImage(camera);
         divider = 0;
      }
      else divider++;
      
      if(!track) return (imageCenter != -1);
      
      // Turn the wheels to allow rotation in an arc
      TrackFrontSteering(0);
      TrackRearSteering(PULSESPER90);
      
      // If no image is found, steer based on the gyro angle
      if(imageCenter == -1)
      {
//         correctionSpeed = Abs(gyroAngle) / GYROAPPROACHDISTANCE * 0.3f + 0.1f;
//         if(correctionSpeed > 0.4f) correctionSpeed = 0.4f;
//         
//         if(gyroAngle - GYRODEADBANDANGLE > 0.0f) correctionSpeed *= -1.0f;
//         else if(gyroAngle + GYRODEADBANDANGLE >= 0.0f) correctionSpeed = 0.0f;
//         
//         rearMotor->Set(correctionSpeed);
      }
      
      // Otherwise, move to centre the target from the camera image
      else
      {
         DISPLAYINTEGER(5, imageCenter);
         correctionSpeed = Abs((float)imageCenter - IMAGEWIDTH / 2.0f) / (float)IMAGEAPPROACHDISTANCE * 0.6f;
         if(correctionSpeed > 0.6f) correctionSpeed = 0.6f;
         
         // Offset the center of the image according to the offset of the camera from the robot's center
         imageCenter += IMAGEOFFSET;
         
         if(imageCenter - IMAGEDEADBAND > (int)IMAGEWIDTH / 2) correctionSpeed *= -1.0f;
         else if(imageCenter + IMAGEDEADBAND >= (int)IMAGEWIDTH / 2) correctionSpeed = 0.0f;
         
         rearMotor->Set(correctionSpeed);
      }
      
      // Return true if the image is visible and false otherwise
      if(imageCenter != -1) return 1;
      else return 0;
   }
}; /* Semicolons are not used after functions, but classes are treated
more like datatypes, so they DO have a semicolon after them. To think some
people say that object-oriented makes sense :/ */

// 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;
}

// Converts the width of the rectangle to a distance in feet
double WidthToDistance(double x)
{
   double result; // The resut of the calculations
   
   result = GOALWIDTH * IMAGEWIDTH / x / 2;
   result = result / tan(HALFCAMERAVIEWINGANGLE * Pi / 180);
   
   return result;
}

// Square the argument sent to it, preserving the sign
void SquareSign(float *x)
{
   if((*x) < 0) *x = -pow((*x), 2);
   else *x = pow((*x), 2);
}

/* The entry point is FRC_UserProgram_StartupLibraryInit. This macro tells
the compiler to use the SimpleTracker class. There are a series of
"under-the-hood" operations so that the program starts in this class
instead of searching for int main(void). */
START_ROBOT_CLASS(SimpleTracker);

The header file:

Code:


/* 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(3)
#define LOCKMODEBUTTON rightStick->GetRawButton(5)

#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 HIGHGEARBUTTON leftStick->GetRawButton(5)
#define LOWGEARBUTTON leftStick->GetRawButton(4)
#define NEUTRALGEARBUTTON leftStick->GetRawButton(3)

#define BRIDGEMANIPULATEBUTTON leftStick->GetRawButton(2)
#define PIVOTMODEBUTTON rightStick->GetRawButton(2)

#define GYRORESETBUTTON rightStick->GetRawButton(4)

#define AUTOSHOOTBYDISTANCE speedStick->GetRawButton(1)
#define SHOOTBUTTON speedStick->GetRawButton(3)
#define SOLOMAGAZINEBUTTON speedStick->GetRawButton(3)
#define FIRSTFIREBUTTON speedStick->GetRawButton(4)
#define SECONDFIREBUTTON speedStick->GetRawButton(3)
#define THIRDFIREBUTTON speedStick->GetRawButton(5)
#define TARGETMODEENABLE speedStick->GetRawButton(8)
#define TARGETMODEDISABLE speedStick->GetRawButton(9)

#define MAGAZINEENABLEBUTTON speedStick->GetRawButton(10)
#define LATERALCOLLECTORBUTTON speedStick->GetRawButton(10)
#define TRANSVERSECOLLECTORBUTTON speedStick->GetRawButton(10)
#define LATERALREVERSEBUTTON speedStick->GetRawButton(7)
#define TRANSVERSEREVERSEBUTTON speedStick->GetRawButton(7)

// Port numbers
// Motors are on PWM ports
#define FRONTMOTORPORT 3
#define REARMOTORPORT 4
#define FRONTSTEERINGMOTORPORT 2
#define REARSTEERINGMOTORPORT 1
#define TOPSHOOTINGMOTORPORT 10
#define BOTTOMSHOOTINGMOTORPORT 9

// 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 13
#define DRIVEENCODERPORTB 14

// Light sensors for encoders are also digital inputs
#define DRIVEWHEELENCODERPORT 5
#define ARMSTARTPOSITIONPORT 9
#define ARMHITPOSITIONPORT 10

// Limit switches are digital inputs
#define ARMDOWNSWITCHPORT 7
#define ARMUPSWITCHPORT 8

// The gyro is on an analog channel
#define STEERINGGYROPORT 2

// Relays have their own ports
#define MAGAZINERELAYPORT 2
#define MAGAZINERELAY2PORT 5
#define LATERALCOLLECTORPORT 1
#define TRANSVERSECOLLECTORPORT 3
#define LATERALCOLLERTOR2PORT 4

// Constants
// Hardware constants
#define GYROSENSITIVITY 0.007f // Volts per degree per second
#define PULSESPER90 1800 // Number of encoder pulses in a 90-degree turn
#define STEERINGDEADBAND 50 // Number of encoder pulses of proximity needed for the steering (lower numbers are more accurate, but can cause hunting)
#define STEERINGAPPROACHDISTANCE 80 // Number of encoder pulses of proximity after which the wheels begin to slow down
#define CAMERAFREQUENCY 2000 // The number of times the Is-lied by this constant
#define HALFCAMERAVIEWINGANGLE 23.5 // Half of the viewing angle of the camera
#define IMAGEWIDTH 320.0 // The width of the image from the camera
#define IMAGEHEIGHT 240.0 // The height of the image from the camera
#define ENCODERFREQUENCY 300 // The number of times the IsOperatorControl() loop must run before updating the encoder speed
#define MAGAZINEALTERNATORLIMIT 800 // The number of time the IsOpteratorControl() loop must run before the magazine alternator resets

// Hardware threshold constants
#define JOYSTICKDEADBANDX 0.100f
#define JOYSTICKDEADBANDY 0.100f
#define GYRODEADBANDANGLE 3.0f
#define SHOOTERDEADBAND 0.0f
#define SERVODEADBAND 2.0f

// Camera HSL Image Thresholds
#define HUEMAX 255
#define HUEMIN 0
#define SATURATIONMAX 15
#define SATURATIONMIN 0
#define LUMINANCEMAX 255
#define LUMINANCEMIN 200 // 160
#define REDMIN 190
#define REDMAX 255
#define BLUEMIN 0
#define BLUEMAX 150
#define GREENMIN 0
#define GREENMAX 150
#define MINIMUMAREA 220

// The deadband for the the horizontal pivoting to target
#define IMAGEDEADBAND 10
#define IMAGEAPPROACHDISTANCE 60

// Approching deadband for aligning with the gyro
#define GYROAPPROACHDISTANCE 15

// Other constants
#define Pi 3.1415926535897932384626433832795028841971f
#define SWERVEMODE 0
#define MONSTERMODE 1
#define CARMODE 2
#define GOALWIDTH 2.0
#define STICKDIRECTIONX -1 // Use this to control whether or not the x-axis of the joysticks is inverted
#define LOWGEARPOSITION 160.0f // Angles for the gear-shifting servos
#define HIGHGEARPOSITION 80.0f
#define NEUTRALGEARPOSITION 120.0f
#define ARMENGAGEPOSITION 60.0f
#define MAGAZINEREVERSETIME 400 // Time for magazine to reverse before shooting
#define SERVOPAUSETIME 800 // Time to wait for arm-engaging servo to shift
#define IMAGEOFFSET 0 // Counteract the offset of the camera from the robot's center
#define AUTONOMOUSDISTANCE 2000 // Maximum distance to travel in autonomous
#define FINALAUTONOMOUSDISTANCE 19.0f // Final distance from target once robot stops in autonomous at bridge
#define MAGAZINEPAUSETIME 400 // Amount of time to wait during autonomous for rollers to spin up to speed

// Shooter constants
#define FIRESPEEDONE 0.40f // Pre-set firing speeds
#define FIRESPEEDTWO 0.6f
#define FIRESPEEDTHREE 0.98f
#define ROLLERRATIO (1.0f/5.0f) // The ratio of the bottom roller's speed to the top roller's speed (0.5 halves the bottom)
#define LINEARDISTANCEKT 0.015f // The camera distance is multiplied by this constant ("m" in y = mx + b) -- for top net (> 9')
#define CONSTANTDISTANCEKT 0.53f // The camera distance has this added to it ("b" in y = mx + b) -- for top net (> 9')
#define LINEARDISTANCEKB 0.01125f // For bottom net (< 9')
#define CONSTANTDISTANCEKB 0.4401f // For bottom net (< 9')

It's very inconvenient to read on this forum. It's probably best if you can manage to copy and paste it into a text editor, or better yet, a C compiler (like Dev-C++, which is free and trusted). I hope the tutorials have made things better Wink

Brendan van Ryn

Posts : 95
Join date : 2010-09-30
Age : 24

Back to top Go down

Day Before UOIT

Post  Brendan van Ryn on Wed Mar 07, 2012 11:35 pm

Here is everything as of now, right before our first competition. A lot is still commented out, but everything we've added, changed, and tested seems to be perfectly functional, so we're hoping that no problems arise once those other sections are uncommented.

Code:


/******************************************************************************
 *
 *    Sprockets 2012 Base Code
 *    Authors:    Brendan van Ryn
 *             Stuart Sullivan
 *             Aliah McCalla
 *             Will Surmak
 *              Jacob 't Mannetje
 *             Caleb
 *    Last Updated:   15/02/2012
 *
 ******************************************************************************/



/* Robot configuration macros. Do not change. */
#define NEW 1
#define OLD 2

/* Robot configuration */
#define ROBOT NEW // Set this to either "OLD" or "NEW", without the quotation marks, to configure for the given robot

/* Set the correct macros based on which robot is being used. Do not change this. */
#if !defined ROBOT
#warning "Robot version not specified. Assuming new configuration..."
#include "NewRobot.h"
#elif ROBOT == NEW
#include "NewRobot.h"
#elif ROBOT == OLD
#include "OldRobot.h"
#else
#warning "Robot configuration must be either 'OLD' or 'NEW'. Assuming new..."
#include "NewRobot.h"
#endif

// Header file for WPI standard hardware/robot overhead.
#include "WPILib.h"

// Include the libraries for the camera
#include "vision/AxisCamera.h"

// Header file for math functions
#include <math.h>

// Macro Functions
// Clears the Driver Station output display
#define CLEARMESSAGE DriverStationLCD::GetInstance()->Clear()

// Prints an integer, a, on the Driver Station on line, b.
#define DISPLAYINTEGER(b, a) DriverStationLCD::GetInstance()->Printf(DriverStationLCD::kUser_Line ## b, 1, "%d", a); DriverStationLCD::GetInstance()->UpdateLCD()

// Prints a string, a, on the Driver Station on line, b.
#define DISPLAYSTRING(b, a) DriverStationLCD::GetInstance()->Printf(DriverStationLCD::kUser_Line ## b, 1, a); DriverStationLCD::GetInstance()->UpdateLCD()

// Prints a float, a, on the Driver Station on line, b.
#define DISPLAYFLOAT(b, a) DriverStationLCD::GetInstance()->Printf(DriverStationLCD::kUser_Line ## b, 1, "%0.2f", a); DriverStationLCD::GetInstance()->UpdateLCD()

// 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();}

// 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
double WidthToDistance(double); // Returns the distance from the camera depending on the width
void SquareSign(float*); // Squares the argument sent to it, preserving the sign


/* Our main (and this year, only) class. Don't worry about the word
"class" as we won't do much Object-Oriented stuff, but in the layout
provided to use for the robot code, we must have this one class. The
code for the robot goes here, along with all variables we need. It has
always been called "SimpleTracker" for whatever reason". */
class SimpleTracker : public SimpleRobot // Just ignore this, essentially. Treat it as "int main(void)" for simplicity.
{
    // "Global" variables are declared here.
   Joystick *leftStick; // Left driver joystick
   Joystick *rightStick; // Right driver joystick
   Joystick *speedStick; // Third driver joystick
    Jaguar *frontMotor; // Front-left drive motor
    Jaguar *rearMotor; // Rear-left drive motor
    Victor *frontSteer; // Front steering motor
    Victor *rearSteer; // Rear steering motor
    Jaguar *topShooter; // Top shooting roller
    Jaguar *bottomShooter; // Bottom shooting roller
    Encoder *frontSteeringEncoder; // Front steering encoder
    Encoder *rearSteeringEncoder; // Rear steering encoder
    Encoder *driveEncoder; // Drive wheel encoder
    Gyro *steeringGyro; // Steering gyro
    HSLImage *cameraImage; // Hue/Saturation/Luminence image from camera
    BinaryImage *binaryImage; // The object used to store and edit the filtered image
    Image *imaqImage; // Stores the image in a format that the built-in imaq functions can use
    Relay *magazineBelt; // Magazine conveyor
    Relay *magazineBelt2; // Second Motor for the Magazine
    Relay *lateralCollector; // Lateral collector belts
    Relay *transverseCollector; // Transverse collector belts
    Relay *lateralCollector2; // The supplementry motor for the lateral collector belts
    DigitalInput *driveWheelEncoder; // Light sensor for drive wheel encoding
    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
    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
    double maxTargetWidth; // Width of clearest target in view
    double maxTargetHeight; // Height of clearest target in view
   
    /* This "public" means that these variables are available outside of
    this class...but we only have one, so it essentially does nothing.
    "SimpleTracker(void)" is called a constructor. Again, don't worry about
    the details. This is where our object-based variables (hardware components)
    are initialized. This means we assign them a particular port. */ 
    public:
   SimpleTracker(void)
   {
        // Assign object pointers here.     
        frontMotor = new Jaguar(FRONTMOTORPORT); // Driving Motors
        rearMotor = new Jaguar(REARMOTORPORT);
//       
//        frontSteer = new Victor(FRONTSTEERINGMOTORPORT); // Steering Motors
//        rearSteer = new Victor(REARSTEERINGMOTORPORT);
       
        topShooter = new Jaguar(TOPSHOOTINGMOTORPORT); // Shooter motors
        bottomShooter = new Jaguar(BOTTOMSHOOTINGMOTORPORT);
 
        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
//       
//        magazineBelt = new Relay(MAGAZINERELAYPORT); // Relays
//        magazineBelt2 = new Relay(MAGAZINERELAY2PORT);
//        lateralCollector = new Relay(LATERALCOLLECTORPORT);
//        transverseCollector = new Relay(TRANSVERSECOLLECTORPORT);
//        lateralCollector2 = new Relay(LATERALCOLLERTOR2PORT);
//       
        armStartSensor = new DigitalInput(ARMSTARTPOSITIONPORT); // Bridge manipulator light sensors
        armHitSensor = new DigitalInput(ARMHITPOSITIONPORT);
       
        armDownSwitch = new DigitalInput(ARMDOWNSWITCHPORT); // The bridge manipulator limit switches
        armUpSwitch = new DigitalInput(ARMUPSWITCHPORT);
       
        autonomousSwitchOne = new DigitalInput(AUTONOMOUSSWITCHPORTONE); // The autonomous toggle switches
        autonomousSwitchTwo = new DigitalInput(AUTONOMOUSSWITCHPORTTWO);
       
        frontDriveServo = new Servo(FRONTSERVOPORT); // Gear shifting servos
        rearDriveServo = new Servo(REARSERVOPORT);
        armEngageServo = new Servo(ARMENGAGESERVOPORT); // Servo to engage bridge manipulator
       
        // Initialize encoders
        /*frontSteeringEncoder->Reset();
        frontSteeringEncoder->Start();
        rearSteeringEncoder->Reset();
        rearSteeringEncoder->Start();*/
        driveEncoder->Reset();
        driveEncoder->Start();
       
//        // Initialize gyro
//        steeringGyro->Reset();
//        steeringGyro->SetSensitivity(GYROSENSITIVITY); // Changes the sensitivity of the gyro
//               
        // Enable camera and wait for a bit
        AxisCamera &camera = AxisCamera::GetInstance();
        Wait(8.0);
       
        // Set the camera's preferences
        camera.WriteResolution(camera.kResolution_320x240); // Make sure this resolution matches the width and height macros
        camera.WriteCompression(0);
        camera.WriteBrightness(50);
        camera.WriteColorLevel(256);
   }
   
    /* 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 speedx = 0.0f, speedy = 0.0f; // The x- and y-positions of the third speed-control shift
      float x, y; // The average x- and y-positions between the two 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; // Allow different angles for the front and back sides, for use in different steering modes
      int waitForSteering; // Should the drive motors wait for the wheels to finish turning?
      int frontFinished, rearFinished; // Store whether each driving side has finished turning
      int steeringMode = SWERVEMODE; // Store the current mode of steering (0 = strafe, 1 = monster, 2 = car)
      int currentButtonState = 0, previousButtonState = 0; // Store the current and previous states of the manual alignment buttons
      double distance = 0.0; // The temp variable to display distance, used to test the distance formula that is in progress
      int dividerCount = 0; // Count the number of times the loop has been executed
      int driveWheelEncoderValue; // Distance travelled by robot
      int targetFound; // Has the target been centred?
      int targetMode = 0; // Are we in targetting mode?
      float frontGearSetting = LOWGEARPOSITION; // Store the angle setting for the gear-shifting servos
      float rearGearSetting = LOWGEARPOSITION;
      float armPosition = LOWGEARPOSITION; // Stores the angle setting for the bridge-manipulator servo
      int armStage = 0; // Clean up for arm engagement
      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 fireReady = 0; // Are the rollers up to firing speed?
      float fireSpeed = 0.0f; // Firing speed for shooter based on camera distance
      int brakeValue = -1; // Encoder value to maintain when in brake mode
   
      // Acquire reference to axis camera
      AxisCamera &camera = AxisCamera::GetInstance();
      
      // Reset the drive and shooter encoders
      driveEncoder->Reset();
      driveEncoder->Start();
      
      // This code is run repeatedly so long as the robot is in teleop mode.
      while (IsOperatorControl())
      {
         /* This is a fancy, convoluted way of telling the operating-system
         software that the code hasn't frozen. */
         GetWatchdog().Feed();
         
         /* Message displays */
         CLEARMESSAGE;
         DISPLAYPRINTF(1, "Distance: %0.2f", distance) // Distance to target, in feet
         DISPLAYPRINTF(2, "Encoder: %d", driveEncoder->Get()) // Encoder value from drive wheel
         DISPLAYPRINTF(3, "Top Shooter: %0.2f", topShooter->Get()) // Display the top shooter speed reading
         DISPLAYPRINTF(4, "Bottom Shooter: %0.2f", bottomShooter->Get()) // Display the bottom shooter speed reading
         if(targetFound != -1) DISPLAYPRINTF(5, "Alignment: %d", targetFound - (int)IMAGEWIDTH / 2) // Display the position of the topmost target
         else DISPLAYPRINTF(5, "Target not found") // Display that the target is not visible
         
         // Get joystick positions and square, preserving sign, to reduce sensitivity
         leftx = STICKDIRECTIONX * leftStick->GetRawAxis(1); SquareSign(&leftx);
         lefty = leftStick->GetRawAxis(2); SquareSign(&lefty);
         rightx = STICKDIRECTIONX * rightStick->GetRawAxis(1); SquareSign(&rightx);
         righty = rightStick->GetRawAxis(2); SquareSign(&righty);
         speedx = speedStick->GetRawAxis(1);
         speedy = speedStick->GetRawAxis(2);
         
         // Prevent back-rolling
         if(speedy > 0) speedy = 0.0f;
         
         // Update the encoder speed every few times through this loop
         if(dividerCount >= ENCODERFREQUENCY)
         {
            // Get the distance and display it
            targetFound = GetProcessedImage(camera);
            distance = WidthToDistance(maxTargetWidth);
         }
         else dividerCount++;
//         
         // 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++;
//         
//         /* Teleop code here. */
//         // Check  drive mode buttons
//         if(SWERVEMODEBUTTON) steeringMode = SWERVEMODE;
//         else if(MONSTERMODEBUTTON) steeringMode = MONSTERMODE;
//         else if(CARMODEBUTTON) steeringMode = CARMODE;
//         
         // Check shooter buttons
         if(FIRSTFIREBUTTON) fireReady = SetRollerSpeed(FIRESPEEDONE), preFire = 1;
         else if(SECONDFIREBUTTON) fireReady = SetRollerSpeed(FIRESPEEDTWO), preFire = 1;
         else if(THIRDFIREBUTTON) fireReady = SetRollerSpeed(FIRESPEEDTHREE), preFire = 1;
         else preFire = 0, fireReady = 1;
//         
//         // Enter or exit targeting mode
//         if(TARGETMODEENABLE) targetMode = 1, magazineReverseCount = MAGAZINEREVERSETIME;
//         else if(TARGETMODEDISABLE) targetMode = 0;
         
//         // Reset the gyro reading
//         if(GYRORESETBUTTON) steeringGyro->Reset();
         
         // Check gear shifting buttons
         if(HIGHGEARBUTTON && !targetMode) frontGearSetting = rearGearSetting = HIGHGEARPOSITION, armPosition = LOWGEARPOSITION;
         else if(LOWGEARBUTTON || targetMode) frontGearSetting = rearGearSetting = LOWGEARPOSITION, armPosition = LOWGEARPOSITION;
         else if(NEUTRALGEARBUTTON && !targetMode) frontGearSetting = LOWGEARPOSITION, rearGearSetting = NEUTRALGEARPOSITION, armPosition = LOWGEARPOSITION;
         else if(BRIDGEMANIPULATEBUTTON && !targetMode && armPosition != ARMENGAGEPOSITION) frontGearSetting = LOWGEARPOSITION, rearGearSetting = NEUTRALGEARPOSITION, armPosition = ARMENGAGEPOSITION, servoPauseCount = SERVOPAUSETIME, armStage = 0;
         else if(ARMUNWINDBUTTON && !targetMode) frontGearSetting = LOWGEARPOSITION, rearGearSetting = NEUTRALGEARPOSITION, armPosition = ARMENGAGEPOSITION, servoPauseCount = -1, armStage = 5;
         
         // 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 an invalid value in the brake value
         if(!BRAKEMODEBUTTON) brakeValue = -1;
//         
//         // Check belt buttons
//         if(MAGAZINEENABLEBUTTON) magazineBelt->Set(Relay::kForward), magazineBelt2->Set(Relay::kForward), speedy = 0.7, magazineReverseCount = MAGAZINEREVERSETIME;
//         else if(SOLOMAGAZINEBUTTON && fireReady)
//         {
//            if(magazineAlternator > MAGAZINEALTERNATORLIMIT / 8) magazineBelt->Set(Relay::kForward), magazineBelt2->Set(Relay::kForward);
//            else magazineBelt->Set(Relay::kOff), magazineBelt2->Set(Relay::kOff);
//         }
//         else if(magazineReverseCount > 0 && !targetMode) magazineBelt->Set(Relay::kReverse), magazineBelt2->Set(Relay::kReverse), magazineReverseCount--;
//         else magazineBelt->Set(Relay::kOff), magazineBelt2->Set(Relay::kOff);
//         if(LATERALCOLLECTORBUTTON) lateralCollector->Set(Relay::kReverse), lateralCollector2->Set(Relay::kReverse);
//         else if(LATERALREVERSEBUTTON) magazineBelt->Set(Relay::kReverse), magazineBelt2->Set(Relay::kReverse), lateralCollector->Set(Relay::kForward), lateralCollector2->Set(Relay::kForward);
//         else lateralCollector->Set(Relay::kOff), lateralCollector2->Set(Relay::kOff);
//         if(TRANSVERSECOLLECTORBUTTON) transverseCollector->Set(Relay::kReverse);
//         else if(TRANSVERSEREVERSEBUTTON) transverseCollector->Set(Relay::kForward);
//         else transverseCollector->Set(Relay::kOff);
//         
         // If the total y-deflection for each joystick is below the threshold, we shouldn't count it at all
         if(Abs(lefty) < JOYSTICKDEADBANDY) lefty = 0.0f;
         if(Abs(righty) < JOYSTICKDEADBANDY) righty = 0.0f;
            
         // Average the joystick positions
         x = (leftx + rightx) / 2.0f;
         y = (lefty + righty) / 2.0f;
//                  
//         // Store the currentButtonState from the previous iteration
//         previousButtonState = currentButtonState;
//         
         // Update the camera image
//         targetFound = FindTarget(camera, 0);
//         
//         // If the manual-alignment buttons are pressed
//         if(MANUALALIGNENABLEBUTTON)
//         {
//            currentButtonState = 1;
//            if(MANUALALIGNLEFTREAR) rearSteer->Set(-0.6f);
//            else if(MANUALALIGNRIGHTREAR) rearSteer->Set(0.6f);
//            else rearSteer->Set(0);
//               
//            if(MANUALALIGNLEFTFRONT) frontSteer->Set(-0.7f);
//            else if(MANUALALIGNRIGHTFRONT)frontSteer->Set(0.7f);
//            else frontSteer->Set(0);
//         }
//         
         // Sets the speed dependent on the distance to top target
         if(AUTOSHOOTBYDISTANCE) SetRollerSpeed(LINEARDISTANCEKT * distance + CONSTANTDISTANCEKT);
//         
         // If we are manipulating the bridge
         /*else*/ if(armPosition == ARMENGAGEPOSITION && servoPauseCount == 0)
         {
            // Drive rear wheels off of joystick
            if(Abs(x) < JOYSTICKDEADBANDX) frontMotor->Set(lefty);
            
            if(!BRIDGEMANIPULATEBUTTON) armStage = 4;
            
            /// Drive winch until first light sensor is triggered (arm is in primed position)
            if(!armStartSensor->Get() && armStage == 0) rearMotor->Set(0.9f);
            else if(armStage == 0) armStage = 1;
            
            // Stop winch until the light sensor on the bridge dector is triggered (bridge is hit)
            if(!armHitSensor->Get() && armStage == 1) rearMotor->Set(0.0f);
            else if(armStage == 1) armStage = 2;
            
            // Continue to run winch until bottom switch is hit
            if(!armDownSwitch->Get() && armStage == 2) rearMotor->Set(0.9f);
            else if(armStage == 2) armStage = 3;
               
            // Wait for manipulator button to be released
            if(BRIDGEMANIPULATEBUTTON && armStage == 3) rearMotor->Set(0.0f);
            else if(armStage == 3) armStage = 4;
            
            // Return arm
            if(armUpSwitch->Get() && armStage == 4) rearMotor->Set(-0.6f);
            else if(armStage == 4) armStage = 5;
            
            // Disengage arm, set drive units to low gear, and stop winch motor; reset the armStage for next time
            if(armStage == 5)
            {
               rearGearSetting = LOWGEARPOSITION;
               frontGearSetting = LOWGEARPOSITION;
               armPosition = LOWGEARPOSITION;
               
               rearMotor->Set(0.0f);
            }
         }
         
         // The lock button sets the wheels straight sideways for aligning on the bridge
         else if(LOCKMODEBUTTON)
         {   
            TrackFrontSteering(PULSESPER90FRONT);
            TrackRearSteering(PULSESPER90REAR);
            frontMotor->Set(y);
            rearMotor->Set(y);
         }
         
         // The brake button holds us still
         else if(BRAKEMODEBUTTON)
         {
            // Store value to hold
            if(brakeValue == -1) brakeValue = driveWheelEncoderValue;
            
            // Allow the robot to move still based on driver input
            if(y > JOYSTICKDEADBANDY) brakeValue++;
            else if(y < -JOYSTICKDEADBANDY) brakeValue--;
            
            TrackDrive(brakeValue);
         }
            
         // The pivot mode separate from the rest of the drive
         else if(PIVOTMODEBUTTON)
         {
            TrackFrontSteering(0);
            TrackRearSteering(PULSESPER90REAR);
            frontMotor->Set(0);
            rearMotor->Set(righty);
         }
         
         // If we are in targetting mode
         else if(targetMode)
         {
            // Track the target
            targetFound = FindTarget(camera, 1);
            
            // If it is visible, prepare to shoot
            if(targetFound)
            {
               // Get the distance to the target
               distance = WidthToDistance(maxTargetWidth);
               
               // Once the target is in view, set the roller speed based on distance
               if(distance > 9) fireSpeed = LINEARDISTANCEKT * distance + CONSTANTDISTANCEKT;
               else fireSpeed = LINEARDISTANCEKB * distance + CONSTANTDISTANCEKB;
               if(fireSpeed < 0.0f) fireSpeed = 0.0f;
               fireReady = SetRollerSpeed(fireSpeed);
                  
               // Reverse the magazine for a bit
               if(magazineReverseCount > 0)
               {
                  magazineBelt->Set(Relay::kReverse), magazineBelt2->Set(Relay::kReverse);
                  magazineReverseCount--;
               }
               
               // Then, move it forward for shooting
               else if(SHOOTBUTTON && fireReady) magazineBelt->Set(Relay::kForward), magazineBelt2->Set(Relay::kForward);
            }
            
            // Otherwise, wait
            else
            {
               distance = 0.0f;
               topShooter->Set(0.0f);
               bottomShooter->Set(0.0f);
               
               // Reverse the magazine for a bit
               if(magazineReverseCount > 0)
               {
                  magazineBelt->Set(Relay::kReverse), magazineBelt2->Set(Relay::kReverse);
                  magazineReverseCount--;
               }
               
               // Then, stop it
               else magazineBelt->Set(Relay::kOff), magazineBelt2->Set(Relay::kOff);
            }
         }
                  
         // Otherwise, if the deflection left or right is less than a certain amount, we simply drive tank-style
         // Abs simply ensures that x is positive so that we don't need two separate if-statements
         else if(Abs(x) < JOYSTICKDEADBANDX)
         {
            // Tank drive
            // Set the drive motors according to their respective joysticks
            frontMotor->Set(lefty);
            rearMotor->Set(righty);
            
            if(!preFire && !AUTOSHOOTBYDISTANCE)
            {
               topShooter->Set(speedy); // Set roller speeds manually
               bottomShooter->Set(-speedy);
            }
            currentButtonState = 0;
            
//            TrackFrontSteering(0);
//            TrackRearSteering(0);
         }
//         
//         else
//         {
//            // Swerve drive
//            // Determine the total deflection of the joysticks to use as the speed setting
//            swerveSpeed = sqrt(x*x + y*y); // a^2 + b^2 = c^2
//            swerveAngle = -Abs(atan(x/y)); // x/y = tan(theta)
//            swerveAngle = swerveAngle * 180 / Pi; // 1 degree = 180/Pi radians
//            swerveAngle = swerveAngle / 90.0f; // Convert from degrees to a percentage of 90 degrees
//            
//            // Set roller speeds manually
//            if(!preFire)
//            {
//               topShooter->Set(speedy);
//               bottomShooter->Set(-speedy);
//            }
//            
//            /* If the y-component is zero, we should try to move straight left or right. Therefore, we
//             * need to wait for the wheels to finish turning before allowing the drive motors to move. The
//             * angle, for some reason, also needs to be inverted when moving straight left or right. */
//            if(Abs(y) < JOYSTICKDEADBANDY)
//            {
//               waitForSteering = 1;
//               if(x < 0.0f) swerveAngle = 1.0f, swerveSpeed = -swerveSpeed;
//               else if(x > 0.0f) swerveAngle = -1.0f;
//            }
//            else waitForSteering = 0;
//                        
//            // Determine the quadrant of the joystick and convert the speed or angle appropriately
//            // Quadrant one is assumed, except for the following conditions
//            if(x < 0 && y >= 0) swerveAngle = -swerveAngle; // Quadrant two
//            else if(x < 0 && y < 0) swerveSpeed = -swerveSpeed; // Quadrant three
//            else if(x >= 0 && y < 0) // Quadrant Four
//            {
//               swerveSpeed = -swerveSpeed;
//               swerveAngle = -swerveAngle;               
//            }
//            
//            // If we are in monster mode and travelling faster than 80% max speed, we switch to car automatically
//            if(Abs(swerveSpeed) > 1.2f && steeringMode == MONSTERMODE) steeringMode = CARMODE;
//         
//            // Put steer-angle and drive-direction changes here for the other steering modes (monster, car, auto-monster, etc.)
//            if(steeringMode == MONSTERMODE)
//            {
//               if(swerveSpeed <= 0) swerveAngleFront = -0.75f * swerveAngle, swerveAngleRear = 0.75f * swerveAngle;
//               else swerveAngleFront = 0.75f * swerveAngle, swerveAngleRear = -0.75f * swerveAngle;
//            }
//            else if(steeringMode == CARMODE)
//            {
//               if(swerveSpeed <= 0) swerveAngleFront = -0.5f * swerveAngle, swerveAngleRear = 0.0f;
//               else swerveAngleRear = -0.5f * swerveAngle, swerveAngleFront = 0.0f;
//            }
//            
//            else
//            {
//               swerveAngleFront = -swerveAngle;
//               swerveAngleRear = -swerveAngle;
//            }
//            
//             // Convert front and rear angle percentages into pulses
//             swerveAngleFront *= PULSESPER90FRONT;
//             swerveAngleRear *= PULSESPER90REAR;
          
//            /* Call pre-written functions that will use the encoder to get the wheels to gradually approach the desired
//             * position. This function will return zero when the steering is finished.*/
//            frontFinished = TrackFrontSteering((int)swerveAngleFront);
//            rearFinished = TrackRearSteering((int)swerveAngleRear);
//            
//            // If the steering has finished, set the drive motors to move
//            if((frontFinished == 0 && rearFinished == 0) || !waitForSteering)
//            {
//               frontMotor->Set(swerveSpeed);
//               rearMotor->Set(swerveSpeed);
//            }
//            
//            currentButtonState = 0;
//         }
//         
//         // If the manual alignment buttons have JUST been released, reset the encoders again
//         if(previousButtonState == 1 && currentButtonState == 0)
//         {
//            frontSteeringEncoder->Reset();
//            frontSteeringEncoder->Start();
//            rearSteeringEncoder->Reset();
//            rearSteeringEncoder->Start();
//            frontSteer->Set(0);
//            rearSteer->Set(0);
//         }
        }
   } // 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();
      else if(firstSwitch && !secondSwitch) AutonomousTwo();
      else if(secondSwitch && !firstSwitch) AutonomousThree();
      else
      {
         CLEARMESSAGE;
         DISPLAYSTRING(1, "No autonomous switch");
      }
   } // End of Autonomous
   
   /* This function handles the complicated problem of quickly, but accurately, moving the
    * front wheels to their correct position, as passed 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 < STEERINGDEADBAND) speed = 0.0f;
      else if(frontSteeringEncoderValue < targetPosition) speed = (float)remainingSteeringDistance / (float)STEERINGAPPROACHDISTANCE;
      else speed = (float)-remainingSteeringDistance / (float)STEERINGAPPROACHDISTANCE;
      
      // 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 < STEERINGDEADBAND) speed = 0.0f;
      else if(rearSteeringEncoderValue < targetPosition) speed = (float)remainingSteeringDistance / (float)STEERINGAPPROACHDISTANCE;
      else speed = (float)-remainingSteeringDistance / (float)STEERINGAPPROACHDISTANCE;
      
      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;
   }
   
   /* Take the camera image and process it, returning the x-position of the largest particle. This should represent the
   horizontal centre of the most in-focus target. Comparing this value to the image width (320) should allow us to
   centre the image by rotating the robot. If the targets are out of view, -1 is returned. */
   int GetProcessedImage(AxisCamera &camera)
   {
      register int i, j, k;
      int numberOfParticles, highestParticle, indices[4];
      double maxHeight = -1, width, height, horizontalPosition, verticalPosition;
      double targetAreas[4];
      
      // Set the target areas to a sentinel value to detect empty positions
      for(i = 0; i < 4; i++) targetAreas[i] = -1.0f;
      
      // Save the image as a BinaryImage Object
      cameraImage = camera.GetImage();
      //binaryImage = cameraImage->ThresholdHSLHUEMIN, HUEMAX, SATURATIONMIN, SATURATIONMAX, LUMINANCEMIN, LUMINANCEMAX);
      binaryImage = cameraImage->ThresholdRGB(REDMIN, REDMAX, GREENMIN, GREENMAX, BLUEMIN, BLUEMAX);
      imaqImage = binaryImage->GetImaqImage();
      
      // Perform the Convex Hull operation
      imaqConvexHull(imaqImage, imaqImage, 1);
      
      // Count the number of distinct sections remaining
      imaqCountParticles(imaqImage, 2, &numberOfParticles);
            
       // Save image to the cRIO's memory (enable for debugging purposes only)
       //cameraImage->Write("camIm4.jpeg");
       //binaryImage->Write("binIm4.bmp");
      
       // For each particle found
       for(i = 0; i < numberOfParticles; i++)
       {
          // Get the width and height of the given particle
          imaqMeasureParticle(imaqImage, i, 0, IMAQ_MT_BOUNDING_RECT_WIDTH, &width);
          imaqMeasureParticle(imaqImage, i, 0, IMAQ_MT_BOUNDING_RECT_HEIGHT, &height);
          
          // Search through array of the four largest particles
          // Add the current one to the array if it overcomes any already there
          // Also store the index of each
          for(j = 0; j < 4; j++)
          {
             // If we find an empty spot, just fill it
             if(targetAreas[j] == -1.0f)
             {
                targetAreas[j] = width * height;
                indices[j] = i;
                break;
             }
             
             // If the current particle exceeds one already in the array
             if(width * height > targetAreas[j])
             {
                // Move all other entries back one spot
                for(k = 3; k > j; k--)
                {
                   targetAreas[k] = targetAreas[k-1];
                   indices[k] = indices[k-1];
                }
                
                // Store the current one
                targetAreas[j] = width * height;
                indices[j] = i;
                break;
             }
          }
       }
      
       // Search through the recorded target candidates
       for(i = 0; i < 4; i++)
       {
          // Skip any empty spots
          if(targetAreas[i] == -1) continue;
          
          // Find the specific target's vertical position
          imaqMeasureParticle(imaqImage, indices[i], 0, IMAQ_MT_CENTER_OF_MASS_Y, &verticalPosition);
          
          // If it's current position is greater than the largest recorded so far, store it as the highest
          if(verticalPosition > maxHeight)
          {
             maxHeight = verticalPosition;
             highestParticle = indices[i];
          }
       }
      
       // Find the x-position of the centre of the top target, with a few assumptions made
       imaqMeasureParticle(imaqImage, highestParticle, 0, IMAQ_MT_CENTER_OF_MASS_X, &horizontalPosition);
       imaqMeasureParticle(imaqImage, highestParticle, 0, IMAQ_MT_BOUNDING_RECT_WIDTH, &maxTargetWidth);
      
       // Free memory
       delete binaryImage;
       delete cameraImage;
      
       // If the largest particle is outside of the threshold, return -1 to indicate that the targets aren't visible
       if(targetAreas[highestParticle] < MINIMUMAREA) return -1;
      
       // Return this x-position
       return (int)horizontalPosition;
   }
   
   // Maintains a specific speed for the shooting rollers. Returns true when rollers are at that speed
   int SetRollerSpeed(float targetSpeed)
   {
      /* Insert time delay */
      
      topShooter->Set(-targetSpeed * ROLLERRATIO);
      bottomShooter->Set(targetSpeed);
      
      return 1;
   }
   
   /* Track the target with the camera if in view, otherwise rotate to gryo angle zero. Return true if aimed at target
    * and false otherwise. */
   int FindTarget(AxisCamera &camera, int track)
   {
      static int divider = 0, imageCenter;
      float gyroAngle, correctionSpeed;
      
//      // Get the angle from the gyro
//      gyroAngle = steeringGyro->GetAngle();
      
      // Every few times through the loop, update the camera image information
      if(divider >= CAMERAFREQUENCY)
      {
         imageCenter = GetProcessedImage(camera);
         divider = 0;
      }
      else divider++;
      
      if(!track) return (imageCenter != -1);
      
      // Turn the wheels to allow rotation in an arc
      TrackFrontSteering(0);
      TrackRearSteering(PULSESPER90REAR);
      
      // If no image is found, steer based on the gyro angle
      if(imageCenter == -1)
      {
//         correctionSpeed = Abs(gyroAngle) / GYROAPPROACHDISTANCE * 0.3f + 0.1f;
//         if(correctionSpeed > 0.4f) correctionSpeed = 0.4f;
//         
//         if(gyroAngle - GYRODEADBANDANGLE > 0.0f) correctionSpeed *= -1.0f;
//         else if(gyroAngle + GYRODEADBANDANGLE >= 0.0f) correctionSpeed = 0.0f;
//         
//         rearMotor->Set(correctionSpeed);
      }
      
      // Otherwise, move to centre the target from the camera image
      else
      {
         DISPLAYINTEGER(5, imageCenter);
         correctionSpeed = Abs((float)imageCenter - IMAGEWIDTH / 2.0f) / (float)IMAGEAPPROACHDISTANCE * 0.6f;
         if(correctionSpeed > 0.6f) correctionSpeed = 0.6f;
         
         // Offset the center of the image according to the offset of the camera from the robot's center
         imageCenter += IMAGEOFFSET;
         
         if(imageCenter - IMAGEDEADBAND > (int)IMAGEWIDTH / 2) correctionSpeed *= -1.0f;
         else if(imageCenter + IMAGEDEADBAND >= (int)IMAGEWIDTH / 2) correctionSpeed = 0.0f;
         
         rearMotor->Set(correctionSpeed);
      }
      
      // Return true if the image is visible and false otherwise
      if(imageCenter != -1) return 1;
      else return 0;
   }
   
   // This function attempts to maintain a specific encoder value on the drive block
   float TrackDrive(int targetPosition)
   {
      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;
      if(distance > 1.0f) distance = 1.0f;
      
      // Move forward or backward at a proportional pace until within a deadband
      if(currentPosition < targetPosition - DRIVEDEADBAND) frontMotor->Set(distance), rearMotor->Set(distance);
      else if(currentPosition > targetPosition + DRIVEDEADBAND) frontMotor->Set(-distance), rearMotor->Set(-distance);
      else frontMotor->Set(0.0f), rearMotor->Set(0.0f);
      
      // Return motor speed
      return frontMotor->Get();
   }
   
   // Code for our first possible autonomous mode
   void AutonomousOne(void)
   {
      // Variables
      float gyroAngle = 0; // Current angle from home as read by the gyro
      int encoderValue = 0; // Stores the encoder distance traveled by the robot
      int armStage = 0; // Current stage for bridge manipulator
      int autoPauseCount = SERVOPAUSETIME; // The amount of time to wait for the servos to reach their target position
      int magazinePauseCount = MAGAZINEPAUSETIME; // The amount of time to wait for the rollers to reach speed
      
      // Acquire reference to axis camera
      AxisCamera &camera = AxisCamera::GetInstance();
      
      // Reset the drive encoder and gyro
//      driveEncoder->Reset();
//      driveEncoder->Start();
//      steeringGyro->Reset();
      
      // This code is run repeatedly so long as the robot is in autonomous mode (lasting 15 seconds).
      while (IsAutonomous())
      {
         /* Tell the system that the code hasn't frozen. */
         GetWatchdog().Feed();
         
         /* Autonomous code here. */
//         // Get the gyro angle
//         gyroAngle = steeringGyro->GetAngle();
//         
//         // Get the pulse count from the drive encoder
//         encoderValue = AbsI(driveEncoder->Get());
                  
         // Message displays
         CLEARMESSAGE;
         DISPLAYFLOAT(1, gyroAngle);  // The float value of the gyro angle
         DISPLAYINTEGER(2, encoderValue); // Current position of encoder
         DISPLAYINTEGER(3, armStage);
         
         /* Simple, straight-movement autonomous */
         // Turn the rear wheels left to correct if we are off to the left
         // Otherwise, turn the wheels right to correct if we are off to the right
         // Otherwise, move straight
//         if(gyroAngle + GYRODEADBANDANGLE < 0.0f) TrackRearSteering(PULSESPER90REAR / 8);
//         else if(gyroAngle - GYRODEADBANDANGLE > 0.0f) TrackRearSteering(-PULSESPER90REAR / 8);
//         else TrackRearSteering(0);
         
         // Keep the front wheels straight and drive forward slowly
//         TrackFrontSteering(0);
                  
         // Until we have hit the bridge, move forward
         if(armStage < 2)
         {
            // Set gears to front drive and arm engagement
            frontDriveServo->SetAngle(LOWGEARPOSITION);
            rearDriveServo->SetAngle(NEUTRALGEARPOSITION);
            armEngageServo->SetAngle(ARMENGAGEPOSITION);
            
            // Give the servos time to get into place
            if(autoPauseCount > 0)
            {
               autoPauseCount--;
               continue;
            }
            
            // Drive
            frontMotor->Set(0.4);
            
            /// Drive winch until first light sensor is triggered (arm is in primed position)
            if(!armStartSensor->Get() && armStage == 0) rearMotor->Set(0.6);
            else if(armStage == 0) armStage = 1;
                        
            // Stop winch until the light sensor on the bridge dector is triggered (bridge is hit) or we travel too far (probably missing)
            if((!armHitSensor->Get() || encoderValue > AUTONOMOUSDISTANCE) && armStage == 1) rearMotor->Set(0.0f);
            else if(armStage == 1) armStage = 2;
         }
         
         // We have hit the bridge now
         else if(armStage < 4)
         {
            // Set gears to front drive and arm engagement
            frontDriveServo->SetAngle(LOWGEARPOSITION);
            rearDriveServo->SetAngle(NEUTRALGEARPOSITION);
            armEngageServo->SetAngle(ARMENGAGEPOSITION);
            
            // Stop
            frontMotor->Set(0.0f);
            
            // Continue to run winch until bottom switch is hit
            if(!armDownSwitch->Get() && armStage == 2) rearMotor->Set(0.9f);
            else if(armStage == 2) armStage = 3;
            
            // Return arm to home position
            if(armUpSwitch->Get() && armStage == 3) rearMotor->Set(-0.6f);
            else if(armStage == 3) armStage = 4;
         }
         
         // Bridge tipping is complete
         else
         {
            // Return gears to low
            frontDriveServo->Set(LOWGEARPOSITION);
            rearDriveServo->Set(LOWGEARPOSITION);
            armEngageServo->Set(LOWGEARPOSITION);
            
            // Spin the rollers at the correct launching speed for the top target
            SetRollerSpeed(FINALAUTONOMOUSDISTANCE * LINEARDISTANCEKT + CONSTANTDISTANCEKT);
            
            // Wait for the rollers to get up to speed, the launch the balls
            if(magazinePauseCount > 0) magazinePauseCount--;
            else magazineBelt->Set(Relay::kForward), magazineBelt2->Set(Relay::kForward);
               
            // Stop winch and drive
            frontMotor->Set(0.0f);
            rearMotor->Set(0.0f);
            
            // Code to move elsewhere or shoot goes here
         }
      } // End of IsAutonomous
   }
   
   // Second of the possible autonomous modes
   void AutonomousTwo(void)
   {
      // Variables
      int rollerPause = MAGAZINEPAUSETIME; // The time to wait for the rollers to spin up before enabling the magazine
      
      while(IsAutonomous())
      {
         /* Tell the system that the code hasn't frozen. */
         GetWatchdog().Feed();
         
         // Wait for the rollers to spin up before engaging the magazine belts to fire
         if(rollerPause > 0) rollerPause--;
         else magazineBelt->Set(Relay::kForward), magazineBelt2->Set(Relay::kForward);
         
         // Spin up the rollers to the preset speed for the key shot
         SetRollerSpeed(FIRESPEEDTWO);
      }
   }
   
   // Third of the possible autonomous modes
   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 gyroAngle = 0; // Current angle from home as read by the gyro
      int encoderValue = 0; // Stores the encoder distance traveled by the robot
      int armStage = 0; // Current stage for bridge manipulator
      
      while(IsAutonomous())
      {
         /* Tell the system that the code hasn't frozen. */
         GetWatchdog().Feed();
         
         // 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(FIRESPEEDTWO);
            rollerPause--;
         }
         
         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);
         }
         
         // Proceed to next stage
         else
         {
            armStage = -1;
            SetRollerSpeed(0.0f);
         }
         
         // Get the gyro angle
//         gyroAngle = steeringGyro->GetAngle();
//         
//         // Get the pulse count from the drive encoder
//         encoderValue = AbsI(driveEncoder->Get());
                  
         /* Simple, straight-movement autonomous */
         // Turn the rear wheels left to correct if we are off to the left
         // Otherwise, turn the wheels right to correct if we are off to the right
         // Otherwise, move straight
//         if(gyroAngle + GYRODEADBANDANGLE < 0.0f) TrackRearSteering(PULSESPER90REAR / 8);
//         else if(gyroAngle - GYRODEADBANDANGLE > 0.0f) TrackRearSteering(-PULSESPER90REAR / 8);
//         else TrackRearSteering(0);
         
         // Keep the front wheels straight and drive forward slowly
//         TrackFrontSteering(0);

         // Wait for shooting to finish before continuing
         if(armStage == -1) continue;
         
         // Until we have hit the bridge, move forward
         if(armStage < 2)
         {
            // Set gears to front drive and arm engagement
            frontDriveServo->SetAngle(LOWGEARPOSITION);
            rearDriveServo->SetAngle(NEUTRALGEARPOSITION);
            armEngageServo->SetAngle(ARMENGAGEPOSITION);
            
            // Drive
            frontMotor->Set(0.4);
            
            /// Drive winch until first light sensor is triggered (arm is in primed position)
            if(!armStartSensor->Get() && armStage == 0) rearMotor->Set(0.6);
            else if(armStage == 0) armStage = 1;
                        
            // Stop winch until the light sensor on the bridge dector is triggered (bridge is hit) or we travel too far (probably missing)
            if((!armHitSensor->Get() || encoderValue > AUTONOMOUSDISTANCE) && armStage == 1) rearMotor->Set(0.0f);
            else if(armStage == 1) armStage = 2;
         }
         
         // We have hit the bridge now
         else if(armStage < 4)
         {
            // Set gears to front drive and arm engagement
            frontDriveServo->SetAngle(LOWGEARPOSITION);
            rearDriveServo->SetAngle(NEUTRALGEARPOSITION);
            armEngageServo->SetAngle(ARMENGAGEPOSITION);
            
            // Stop
            frontMotor->Set(0.0f);
            
            // Continue to run winch until bottom switch is hit
            if(!armDownSwitch->Get() && armStage == 2) rearMotor->Set(0.9f);
            else if(armStage == 2) armStage = 3;
            
            // Return arm to home position
            if(armUpSwitch->Get() && armStage == 3) rearMotor->Set(-0.6f);
            else if(armStage == 3) armStage = 4;
         }
         
         // Bridge tipping is complete
         else
         {
            // Return gears to low
            frontDriveServo->Set(LOWGEARPOSITION);
            rearDriveServo->Set(LOWGEARPOSITION);
            armEngageServo->Set(LOWGEARPOSITION);
            
            // Stop winch and drive
            frontMotor->Set(0.0f);
            rearMotor->Set(0.0f);
         }
      }
   }
}; /* Semicolons are not used after functions, but classes are treated
more like datatypes, so they DO have a semicolon after them. To think some
people say that object-oriented makes sense :/ */

// 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;
}

// Converts the width of the rectangle to a distance in feet
double WidthToDistance(double x)
{
   double result; // The resut of the calculations
   
   result = GOALWIDTH * IMAGEWIDTH / x / 2;
   result = result / tan(HALFCAMERAVIEWINGANGLE * Pi / 180);
   
   return result;
}

// Square the argument sent to it, preserving the sign
void SquareSign(float *x)
{
   if((*x) < 0) *x = -pow((*x), 2);
   else *x = pow((*x), 2);
}

/* The entry point is FRC_UserProgram_StartupLibraryInit. This macro tells
the compiler to use the SimpleTracker class. There are a series of
"under-the-hood" operations so that the program starts in this class
instead of searching for int main(void). */
START_ROBOT_CLASS(SimpleTracker);

The header file:
Code:

/* 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 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 HIGHGEARBUTTON leftStick->GetRawButton(5)
#define LOWGEARBUTTON leftStick->GetRawButton(4)
#define NEUTRALGEARBUTTON leftStick->GetRawButton(3)
#define ARMUNWINDBUTTON speedStick->GetRawButton(8)

#define BRIDGEMANIPULATEBUTTON rightStick->GetRawButton(7)
#define PIVOTMODEBUTTON rightStick->GetRawButton(2)

#define GYRORESETBUTTON rightStick->GetRawButton(11)

#define AUTOSHOOTBYDISTANCE speedStick->GetRawButton(4)
#define SHOOTBUTTON (speedStick->GetRawButton(1) && (speedStick->GetRawButton(2) || speedStick->GetRawButton(3) || speedStick->GetRawButton(4) || speedStick->GetRawButton(5)))
#define SOLOMAGAZINEBUTTON speedStick->GetRawButton(16)
#define FIRSTFIREBUTTON speedStick->GetRawButton(2)
#define SECONDFIREBUTTON speedStick->GetRawButton(3)
#define THIRDFIREBUTTON speedStick->GetRawButton(5)
#define TARGETMODEENABLE speedStick->GetRawButton(6)
#define TARGETMODEDISABLE speedStick->GetRawButton(7)

#define MAGAZINEENABLEBUTTON speedStick->GetRawButton(11)
#define LATERALCOLLECTORBUTTON speedStick->GetRawButton(11)
#define TRANSVERSECOLLECTORBUTTON speedStick->GetRawButton(11)
#define LATERALREVERSEBUTTON speedStick->GetRawButton(10)
#define TRANSVERSEREVERSEBUTTON speedStick->GetRawButton(10)

// Port numbers
// Motors are on PWM ports
#define FRONTMOTORPORT 3
#define REARMOTORPORT 4
#define FRONTSTEERINGMOTORPORT 2
#define REARSTEERINGMOTORPORT 1
#define TOPSHOOTINGMOTORPORT 10
#define BOTTOMSHOOTINGMOTORPORT 9

// 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 13
#define DRIVEENCODERPORTB 14

// Light sensors for encoders are also digital inputs
#define ARMSTARTPOSITIONPORT 9
#define ARMHITPOSITIONPORT 10

// Limit switches are digital inputs
#define ARMDOWNSWITCHPORT 7
#define ARMUPSWITCHPORT 8

// The gyro is on an analog channel
#define STEERINGGYROPORT 2

// Autonomous switches enable/disable digital inputs
#define AUTONOMOUSSWITCHPORTONE 5
#define AUTONOMOUSSWITCHPORTTWO 6

// Relays have their own ports
#define MAGAZINERELAYPORT 2
#define MAGAZINERELAY2PORT 5
#define LATERALCOLLECTORPORT 1
#define TRANSVERSECOLLECTORPORT 3
#define LATERALCOLLERTOR2PORT 4

// Constants
// Hardware constants
#define GYROSENSITIVITY 0.007f // Volts per degree per second
#define PULSESPER90FRONT 1800 // Number of encoder pulses in a 90-degree turn (front steering)
#define PULSESPER90REAR 1800 // Number of encoder pulses in a 90-degree turn (rear steering)
#define STEERINGDEADBAND 50 // Number of encoder pulses of proximity needed for the steering (lower numbers are more accurate, but can cause hunting)
#define STEERINGAPPROACHDISTANCE 80 // Number of encoder pulses of proximity after which the wheels begin to slow down
#define CAMERAFREQUENCY 2000 // The number of times the Is-lied by this constant
#define HALFCAMERAVIEWINGANGLE 23.5 // Half of the viewing angle of the camera
#define IMAGEWIDTH 320.0 // The width of the image from the camera
#define IMAGEHEIGHT 240.0 // The height of the image from the camera
#define ENCODERFREQUENCY 300 // The number of times the IsOperatorControl() loop must run before updating the encoder speed
#define MAGAZINEALTERNATORLIMIT 800 // The number of time the IsOpteratorControl() loop must run before the magazine alternator resets

// Hardware threshold constants
#define JOYSTICKDEADBANDX 0.100f // Minimum detectable joystick deflection
#define JOYSTICKDEADBANDY 0.100f
#define GYRODEADBANDANGLE 3.0f // Allowable error for gyro angle when tracking
#define SHOOTERDEADBAND 0.0f // Allowable error for shooter speed
#define SERVODEADBAND 2.0f // Allowable error for servos
#define DRIVEAPPROACHDISTANCE 100.0f // Distance at which the drive motors begin to slow during brake mode
#define DRIVEDEADBAND 5.0f // Allowable error for brake mode encoder

// Camera HSL Image Thresholds
#define HUEMAX 255
#define HUEMIN 0
#define SATURATIONMAX 15
#define SATURATIONMIN 0
#define LUMINANCEMAX 255
#define LUMINANCEMIN 200 // 160
#define REDMIN 190
#define REDMAX 255
#define BLUEMIN 0
#define BLUEMAX 150
#define GREENMIN 0
#define GREENMAX 150
#define MINIMUMAREA 220

// The deadband for the the horizontal pivoting to target
#define IMAGEDEADBAND 10
#define IMAGEAPPROACHDISTANCE 60

// Approching deadband for aligning with the gyro
#define GYROAPPROACHDISTANCE 15

// Other constants
#define Pi 3.1415926535897932384626433832795028841971f
#define SWERVEMODE 0
#define MONSTERMODE 1
#define CARMODE 2
#define GOALWIDTH 2.0
#define STICKDIRECTIONX -1 // Use this to control whether or not the x-axis of the joysticks is inverted
#define LOWGEARPOSITION 160.0f // Angles for the gear-shifting servos
#define HIGHGEARPOSITION 80.0f
#define NEUTRALGEARPOSITION 120.0f
#define ARMENGAGEPOSITION 60.0f
#define MAGAZINEREVERSETIME 400 // Time for magazine to reverse before shooting
#define SERVOPAUSETIME 800 // Time to wait for arm-engaging servo to shift
#define IMAGEOFFSET 0 // Counteract the offset of the camera from the robot's center
#define AUTONOMOUSDISTANCE 2000 // Maximum distance to travel in autonomous
#define FINALAUTONOMOUSDISTANCE 19.0f // Final distance from target once robot stops in autonomous at bridge
#define MAGAZINEPAUSETIME 400 // Amount of time to wait during autonomous for rollers to spin up to speed
#define AUTOSHOOTPAUSETIME 2000 // Amount of time to wait during autonomous before moving (third mode only)

// Shooter constants
#define FIRESPEEDONE 0.40f // Pre-set firing speeds
#define FIRESPEEDTWO 0.6f
#define FIRESPEEDTHREE 0.98f
#define ROLLERRATIO (1.0f/5.0f) // The ratio of the bottom roller's speed to the top roller's speed (0.5 halves the bottom)
#define LINEARDISTANCEKT 0.015f // The camera distance is multiplied by this constant ("m" in y = mx + b) -- for top net (> 9')
#define CONSTANTDISTANCEKT 0.53f // The camera distance has this added to it ("b" in y = mx + b) -- for top net (> 9')
#define LINEARDISTANCEKB 0.01125f // For bottom net (< 9')
#define CONSTANTDISTANCEKB 0.4401f // For bottom net (< 9')

Brendan van Ryn

Posts : 95
Join date : 2010-09-30
Age : 24

Back to top Go down

Current Code?

Post  Jacob on Wed Mar 21, 2012 7:05 pm

Would you be able to post the current robot code along with the header file? I finished reading the tutorials (a bit late, but oh well Wink ) and I thought it would be useful for me to read through the code and the header file and see if I understand. cyclops

Jacob

Posts : 16
Join date : 2011-10-04

Back to top Go down

Code Update

Post  Brendan van Ryn on Wed Mar 21, 2012 8:36 pm

Unfortunately, due to being rushed, I don't have a current version of the robot code, but I'll make sure to post one tomorrow Wink

Brendan van Ryn

Posts : 95
Join date : 2010-09-30
Age : 24

Back to top Go down

Current Code?

Post  Jacob on Wed Mar 21, 2012 9:51 pm

Ok, thanks Very Happy

Jacob

Posts : 16
Join date : 2011-10-04

Back to top Go down

Code Update

Post  Brendan van Ryn on Thu Mar 22, 2012 8:11 pm

So, here it is, in it's uncommented glory. More changes still need to be made, so this isn't a final version by any means (but none ever are, I suppose). Unfortunately, the camera stuff has all been phased out. This means that we no longer have any examples of for loops, arrays, sorting, etc. in our robot code, but you guys should already know that based off of the tutorials, so hopefully it's not a big issue. On the plus side, it significantly reduces the size of the code Wink

The header file (many of these macros aren't needed any more, but are kept for safety):
Code:


/* 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 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 HIGHGEARBUTTON leftStick->GetRawButton(5)
#define LOWGEARBUTTON leftStick->GetRawButton(4)
#define NEUTRALGEARBUTTON leftStick->GetRawButton(3)
#define ARMUNWINDBUTTON speedStick->GetRawButton(8)

#define BRIDGEMANIPULATEBUTTON speedStick->GetRawButton(7)
#define PIVOTMODEBUTTON rightStick->GetRawButton(2)

#define GYRORESETBUTTON rightStick->GetRawButton(11)

#define AUTOSHOOTBYDISTANCE speedStick->GetRawButton(6)
#define SHOOTBUTTON (speedStick->GetRawButton(1) && (speedStick->GetRawButton(2) || speedStick->GetRawButton(3) || speedStick->GetRawButton(4) || speedStick->GetRawButton(5)))
#define SOLOMAGAZINEBUTTON speedStick->GetRawButton(16)
#define FIRSTFIREBUTTON (speedStick->GetRawButton(4) || speedStick->GetRawButton(2))
#define SECONDFIREBUTTON speedStick->GetRawButton(3)
#define THIRDFIREBUTTON speedStick->GetRawButton(5)
#define SPECIALBUTTON speedStick->GetRawButton(9)
#define TARGETMODEENABLE speedStick->GetRawButton(9)
#define TARGETMODEDISABLE speedStick->GetRawButton(9)

#define MAGAZINEENABLEBUTTON speedStick->GetRawButton(11)
#define LATERALCOLLECTORBUTTON speedStick->GetRawButton(11)
#define TRANSVERSECOLLECTORBUTTON speedStick->GetRawButton(11)
#define LATERALREVERSEBUTTON speedStick->GetRawButton(10)
#define TRANSVERSEREVERSEBUTTON speedStick->GetRawButton(10)

// Port numbers
// Motors are on PWM ports
#define FRONTMOTORPORT 3
#define REARMOTORPORT 4
#define FRONTSTEERINGMOTORPORT 2
#define REARSTEERINGMOTORPORT 1
#define TOPSHOOTINGMOTORPORT 10
#define BOTTOMSHOOTINGMOTORPORT 9

// 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
#define SHOOTERENCODERPORTA 11
#define SHOOTERENCODERPORTB 12

// Light sensors for encoders are also digital inputs
#define ARMSTARTPOSITIONPORT 9
#define ARMHITPOSITIONPORT 10

// Limit switches are digital inputs
#define ARMDOWNSWITCHPORT 7
#define ARMUPSWITCHPORT 8

// The gyro is on an analog channel
#define STEERINGGYROPORT 2

// Autonomous switches enable/disable digital inputs
#define AUTONOMOUSSWITCHPORTONE 5
#define AUTONOMOUSSWITCHPORTTWO 6

// Relays have their own ports
#define MAGAZINERELAYPORT 2
#define MAGAZINERELAY2PORT 5
#define LATERALCOLLECTORPORT 1
#define TRANSVERSECOLLECTORPORT 3
#define LATERALCOLLERTOR2PORT 4

// 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 steering (lower numbers are more accurate, but can cause hunting)
#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 steering (lower numbers are more accurate, but can cause hunting)
#define STEERINGAPPROACHDISTANCEREAR 60 // Number of encoder pulses of proximity after which the wheels begin to slow down
#define CAMERAFREQUENCY 2000 // The number of times the Is-lied by this constant
#define HALFCAMERAVIEWINGANGLE 23.5 // Half of the viewing angle of the camera
#define IMAGEWIDTH 320.0 // The width of the image from the camera
#define IMAGEHEIGHT 240.0 // The height of the image from the camera
#define ENCODERPERIOD 80 // The number of times the IsOperatorControl() loop must run before updating the roller speed
#define SAMPLENUMBER 8 // The number of samples to average is the roller speed
#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
#define SHOOTTIME -1200

// 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 SERVODEADBAND 2.0f // Allowable error for servos
#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

// Camera HSL Image Thresholds
#define HUEMAX 255
#define HUEMIN 0
#define SATURATIONMAX 15
#define SATURATIONMIN 0
#define LUMINANCEMAX 255
#define LUMINANCEMIN 200 // 160
#define REDMIN 190
#define REDMAX 255
#define BLUEMIN 0
#define BLUEMAX 150
#define GREENMIN 0
#define GREENMAX 150
#define MINIMUMAREA 220

// The deadband for the the horizontal pivoting to target
#define IMAGEDEADBAND 10
#define IMAGEAPPROACHDISTANCE 60

// Approching deadband for aligning with the gyro
#define GYROAPPROACHDISTANCE 15

// Other constants
#define Pi 3.1415926535897932384626433832795028841971f
#define SWERVEMODE 0
#define MONSTERMODE 1
#define CARMODE 2
#define GOALWIDTH 2.0
#define STICKDIRECTIONX -1 // Use this to control whether or not the x-axis of the joysticks is inverted
#define LOWGEARPOSITION 160.0f // Angles for the gear-shifting servos
#define HIGHGEARPOSITION 70.0f
#define NEUTRALGEARPOSITION 110.0f
#define ARMENGAGEPOSITION 70.0f http://100.0f
#define ARMDISENGAGEPOSITION 120.0f http://60.0f
#define MAGAZINEREVERSETIME 700 // Time for magazine to reverse before shooting
#define SERVOPAUSETIME 1000 // Time to wait for arm-engaging servo to shift
#define IMAGEOFFSET 0 // Counteract the offset of the camera from the robot's center
#define AUTONOMOUSDISTANCE 10000 // Maximum distance to travel in autonomous
#define FINALAUTONOMOUSDISTANCE 19.0f // Final distance from target once robot stops in autonomous at bridge
#define MAGAZINEPAUSETIME 400 // Amount of time to wait during autonomous for rollers to spin up to speed
#define AUTOSHOOTPAUSETIME 2000 // Amount of time to wait during autonomous before moving (third mode only)
#define SHOOTERAPPROACHSPEED 50 // Distance from target speed at which the rollers begin to ramp down
#define SHOOTERCONSTANTSPEED 0.05f // Minimum power value for the shooter (a maintenance value)
#define SHOOTERDEADBAND 5 // Distance from target speed at which the rollr speed is considered to be within a good enough threshold

// Shooter constants
#define FIRESPEEDONE 0.50f // Pre-set firing speeds
#define FIRESPEEDTWO 0.78f
#define FIRESPEEDTHREE 0.98f
#define ROLLERRATIO (1.0f/5.0f) // The ratio of the bottom roller's speed to the top roller's speed (0.5 halves the bottom)
#define LINEARDISTANCEKT 0.015f // The camera distance is multiplied by this constant ("m" in y = mx + b) -- for top net (> 9')
#define CONSTANTDISTANCEKT 0.53f // The camera distance has this added to it ("b" in y = mx + b) -- for top net (> 9')
#define LINEARDISTANCEKB 0.01125f // For bottom net (< 9')
#define CONSTANTDISTANCEKB 0.4401f // For bottom net (< 9')

Removing those unneeded macros will be a good cleanup thing to do once the season's over, along with cleaning up some of the source code, below:
Code:


/******************************************************************************
 *
 *    Sprockets 2012 Base Code
 *    Authors:    Brendan van Ryn
 *             Stuart Sullivan
 *             Aliah McCalla
 *             Will Surmak
 *              Caleb McAnuff
 *            Jacob 'tMannetje    
 * Last Updated:   15/02/2012
 *
 ******************************************************************************/



/* Robot configuration macros. Do not change. */
#define NEW 1
#define OLD 2

/* Robot configuration */
#define ROBOT NEW // Set this to either "OLD" or "NEW", without the quotation marks, to configure for the given robot

/* Set the correct macros based on which robot is being used. Do not change this. */
#if !defined ROBOT
#warning "Robot version not specified. Assuming new configuration..."
#include "NewRobot.h"
#elif ROBOT == NEW
#include "NewRobot.h"
#elif ROBOT == OLD
#include "OldRobot.h"
#else
#warning "Robot configuration must be either 'OLD' or 'NEW'. Assuming new..."
#include "NewRobot.h"
#endif

// Header file for WPI standard hardware/robot overhead.
#include "WPILib.h"

// Include the libraries for the camera
#include "vision/AxisCamera.h"

// Header file for math functions
#include <math.h>

// Macro Functions
// Clears the Driver Station output display
#define CLEARMESSAGE DriverStationLCD::GetInstance()->Clear()

// Prints an integer, a, on the Driver Station on line, b.
#define DISPLAYINTEGER(b, a) DriverStationLCD::GetInstance()->Printf(DriverStationLCD::kUser_Line ## b, 1, "%d", a); DriverStationLCD::GetInstance()->UpdateLCD()

// Prints a string, a, on the Driver Station on line, b.
#define DISPLAYSTRING(b, a) DriverStationLCD::GetInstance()->Printf(DriverStationLCD::kUser_Line ## b, 1, a); DriverStationLCD::GetInstance()->UpdateLCD()

// Prints a float, a, on the Driver Station on line, b.
#define DISPLAYFLOAT(b, a) DriverStationLCD::GetInstance()->Printf(DriverStationLCD::kUser_Line ## b, 1, "%0.2f", a); DriverStationLCD::GetInstance()->UpdateLCD()

// 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();}

// 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
void SquareSign(float*); // Squares the argument sent to it, preserving the sign


/* Our main (and this year, only) class. Don't worry about the word
"class" as we won't do much Object-Oriented stuff, but in the layout
provided to use for the robot code, we must have this one class. The
code for the robot goes here, along with all variables we need. It has
always been called "SimpleTracker" for whatever reason". */
class SimpleTracker : public SimpleRobot // Just ignore this, essentially. Treat it as "int main(void)" for simplicity.
{
    // "Global" variables are declared here.
   Joystick *leftStick; // Left driver joystick
   Joystick *rightStick; // Right driver joystick
   Joystick *speedStick; // Third driver joystick
    Victor *frontMotor; // Front-left drive motor
    Victor *rearMotor; // Rear-left drive motor
    Jaguar *frontSteer; // Front steering motor
    Jaguar *rearSteer; // Rear steering motor
    Jaguar *topShooter; // Top shooting roller
    Jaguar *bottomShooter; // Bottom shooting roller
    Encoder *frontSteeringEncoder; // Front steering encoder
    Encoder *rearSteeringEncoder; // Rear steering encoder
    Encoder *driveEncoder; // Drive wheel encoder
    Encoder *shootingEncoder; // Encoder for shooting roller
    Gyro *steeringGyro; // Steering gyro
    HSLImage *cameraImage; // Hue/Saturation/Luminence image from camera
    BinaryImage *binaryImage; // The object used to store and edit the filtered image
    Image *imaqImage; // Stores the image in a format that the built-in imaq functions can use
    Relay *magazineBelt; // Magazine conveyor
    Relay *magazineBelt2; // Second Motor for the Magazine
    Relay *lateralCollector; // Lateral collector belts
    Relay *transverseCollector; // Transverse collector belts
    Relay *lateralCollector2; // The supplementry motor for the lateral collector belts
    DigitalInput *driveWheelEncoder; // Light sensor for drive wheel encoding
    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
    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
    double maxTargetWidth; // Width of clearest target in view
    double maxTargetHeight; // Height of clearest target in view
   
    /* This "public" means that these variables are available outside of
    this class...but we only have one, so it essentially does nothing.
    "SimpleTracker(void)" is called a constructor. Again, don't worry about
    the details. This is where our object-based variables (hardware components)
    are initialized. This means we assign them a particular port. */ 
    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);
       
        topShooter = new Jaguar(TOPSHOOTINGMOTORPORT); // Shooter motors
        bottomShooter = new Jaguar(BOTTOMSHOOTINGMOTORPORT);
 
        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);
        shootingEncoder = new Encoder(SHOOTERENCODERPORTA, SHOOTERENCODERPORTB);
       
        steeringGyro = new Gyro(1, STEERINGGYROPORT); // Analogue devices
       
        magazineBelt = new Relay(MAGAZINERELAYPORT); // Relays
        magazineBelt2 = new Relay(MAGAZINERELAY2PORT);
        lateralCollector = new Relay(LATERALCOLLECTORPORT);
        transverseCollector = new Relay(TRANSVERSECOLLECTORPORT);
        lateralCollector2 = new Relay(LATERALCOLLERTOR2PORT);
       
        armStartSensor = new DigitalInput(ARMSTARTPOSITIONPORT); // Bridge manipulator light sensors
        armHitSensor = new DigitalInput(ARMHITPOSITIONPORT);
       
        armDownSwitch = new DigitalInput(ARMDOWNSWITCHPORT); // The bridge manipulator limit switches
        armUpSwitch = new DigitalInput(ARMUPSWITCHPORT);
       
        autonomousSwitchOne = new DigitalInput(AUTONOMOUSSWITCHPORTONE); // The autonomous toggle switches
        autonomousSwitchTwo = new DigitalInput(AUTONOMOUSSWITCHPORTTWO);
       
        frontDriveServo = new Servo(FRONTSERVOPORT); // Gear shifting servos
        rearDriveServo = new Servo(REARSERVOPORT);
        armEngageServo = new Servo(ARMENGAGESERVOPORT); // Servo to engage bridge manipulator
       
        // Initialize encoders
        frontSteeringEncoder->Reset();
        frontSteeringEncoder->Start();
        rearSteeringEncoder->Reset();
        rearSteeringEncoder->Start();
        driveEncoder->Reset();
        driveEncoder->Start();
        shootingEncoder->Reset();
        shootingEncoder->Start();
       
        // Initialize gyro
        steeringGyro->Reset();
        steeringGyro->SetSensitivity(GYROSENSITIVITY); // Changes the sensitivity of the gyro
   }
   
    /* 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 speedx = 0.0f, speedy = 0.0f; // The x- and y-positions of the third speed-control shift
      float x, y; // The average x- and y-positions between the two 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; // Allow different angles for the front and back sides, for use in different steering modes
      int waitForSteering; // Should the drive motors wait for the wheels to finish turning?
      int frontFinished, rearFinished; // Store whether each driving side has finished turning
      int steeringMode = SWERVEMODE; // Store the current mode of steering (0 = strafe, 1 = monster, 2 = car)
      int currentButtonState = 0, previousButtonState = 0; // Store the current and previous states of the manual alignment buttons
      int driveWheelEncoderValue; // Distance travelled by robot
      int targetMode = 0; // Are we in targetting mode?
      float frontGearSetting = LOWGEARPOSITION; // Store the angle setting for the gear-shifting servos
      float rearGearSetting = LOWGEARPOSITION;
      float armPosition = LOWGEARPOSITION; // Stores the angle setting for the bridge-manipulator servo
      int armStage = 0; // Clean up for arm engagement
      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 fireReady = 0; // Are the rollers up to firing speed?
      int brakeValue = -1; // Encoder value to maintain when in brake mode
      int override = 0; // Override flag for manual winding of bridge manipulator
      float rollerAverage = 0.0f; // Average speed of roller from a set of samples
      int rollerOffset = 0; // Offset for roller encoder so we never have to reset
      int rollerSpeed = 0; // Current speed reading in the given sample
      int divider = 0; // A divider counter so we update the roller speed every so often
      
      // Reset the drive and shooter encoders
      driveEncoder->Reset();
      driveEncoder->Start();
      shootingEncoder->Reset();
      shootingEncoder->Start();
      
      // This code is run repeatedly so long as the robot is in teleop mode.
      while(IsOperatorControl())
      {
         /* This is a fancy, convoluted way of telling the operating-system
         software that the code hasn't frozen. */
         GetWatchdog().Feed();
         
         /* Message displays */
         CLEARMESSAGE;
         //DISPLAYPRINTF(1, "Encoder: %d", driveEncoder->Get()) // Encoder value from drive wheel
         //DISPLAYPRINTF(2, "Front Steering: %0.2f", frontSteer->Get()) // Display the top shooter speed reading
         //DISPLAYPRINTF(3, "Rear Steering: %0.2f", rearSteer->Get()) // Display the bottom shooter speed reading
         DISPLAYPRINTF(2, "Swerve: %0.2f", swerveAngle) // Turning angle for steering
         DISPLAYPRINTF(3, "Shooter speed: %0.3f", rollerAverage) // Average speed from a set of samples for roller
         DISPLAYPRINTF(4, "Roller setting: %0.4f", speedy) // The speed value from the joystick
         
         // Get joystick positions and square, preserving sign, to reduce sensitivity
         leftx = STICKDIRECTIONX * leftStick->GetRawAxis(1); //SquareSign(&leftx);
         lefty = leftStick->GetRawAxis(2); //SquareSign(&lefty);
         rightx = STICKDIRECTIONX * rightStick->GetRawAxis(1); //SquareSign(&rightx);
         righty = rightStick->GetRawAxis(2); //SquareSign(&righty);
         speedx = speedStick->GetRawAxis(1);
         //speedy = speedStick->GetRawAxis(2);
         speedy = (speedStick->GetRawAxis(3) - 1.0f) / 2.0f;
         
         // After a set period of time
         if(divider > ENCODERPERIOD)
         {
            // Reset divider
            divider = 0;
            
            // Get the number of pulses passed between this sample and the previous sample
            rollerSpeed = AbsI(shootingEncoder->Get()) - rollerOffset;
            
            // Update value for next sample
            rollerOffset += rollerSpeed;
            
            // Update the average speed
            rollerAverage = (rollerAverage * (SAMPLENUMBER - 1) + rollerSpeed) / SAMPLENUMBER;
            
            // Truncate to three decimal places (prevents division remnants from being a problem)
            rollerAverage = (int)(rollerAverage * 1000.0f) / 1000.0f;
         }
         else divider++;
         
         // Prevent back-rolling
         if(speedy > 0) speedy = 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++;
         
         /* Teleop code here. */
         // Check  drive mode buttons
         if(SWERVEMODEBUTTON) steeringMode = SWERVEMODE;
         else if(MONSTERMODEBUTTON) steeringMode = MONSTERMODE;
         else if(CARMODEBUTTON) steeringMode = CARMODE;
         
         // Check shooter buttons
         if(FIRSTFIREBUTTON) fireReady = SetRollerSpeed(FIRESPEEDONE, rollerAverage), preFire = 1;
         else if(SECONDFIREBUTTON) fireReady = SetRollerSpeed(FIRESPEEDTWO, rollerAverage), preFire = 1;
         else if(THIRDFIREBUTTON) fireReady = SetRollerSpeed(FIRESPEEDTHREE, rollerAverage), preFire = 1;
         else preFire = 0, fireReady = 1;
         
         // Reset the gyro reading
         if(GYRORESETBUTTON) steeringGyro->Reset();
         
         // Check gear shifting buttons
         if(HIGHGEARBUTTON && !targetMode) frontGearSetting = rearGearSetting = HIGHGEARPOSITION, armPosition = ARMDISENGAGEPOSITION, override = 0;
         else if(LOWGEARBUTTON || targetMode) frontGearSetting = rearGearSetting = LOWGEARPOSITION, armPosition = ARMDISENGAGEPOSITION, override = 0;
         else if(NEUTRALGEARBUTTON && !targetMode) frontGearSetting = LOWGEARPOSITION, rearGearSetting = NEUTRALGEARPOSITION, armPosition = ARMDISENGAGEPOSITION, override = 0;
         else if(BRIDGEMANIPULATEBUTTON && !targetMode && armPosition != ARMENGAGEPOSITION) frontGearSetting = LOWGEARPOSITION, rearGearSetting = NEUTRALGEARPOSITION, armPosition = ARMENGAGEPOSITION, servoPauseCount = SERVOPAUSETIME, armStage = 0, override = 0;
         else if(ARMUNWINDBUTTON && !targetMode) frontGearSetting = LOWGEARPOSITION, rearGearSetting = NEUTRALGEARPOSITION, armPosition = ARMENGAGEPOSITION, override = 1;
         else if(override && !ARMUNWINDBUTTON) frontGearSetting = rearGearSetting = LOWGEARPOSITION, armPosition = ARMDISENGAGEPOSITION, override = 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 an invalid value in the brake value
         if(!BRAKEMODEBUTTON) brakeValue = -1;
         
         // Check belt buttons
         if(MAGAZINEENABLEBUTTON || SHOOTBUTTON)
         {
            if(magazineAlternator < MAGAZINEALTERNATORLIMITONE) magazineBelt->Set(Relay::kForward), magazineBelt2->Set(Relay::kForward);
            else magazineBelt->Set(Relay::kReverse), magazineBelt2->Set(Relay::kReverse);
            
            speedy = 0.7;
            magazineReverseCount = MAGAZINEREVERSETIME;
         }
         /*else if(SOLOMAGAZINEBUTTON && fireReady)
         {
            if(magazineAlternator > MAGAZINEALTERNATORLIMIT / 8) magazineBelt->Set(Relay::kForward), magazineBelt2->Set(Relay::kForward);
            else magazineBelt->Set(Relay::kOff), magazineBelt2->Set(Relay::kOff);
         }*/
         else if(magazineReverseCount > 0 && !targetMode) magazineBelt->Set(Relay::kReverse), magazineBelt2->Set(Relay::kReverse), magazineReverseCount--;
         else magazineBelt->Set(Relay::kOff), magazineBelt2->Set(Relay::kOff);
         if(LATERALCOLLECTORBUTTON) lateralCollector->Set(Relay::kReverse), lateralCollector2->Set(Relay::kReverse);
         else if(LATERALREVERSEBUTTON) magazineBelt->Set(Relay::kReverse), magazineBelt2->Set(Relay::kReverse), lateralCollector->Set(Relay::kForward), lateralCollector2->Set(Relay::kForward);
         else lateralCollector->Set(Relay::kOff), lateralCollector2->Set(Relay::kOff);
         if(TRANSVERSECOLLECTORBUTTON) transverseCollector->Set(Relay::kReverse);
         else if(TRANSVERSEREVERSEBUTTON) transverseCollector->Set(Relay::kForward);
         else transverseCollector->Set(Relay::kOff);
         
         // If the total y-deflection for each joystick is below the threshold, we shouldn't count it at all
         if(Abs(lefty) < JOYSTICKDEADBANDY) lefty = 0.0f;
         if(Abs(righty) < JOYSTICKDEADBANDY) righty = 0.0f;
            
         // Average the joystick positions
         x = (leftx + rightx) / 2.0f;
         y = (lefty + righty) / 2.0f;
                  
         // Store the currentButtonState from the previous iteration
         previousButtonState = currentButtonState;
         
         // Bridge-manipulate not part of state-machine
         // If we are manipulating the bridge
         if(armPosition == ARMENGAGEPOSITION && servoPauseCount <= 0 && !override)
         {
            if(!BRIDGEMANIPULATEBUTTON) armStage = 4;
            
            /// Drive winch until first light sensor is triggered (arm is in primed position)
            if(!armStartSensor->Get() && armStage == 0) rearMotor->Set(0.5f);
            else if(armStage == 0) armStage = 1;
            
            // Stop winch until the light sensor on the bridge dector is triggered (bridge is hit)
            if(!armHitSensor->Get() && armStage == 1) rearMotor->Set(0.0f);
            else if(armStage == 1) armStage = 2;
            
            // Continue to run winch until bottom switch is hit
            if(!armDownSwitch->Get() && armStage == 2) rearMotor->Set(0.85f);
            else if(armStage == 2) armStage = 3;
               
            // Wait for manipulator button to be released
            if(BRIDGEMANIPULATEBUTTON && armStage == 3) rearMotor->Set(0.0f);
            else if(armStage == 3) armStage = 4;
            
            // Return arm
            if(!armUpSwitch->Get() && armStage == 4) rearMotor->Set(-0.4f);
            else if(armStage == 4) armStage = 5;
            
            // Disengage arm, set drive units to low gear, and stop winch motor; reset the armStage for next time
            if(armStage == 5)
            {
               rearGearSetting = LOWGEARPOSITION;
               frontGearSetting = LOWGEARPOSITION;
               armPosition = LOWGEARPOSITION;
               
               rearMotor->Set(0.0f);
            }
         }
                  
         
         // If the manual-alignment buttons are pressed
         if(MANUALALIGNENABLEBUTTON)
         {
            currentButtonState = 1;
            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);
         }
         
         // The lock button sets the wheels straight sideways for aligning on the bridge
         else if(LOCKMODEBUTTON)
         {   
            TrackFrontSteering(PULSESPER90FRONT);
            TrackRearSteering(PULSESPER90REAR);
            frontMotor->Set(y);
            rearMotor->Set(y);
         }
         
         // The brake button holds us still
         else if(BRAKEMODEBUTTON)
         {
            // Store value to hold
            if(brakeValue == -1) brakeValue = driveWheelEncoderValue;
            
            // Allow the robot to move still based on driver input
            if(y > JOYSTICKDEADBANDY) brakeValue += 2;
            else if(y < -JOYSTICKDEADBANDY) brakeValue -= 2;
            TrackFrontSteering(0);
            TrackRearSteering(0);
            
            TrackDrive(brakeValue);
         }
      
         else if(TANKTURNINGMODEBUTTON)
         {
            // Turn the wheels in opposite directions
            TrackFrontSteering(PULSESPER90FRONT);
            TrackRearSteering(-PULSESPER90REAR);
            
            // Allow the ability to move the wheels
            frontMotor->Set(lefty);
            rearMotor->Set(righty);
         }
            
         // The pivot mode separate from the rest of the drive
         else if(PIVOTMODEBUTTON)
         {
            TrackFrontSteering(0);
            TrackRearSteering(PULSESPER90REAR);
            frontMotor->Set(0);
            rearMotor->Set(righty);
         }
                  
         // Otherwise, if the deflection left or right is less than a certain amount, we simply drive tank-style
         // Abs simply ensures that x is positive so that we don't need two separate if-statements
         else if(Abs(x) < JOYSTICKDEADBANDX)
         {
            // Tank drive
            // 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 && !override)) rearMotor->Set(righty);
            
            if(!preFire && !AUTOSHOOTBYDISTANCE)
            {
               topShooter->Set(speedy); // Set roller speeds manually
               bottomShooter->Set(-speedy);
            }
            
            currentButtonState = 0;
            
            TrackFrontSteering(0);
            TrackRearSteering(0);
         }
         
         else
         {
            // Swerve drive
            
            waitForSteering = 0;
            
            // Determine the total deflection of the joysticks to use as the speed setting
            swerveSpeed = sqrt(x*x + y*y); // a^2 + b^2 = c^2
            if(Abs(y) < JOYSTICKDEADBANDY)
            {
               if(x > 0.0f) swerveAngle = -1.5707963267949;
               else swerveAngle = 1.5707963267949;
            }
            else swerveAngle = -Abs(atan(x/y)); // x/y = tan(theta)
            swerveAngle = swerveAngle * 180 / Pi; // 1 degree = 180/Pi radians
            swerveAngle = swerveAngle / 90.0f; // Convert from degrees to a percentage of 90 degrees
            
            // Set roller speeds manually
            if(!preFire)
            {
               topShooter->Set(speedy);
               bottomShooter->Set(-speedy);
            }
                        
            // Determine the quadrant of the joystick and convert the speed or angle appropriately
            // Quadrant one is assumed, except for the following conditions
            if(x < 0 && y >= 0) swerveAngle = -swerveAngle; // Quadrant two
            else if(x < 0 && y < 0) swerveSpeed = -swerveSpeed; // Quadrant three
            else if(x >= 0 && y < 0) // Quadrant Four
            {
               swerveSpeed = -swerveSpeed;
               swerveAngle = -swerveAngle;               
            }
            
            // If we are in monster mode and travelling faster than 80% max speed, we switch to car automatically
            if(Abs(swerveSpeed) > 1.2f && steeringMode == MONSTERMODE) steeringMode = CARMODE;
         
            // Put steer-angle and drive-direction changes here for the other steering modes (monster, car, auto-monster, etc.)
            if(steeringMode == MONSTERMODE)
            {
               if(swerveSpeed <= 0) swerveAngleFront = -0.75f * swerveAngle, swerveAngleRear = 0.75f * swerveAngle;
               else swerveAngleFront = 0.75f * swerveAngle, swerveAngleRear = -0.75f * swerveAngle;
            }
            else if(steeringMode == CARMODE)
            {
               if(swerveSpeed <= 0) swerveAngleFront = -0.5f * swerveAngle, swerveAngleRear = 0.0f;
               else swerveAngleRear = -0.5f * swerveAngle, swerveAngleFront = 0.0f;
            }
            
            else
            {
               swerveAngleFront = -swerveAngle;
               swerveAngleRear = -swerveAngle;
            }
            
             // Convert front and rear angle percentages into pulses
             swerveAngleFront *= PULSESPER90FRONT;
             swerveAngleRear *= PULSESPER90REAR;
          
            /* Call pre-written functions that will use the encoder to get the wheels to gradually approach the desired
             * position. This function will return zero when the steering is finished.*/
            frontFinished = TrackFrontSteering((int)swerveAngleFront);
            rearFinished = TrackRearSteering((int)swerveAngleRear);
            
            // If the steering has finished, set the drive motors to move
            if((frontFinished == 0 && rearFinished == 0) || !waitForSteering)
            {
               frontMotor->Set(swerveSpeed);
               
               // Only move rear wheels if bride manipulate mode is not engaged
               if(!(armPosition == ARMENGAGEPOSITION && servoPauseCount <= 0 && !override)) rearMotor->Set(swerveSpeed);
            }
            
            currentButtonState = 0;
         }
         
         // If the manual alignment buttons have JUST been released, reset the encoders again
         if(previousButtonState == 1 && currentButtonState == 0)
         {
            frontSteeringEncoder->Reset();
            frontSteeringEncoder->Start();
            rearSteeringEncoder->Reset();
            rearSteeringEncoder->Start();
            frontSteer->Set(0);
            rearSteer->Set(0);
         }
        }
   } // 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
      
      // Reset shooting encoders
      shootingEncoder->Reset();
      shootingEncoder->Start();
      
      // Store the values of the switches for the autonomous modes
      firstSwitch = autonomousSwitchOne->Get();
      secondSwitch = autonomousSwitchTwo->Get();
      
      AutonomousOne();
      
//      // Check the states of switches and call the correct autonomous mode
//      if(firstSwitch && secondSwitch) AutonomousOne();
//      else if(firstSwitch && !secondSwitch) AutonomousTwo();
//      else if(secondSwitch && !firstSwitch) AutonomousThree();
//      else
//      {
//         CLEARMESSAGE;
//         DISPLAYSTRING(1, "No autonomous switch");
//      }
   } // End of Autonomous
   
   /* This function handles the complicated problem of quickly, but accurately, moving the
    * front wheels to their correct position, as passed 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 rollers. Returns true when rollers are at that speed
   int SetRollerSpeed(float targetSpeed, float currentSpeed)
   {
      /*float powerSetting;
      
      powerSetting = Abs(targetSpeed - currentSpeed) / SHOOTERAPPROACHSPEED + SHOOTERCONSTANTSPEED;
      if(powerSetting > 1.0f) powerSetting = 1.0f;
      
      if(currentSpeed < targetSpeed) topShooter->Set(powerSetting);
      else topShooter->Set(0.0f);
      
      return (Abs(targetSpeed - currentSpeed) < SHOOTERDEADBAND);*/
      
      topShooter->Set(-targetSpeed * ROLLERRATIO);
      bottomShooter->Set(targetSpeed);
      
      return 1;
   }
   
   // This function attempts to maintain a specific encoder value on the drive block
   float TrackDrive(int targetPosition)
   {
      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;
      if(distance > 1.0f) distance = 1.0f;
      
      // Move forward or backward at a proportional pace until within a deadband
      if(currentPosition < targetPosition - DRIVEDEADBAND) frontMotor->Set(distance), rearMotor->Set(distance);
      else if(currentPosition > targetPosition + DRIVEDEADBAND) frontMotor->Set(-distance), rearMotor->Set(-distance);
      else frontMotor->Set(0.0f), rearMotor->Set(0.0f);
      
      // Return motor speed
      return frontMotor->Get();
   }
   
   // Code for our first possible autonomous mode
   void AutonomousOne(void)
   {
      // Variables
      float gyroAngle = 0; // Current angle from home as read by the gyro 
      int armStage = 0; // Current stage for bridge manipulator
      int autoPauseCount = SERVOPAUSETIME; // The amount of time to wait for the servos to reach their target position
      int magazinePauseCount = MAGAZINEPAUSETIME; // The amount of time to wait for the rollers to reach speed
      int encoderValue = 0; // Value from drive encoder
      int divider = 0; // Count the number of IsAutonomous() loops that have passed
      int rollerSpeed = 0; // Distance roller has traveled since previous sample
      float rollerAverage = 0.0f; // Average speed of rollers based on a set number of samples
      int rollerOffset = 0; // Value of encoder from previous sample (to update current sample)
      
      // Reset the drive encoder and gyro
      steeringGyro->Reset();
      driveEncoder->Reset();
      driveEncoder->Start();
      
      // This code is run repeatedly so long as the robot is in autonomous mode (lasting 15 seconds).
      while (IsAutonomous())
      {
         /* Tell the system that the code hasn't frozen. */
         GetWatchdog().Feed();
         
         /* Autonomous code here. */
         // Get the gyro angle
         gyroAngle = steeringGyro->GetAngle();
                  
         // Get the encoder value
         encoderValue = driveEncoder->Get();
         
         // After a set period of time
         if(divider > ENCODERPERIOD)
         {
            // Reset divider
            divider = 0;
            
            // Get the number of pulses passed between this sample and the previous sample
            rollerSpeed = AbsI(shootingEncoder->Get()) - rollerOffset;
            
            // Update value for next sample
            rollerOffset += rollerSpeed;
            
            // Update the average speed
            rollerAverage = (rollerAverage * (SAMPLENUMBER - 1) + rollerSpeed) / SAMPLENUMBER;
            
            // Truncate to three decimal places (prevents division remnants from being a problem)
            rollerAverage = (int)(rollerAverage * 1000.0f) / 1000.0f;
         }
         else divider++;
         
         // Message displays
         CLEARMESSAGE;
         DISPLAYPRINTF(1, "Move then shoot"); // Display mode of autonomous
         DISPLAYPRINTF(1, "Gyro: %0.2f", gyroAngle);  // The float value of the gyro angle
         DISPLAYPRINTF(2, "Stage (0-4): %d", armStage); // The stage of autonomous
         DISPLAYPRINTF(3, "Encoder: %d", encoderValue); // The value of the encoder
         
         /* Simple, straight-movement autonomous */
         // Turn the rear wheels left to correct if we are off to the left
         // Otherwise, turn the wheels right to correct if we are off to the right
         // Otherwise, move straight
         if(gyroAngle + GYRODEADBANDANGLE < 0.0f) TrackRearSteering(PULSESPER90REAR / 8);
         else if(gyroAngle - GYRODEADBANDANGLE > 0.0f) TrackRearSteering(-PULSESPER90REAR / 8);
         else TrackRearSteering(0);
         
         // Keep the front wheels straight and drive forward slowly
         TrackFrontSteering(0);
                  
         // Until we have hit the bridge, move forward
         if(armStage < 2)
         {
            // Set gears to front drive and arm engagement
            frontDriveServo->SetAngle(LOWGEARPOSITION);
            rearDriveServo->SetAngle(NEUTRALGEARPOSITION);
            armEngageServo->SetAngle(ARMENGAGEPOSITION);
            
            // Give the servos time to get into place
            if(autoPauseCount > 0)
            {
               autoPauseCount--;
               continue;
            }
            
            // Drive
            frontMotor->Set(0.4);
            
            /// Drive winch until first light sensor is triggered (arm is in primed position)
            if(!armStartSensor->Get() && armStage == 0) rearMotor->Set(0.45);
            else if(armStage == 0) armStage = 1;
                        
            // Stop winch until the light sensor on the bridge dector is triggered (bridge is hit) or we travel too far (probably missing)
            if((!armHitSensor->Get() || encoderValue > AUTONOMOUSDISTANCE) && armStage == 1) rearMotor->Set(0.0f);
            else if(armStage == 1) armStage = 2;
         }
         
         // We have hit the bridge now
         else if(armStage < 4)
         {
            // Set gears to front drive and arm engagement
            frontDriveServo->SetAngle(LOWGEARPOSITION);
            rearDriveServo->SetAngle(NEUTRALGEARPOSITION);
            armEngageServo->SetAngle(ARMENGAGEPOSITION);
            
            // Stop
            frontMotor->Set(0.0f);
            
            // Continue to run winch until bottom switch is hit
            if(!armDownSwitch->Get() && armStage == 2) rearMotor->Set(0.75f);
            else if(armStage == 2) armStage = 3;
            
            // Return arm to home position
            if(!armUpSwitch->Get() && armStage == 3) rearMotor->Set(-0.4f);
            else if(armStage == 3) armStage = 4;
         }
         
         // Bridge tipping is complete
         else
         {
            // Return gears to low
            frontDriveServo->Set(LOWGEARPOSITION);
            rearDriveServo->Set(LOWGEARPOSITION);
            armEngageServo->Set(LOWGEARPOSITION);
            
            /*// Spin the rollers at the correct launching speed for the top target
            SetRollerSpeed(FINALAUTONOMOUSDISTANCE * LINEARDISTANCEKT + CONSTANTDISTANCEKT, rollerAverage);
            
            // Wait for the rollers to get up to speed, the launch the balls
            if(magazinePauseCount > 0) magazinePauseCount--;
            else magazineBelt->Set(Relay::kForward), magazineBelt2->Set(Relay::kForward);*/
               
            // Stop winch and drive
            frontMotor->Set(0.0f);
            rearMotor->Set(0.0f);
            
            // Code to move elsewhere or shoot goes here
         }
      } // End of IsAutonomous
   }
   
   // Second of the possible autonomous modes
   void AutonomousTwo(void)
   {
      // Variables
      int rollerPause = MAGAZINEPAUSETIME; // The time to wait for the rollers to spin up before enabling the magazine
      int divider = 0; // Count the number of IsAutonomous() loops that have passed
      int rollerSpeed = 0; // Distance roller has traveled since previous sample
      float rollerAverage = 0.0f; // Average speed of rollers based on a set number of samples
      int rollerOffset = 0; // Value of encoder from previous sample (to update current sample)
      
      while(IsAutonomous())
      {
         /* Tell the system that the code hasn't frozen. */
         GetWatchdog().Feed();
         
         // After a set period of time
         if(divider > ENCODERPERIOD)
         {
            // Reset divider
            divider = 0;
            
            // Get the number of pulses passed between this sample and the previous sample
            rollerSpeed = AbsI(shootingEncoder->Get()) - rollerOffset;
            
            // Update value for next sample
            rollerOffset += rollerSpeed;
            
            // Update the average speed
            rollerAverage = (rollerAverage * (SAMPLENUMBER - 1) + rollerSpeed) / SAMPLENUMBER;
            
            // Truncate to three decimal places (prevents division remnants from being a problem)
            rollerAverage = (int)(rollerAverage * 1000.0f) / 1000.0f;
         }
         else divider++;
         
         frontDriveServo->Set(LOWGEARPOSITION);
         rearDriveServo->Set(LOWGEARPOSITION);
         armEngageServo->Set(ARMDISENGAGEPOSITION);
         
         // Wait for the rollers to spin up before engaging the magazine belts to fire
         if(rollerPause > 0) magazineBelt->Set(Relay::kOff), magazineBelt2->Set(Relay::kOff), rollerPause--;
         else if(rollerPause > SHOOTTIME) magazineBelt->Set(Relay::kForward), magazineBelt2->Set(Relay::kForward), rollerPause--;
         else rollerPause = MAGAZINEPAUSETIME;
         
         // Spin up the rollers to the preset speed for the key shot
         SetRollerSpeed(FIRESPEEDTWO, rollerAverage);
      }
   }
   
   // Third of the possible autonomous modes
   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 gyroAngle = 0; // Current angle from home as read by the gyro
      int armStage = 0; // Current stage for bridge manipulator
      int encoderValue = 0; // Value from drive encoder
      int divider = 0; // Count the number of IsAutonomous() loops that have passed
      int rollerSpeed = 0; // Distance roller has traveled since previous sample
      float rollerAverage = 0.0f; // Average speed of rollers based on a set number of samples
      int rollerOffset = 0; // Value of encoder from previous sample (to update current sample)
      
      // Display messages
      CLEARMESSAGE;
      DISPLAYPRINTF(1, "Shoot only"); // Display which autonomous mode is being used
      
      // 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 move"); // Display which autonomous mode is being used
         DISPLAYPRINTF(2, "Gyro: %0.2f", gyroAngle);  // The float value of the gyro angle
         DISPLAYPRINTF(3, "Stage (0-4): %d", armStage); // The stage of autonomous
         DISPLAYPRINTF(4, "Encoder: %d", encoderValue); // The value of the encoder
         
         // After a set period of time
         if(divider > ENCODERPERIOD)
         {
            // Reset divider
            divider = 0;
            
            // Get the number of pulses passed between this sample and the previous sample
            rollerSpeed = AbsI(shootingEncoder->Get()) - rollerOffset;
            
            // Update value for next sample
            rollerOffset += rollerSpeed;
            
            // Update the average speed
            rollerAverage = (rollerAverage * (SAMPLENUMBER - 1) + rollerSpeed) / SAMPLENUMBER;

            // Truncate to three decimal places (prevents division remnants from being a problem)
            rollerAverage = (int)(rollerAverage * 1000.0f) / 1000.0f;
         }
         else divider++;
         
         // 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(FIRESPEEDTWO, rollerAverage);
            rollerPause--;
         }
         
         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, rollerAverage);
         }
         
         // Proceed to next stage
         else
         {
            armStage = -1;
            SetRollerSpeed(0.0f, rollerAverage);
         }
         
         // Get the gyro angle
         gyroAngle = steeringGyro->GetAngle();
         
         // Get the drive encoder value
         encoderValue = driveEncoder->Get();
         
         /* Simple, straight-movement autonomous */
         // Turn the rear wheels left to correct if we are off to the left
         // Otherwise, turn the wheels right to correct if we are off to the right
         // Otherwise, move straight
         if(gyroAngle + GYRODEADBANDANGLE < 0.0f) TrackRearSteering(PULSESPER90REAR / 8);
         else if(gyroAngle - GYRODEADBANDANGLE > 0.0f) TrackRearSteering(-PULSESPER90REAR / 8);
         else TrackRearSteering(0);
         
         // Keep the front wheels straight and drive forward slowly
         TrackFrontSteering(0);

         // Wait for shooting to finish before continuing
         if(armStage == -1) continue;
         
         // Until we have hit the bridge, move forward
         if(armStage < 2)
         {
            // Set gears to front drive and arm engagement
            frontDriveServo->SetAngle(LOWGEARPOSITION);
            rearDriveServo->SetAngle(NEUTRALGEARPOSITION);
            armEngageServo->SetAngle(ARMENGAGEPOSITION);
            
            // Drive
            frontMotor->Set(0.4);
            
            /// Drive winch until first light sensor is triggered (arm is in primed position)
            if(!armStartSensor->Get() && armStage == 0) rearMotor->Set(0.45);
            else if(armStage == 0) armStage = 1;
                        
            // Stop winch until the light sensor on the bridge dector is triggered (bridge is hit) or we travel too far (probably missing)
            if((!armHitSensor->Get() || encoderValue > AUTONOMOUSDISTANCE) && armStage == 1) rearMotor->Set(0.0f);
            else if(armStage == 1) armStage = 2;
         }
         
         // We have hit the bridge now
         else if(armStage < 4)
         {
            // Set gears to front drive and arm engagement
            frontDriveServo->SetAngle(LOWGEARPOSITION);
            rearDriveServo->SetAngle(NEUTRALGEARPOSITION);
            armEngageServo->SetAngle(ARMENGAGEPOSITION);
            
            // Stop
            frontMotor->Set(0.0f);
            
            // Continue to run winch until bottom switch is hit
            if(!armDownSwitch->Get() && armStage == 2) rearMotor->Set(0.75f);
            else if(armStage == 2) armStage = 3;
            
            // Return arm to home position
            if(!armUpSwitch->Get() && armStage == 3) rearMotor->Set(-0.4f);
            else if(armStage == 3) armStage = 4;
         }
         
         // Bridge tipping is complete
         else
         {
            // Return gears to low
            frontDriveServo->Set(LOWGEARPOSITION);
            rearDriveServo->Set(LOWGEARPOSITION);
            armEngageServo->Set(LOWGEARPOSITION);
            
            // Stop winch and drive
            frontMotor->Set(0.0f);
            rearMotor->Set(0.0f);
         }
      }
   }
}; /* Semicolons are not used after functions, but classes are treated
more like datatypes, so they DO have a semicolon after them. To think some
people say that object-oriented makes sense :/ */

// 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;
}

// Square the argument sent to it, preserving the sign
void SquareSign(float *x)
{
   if((*x) < 0) *x = -pow((*x), 2);
   else *x = pow((*x), 2);
}

/* The entry point is FRC_UserProgram_StartupLibraryInit. This macro tells
the compiler to use the SimpleTracker class. There are a series of
"under-the-hood" operations so that the program starts in this class
instead of searching for int main(void). */
START_ROBOT_CLASS(SimpleTracker);

Let me know if anything still doesn't make sense Wink

Brendan van Ryn

Posts : 95
Join date : 2010-09-30
Age : 24

Back to top Go down

code update

Post  Jacob on Thu Mar 22, 2012 8:41 pm

Thanks Brendan Very Happy

Jacob

Posts : 16
Join date : 2011-10-04

Back to top Go down

Final Version

Post  Brendan van Ryn on Sun Apr 01, 2012 8:41 pm

Here's the final version of the code from after GTR. I took the liberty of cleaning things up a bit by making sure that any missing comments were filled in and that any unused remnants of older code were removed. Take a look, because any modifications from now on will be made by you guys Wink

Code:


/******************************************************************************
 *
 *    Sprockets 2012
 *    Authors:    Brendan van Ryn
 *             Stuart Sullivan
 *             Aliah McCalla
 *             Will Surmak
 *              Caleb McAnuff
 *            Jacob 'tMannetje    
 *             Daniel "Raphael" Garza
 * Last Updated:   1/04/2012
 *
 ******************************************************************************/


// 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>

// 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();}

// 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
    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);
       
        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; // The direction of the drive (allows quick reversing)
      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
         leftx = direction * leftStick->GetRawAxis(1);
         lefty = direction * leftStick->GetRawAxis(2);
         rightx = direction * 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 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;
         }
         
         // 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;
            
         // Average the joystick positions
         x = (leftx + rightx) / 2.0f;
         y = (lefty + righty) / 2.0f;
                  
         // Store the states of the toggle buttons from the previous iteration
         previousButtonState = currentButtonState;
         reverseTogglePrevious = reverseToggleCurrent;
         
         // If we are manipulating the bridge
         if(armPosition == ARMENGAGEPOSITION && servoPauseCount <= 0 && !armOverride)
         {
            // Exit if the button has been released
            if(!BRIDGEMANIPULATEBUTTON) armStage = 4;
            
            // Drive winch until first light sensor is triggered (arm is in primed position)
            if(!armStartSensor->Get() && armStage == 0) rearMotor->Set(0.5f);
            else if(armStage == 0) armStage = 1;
            
            // Stop winch until the light sensor on the bridge dector is triggered (bridge is hit)
            if(!armHitSensor->Get() && armStage == 1) rearMotor->Set(0.0f);
            else if(armStage == 1) armStage = 2;
            
            // Continue to run winch until bottom switch is hit
            if(!armDownSwitch->Get() && armStage == 2) rearMotor->Set(0.85f);
            else if(armStage == 2) armStage = 3;
               
            // Wait for manipulator button to be released
            if(BRIDGEMANIPULATEBUTTON && armStage == 3) rearMotor->Set(0.0f);
            else if(armStage == 3) armStage = 4;
            
            // Return arm
            if(!armUpSwitch->Get() && armStage == 4) rearMotor->Set(-0.2f);
            else if(armStage == 4) armStage = 5;
            
            // Disengage arm, set drive units to low gear, and stop winch motor; reset the armStage for next time
            if(armStage == 5)
            {
               rearGearSetting = LOWGEARPOSITION;
               frontGearSetting = LOWGEARPOSITION;
               armPosition = LOWGEARPOSITION;
               
               rearMotor->Set(0.0f);
            }
         }
                  
         // 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(righty);
            
            // Allow manual firing if no automatic firing is in use
            if(!preFire) SetRollerSpeed(speedz, shooterSpeed);
         }
                  
         /* 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;
         
            // 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 and we 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;
      if(distance > 1.0f) distance = 1.0f;
      
      // 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)
   {
      // 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);
         armEngageServo->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(FIRESPEEDTWO, 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 < STRAFEDISTANCE)
         {
            TrackFrontSteering((int)(-PULSESPER90FRONT * 0.78));
            TrackRearSteering((int)(-PULSESPER90REAR * 0.78));
            frontMotor->Set(0.75f);
            rearMotor->Set(0.75f);
         }
         
         // Stop
         else
         {
            TrackFrontSteering(0);
            TrackRearSteering(0);
            frontMotor->Set(0.0f);
            rearMotor->Set(0.0f);
         }
      }
   }
   
   // Second of the possible autonomous modes
   void AutonomousTwo(void)
   {   
      while(IsAutonomous())
      {
         /* Tell the system that the code hasn't frozen. */
         GetWatchdog().Feed();
         
         // Display which autonomous mode we're in
         CLEARMESSAGE;
         DISPLAYPRINTF(1, "Shoot only");
         
         // Keep the wheels straight
         TrackFrontSteering(0);
         TrackRearSteering(0);
         
         // Set the servos
         frontDriveServo->SetAngle(LOWGEARPOSITION);
         rearDriveServo->SetAngle(LOWGEARPOSITION);
         armEngageServo->SetAngle(ARMDISENGAGEPOSITION);
         
         // Spin up the rollers to the preset speed for the key shot
         SetRollerSpeed(FIRESPEEDTWO, rollerVoltage->GetVoltage());
         
         // Move belts to fire
         magazineBelt->Set(Relay::kForward), magazineBelt2->Set(Relay::kForward);
         
         // Run the collectors in case another robot feeds us
         transverseCollector->Set(Relay::kReverse);
         lateralCollector->Set(COLLECTORSPEED);
      }
   }
   
   // Third of the possible autonomous modes
   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 gyroAngle = 0; // Current angle from home as read by the gyro
      float shooterSpeed = 0.0f; // Current voltage reading
      int armStage = 0; // Current stage for bridge manipulator
      int encoderValue = 0; // Value from drive encoder
      int downPauseCount = ARMDOWNTIME; // Amount of time to wait while arm is down
      int servoPauseCount = SERVOPAUSETIME; // Time to wait for servos to shift
      float rearSetting = LOWGEARPOSITION, frontSetting = LOWGEARPOSITION, armSetting = LOWGEARPOSITION; // Servo posiions
      
      // 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();
         
         // Display messages
         CLEARMESSAGE;
         DISPLAYPRINTF(1, "Shoot then tip bridge"); // Display which autonomous mode is being used
         DISPLAYPRINTF(2, "Gyro: %0.2f", gyroAngle);  // The float value of the gyro angle
         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);
         armEngageServo->SetAngle(armSetting);
         
         // 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(FIRESPEEDTWO, shooterSpeed);
            rollerPause--;
            
            // Keep wheels straight
            TrackFrontSteering(0);
            TrackRearSteering(0);
            
            // Set servos
            frontSetting = LOWGEARPOSITION;
            rearSetting = NEUTRALGEARPOSITION;
            armSetting = ARMENGAGEPOSITION;
         }
         
         // 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(FIRESPEEDTWO, shooterSpeed);
            
            // Keep wheels straight
            TrackFrontSteering(0);
            TrackRearSteering(0);
            
            // Set servos
            frontSetting = LOWGEARPOSITION;
            rearSetting = LOWGEARPOSITION;
            armSetting = LOWGEARPOSITION;
         }
         
         // Wait for servos to shift
         else if(servoPauseCount > 0)
         {
            servoPauseCount--;
            
            // Set servos
            frontSetting = LOWGEARPOSITION;
            rearSetting = NEUTRALGEARPOSITION;
            armSetting = ARMENGAGEPOSITION;
         }
         
         // Perform bridge manipulation
         else if(armStage < 3)
         {
            // Stop rollers
            SetRollerSpeed(0.0f, shooterSpeed);
         
            // Get the gyro angle
            gyroAngle = steeringGyro->GetAngle();
            
            // Get the drive encoder value
            encoderValue = AbsI(driveEncoder->Get());
            
            // Turn left or right based on gyro reading to stay straight
            if(gyroAngle + GYRODEADBANDANGLE < 0.0f) TrackRearSteering(PULSESPER90REAR / 8);
            else if(gyroAngle - GYRODEADBANDANGLE > 0.0f) TrackRearSteering(-PULSESPER90REAR / 8);
            else TrackRearSteering(0);
            
            // Keep the front wheels straight and drive forward slowly
            TrackFrontSteering(0);
            
            // Set servos
            frontSetting = LOWGEARPOSITION;
            rearSetting = NEUTRALGEARPOSITION;
            armSetting = ARMENGAGEPOSITION;
            
            // Drive forward
            frontMotor->Set(0.5f);
            
            // Drive winch until first light sensor is triggered (arm is in primed position)
            if(!armStartSensor->Get() && armStage == 0) rearMotor->Set(0.5f);
            else if(armStage == 0) armStage = 1;
            
            // Stop winch until the light sensor on the bridge dector is triggered (bridge is hit) or we travel too far on the encoder
            if(!armHitSensor->Get() && encoderValue < AUTONOMOUSDISTANCE && armStage == 1) rearMotor->Set(0.0f);
            else if(armStage == 1) armStage = 2;
            
            // Continue to run winch until bottom switch is hit
            if(!armDownSwitch->Get() && armStage == 2) rearMotor->Set(0.85f);
            else if(armStage == 2) armStage = 3;
         }
         
         // Wait for a bit
         else if(armStage == 3)
         {
            frontMotor->Set(0.0f);
            rearMotor->Set(0.0f);
            
            if(downPauseCount <= 0) armStage = 4;
            else downPauseCount--;
         }
         
         // Stop moving and return arm to hom
         else if(armStage == 4)
         {
            frontMotor->Set(0.0f);
            
            if(!armUpSwitch->Get()) rearMotor->Set(-0.2f);
            else armStage = 5;
         }
         
         // Bridge tipping is complete
         else
         {
            // Return gears to low
            frontSetting = LOWGEARPOSITION;
            rearSetting = LOWGEARPOSITION;
            armSetting = LOWGEARPOSITION;
            
            // Stop winch and drive
            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);

And the header file:
Code:


/* 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 speedStick->GetRawButton(9)

#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 HIGHGEARBUTTON leftStick->GetRawButton(5)
#define LOWGEARBUTTON leftStick->GetRawButton(4)
#define NEUTRALGEARBUTTON leftStick->GetRawButton(3)
#define ARMUNWINDBUTTON rightStick->GetRawButton(11)

#define BRIDGEMANIPULATEBUTTON speedStick->GetRawButton(7)
#define PIVOTMODEBUTTON rightStick->GetRawButton(2)

#define SHOOTBUTTON (speedStick->GetRawButton(1) && (speedStick->GetRawButton(3) || speedStick->GetRawButton(4) || speedStick->GetRawButton(5) || speedStick->GetRawButton(8)))
#define FIRSTFIREBUTTON speedStick->GetRawButton(4)
#define SECONDFIREBUTTON speedStick->GetRawButton(3)
#define THIRDFIREBUTTON speedStick->GetRawButton(5)
#define FOURTHFIREBUTTON speedStick->GetRawButton(8)

#define MAGAZINEENABLEBUTTON speedStick->GetRawButton(11)
#define MAGAZINEREVERSEBUTTON speedStick->GetRawButton(10)
#define COLLECTORENABLEBUTTON speedStick->GetRawButton(11)
#define COLLECTORREVERSEBUTTON speedStick->GetRawButton(10)

// 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

// 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
#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 SHOOTERAPPROACHSPEED 0.60f // 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.5f // 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 STRAFEDISTANCE 1900 // Distance to strafe in first autonomous mode
#define ARMDOWNTIME 8000 // Time in autonomous mode to pause when the arm first lowers

// Shooter constants
#define FIRESPEEDONE 5.65f // Pre-set firing speeds
#define FIRESPEEDTWO 6.80f
#define FIRESPEEDTHREE 3.2f
#define FIRESPEEDFOUR 2.0f

Brendan van Ryn

Posts : 95
Join date : 2010-09-30
Age : 24

Back to top Go down

The Chess Assignment

Post  Brendan van Ryn on Mon Apr 09, 2012 5:28 pm

Several days ago, Will came to me about one of the programming assignments I gave you guys: given the starting position of a knight on a chess board, calculate the minimum number of moves required to reach a given target position. Below is my solution to the assignment, commented just a bit to try and help out. There are quite a few important things that this program introduces, so I've explained them a bit below. Many of these concepts are a bit on the advanced side of things, so I don't plan to go into too much detail. If you want to learn more about any of them, you can definitely find these topics in any good C book or online tutorial, or you could even ask me in person anytime (especially at our programming meetings). So, here it is:

Code:

/* Header files */
#include <stdio.h> // Input and output functions
#include <stdlib.h> // Utility functions

/* Macros */
// Indicate the direction of movement
#define DELTA_NONE 0 // No movement (used for first move)
#define DELTA_X1Y2 1 // Right one and down two
#define DELTA_X1YN2 2 // Right one and up two
#define DELTA_XN1Y2 3 // Left one and down two
#define DELTA_XN1YN2 4 // Left one and up two
#define DELTA_X2Y1 5 // Right two and down one
#define DELTA_X2YN1 6 // Right two and up one
#define DELTA_XN2Y1 7 // Left two and down one
#define DELTA_XN2YN1 8 // Left two and up one

// The maximum number of moves allowed
#define MAX_MOVES 6

/* Global variables (accessible to ALL functions) */
int positionStack[64][2]; // The x- and y- positions of all of the squares we've been to so far
int stackTop = 0; // The number of moves so far

/* Function prototypes */
int FindMoves(int, int, char, int, int); // Find number of moves to move from start to end

/* Main program starts here */
int main(void)
{
    /* Local variables */
    int startx, starty; // X- and y-coordinates of the knight's starting position
    int endx, endy; // X- and y-coordinates of the knight's target position
    int ret; // Store result of FindMoves() function, and act as a temporary variable for error-checking
    char input[50]; // Store the raw input from the user
    char *remain, *remain2; // When strtol() converts a string to a number, the rest of the string is stored in these variables
   
    // Get the starting position from the user and store the x- and y-components
    printf("Enter the knight's starting position (x y): ");
    fgets(input, 50, stdin);
    startx = strtol(input, &remain, 10);
    starty = strtol(remain, &remain2, 10);
   
    // Get the target position from the user and split into the x- and y-components
    printf("Enter the knight's target position (x y): ");
    fgets(input, 50, stdin);
    endx = strtol(input, &remain, 10);
    endy = strtol(remain, &remain2, 10);
   
    // Check if the starting or ending positions are off of the chess board
    ret = 1;
    ret = ret && (startx >= 0 && startx < 8);
    ret = ret && (starty >= 0 && starty < 8);
    ret = ret && (endx >= 0 && endx < 8);
    ret = ret && (endy >= 0 && endy < 8);
   
    // If so, display and eror message and exit
    if(!ret)
    {
        printf("\nError: Coordinates must be in the range [0-7]\r\n");
        system("pause");
        return 1;
    }
   
    // Otherwise, find the number of moves required
    ret = FindMoves(startx, starty, DELTA_NONE, endx, endy);
   
    // If, somehow, it is impossible to solve the problem, display so. Otherwise, display the result
    if(ret == -1) printf("\nError: It is impossible to reach target position from starting position.\r\n");
    else printf("\n%d moves are required.\r\n", ret);
   
    // Pause and exit
    system("pause");
    return 0;
}

/* Find the number of moves required to get from (x,y) on the chess board to (targetx,targey). The delta value tells the
function where the previous position is in relation to itself so that we don't backtrack on a position we were already. This
returns the number of moves required, or -1 if it is impossible. */
int FindMoves(int x, int y, char delta, int targetx, int targety)
{
    // Local variables
    register int i; // Index for loops
    int ret[8]; // The return values of the child functions
    int minimum; // The smallest number of moves returned by the child functions
    int index; // The index in the ret[] array of this smallest value
   
    // If the current position is equal to the target position, zero moves are required
    if(x == targetx && y == targety) return 0;       
   
    // If the current position is out of bounds, return an error
    if(x >= 8 || y >= 8 || x < 0 || y < 0) return -1;
   
    // If more than the maximum number of moves have been made, return an error
    if(stackTop > MAX_MOVES) return -1;
   
    // Search through the array of previous positions
    for(i = 0; i < stackTop; i++)
      // If a match is found, we have been to this position before and are loop in a circle, so return an error
      if(positionStack[i][0] == x && positionStack[i][1] == y)
      return -1;
   
    // Add the current position to the array (a "stack", actually), and add one to the move counter (push)
    positionStack[stackTop][0] = x;
    positionStack[stackTop][1] = y;
    stackTop++;
   
    // Initialize the array to error values
    for(i = 0; i < 8; i++) ret[i] = -1;
   
    // For each possible move that does not back track, make the move and then check how many more moves are required
    if(delta != DELTA_XN2YN1) ret[0] = FindMoves(x+2, y+1, DELTA_X2Y1, targetx, targety);
    if(delta != DELTA_XN2Y1) ret[1] = FindMoves(x+2, y-1, DELTA_X2YN1, targetx, targety);
    if(delta != DELTA_X2YN1) ret[2] = FindMoves(x-2, y+1, DELTA_XN2Y1, targetx, targety);
    if(delta != DELTA_X2Y1) ret[3] = FindMoves(x-2, y-1, DELTA_XN2YN1, targetx, targety);
    if(delta != DELTA_XN1YN2) ret[4] = FindMoves(x+1, y+2, DELTA_X1Y2, targetx, targety);
    if(delta != DELTA_XN1Y2) ret[5] = FindMoves(x+1, y-2, DELTA_X1YN2, targetx, targety);
    if(delta != DELTA_X1YN2) ret[6] = FindMoves(x-1, y+2, DELTA_XN1Y2, targetx, targety);
    if(delta != DELTA_X1Y2) ret[7] = FindMoves(x-1, y-2, DELTA_XN1YN2, targetx, targety);
   
    // Look through the returned move counts
    for(i = 0, index = -1; i < 8; i++)
      // If no minimum has been stored or the current value is less than the current minimum
      if((index == -1 || ret[i] < minimum) && ret[i] >= 0)
      {
            // Update the minimum value and store the index of this value
            index = i;
            minimum = ret[i];
      }
   
    // Decrement the move counter now that our move is complete (pop the stack)
    stackTop--;
   
    // If all of the return values were errors, it is impossible to get here from there, so return an error
    if(index == -1) return -1;
   
    // Otherwise, add this move to the count of moves and return that
    return minimum + 1;
}

The first thing I want to explain quickly is the "strtol()" function and how it works. The function is defined as:

int strtol(char *toConvert, char **remain, int radix);

Strtol() takes the string (char array) "toConvert" and searches for a number. It returns the first number it finds as an int, and the rest of the string is stored in "remain". Remain is a pointer to a pointer, which may sound confusing at first, but remember in scanf() how, when we wanted a function to store a value in one of the parameters we pass to it, we passed the address using the ampersand? This is the same principle, but instead of sending the address of a variable, we're sending the address of an array (and whenever you pass an array, you're already passing the address of the first element in that array). An array is already an address, so now we're sending the address of an address, hence the double asterisks. So, after the function returns, "remain" points to the character array that holds the remainder of the string--the part after the number it found. Feel free to ask about pointers at one of our lessons later. So, in the above program, when we pass "&remain" to the function, the function alters "remain" so that it is now, essentially, the remainder of the string. So when we pass it to the next call to strtol(), the string "remain" contains only the second number of the coordinate, since the function already removed the first. Finally, "radix" is the base of the number. We pass "10" for a standard, base-ten number. Passing "2" would indicate that the string is in binary format, and the function will parse it as such.

Next, I'd like to highlight the lines in the main() function beginning with "ret = 1". Back when I explained if-statements, I underscored how C considers any non-zero value to be true, and zero to be false. To check if the positions are within the acceptable range, we could use a series of if-statments or a set of OR operators (||) within a signle if-statement. However, these ways of solving the problem are messy and difficult to read. The first is somewhat wasteful and requires a flag variable anyway, and the second creates a very long condition statement. Instead, each condition is checked individually, and the result is either true (1) or false (0). This value is then ANDed (&&) with ret. If ret is equal to one and the result is one, the new value of ret remains one (true). If ret is equal to zero, however, it remains zero and never returns to one. Finally, if the result of the condition is zero (false), the result of the AND is also zero, so ret become zero and, as per the above statement, remains false for the rest of the conditions. In this way, after all of the checks, ret will be true if and only if all of the previous conditions were true, allowing us to write one if-statement checking the value of ret, rather than several. This is just one of the ways that C works to make the programmer's job easier by allowing him or her to do whatever he or she wishes, without getting in the way. The parentheses here are just to make sure that the conditional operations (>,<,etc.) are evaluted before the equals sign (=). I can talk about this more in-person if necessary.

Now let's evaluate the "positionStack" array. It has an unusual name, and for good reason. This particular array is a special data structure called a "stack". It is very similar to a normal array, but it is organized differently. In a normal array, data is stored and read randomly from any position (or "index") in said array. However, you can think of a stack as being a literal stack of objects--let's use books as an example. If you have a stack of books, you can add another book to the top of that stack, or remove the top book from the stack, but you can't (or at least, shouldn't) add or remove books from the middle or bottom of the stack. This is the same in programming: you have an array that stores the data, and a variable that stores the position of the top of the stack. Items are "pushed" onto the top of the stack and "popped" off of the top of the stack. How this works is simple: "stackTop" starts at zero, because no items are in the array; then, when a position is added to the array, it places it at positionStack[stackTop][x], using "stackTop" to ensure that it is being inserted in the correct spot (the top of the stack). "stackTop" is then incremented to update it to the new position of the top of the stack. When a value is removed from the stack, "stackTop" is decremented. The positionStack in this program is used to track all of the locations on the chess board that we've been to so far. This makes sure that we never run around in a circle because we can check to see if the position we're currently at has already been visited, in which case we've looped around. Each time we move to a new position, we "push" that position onto the stack, and when we return back to where we used to be, we "pop" the position back off. There is plenty of information online to explain the operation of stacks (which are especially crucial at the lower level), but as always, you can post questions online or ask at the next lesson.

Finally, the most interesting aspect of this assignment is how the program actually calculates the number of moves. All of the work is done by the one, simple function, "FindMoves". It simply receives the current position of the knight, the target position, and a delta value (which I'll explain later). FindMoves() returns the number of moves required to get from the start to the end. The function starts out by performing some simple steps. First, it checks to see if the current position of the knight matches the target position. If so, the knight has reached its destination, and zero more moves are required (notice how I said zero MORE moves, as some moves might already have taken place, as we'll see). Next, the function checks to see if the current position of the knight is out of bounds. If so, it returns the error value of -1. This is a good error value because it is impossible to have a negative number of moves, so it's easy to check for this value. Since the main() function already checked that the knight's starting position is valid, checking the current position might seem strange, but again, it will become clear soon. The next step we take is checking the value of "stackTop". Since stackTop is incremented each time we make a move (and push that move onto the stack), it also represents the number of moves we've tried so far. It should never take the knight more than six moves to get from one square to another, so if more than six moves have been made, we return the same error value of -1. Finally, we run through the positionStack and check each position stored in it. If any of the previous positions we've been to match the current position we're at, we have looped around in a circle and should stop--a -1 is returned in this case as well. If the position has not yet been visited, we push it onto the stack while we perform further moves. Then, we pop the position back off of the stack.

Now, if you ignore the code in between and leave only the "stackTop--;" line and the "return" line, you have a function that will return zero when the knight is at it's target position, one if the knight is not at it's target position, and negative one if the knight cannot reach it's target position from where it is (or too many moves have been required or we've looped around; in either case, we obviously haven't found the shortest path as we were asked to). This is all fine, but it only checks the knight's current position. So, what we employ is a trick called "recursion". The function calls itself with different parameters. If the other checks up until this point have been clear, we move the knight left two and up one (by adding to the x- and y-positions) and then call FindMoves() again. Instead of imagining the computer jumping back to the top of the function, like a loop would do, I'd like you to imagine a brand new copy of that function being made. When a function calls itself, a new copy of that function is made, and the computer runs THAT copy. When it is done, it returns to the spot in the original copy where the new copy was called--and this can be done many times. This is a hard concept to grasp--it's called recursion. At each position that the function is called with, it checks to see if it's reached its target. If so, it returns zero. Otherwise, it calls new copies of the function again for each new position the knight can move to from its current position. Eventually, the target position will be reached and the function will return zero. The function that called that copy will then find that value by searching through the return values for the smallest number that is not an error (-1). This represents the fewest moves required to get from the current position to the NEXT position, so it adds one for the current move and returns that. As they return up the line, the parent (first) copy of the function will receive a variety of possible paths, each with a different number of moves, so it takes the smallest number and uses that. It's a complicated concept to grasp, but it's a useful one when used correctly as the code is small, and compact, and easy to write. I think I'll likely explain this in person if any interest is shown because it really is a great example of how computers can make solving problems much easier. The code I didn't expressly talk about just searches for the smallest value returned from its child copies so it knows the shortest route. Additionally, the "delta" value just tells the function which way the knight moved to get to where it is. This way, it knows not to jump back and forth between two spots over an over again.

So, there it is, and I hope I did a reasonable job of explaining these concepts. Try to familiarize yourselves with all of them, as they come in handy, but I'll understand about recursion if you still need more help. There's a wise saying that says "To understand recursion, you must first understand recursion," and it's really quite true. I hope this was interesting, and I'll see you guys tomorrow Wink

Brendan van Ryn

Posts : 95
Join date : 2010-09-30
Age : 24

Back to top Go down

The 2011 Code

Post  Brendan van Ryn on Fri Jun 08, 2012 11:11 pm

Apparently there is work to be done on the 2011 code. I don't know much of the modifications made to it at the elementary school demos, but this version below is the last backup from the actual 2011 season. I'm posting this as a means of helping you guys become familiar with it should you need to work with it. It is much messier, much longer, much less organized, and much more poorly commented than this year's, so I understand some apprehension upon seeing it, but take a look anyway if you will (as proof of what I said above, the forum said this post was too long, so it had to be broken in to two pieces; my tutorials don't seem so long any more, now do they?):

Code:

/******************************************************************************
 *
 *    Sprockets 2011 Code
 *    Authors:    Duane Jeffery
 *             Matt Aceto
 *            Brendan van Ryn
 *             Terry Shi
 *             Stuart Sullivan
 *            
 *    Last Updated:   2/3/2011
 *
 ******************************************************************************/

/* Note that for "historical" reasons, the rear steering unit is called the left steering unit,
 * and the right the front */

/* The autonomous move speed is found by searching for two "at" symbols, side-by-side (@@) */
/* The LOCKBUTTONSPEEDLIMIT is multiplied by the drive motor speeds we the lock button is pressed.
 * For example, 0.5 represents 50% speed. If in doubt, set to 1.0f. The arm heights may also need
 * editing, and the auto-align button (designated by the //x marker) has been assignd to an imaginary
 * button to prevent accidently being presed because it was having trouble. Re-asign it to button 4
 * on the left stick if you need it again. Finally, some swerve-drive synchronization code has been
 * added, but if it's causing problems you could revert to one of the numbered and dated backups
 * in the "Robotics 2011" folder via copying and pasting. Through the same method, please make at
 * least one backup at the end of each day (just a text file) and place in said folder with an
 * incremental name (seriously, use the EXACT same naming convention). */

// Header files
#include "WPILib.h"
#include "vision\AxisCamera.h"
#include "math.h"
#include "stdlib.h"

// Drive system definitions. Use these to specify which drive system is in use.
#define MECANUM 32
#define SWERVE 51

// Handles the tedious driverstation output overhead
// Simply pass a number and it will be sent to the driver station
#define CLEARMESSAGE DriverStationLCD::GetInstance()->Clear()
#define DISPLAYMESSAGE(b, a) DriverStationLCD::GetInstance()->Printf(DriverStationLCD::kUser_Line ## b, 1, "%d", a); DriverStationLCD::GetInstance()->UpdateLCD()
#define DISPLAYSTRING(b, a) DriverStationLCD::GetInstance()->Printf(DriverStationLCD::kUser_Line ## b, 1, a); DriverStationLCD::GetInstance()->UpdateLCD()

// Autonomous mode definitions. Use these to specify which autonomous mode to use.
#define STRAIGHT 0
#define BRANCH 1

// Determine which drive system we are using
// If the drive system is defined as MECANUM, the mecanum
// drive code is used. If it is defined as SWERVE, the swerve
// drive code is used. Otherwise, the compiler generates an error
#define DRIVESYSTEM SWERVE
#if (DRIVESYSTEM !=  MECANUM && DRIVESYSTEM != SWERVE) || !defined DRIVESYSTEM
#error "Drive system not specified"
#endif

// Ultrasonic stopping distance for autonomous
// Branch angle for y-autonomous
#define BRANCHSTOPDISTANCE 400
#define BRANCHANGLE -0.12

#define AUTO1STOPDISTANCE 1375
#define AUTOSTOPDIFFERENCE 590
#define AUTO2STOPDISTANCE AUTO1STOPDISTANCE + AUTOSTOPDIFFERENCE

// Steering mode macros for the swerve drive function
#define STRAFE 0
#define TANK 1
#define MONSTER 2
#define CAR 3
#define AUTOMONSTER_LEFT 4
#define AUTOMONSTER_RIGHT 5

// Number of pulses per rotation on the steering encoders
// To acheive a pulse value, multiply an angle by this value
// AUTOALIGNSTOP is a time limit before the autoalign function should automatically exit
#define PULSES_PER_STEER (1020)
#define AUTOALIGNSTOP 1000

// Speed of claw rollers
#define CLAWROLLERSPEED 0.6

#define LANECHANGE 1700

// Motor macros to be changed depending on whether we use Jaguars or Victors
#define LPDRIVEMOTOR Jaguar*
#define DRIVEMOTOR(a) Jaguar(a)

// Joystick deadbands
#define JOYSTICK_DEADBAND_X 0.0525f
#define JOYSTICK_DEADBAND_Y 0.100f

// Steering synchronization macros
#define STEERINGSYNCDEADBAND 5 // Manimum difference between encoders for synchronization to take effect
#define STEERINGSYNCDISTANCE 100 // Maximum distance between drive units before one steering motor is stopped completely
#define STEERINGSYNCDIVIDER 200 // Resolution of the steering synchronization code (smaller values means more frequent synchronization)

// When the lock button is pressed, multiply the drive speed by this to reduce chances of tipping
#define LOCKBUTTONLIMIT 0.625

// This represents a percentage of the maximum speed at which the steering motors should operate in the
// TrackSteering() function
#define TRACK_STEERING_SPEED_LIMIT 0.7

// Button Macros
// An x indicates that an imaginary button was specified, such as for an untested feature
#define CLAWREVERSEBUTTON !clawStick->GetRawButton(1)
#define PIVOTPOSITIONHOME clawStick->GetRawButton(2)
#define PIVOTPOSITIONMIDDLE clawStick->GetRawButton(4)
#define PIVOTPOSITIONLOW clawStick->GetRawButton(80) // x
#define PIVOTPOSITIONFULL clawStick->GetRawButton(5)
#define EXTENDBUTTON clawStick->GetRawButton(6)
#define RETRACTBUTTON clawStick->GetRawButton(7)
#define LOCKBUTTONPRESSED leftStick->GetRawButton(2)
#define PIVOTDOWNBUTTON clawStick->GetRawButton(10)
#define PIVOTUPBUTTON clawStick->GetRawButton(11)
#define PIVOTPOSITIONHIGH clawStick->GetRawButton(30) // x
#define STEERALIGNBUTTON leftStick->GetRawButton(10)
#define MONSTERENABLEBUTTON leftStick->GetRawButton(1)
#define DEPLOYMINIBOT leftStick->GetRawButton(6)
#define RETRACTMINIBOT leftStick->GetRawButton(7)
#define SWERVEENABLEBUTTON rightStick->GetRawButton(1)
#define CARENABLEBUTTON rightStick->GetRawButton(20) // x
#define EXITSTEERALIGN leftStick->GetRawButton(11)
#define PICKUPBUTTON clawStick->GetRawButton(9)
#define DRIVEREVERSEOFF (leftStick->GetRawButton(4) || leftStick->GetRawButton(5) || leftStick->GetRawButton(3))
#define DRIVEREVERSEON (rightStick->GetRawButton(4) || rightStick->GetRawButton(5) || rightStick->GetRawButton(3))
#define PREPICKUPBUTTON clawStick->GetRawButton(8)

// Slot for second digital sidecar
#define DIGITALSIDECARTWO 6

// Autonomous travel time (how long should it strafe after the branch?)
#define AUTONOMOUSTRAVELTIME 3300

// Autonomous straight section time
#define AUTONOMOUSSTRAIGHT 1600

// Autonomous manipulation time
#define MANIPULATIONTIME 6000

// Delay between arriving at rack and deploying tube in autonomous
#define RETRACTTIME 3000

// Amount of time for the tube to be manipulated when the claw switch is triggered
#define AUTOMANIPULATETIME 1750

// Travel speed an angle along branch
#define BRANCHSPEED 0.5

// Port number macros in case things get moved around
#define RIGHTJOYSTICKPORT 1
#define LEFTJOYSTICKPORT 4
#define CLAWJOYSTICKPORT 3

#define LIGHTSENSORPORT1 1
#define LIGHTSENSORPORT2 2
#define LIGHTSENSORPORT3 3
#define LIGHTSENSORPORT4 13

#define ARMENCCHANNELA 7
#define ARMENCCHANNELB 6
#define LEFTSTEERINGENCODERA 12
#define LEFTSTEERINGENCODERB 11
#define RIGHTSTEERINGENCODERA 14
#define RIGHTSTEERINGENCODERB 13

#define EXTENDLIMITSWITCHPORT 4
#define RETRACTLIMITSWITCHPORT 8
#define PIVOTLIMITSWITCHUPPORT 10
#define PIVOTLIMITSWITCHDOWNPORT 9
#define MINIBOTLIMITSWITCHPORT 1

#define STEERINGLIMITFRONTRIGHT 2
#define STEERINGLIMITFRONTLEFT 3
#define STEERINGLIMITBACKRIGHT 4
#define STEERINGLIMITBACKLEFT 5

#define LFMOTORPORT 1
#define LRMOTORPORT 2
#define RFMOTORPORT 3
#define RRMOTORPORT 4
#define LEFTSTEERINGPORT 5
#define RIGHTSTEERINGPORT 6

#define EXTENDMOTORPORT 9
#define PIVOTMOTORPORT 10
#define CLAWTOPMOTORPORT 7
#define CLAWBOTTOMMOTORPORT 8

#define MINIBOTDEPLOYPORT 1
#define MINIBOTRETRACTPORT 2
#define BRAKEPORT 3
#define MINIBOTDEPLOYPORT2 4

#define COMPRESSORSWITCHPORT 12
#define COMPRESSORSPIKEPORT 8
#define CLAWSWITCHPORT 9

#define ULTRAPORT 3
#define ARMLIGHTSENSOR 10

#define SWERVEBLOCKENCODER1 8
#define SWERVEBLOCKENCODER2 7

#define AUTOSWITCHPORT 12

#define PREPICKUPEXTEND 35

// Arm positions
#define ARM_FULLUP 140
#define ARM_RACKTOP 120
#define ARM_RACKMIDDLE 30
#define ARM_RACKBOTTOM 12
#define ARM_HOME 0

// Arm extension limits
#define ARMEXTENDHOME 33
#define ARMEXTENDRACK 60

// Movement speed limits
#define ARMLIMITHEIGHT 25
#define ARMLIMITFACTOR 0.8

// Arm approach distances and speeds
#define ARMAPPROACHUP 25
#define ARMAPPROACHDOWN 25
#define ARMUPSPEED 1.0
#define ARMDOWNSPEED -1.0
#define ARMEXTENDSPEED 1
#define ARMDEADBAND 6

// Steering approach distance
#define STEERING_APPROACH_DISTANCE 100.0f
#define STEERING_DEADBAND 0.03f

// Function prototypes
float Abs(float); // Absolute value function

class MecanumDrive
{
public:
   LPDRIVEMOTOR lfDrive; // Create a pointer to a Jag/Victor for the LF Motor
   LPDRIVEMOTOR lrDrive; // Create a pointer to a Jag/Victor for the LR Motor
   LPDRIVEMOTOR rfDrive; // Create a pointer to a Jag/Victor for the RF Motor
   LPDRIVEMOTOR rrDrive; // Create a pointer to a Jag/Victor for the RR Motor
   Victor* leftSteer; // Create a pointer to a Jag/Victor for the left steering motor
   Victor* rightSteer; // Create a pointer to a Jag/Victor for the right steering motor
   Victor* extendMotor; // Create a pointer to a victor for the arm extend motor
   Victor* pivotMotor; // Create a pointer to a victor for the arm pivot motor
   Victor* topclawmotor; // Create a pointer to a victor for the top claw motor
   Victor* bottomclawmotor; // Create a pointer to a victor for the bottom claw motor
   Encoder *leftSteeringEncoder; // Create a pointer to an encoder for the left steering motor
   Encoder *rightSteeringEncoder; // Create a pointer to an encoder for the right steering motor
   RobotDrive *robotDrive; // Create a pointer to a default drive system
   DigitalInput *frontleftSteerLimit; // These limit switches prevent wheel rotation beyond limits
   DigitalInput *frontrightSteerLimit; //
   DigitalInput *backleftSteerLimit; //
   DigitalInput *backrightSteerLimit; //
   DigitalInput *ClawSwitch; // Limit switch on claw to detect tube presence
   float output_speed; // Used by swerve drive
   bool flag2, flag3; // Various flags used by swerve drive
   int DriveReverse; // Either 1 or -1 for forward or reversed drive, respectively
   float y_sign; // Sign of joystick y-movement for the drive reverse feature
   int oldLeftEncoderValue, oldRightEncoderValue; // Hold the values of the encoders since the last time the swerve-synchronizing code was called
   int trip; // Increment each time third sensor is tripped, to check for consistency and prevent noise from being a problem

public:
   MecanumDrive(UINT32 lfMotor, UINT32 lrMotor, UINT32 rfMotor, UINT32 rrMotor)
   {
      lfDrive = new DRIVEMOTOR(lfMotor); // Create drive motor pointers
      lrDrive = new DRIVEMOTOR(lrMotor);
      rfDrive = new DRIVEMOTOR(rfMotor);
      rrDrive = new DRIVEMOTOR(rrMotor);
      extendMotor = new Victor(EXTENDMOTORPORT); // Create pointer to arm extend motor
      pivotMotor = new Victor(PIVOTMOTORPORT); // Create pointer to arm pivot motor
      topclawmotor = new Victor(CLAWTOPMOTORPORT); //Create a pointer to the top claw motor
      bottomclawmotor = new Victor(CLAWBOTTOMMOTORPORT); // Create a pointer to the bottom claw motor
      robotDrive = new RobotDrive(lfMotor, lrMotor, rfMotor, rrMotor); // The mecanum function assumes that a robot drive with this setup has been created
      frontrightSteerLimit = new DigitalInput(DIGITALSIDECARTWO, STEERINGLIMITFRONTRIGHT); // Create steering limit switches
      frontleftSteerLimit = new DigitalInput(DIGITALSIDECARTWO, STEERINGLIMITFRONTLEFT);
       backrightSteerLimit = new DigitalInput(DIGITALSIDECARTWO, STEERINGLIMITBACKRIGHT);
      backleftSteerLimit = new DigitalInput(DIGITALSIDECARTWO, STEERINGLIMITBACKLEFT);
      ClawSwitch = new DigitalInput(DIGITALSIDECARTWO, CLAWSWITCHPORT); // Create a limit switch to detect tube presence in claw

      leftSteeringEncoder = new Encoder(LEFTSTEERINGENCODERA, LEFTSTEERINGENCODERB); // Encoders to track rotation positions of motors
      rightSteeringEncoder = new Encoder(RIGHTSTEERINGENCODERA, RIGHTSTEERINGENCODERB);
      leftSteer = new Victor(LEFTSTEERINGPORT); // Motor controller for steering movement
      rightSteer = new Victor(RIGHTSTEERINGPORT);
      DriveReverse = 1; // Store the state of the drive reverse feature (1 = forward)
      reset_variables(); // Reset and start all encoders and clear various state variables

   }//End of MecanumDrive constructor

   // Track the line and check the ultrasonic sensor distance for autonomous
   int MoveSwerve(int leftSensor, int rightSensor, int endOfLineSensor, int armPivot, AnalogChannel *ultraDistance, int AutoMode, int BlockEncoder)
   {
      int AUTOSTOPDISTANCE; // Stores the distance to stop (as read by the ultrasonic sensor)
      
      if(AutoMode) AUTOSTOPDISTANCE = 500; // Straightline stopping distance
      else AUTOSTOPDISTANCE = 690; // Y-branch stopping distance
      
      // If we are within the decided distance from the wall
      if((ultraDistance->GetVoltage() * 1000 < AUTOSTOPDISTANCE && BlockEncoder == 0) || Abs(BlockEncoder) > AUTO1STOPDISTANCE)
      {
         // Increment a trip counter
         trip++;
         
         // If we have been within the given distance for long enough, we can hopefully
         // Rule out error on the part of the sensor, and stop, returning our state
         if(trip > 320)
         {
            lfDrive->Set(0);
            lrDrive->Set(0);
            rfDrive->Set(0);
            rrDrive->Set(0);
            
            // Robot should stop moving
            return 1;
         }
      }
      
      // Anything too short to reach 320 was probably just interference from debris, so lower the trip value again
      else if(trip > 0) trip--;
      
      // If the right sensor hits the line
      if (rightSensor)
      {
         // Stop the left motors
         lfDrive->Set(0);
         lrDrive->Set(0);
         rfDrive->Set(0.9);
         rrDrive->Set(0.9);
      }

      // If the right sensor has not hit the line, but the left one has,
      // set the motors to turn right
      else if (leftSensor)
      {
         lfDrive->Set(0.9);
         lrDrive->Set(0.9);
         rfDrive->Set(0);
         rrDrive->Set(0);
      }

      // If neither sensor has hit the line, move forward normally
      else
      {
         lfDrive->Set(0.57);
         lrDrive->Set(0.57);
         rfDrive->Set(0.57);
         rrDrive->Set(0.57);
      }

      // Robot should continue moving
      return 0;
   }
   
   // Track the line and check the ultrasonic sensor distance for autonomous
   int MoveReverse(int leftSensor, int rightSensor, int clawSwitch)
   {
         if(!clawSwitch)
         {
               lfDrive->Set(0);
               lrDrive->Set(0);
               rfDrive->Set(0);
               rrDrive->Set(0);
               
               // Robot should stop moving
               return 1;
         }
         
         // If the right sensor hits the line
         if (rightSensor)
         {
            // Stop the left motors
            lfDrive->Set(0);
            lrDrive->Set(0);
            rfDrive->Set(-0.9);
            rrDrive->Set(-0.9);
         }

         // If the right sensor has not hit the line, but the left one has,
         // set the motors to turn right
         else if (leftSensor)
         {
            lfDrive->Set(-0.9);
            lrDrive->Set(-0.9);
            rfDrive->Set(0);
            rrDrive->Set(0);
         }

         // If neither sensor has hit the line, move forward normally
         else
         {
            lfDrive->Set(-0.4);
            lrDrive->Set(-0.4);
            rfDrive->Set(-0.4);
            rrDrive->Set(-0.4);
         }

         // Robot should continue moving
         return 0;
   }

   // Swerve drive function
   void DriveSwerve(Joystick *leftStick, Joystick *rightStick, Joystick *clawStick, DigitalInput *ArmPivotLimitDown, DigitalInput *ArmPivotLimitUp, DigitalInput *ArmRetractLimit, DigitalInput *ArmExtendLimit, Encoder *ArmEncoder, int ArmTarget, int ArmPosition, char AutonomousOverride, int ArmPickup, int ArmDeploy, int ArmExtendTarget)
   {
      static int monsterenabled = 0, carenabled = 0, align = 0, rotateSwitch = 0; // Steering modes and alignment state variables
      static int syncDivider = 0; // We only call run the synchronization code periodically
      static int aligntime = 0; // We need to give the wheels a little extra time after they reach their goal
      float right_x, right_y, left_x, left_y, clawy; // Displacements of joystick axes
      int SteerMode; // Between TANK, MONSTER, STRAFE, AUTOMONSTER(RIGHT/LEFT), and CAR
      
      if(DRIVEREVERSEON) DriveReverse = -1;
      if(DRIVEREVERSEOFF) DriveReverse = 1;
      
      if (AutonomousOverride == 1)
      {
         right_x = left_x = -0.32;
         right_y = left_y = 0.45;
      }

      else if (AutonomousOverride == 3)
      {
         right_x = left_x = 0.32;
         right_y = left_y = 0.45;
      }

      else if (AutonomousOverride == 2)
      {
         // Return wheels to home and stop
         right_x = 0.01;
         right_y = 0.01;
         left_x = 0.01;
         left_y = 0.01;
      }

      // If the wheels are turned full left, indicating the end of the first stage of auto-align,
      // reset the encoders so that this new position is considered center. Continue to stage three
      else if (align == 2)
      {
         align = 3;
         DriverStationLCD::GetInstance()->Clear();
         DriverStationLCD::GetInstance()->Printf(DriverStationLCD::kUser_Line1, 1, "mode 2");
         DriverStationLCD::GetInstance()->UpdateLCD();
         rightSteeringEncoder->Reset();
         leftSteeringEncoder->Reset();
      }

      // If we are in the final stage of wheel alignment, we set the goal to "full-right", which should translate to centre
      // because the wheels are turned full-left.
      // Once the wheels are centred, we reset the encoders again and exit wheel-alignment mode
      else if (align >= 3)
      {
         DriverStationLCD::GetInstance()->Clear();
         DriverStationLCD::GetInstance()->Printf(DriverStationLCD::kUser_Line1, 1, "mode 3");
         DriverStationLCD::GetInstance()->Printf(DriverStationLCD::kUser_Line2, 1, "%d",
               leftSteeringEncoder->Get());
         DriverStationLCD::GetInstance()->Printf(DriverStationLCD::kUser_Line3, 1, "%d",
               rightSteeringEncoder->Get());
         DriverStationLCD::GetInstance()->UpdateLCD();

         right_x = 1.0;
         left_x = 1.0;
         right_y = 0.01;
         left_y = 0.01;

         // If the steering motors have finished centering, reset them to center
         // and exit auto-align mode
         if(leftSteeringEncoder->Get() > AUTOALIGNSTOP && rightSteeringEncoder->Get() < -AUTOALIGNSTOP)
         {
            aligntime--;
            if(aligntime < 1)
            {
               leftSteeringEncoder->Reset();
               rightSteeringEncoder->Reset();
               align = 0;
            }
         }
         
         else aligntime++;
      }

      else
      {
         // Read position of left and right drive joysticks
         right_x = rightStick->GetRawAxis(1);
         right_y = rightStick->GetRawAxis(2);
         left_x = leftStick->GetRawAxis(1);
         left_y = leftStick->GetRawAxis(2);
      }
      
      // Get position of claw y-axis
      clawy = clawStick->GetRawAxis(2);

      // If the claw stick is less than the deadband, ignore it
      if (Abs(clawy) <= JOYSTICK_DEADBAND_Y)
         clawy = 0.0f;

      // Check the monster/car/swerve buttons
      if (SWERVEENABLEBUTTON)
         monsterenabled = 0, carenabled = 0;
      else if (MONSTERENABLEBUTTON)
         monsterenabled = 1, carenabled = 0;
      else if (CARENABLEBUTTON)
         monsterenabled = 0, carenabled = 1;

      // If the auto-align button is pressed, set the align flag and enter strafe mode
      if (STEERALIGNBUTTON)
      {
         align = 1;
         SteerMode = STRAFE;
         monsterenabled = 0;
         carenabled = 0;
      }
      
      // In case something goes wrong, this button exits auto-align mode prematurely
      if (EXITSTEERALIGN && align > 0)
      {
         leftSteeringEncoder->Reset();
         rightSteeringEncoder->Reset();
         align = 0;
      }

      // If the arm should extend, set the motor forward
      // If the arm should retract, set the motor backward
      // Otherwise, stop any previous motion
      if ((EXTENDBUTTON || ArmPickup == 1) && ArmExtendLimit->Get())
         extendMotor->Set(1);
      else if ((RETRACTBUTTON || ArmDeploy || ArmPickup == 2) && ArmRetractLimit->Get())
         extendMotor->Set(-1);
      else if(ArmExtendTarget == -1) // *&^%$#@!~`
         extendMotor->Set(0);

      // If the arm should pivot up, and the arm has not hit the limit switch, set the motor forward
      // If the arm should pivot down, and the arm has not hit the limit switch, set the motor backward
      // Otherwise, stop any previous motion
      if (PIVOTUPBUTTON && ArmPivotLimitUp->Get())
         pivotMotor->Set(ARMUPSPEED);
      else if (PIVOTDOWNBUTTON && ArmPivotLimitDown->Get())
         pivotMotor->Set(ARMDOWNSPEED);
      else if (ArmTarget == -1)
         pivotMotor->Set(0);

      // If the arm hits the lower limit switch, reset the encoder
      if (!ArmPivotLimitDown->Get())
         ArmEncoder->Reset();
      
      if(!ClawSwitch->Get() && clawy > 0 && !CLAWREVERSEBUTTON) clawy = 0;
      
      /*// If the tube triggers the claw switch, enter rotate mode
      // Otherwise, exit and reset the time counter
      if(!ClawSwitch->Get()) rotateSwitch = 1;
      else rotateSwitch = 0, rotateTime = 0;*/
      
      // If it's in auto-pickup mode, the rollers should suck in
      // Unless the switch has been hit
      if(ArmPickup == 1 && !rotateSwitch)
      {
         topclawmotor->Set(-1);
         bottomclawmotor->Set(1);
      }
      
      /*// If the switch has been hit, manipulate the tube for a period of time
      // to point it upward
      else if(ArmPickup && rotateTime < AUTOMANIPULATETIME)
      {
         rotateTime++;
         topclawmotor->Set(-1);
         bottomclawmotor->Set(-1);
      }*/
      
      // If it's in auto-deploy mode, the rollers should eject
      else if(ArmDeploy)
      {
         topclawmotor->Set(1);
         bottomclawmotor->Set(-1);
      }
      
      // If the trigger on the claw stick is pressed, set both motors in the same
      // direction, according to the joystick axis
      else if(!CLAWREVERSEBUTTON)
      {
         topclawmotor->Set(-clawy);
         bottomclawmotor->Set(clawy);
      }
      
      else
      {
         // Otherwise, spin the motors inverse to each other.
         // The motor that spins outward travels more slowly to
         // prevent the tube from sliding out.
         if(clawy > 0)
         {
            topclawmotor->Set(-clawy);
            bottomclawmotor->Set(-clawy / 2);
         }
         
         else
         {
            topclawmotor->Set(-clawy / 2);
            bottomclawmotor->Set(-clawy);
         }
      }

      // If both joysticks have been tilted sideways by more than the dead band amount and monster mode is enabled, set to monster mode
      // Otherwise, if both joysticks have been tilted sideways by more than the dead band amount, set to strafe mode
      // Otherwise, set to tank mode
      if ((Abs(right_x) > JOYSTICK_DEADBAND_X && Abs(left_x) > JOYSTICK_DEADBAND_X) || LOCKBUTTONPRESSED)
      {
         if (monsterenabled)
            SteerMode = MONSTER;
         else if (carenabled)
            SteerMode = CAR;
         else
            SteerMode = STRAFE;
      }
      
      // If the left joystick points forward and the right points backward, enter full-angle monster clockwise
      else if(left_y > JOYSTICK_DEADBAND_Y && right_y < -JOYSTICK_DEADBAND_Y)
      {
         SteerMode = AUTOMONSTER_RIGHT;
      }
      
      // If the right joystick points backward and the left joystick points backward, enter full-angle monster counter-clockwise
      else if(left_y < -JOYSTICK_DEADBAND_Y && right_y > JOYSTICK_DEADBAND_Y)
      {
         SteerMode = AUTOMONSTER_LEFT;
      }
      
      else SteerMode = TANK;

      //*******************************************************************************************
      //STEERING ALIGNMENT
      //*******************************************************************************************
      // If steering alignment buttons are held on joystick and the auto-align mode is not running
      if (rightStick->GetRawButton(8) && rightStick->GetRawButton(9) && !align)
      {

         if (rightStick->GetRawButton(6))
         { //move left steering to right
            if (backrightSteerLimit->Get())
               leftSteer->Set(-0.4);
            else
               leftSteer->Set(0);

            leftSteeringEncoder->Stop();
            leftSteeringEncoder->Reset();
            flag2 = true; //***j
         }

         else if (rightStick->GetRawButton(7))
         { //move lrft steering to left
            if (backleftSteerLimit->Get())
               leftSteer->Set(0.4);
            else
               leftSteer->Set(0);

            leftSteeringEncoder->Stop();
            leftSteeringEncoder->Reset();
            flag2 = true; //***j
         }

         else if (flag2)
         {
            leftSteer->Set(0);
            leftSteeringEncoder->Start();
            flag2 = false; //***j
         }

         if (rightStick->GetRawButton(10))
         { //move right steering to right
            if (frontrightSteerLimit->Get())
               rightSteer->Set(-0.4);
            else
               rightSteer->Set(0);

            rightSteeringEncoder->Stop();
            flag3 = true;
            rightSteeringEncoder->Reset();
         } else if (rightStick->GetRawButton(11))
         { //move right steering to left
            if (frontleftSteerLimit->Get())
               rightSteer->Set(0.4);
            else
               rightSteer->Set(0);

            rightSteeringEncoder->Stop();
            flag3 = true;
            rightSteeringEncoder->Reset();
         } else if (flag3)
         {
            rightSteer->Set(0);
            rightSteeringEncoder->Start();
            flag3 = false;
         }
      } //end of if steering alignment buttons are held on joystick
      
      // If auto-align has been initiated, move until both limit switches are hit
      else if (align == 1)
      {

         DriverStationLCD::GetInstance()->Clear();
         DriverStationLCD::GetInstance()->Printf(DriverStationLCD::kUser_Line1, 1, "mode 1");
         DriverStationLCD::GetInstance()->UpdateLCD();

         if (frontleftSteerLimit->Get())
            rightSteer->Set(0.5);
         else
            rightSteer->Set(0);

         if (backrightSteerLimit->Get())
            leftSteer->Set(-0.5);
         else
            leftSteer->Set(0);

         if (!rightSteer->Get() && !leftSteer->Get())
            align = 2;
      }

      //if steering position must come from drive joystick position (x,y)
      else
      {
         //read position of each steering encoder
         float leftSteeringEncoderVal = leftSteeringEncoder->Get();
         float rightSteeringEncoderVal = rightSteeringEncoder->Get();
         float leftSyncPercent = 1.0f; // These are used to scale the steering motor speeds
         float rightSyncPercent = 1.0f; // to synchronize them
         
         //normalize the values (convert from counted integer >> real number from +1 to -1)
         leftSteeringEncoderVal = leftSteeringEncoderVal / PULSES_PER_STEER;
         rightSteeringEncoderVal = rightSteeringEncoderVal / PULSES_PER_STEER;

         float leftgoal = 0;
         float rightgoal = 0;
         
         syncDivider++; // Count another call
         
         // If enough of a delay has been incurred already, perform the synchronization code
         if(syncDivider > STEERINGSYNCDIVIDER)
         {
            // Reset the divider
            syncDivider = 0;
            
            // Determine which steering unit is turning faster
            int deltaLeft;
            int deltaRight;
            
            deltaLeft = oldLeftEncoderValue - (int)leftSteeringEncoderVal;
            deltaRight = oldRightEncoderValue - (int)rightSteeringEncoderVal;
            oldLeftEncoderValue = (int)leftSteeringEncoderVal;
            oldRightEncoderValue = (int)rightSteeringEncoderVal;
            
            if(abs(deltaLeft) > abs(deltaRight) + STEERINGSYNCDEADBAND && abs(deltaRight) > STEERINGSYNCDEADBAND)
            {
               // Left is moving too fast, so adjust
               leftSyncPercent = (float)abs(oldLeftEncoderValue - oldRightEncoderValue) / STEERINGSYNCDISTANCE;
               
               if(leftSyncPercent > 1.0f) leftSyncPercent = 1.0f;
               else if(leftSyncPercent < 0.0f) leftSyncPercent = 0.0f;
               
               leftSyncPercent = 1.0f - leftSyncPercent;
            }
            
            else if(abs(deltaRight) > abs(deltaLeft) + STEERINGSYNCDEADBAND && abs(deltaLeft) > STEERINGSYNCDEADBAND)
            {
               // Right is moving too fast, so adjust
               rightSyncPercent = (float)abs(oldLeftEncoderValue - oldRightEncoderValue) / STEERINGSYNCDISTANCE;
                              
               if(rightSyncPercent > 1.0f) rightSyncPercent = 1.0f;
               else if(rightSyncPercent < 0.0f) rightSyncPercent = 0.0f;
                              
               rightSyncPercent = 1.0f - rightSyncPercent;
            }
         }
         
         if (SteerMode == TANK)
         {
            //point left wheels straignt towards the wide front
            leftgoal = 0;
            rightgoal = 0;
            
            // Reverse the drive if necessary (DriveReverse is either 1 or -1). This is only for tank
            left_y *= DriveReverse;
            right_y *= DriveReverse;
                  
            // Swap the left and right drive settings if the drive has been reversed
            if(DriveReverse == -1)
            {
               float temp;
               
               temp = left_x;
               left_x = right_x;
               right_x = temp;
            }
                  
            // If the arm is above a certain height, reduce movement sensitivity
            if(ArmEncoder->Get() > ARMLIMITHEIGHT)
            {
               lfDrive->Set(left_y * ARMLIMITFACTOR);
               lrDrive->Set(left_y * ARMLIMITFACTOR);
               rfDrive->Set(right_y * ARMLIMITFACTOR);
               rrDrive->Set(right_y * ARMLIMITFACTOR);
            }
            
            // Otherwise, don't limit speed
            else
            {
               lfDrive->Set(left_y);
               lrDrive->Set(left_y);
               rfDrive->Set(right_y);
               rrDrive->Set(right_y);
            }
         }

         else if (SteerMode == STRAFE || SteerMode == MONSTER || SteerMode
               == CAR || SteerMode == AUTOMONSTER_LEFT || SteerMode == AUTOMONSTER_RIGHT)
         {
            float deflection = 0;
            float steer_angle = 0;
            short quadrant = 0;
            float y_avg = 0;
            
            y_avg = (right_y + left_y) / 2;
            if(y_avg == 0.0f) y_avg = JOYSTICK_DEADBAND_Y / 100;
            y_sign = y_avg / Abs(y_avg); y_sign *= -1;

            float x_avg;
            x_avg = (left_x + right_x) / 2;
            
            if(Abs(y_avg) > 0.8 && SteerMode == MONSTER)
            {
               SteerMode = CAR;
            }
            
            //determine output speed based on net joyustick deflection
            deflection = sqrt((y_avg * y_avg) + (x_avg * x_avg)); //Pythagorean Theorem

            //keep number within range. Ignore corners of joystick motion
            if (deflection > 1)
            {
               deflection = 1;
            }
            
            else if (deflection < -1)
            {
               deflection = -1;
            }
            
            // If we are in auto-monster, the deflection should be set to the difference between the two joystick positions
            if(SteerMode == AUTOMONSTER_LEFT || SteerMode == AUTOMONSTER_RIGHT) deflection = Abs(left_y - right_y) / 2;
            
            if (align >= 1)
            { //no drive during alignment mode
               deflection = 0;
            }
            
            // If the arm is above a certain height, reduce drive sensitivity
            if(ArmEncoder->Get() > ARMLIMITHEIGHT) deflection *= ARMLIMITFACTOR;
            
            //determine steering position based on joystick angle (measured in radians)
            steer_angle = atan(Abs(x_avg) / Abs(y_avg)); //trigonometry Tan-1 = Opp / Adj

            //conversion from radians to degrees
            steer_angle = steer_angle * (180 / 3.14159);

            //convert to normalized value (between forward >> 0.85 and sideways >> 0)
            steer_angle = (steer_angle * 0.85) / 90;
            
            // Reverse drive direction if necessary
            deflection *= DriveReverse;
                        
            // If the lock button is pressed, we use a special quadrant
            if(LOCKBUTTONPRESSED)
            {
               quadrant = 7;
               SteerMode = STRAFE;
               deflection *= LOCKBUTTONLIMIT;
            }
            
            //if joysticks are pointed in the right straight deadband
            else if(x_avg > 0 && Abs(y_avg) <= JOYSTICK_DEADBAND_Y)
            {
               quadrant = 6;
            }
            
            //if joysticks are pointed in the left straight deadband
            else if (x_avg < 0 && Abs(y_avg) <= JOYSTICK_DEADBAND_Y)
            {
               quadrant = 5;
            }
            
            //if joysticks are pointed in 4th quadrant
            else if (x_avg > 0 && y_avg >= 0)
            {
               //to make driving easier, when the arm is up, the arm side becomes the "front"
               //when the arm is down, the kicker side becomes the "front"
               quadrant = 4;
            }
            
            //if joysticks are pointed in 3rd quardant
            else if (x_avg < 0 && y_avg > 0)
            {
            quadrant = 3;
            }
            
         //if joysticks are pointed in 2nd quardant
         else if (x_avg < 0 && y_avg <= 0)
         {
            quadrant = 2;
         }
            
         //if joysticks are pointed in 1st quadrant
         else
         {
            quadrant = 1;
         }

         //set direction to drive and angle to steering based on quardant of motion
         if (quadrant == 4)
         {
            rfDrive->Set(deflection);
            rrDrive->Set(deflection);
            lfDrive->Set(deflection);
            lrDrive->Set(deflection);
            leftgoal = steer_angle * -1;
            rightgoal = steer_angle;
         }
         
         else if (quadrant == 3)
         {
            rfDrive->Set(deflection);
            rrDrive->Set(deflection);
            lfDrive->Set(deflection);
            lrDrive->Set(deflection);
            leftgoal = steer_angle;
            rightgoal = steer_angle * -1;
         }
         
         else if (quadrant == 2)
         {
            rfDrive->Set(deflection * -1);
            rrDrive->Set(deflection * -1);
            lfDrive->Set(deflection * -1);
            lrDrive->Set(deflection * -1);
            leftgoal = steer_angle * -1;
            rightgoal = steer_angle;
         }
         
         else if (quadrant == 1)
         {
            rfDrive->Set(deflection * -1);
            rrDrive->Set(deflection * -1);
            lfDrive->Set(deflection * -1);
            lrDrive->Set(deflection * -1);
            leftgoal = steer_angle;
            rightgoal = steer_angle * -1;
         }
         
         else if (quadrant == 5)
         {
            if (align >= 1 || Abs(leftSteer->Get()) > 0.15f)
            {
               rfDrive->Set(0);
               rrDrive->Set(0);
               lfDrive->Set(0);
               lrDrive->Set(0);
               DISPLAYSTRING(4, "Wheels turning");
            }
            
            else
            {
               rfDrive->Set(deflection * -1);
               rrDrive->Set(deflection * -1);
               lfDrive->Set(deflection * -1);
               lrDrive->Set(deflection * -1);
               DISPLAYSTRING(4, "Wheels stopped");
            }
            
            leftgoal = -0.85;
            rightgoal = 0.85;
         }
         
         else if (quadrant == 6)
         {
            if(align >= 1 || Abs(leftSteer->Get()) > 0.15f)
            {
               rfDrive->Set(0);
               rrDrive->Set(0);
               lfDrive->Set(0);
               lrDrive->Set(0);
               DISPLAYSTRING(4, "Wheels turning");
            }
            else
            {
               rfDrive->Set(deflection * -1);
               rrDrive->Set(deflection * -1);
               lfDrive->Set(deflection * -1);
               lrDrive->Set(deflection * -1);
               DISPLAYSTRING(4, "Wheels stopped");
            }
            
            leftgoal = 0.85;
            rightgoal = -0.85;
         }
         
         // If we are in lock-mode
         else if(quadrant == 7)
         {
            // If auto-align is enabled or the steering is still turning
            // stop the drive motors
            if(align >= 1 || Abs(leftSteer->Get()) > 0.15f)
            {
               rfDrive->Set(0);
               rrDrive->Set(0);
               lfDrive->Set(0);
               lrDrive->Set(0);
               DISPLAYSTRING(4, "Wheels turning");
            }
            
            // Otherwise, use the direct joystick x-values to set the motor speed
            else
            {
               rfDrive->Set(-x_avg);
               rrDrive->Set(-x_avg);
               lfDrive->Set(-x_avg);
               lrDrive->Set(-x_avg);
               DISPLAYSTRING(4, "Wheels stopped");
            }
            
            // Set goal position for steering to full right
            leftgoal = 0.85;
            rightgoal = -0.85;
         }
      } //end of if steering in strafe mode
         
      //if MONSTER is steermode, invert the leftsteer goal
      if(SteerMode == MONSTER)
      {
         leftgoal *= -1;
         leftgoal *= DriveReverse;
         rightgoal *= DriveReverse;
         leftgoal *= y_sign;
         rightgoal *= y_sign;
         leftgoal *= 0.75;
         rightgoal *= 0.75;
      }
      
      // In the case of either auto-monster, the deflection has already been calculated,
      // so simply set the wheels to point either left and right or right and left in
      // monster style
      else if(SteerMode == AUTOMONSTER_LEFT)
      {
         leftgoal = -0.58;
         rightgoal = -0.58;
         leftgoal *= DriveReverse;
         rightgoal *= DriveReverse;
      }
      
      else if(SteerMode == AUTOMONSTER_RIGHT)
      {
         leftgoal = 0.58;
         rightgoal = 0.58;
         leftgoal *= DriveReverse;
         rightgoal *= DriveReverse;
      }
      
      // Otherwise, if car is enabled, centre the rear wheels
      else if(SteerMode == CAR)
      {
         if(lfDrive->Get() > 0)
         {
            rightgoal = 0.0f;
            leftgoal = leftgoal /2;
         }
         else
         {
            leftgoal = 0.0f;
            rightgoal = rightgoal /2;
         }
      }

      // If the drive has been reversed, the angle of the strafe should also be inverted
      //leftgoal *= DriveReverse;
      //rightgoal *= DriveReverse;
      
      //if left steering has reached it's target position
      if(Abs(leftSteeringEncoderVal - leftgoal) < STEERING_DEADBAND)
      {
         //turn left steering motor off
         leftSteer->Set(0);
      }

      //if left steering motor is expected to move
      else
      {
         float speedincrement = 1/STEERING_APPROACH_DISTANCE;
         //if left steering motor must move left to reach target
         if(leftSteeringEncoderVal < leftgoal)
         {
            // If the left-rear limit switch is hit and we're travelling left, stop
            if(!backleftSteerLimit->Get()) leftSteer->Set(0);

            //if left steering motor is within approach distance
            else if(leftSteeringEncoderVal + (STEERING_APPROACH_DISTANCE/PULSES_PER_STEER) >= leftgoal)
            {
               //adjust speed proportinal to distance from target
               //printf("Close... Going Right...\r\n");
               leftSteer->Set(speedincrement * Abs((PULSES_PER_STEER*leftgoal) - (PULSES_PER_STEER*leftSteeringEncoderVal)));
            }
            //if left steering motor is far from target
            else
            {
               //go top speed
               //printf("Far... Going Right...\r\n");
               leftSteer->Set(0.9 * leftSyncPercent);
            }
         }

         //if left steering motor must move right to reach target
         else
         {
            // If the steering motor must move right and the right-rear limit switch it hit, stop
            if(!backrightSteerLimit->Get()) leftSteer->Set(0);

            //if left steering motor is within approach distance
            else if(leftSteeringEncoderVal - (STEERING_APPROACH_DISTANCE/PULSES_PER_STEER) <= leftgoal)
            {
               //adjust speed proportinal to distance from target
               //printf("Close... Going Left...\r\n");
               leftSteer->Set(-1 * speedincrement * Abs((PULSES_PER_STEER*leftgoal) - (PULSES_PER_STEER*leftSteeringEncoderVal)));
            }
            //if left steering motor is far from target
            else
            {
               //go top speed
               //printf("Far... Going Left...\r\n");
               leftSteer->Set(-0.9 * leftSyncPercent);
            }
         }
      } //end of if left steering motor is expected to move


      //if right steering has reached it's target position
      if(Abs(rightSteeringEncoderVal - rightgoal) < STEERING_DEADBAND)
      {
         //turn right steering motor off
         rightSteer->Set(0);
      }

      //if right steering motor is expected to move
      else
      {
         float speedincrement = 1/STEERING_APPROACH_DISTANCE;

         //if right steering motor must move left to reach target
         if(rightSteeringEncoderVal < rightgoal)
         {
            // If the front steering motor must move left and the front-left limit switch is triggered, stop
            if(!frontleftSteerLimit->Get()) rightSteer->Set(0);

            //if right steering motor is within approach distance
            else if(rightSteeringEncoderVal + STEERING_APPROACH_DISTANCE >= rightgoal)
            {
               //adjust speed proportinal to distance from target
               rightSteer->Set(speedincrement * Abs((PULSES_PER_STEER*rightgoal) - (PULSES_PER_STEER*rightSteeringEncoderVal)));
            }
            //if right steering motor is far from target
            else
            {
               //go top speed
               rightSteer->Set(0.9 * rightSyncPercent);
            }

         }
         //if right steering motor must move right to reach target
         else
         {
            // If the front steering motor must move right and the front-right limit switch is triggered, stop
            if(!frontrightSteerLimit->Get()) rightSteer->Set(0);

            //if right steering motor is within approach distance
            if(rightSteeringEncoderVal - STEERING_APPROACH_DISTANCE <= rightgoal)
            {
               //adjust speed proportinal to distance from target
               rightSteer->Set(-1 * speedincrement * Abs((PULSES_PER_STEER*rightgoal) - (PULSES_PER_STEER*rightSteeringEncoderVal)));
            }
            //if right steering motor is far from target
            else
            {
               //go top speed
               rightSteer->Set(-0.9 * rightSyncPercent);
            }
         }
      } //end of if right steering motor is expected to move
   } //end of if steering position must come from joystick position (x,y)
} // End of DriveSwerve function

// Reset swerve-drive related variables
void reset_variables(void)
{
   output_speed = 0;
   oldLeftEncoderValue = 0;
   oldRightEncoderValue = 0;
   flag2 = false;
   flag3 = false;
   leftSteeringEncoder->Reset(); //***i
   leftSteeringEncoder->Start(); //***i
   rightSteeringEncoder->Reset(); //***i
   rightSteeringEncoder->Start(); //***i
} // End of reset_variables

// End of "which drive system are we using?"
}; // End of MecanumDrive class

Brendan van Ryn

Posts : 95
Join date : 2010-09-30
Age : 24

Back to top Go down

Part Two

Post  Brendan van Ryn on Fri Jun 08, 2012 11:13 pm

Here's the second half. I'm still a bit baffled about the message length issue, but at any rate:

Code:

class SimpleTracker : public SimpleRobot
{
   MecanumDrive *myRobot; // Pointer to drive system
   Joystick *rightStick; // Right joystick
   Joystick *leftStick; // Left joystick
   Joystick *clawStick; // Claw control joystick
   DigitalInput *LightSensor1; // Light sensors - Left sensor
   DigitalInput *LightSensor2; // - End of line sensor
   DigitalInput *LightSensor3; // - Right sensor
   DigitalInput *LightSensor4; // Reverse sensor
   Encoder *ArmEncoder; // Encoder for arm
   Encoder *BlockEncoder;// Encoder for the steering block
   DigitalInput *ArmRetractLimit; // Telescoping retraction limit switch
   DigitalInput *ArmExtendLimit; // Telescoping extention limit switch
   DigitalInput *ArmPivotLimitDown; // Pivoting limit switch (home)
   DigitalInput *ArmPivotLimitUp; // Pivoting limit switch (full up)
   DigitalInput *MinibotLimit; // Minibot deployment limit switch
   DigitalInput *autonomousSetting; // A jumper to select autonomous modes
   DigitalInput *ArmLightSensor; //The light sensor used for the arm (a getto encoder)
   Victor *ArmExtend; // Arm Extend Motor
   Victor *ArmPivot; // Arm pivot motor
   Victor* topclawmotor; // Create a pointer to a victor for the top claw motor
   Victor* bottomclawmotor; // Create a pointer to a victor for the bottom claw motor
   Solenoid *minibotDeploy; // Pointer to solenoid to deploy minibot
   Solenoid *minibotRetract; // Pointer to solenoid to retract minibot
   Solenoid *engageBrake; // Pointer to solenoid to engage the brake
   Solenoid *minibotDeploy2; // Pointer to second solenoid to deploy minibot
   AnalogChannel *autoSwitch; // Pointer to autonomous analog switch
   Compressor *theCompressor; // Pointer to compressor
   Relay *compressor; // Compressor spike
   DigitalInput *pressureSwitch; // Pressure switch to control compressor
   int ArmEncoderValue; // Current arm encoder position
   int BlockEncoderValue;
   int ExtendEncoderValue; // Current arm extension position
   DigitalInput *ClawSwitch; // Limit switch in claw motor
   DigitalInput *AutonomousSwitch; // Autonomous mode switch
   AnalogChannel *ultraDistance; // Ultrasonic distance sensor for autonomous

public:
   SimpleTracker(void)
   {
      myRobot = new MecanumDrive(LFMOTORPORT, LRMOTORPORT, RFMOTORPORT, RRMOTORPORT); // Instantiate drive system
      rightStick = new Joystick(RIGHTJOYSTICKPORT); // Create the joysticks
      leftStick = new Joystick(LEFTJOYSTICKPORT);
      clawStick = new Joystick(CLAWJOYSTICKPORT);
      LightSensor1 = new DigitalInput(LIGHTSENSORPORT1); // Create the light sensors
      LightSensor2 = new DigitalInput(LIGHTSENSORPORT2);
      LightSensor3 = new DigitalInput(LIGHTSENSORPORT3);
      LightSensor4 = new DigitalInput(DIGITALSIDECARTWO, LIGHTSENSORPORT4);
      ArmEncoder = new Encoder(ARMENCCHANNELA, ARMENCCHANNELB); // Create encoder to gauge arm position
      BlockEncoder = new Encoder(DIGITALSIDECARTWO, SWERVEBLOCKENCODER1, DIGITALSIDECARTWO, SWERVEBLOCKENCODER2);
      ArmRetractLimit = new DigitalInput(RETRACTLIMITSWITCHPORT); // Create limit switch for arm retraction
      ArmExtendLimit = new DigitalInput(EXTENDLIMITSWITCHPORT); //Create a limit switch for arm extention
      ArmPivotLimitUp = new DigitalInput(PIVOTLIMITSWITCHUPPORT); // Create limit switch for arm pivoting (full up)
      ArmPivotLimitDown = new DigitalInput(PIVOTLIMITSWITCHDOWNPORT); // Create limit switch for arm pivoting (full down)
      MinibotLimit = new DigitalInput(DIGITALSIDECARTWO, MINIBOTLIMITSWITCHPORT); // Create limit switch for minibot deployment
      ArmLightSensor = new DigitalInput(DIGITALSIDECARTWO, ARMLIGHTSENSOR); // Creates a lightsensor fo the arm "encoder"
      ArmPivot = new Victor(PIVOTMOTORPORT); // This class needs access to the motor as well
      ArmExtend = new Victor(EXTENDMOTORPORT); // This class needs access to the extending motor as well
      topclawmotor = new Victor(CLAWTOPMOTORPORT); //Create a pointer to the top claw motor
      bottomclawmotor = new Victor(CLAWBOTTOMMOTORPORT); // Create a pointer to the bottom claw motor
      minibotDeploy = new Solenoid(MINIBOTDEPLOYPORT);
      minibotDeploy2 = new Solenoid(MINIBOTDEPLOYPORT2);
      minibotRetract = new Solenoid(MINIBOTRETRACTPORT);
      engageBrake = new Solenoid(BRAKEPORT);
      //autoSwitch = new AnalogChannel(AUTOSWITCHPORT);
      autonomousSetting = new DigitalInput(DIGITALSIDECARTWO, AUTOSWITCHPORT);
      compressor = new Relay(DIGITALSIDECARTWO, COMPRESSORSPIKEPORT);
      pressureSwitch = new DigitalInput(DIGITALSIDECARTWO, COMPRESSORSWITCHPORT);
      ClawSwitch = new DigitalInput(DIGITALSIDECARTWO, CLAWSWITCHPORT); // Create a limit switch to detect tube presence in claw
      ultraDistance = new AnalogChannel(ULTRAPORT);
      AutonomousSwitch = new DigitalInput(DIGITALSIDECARTWO, AUTOSWITCHPORT);
      
      
      //theCompressor = new Compressor(DIGITALSIDECARTWO, COMPRESSORSWITCHPORT, DIGITALSIDECARTWO, COMPRESSORSPIKEPORT);
      
      // Start recording pulses
      ArmEncoder->Start();
      ArmEncoder->Reset();
      
      BlockEncoder->Start();
      BlockEncoder->Reset();

      ExtendEncoderValue = 0; // Initialize extension encoder to start position
      // Enable the compressor
      //theCompressor->Start();
      //Image* cameraImage = frcCreateImage(IMAQ_IMAGE_HSL);
   } // End of SimpleTracker constructor

   void OperatorControl(void)
   {
      int minibotDeployed = 0;
      int ArmTarget = -1; // Target pivot position for arm
      int ExtendTarget = -1; // Target extension position for arm
      int ArmPickup = 0; // Pickup enabled?
      int ArmDeploy = 0; // Auto-deploy enabled?
      int prevlight = 0, curlight = 0; // Previous and current values of arm extension light sensor
      float prevarmext = 0; // Store whether the arm was moving forward or backward before it stopped

      // Reset encoder on arm pivot
      //ArmEncoder->Reset();
      //ArmTarget = -1;
      
      BlockEncoder->Start();
      BlockEncoder->Reset();
      
      // While in operator control 
      while (IsOperatorControl())
      {
         // Reset the "watchdog" timer to show that the processor hasn't frozen
         GetWatchdog().Feed();
         
         float voltage = ultraDistance->GetVoltage();
         
         BlockEncoderValue = BlockEncoder->Get();
         CLEARMESSAGE;
         DISPLAYMESSAGE(1, BlockEncoderValue);
               
         // If we are not at the desired pressure, turn on the compressor
         if(!pressureSwitch->Get()) compressor->Set(Relay::kForward);
         else compressor->Set(Relay::kOff);
         
         // Get the current encoder position
         ArmEncoderValue = ArmEncoder->Get();
         
         curlight = ArmLightSensor->Get();
         if(ArmExtend->Get() != 0) prevarmext = ArmExtend->Get();
         
         if(curlight != prevlight)
         {
            if(prevarmext < 0) ExtendEncoderValue--;
            else if(prevarmext && ArmExtendLimit->Get()) ExtendEncoderValue++;
         }
         
         prevlight = curlight;
         
         if(!ArmRetractLimit->Get()) ExtendEncoderValue = 0;
         
         // Enter auto-pickup upon button command and set arm to pivot to home position
         if(PICKUPBUTTON)
         {
            ArmPickup = 1;
            ExtendTarget = -1;
            ArmTarget = ARM_HOME;
         }
         
         if(!ClawSwitch->Get() && ArmPickup) ArmPickup = 2;
         
         // Send joystick data to drive system as well as limit switch pointers
         // Depending on which drive system we're using (according to the DRIVESYSTEM definition),
         // call the appropriate function
         myRobot->DriveSwerve(leftStick, rightStick, clawStick,
               ArmPivotLimitDown, ArmPivotLimitUp, ArmRetractLimit,
               ArmExtendLimit, ArmEncoder, ArmTarget, ArmEncoderValue, 0, ArmPickup, ArmDeploy, ExtendTarget);

         // If the deploy button is pressed, close all retract solenoids and
         // open the extend solenoids
         if(DEPLOYMINIBOT)
         {
            minibotDeploy->Set(1);
            minibotDeploy2->Set(1);
            minibotRetract->Set(0);
            minibotDeployed = 1;
         }
            
         // If the retract button is pressed, close all deploy solenoids and
         // open the retract solenoids
         else if(RETRACTMINIBOT)
         {
            minibotDeploy->Set(0);
            minibotDeploy2->Set(0);
            minibotRetract->Set(1);
            minibotDeployed = 0;
         }
               
         // If any arm button is pressed, exit auto-pickup
         if(EXTENDBUTTON || RETRACTBUTTON || PIVOTPOSITIONHOME || PIVOTPOSITIONLOW || PIVOTPOSITIONMIDDLE || PIVOTPOSITIONHIGH || PIVOTPOSITIONFULL || PIVOTDOWNBUTTON || PIVOTUPBUTTON || PREPICKUPBUTTON) ArmPickup = 0, ArmDeploy = 0;
         if(ArmDeploy == 1 && ArmPickup != 0) ArmPickup = 0;
         if(!ArmRetractLimit->Get() && ArmPickup != 2)  ArmPickup = 0;
         
         // Read the joystick buttons and set the arm-pivot
         // target position to the corresponding encoder value
         if (PIVOTPOSITIONHOME)
         {
            // If the trigger is pressed, enable auto-deploy
            if(!CLAWREVERSEBUTTON)
            {
               ArmTarget = -1;
               ExtendTarget = -1;
               ArmDeploy = 1;
               ArmPickup = 0;
            }
            
            // Otherwise, go to home position
            else ArmTarget = ARM_HOME, ExtendTarget = 0;
         }
         
         else if (PIVOTPOSITIONLOW)
            ArmTarget = ARM_RACKBOTTOM, ExtendTarget = 0;
         else if (PIVOTPOSITIONMIDDLE)
            ArmTarget = ARM_RACKMIDDLE, ExtendTarget = 0;
         else if (PIVOTPOSITIONHIGH)
            ArmTarget = ARM_RACKTOP, ExtendTarget = 0;
         else if (PIVOTPOSITIONFULL)
            ArmTarget = ARM_FULLUP, ExtendTarget = ARMEXTENDRACK;
         else if(PREPICKUPBUTTON)
            ArmTarget = ARM_HOME, ExtendTarget = PREPICKUPEXTEND;

         // If the manual arm buttons are pressed, disable auto-movement
         if (PIVOTDOWNBUTTON || PIVOTUPBUTTON)
            ArmTarget = -1;
         
         if (EXTENDBUTTON || RETRACTBUTTON)
            ExtendTarget = -1;

         // Negative one indicates that the arm position is being set
         // manually. Otherwise, it is being moved to a preset position
         // and we need to use a PID loop to approach the correct position
         if (ArmTarget >= 0)
         {   
            // If the arm pivot has hit the home position limit switch, reset the encoder and stop
            if (!ArmPivotLimitDown->Get() && ArmTarget < ArmEncoderValue)
            {
               ArmEncoder->Reset();
               ArmPivot->Set(0);
               ArmTarget = -1;
            }

            // If the arm pivot has hit the full up limit switch, stop
            if (!ArmPivotLimitUp->Get() && ArmTarget > ArmEncoderValue)
            {
               ArmPivot->Set(0);
               ArmTarget = -1;
            }

            // If the arm needs to move down
            if (ArmEncoderValue > ArmTarget + ARMDEADBAND)
            {
               if (ArmEncoderValue - ArmTarget < ARMAPPROACHDOWN)
                  ArmPivot->Set(ARMDOWNSPEED + Abs(((ARMAPPROACHDOWN - (ArmEncoderValue - ArmTarget)) / ARMAPPROACHDOWN)));
               else
                  ArmPivot->Set(ARMDOWNSPEED);
            }

            // If the arm needs to move up
            else if (ArmEncoderValue < ArmTarget - ARMDEADBAND)
            {
               if (ArmTarget - ArmEncoderValue < ARMAPPROACHUP)
                  ArmPivot->Set(ARMUPSPEED * 0.4 - Abs(((ARMAPPROACHUP - (ArmTarget - ArmEncoderValue)) / ARMAPPROACHUP)));
               else
                  ArmPivot->Set(ARMUPSPEED);
            }
            
            else ArmPivot->Set(0);
         }
         
         if(ExtendTarget >= 0)
         {
            if(ExtendEncoderValue > ExtendTarget + 1)
            {
               if(ArmRetractLimit->Get()) ArmExtend->Set(-1);
               else ArmExtend->Set(0);
            }
            
            else if(ExtendEncoderValue < ExtendTarget - 1)
            {
               if((ArmExtendLimit->Get() && ArmEncoderValue > 90) || ExtendTarget == PREPICKUPEXTEND) ArmExtend->Set(1);
               else ArmExtend->Set(0);
            }
            
            else ArmExtend->Set(0);
         }
         
         // If auto-deploy is enabled
         if(ArmDeploy)
         {
            // If the arm has not finished retracting
            if(ArmRetractLimit->Get())
            {
               // Eject the tube and retract the arm
               myRobot->topclawmotor->Set(0.5); //@
               myRobot->bottomclawmotor->Set(-0.5); //@
               ArmExtend->Set(-1);
            }
            
            // Otherwise
            else
            {
               // Stop the motors and exit auto-deploy
               myRobot->topclawmotor->Set(0);
               myRobot->bottomclawmotor->Set(0);
               ArmExtend->Set(0);
               //ArmDeploy = 0;
               ArmTarget = ARM_HOME;
            }
         }
         
         if(!ArmPivotLimitDown->Get()) ArmDeploy = 0;
         
         // The arm is moving, disengage the brake. Otherwise, re-engage the brake
         if (ArmPivot->Get())
            engageBrake->Set(0);
         else
            engageBrake->Set(1);
      } // End of loop while in operator control
   } // End of OperatorControl function

   void Autonomous(void)
   {
      // Has the robot reached the end of the line ("T")?
      int stop = 0; // Has the robot reached the end of the line?
      //int strafe = 0; // Is the robot strafing along the branch?
      //int ArmTarget = -1; // Target pivot position for arm
      int ExtendTarget = ARMEXTENDRACK; // Target extension position for arm
      unsigned int timesetting = 0; // Used to track timing of various scripted events
      int prevlight, curlight; // Previous and current states of light sensor on arm extension
      float prevarmext = 0; // Previous setting of arm extension motor
      int trip = 0;
      
      int frontTrip = 0, backTrip = 0;
      int steertripLeft = 0, steertripRight = 0;
      
      BlockEncoder->Start();
      BlockEncoder->Reset();
      myRobot->trip = 0;
      
      // While in autonomous mode
      while (IsAutonomous())
      {
         // Feed "watchdog" to prevent an assumed lockup
         GetWatchdog().Feed();
         
         // Prototypical two-tube autonomous code
         int BlockEncoderValue = BlockEncoder->Get();
         
         // Arm encoder overhead
            // If the light sensor detects a different colour than last time, it has moved
            curlight = ArmLightSensor->Get();
            if(ArmExtend->Get() != 0) prevarmext = ArmExtend->Get(); // Only change the stored motor direction if the motor is running
            if(curlight != prevlight)
            {
               // Increment or decrement encoder value based on direction of extension motor
               if(prevarmext < 0) ExtendEncoderValue--;
               // Stop adding if the exension limit has been hit
               else if(prevarmext > 0 && ArmExtendLimit->Get()) ExtendEncoderValue++;
            }
            
            // Set previous state for next time
            prevlight = curlight;
         
            // If the arm retracts fully, reset value to zero, just in case
            if(!ArmRetractLimit->Get()) ExtendEncoderValue = 0;
            ArmEncoderValue = ArmEncoder->Get();
            
         // End of arm encoder overhead
                  
         // If the robot has not reached the end, track the line
         // When the return value becomes one, the robot should begin executing
         // The scripted events below. 0 == first stage of autonomous (line tracking)
         if (stop == 0) stop = myRobot->MoveSwerve(LightSensor1->Get(), LightSensor3->Get(), LightSensor2->Get(), ArmPivotLimitUp->Get(), ultraDistance, AutonomousSwitch->Get(), BlockEncoderValue);
         
         CLEARMESSAGE;
         DISPLAYMESSAGE(3, (int)(ultraDistance->GetVoltage() * 1000.0f));
         
         // Until the arm has been fully pivoted up, and we haven't finished line-tracking,
         // move it up
         if (stop == 0)
         {
            if (ArmPivotLimitUp->Get()) ArmPivot->Set(1);
            else ArmPivot->Set(0);
         }
         
         // Extend the arm to the desired position, if one exists (-1 means neutral)
         if(ExtendTarget >= 0)
         {
            if(ExtendEncoderValue > ExtendTarget + 1)
            {
               if(ArmRetractLimit->Get()) ArmExtend->Set(-1);
               else ArmExtend->Set(0);
            }
                     
            else if(ExtendEncoderValue < ExtendTarget - 1)
            {
               if(ArmExtendLimit->Get() && ArmEncoderValue > 45) ArmExtend->Set(1);
               else ArmExtend->Set(0);
            }
                     
            else ArmExtend->Set(0);
         }
         
         // Once we've reached the end of the first line, stop and wait for the arm to finish
         // pivoting and extending
         if(stop == 1)
         {
            // Make sure to stop
            myRobot->lfDrive->Set(0);
            myRobot->lrDrive->Set(0);
            myRobot->rfDrive->Set(0);
            myRobot->rrDrive->Set(0);
            
            if(Abs(ArmPivot->Get()) < 0.05 && Abs(ArmExtend->Get()) < 0.05) stop = 2, trip = 0;
         }
            
         else if(stop == 2 && TrackSteeringL2((int)(BRANCHANGLE * PULSES_PER_STEER)))
         {
            //if(ultraDistance->GetVoltage() * 1000.0f < (float)BRANCHSTOPDISTANCE) trip++;
            //else if(trip > 0) trip--;
            
            //if(trip > 320) stop = 3;
            if(Abs(BlockEncoderValue) > AUTO2STOPDISTANCE) stop = 3;
            
            myRobot->lfDrive->Set(0.45);
            myRobot->lrDrive->Set(0.45);
            myRobot->rfDrive->Set(0.45);
            myRobot->rrDrive->Set(0.45);
         }
         
         else if(stop == 3)
         {
            ExtendTarget = -1;
            
            myRobot->lfDrive->Set(0);
            myRobot->lrDrive->Set(0);
            myRobot->rfDrive->Set(0);
            myRobot->rrDrive->Set(0);
            
            // If the arm has not finished retracting, continue to deploy tube with the rollers
            if (ArmRetractLimit->Get())
            {
               myRobot->bottomclawmotor->Set(-CLAWROLLERSPEED);
               myRobot->topclawmotor->Set(CLAWROLLERSPEED);
            }

            // Once the arm has finished retracting, stop the rollers
            else
            {
               myRobot->bottomclawmotor->Set(0);
               myRobot->topclawmotor->Set(0);
            }

            // Retract the arm to the home limit switch
            if (ArmRetractLimit->Get()) ArmExtend->Set(-1);
            else
            {
               ArmExtend->Set(0);
               myRobot->bottomclawmotor->Set(0);
               myRobot->topclawmotor->Set(0);
               BlockEncoder->Reset();
               BlockEncoder->Start();
               stop = 4;
            }
         }
         
         else if(stop == 4 && TrackSteeringL2((int)(BRANCHANGLE * PULSES_PER_STEER)))
         {
            CLEARMESSAGE;
            DISPLAYMESSAGE(2, BlockEncoder->Get());
            if(Abs(BlockEncoder->Get()) > 275) stop = 5;
            myRobot->lfDrive->Set(-0.45);
            myRobot->lrDrive->Set(-0.45);
            myRobot->rfDrive->Set(-0.45);
            myRobot->rrDrive->Set(-0.45);
            if(ArmPivotLimitDown->Get()) ArmPivot->Set(ARMDOWNSPEED);
            else ArmPivot->Set(0);
         }
         
         else if(stop == 5)
         {
            frontTrip = TrackSteeringL2((int)(-0.55 * PULSES_PER_STEER));
            backTrip = TrackSteeringR((int)(-0.55 * PULSES_PER_STEER));
            
            if(frontTrip && backTrip)
            {
               myRobot->lfDrive->Set(-0.55);
               myRobot->lrDrive->Set(-0.55);
               myRobot->rfDrive->Set(-0.55);
               myRobot->rrDrive->Set(-0.55);
            }
            
            if(Abs(BlockEncoder->Get()) > 800) stop = 6;
            if(ArmPivotLimitDown->Get()) ArmPivot->Set(ARMDOWNSPEED);
            else ArmPivot->Set(0);
         }
         
         else if(stop == 6)
         {
            TrackSteeringL2((int)(-0.23 * PULSES_PER_STEER));
            TrackSteeringR((int)(-0.23 * PULSES_PER_STEER));
            myRobot->lfDrive->Set(-0.3);
             myRobot->lrDrive->Set(-0.3);
            myRobot->rfDrive->Set(-0.3);
            myRobot->rrDrive->Set(-0.3);
            if(Abs(BlockEncoder->Get()) > 1200)
                {
                    stop = 7;
                    frontTrip = 0;
                    backTrip = 0;
                }
         }
         
         else if(stop == 7)
         {
                if(LightSensor4->Get()) frontTrip++;
                if(LightSensor3->Get()) backTrip++;
               
                if(backTrip > 100)
                {
                    TrackSteeringL2(0);
                    myRobot->lfDrive->Set(0);
                    myRobot->rfDrive->Set(0);
                }
               
                else
                {
                      TrackSteeringL2((int)(-0.23 * PULSES_PER_STEER));
                      myRobot->lfDrive->Set(-0.25);
                      myRobot->rfDrive->Set(-0.25);
                }
               
                if(frontTrip > 100)
                {
                    TrackSteeringR(0);
                    myRobot->lrDrive->Set(0);
                    myRobot->rrDrive->Set(0);
                }
               
                else
                {
                      TrackSteeringR((int)(-0.23 * PULSES_PER_STEER));
                      myRobot->lrDrive->Set(-0.25);
                      myRobot->rrDrive->Set(-0.25);
                }
               
                if(frontTrip > 100 && backTrip > 100) stop = 8;
            }
           
            else if(stop == 8)
            {
                TrackSteeringR(0);
                TrackSteeringL2(0);
                if(ArmExtendLimit->Get()) ArmExtend->Set(1);
                else ArmExtend->Set(0);
                myRobot->bottomclawmotor->Set(CLAWROLLERSPEED);
                myRobot->topclawmotor->Set(-CLAWROLLERSPEED);
                if(myRobot->MoveReverse(LightSensor2->Get(), LightSensor4->Get(), ClawSwitch->Get())) stop = 9;
            }
         
         else if(stop == 9 || stop == 10)
         {
            TrackSteeringR(0);
            TrackSteeringL2(0);
            myRobot->bottomclawmotor->Set(0);
            myRobot->topclawmotor->Set(0);
             if(stop == 9 && myRobot->MoveSwerve(LightSensor1->Get(), LightSensor3->Get(), LightSensor2->Get(), ArmPivotLimitUp->Get(), ultraDistance, !AutonomousSwitch->Get(), 0)) stop = 10;
            
             if(stop == 10)
             {
                myRobot->lrDrive->Set(0);
                myRobot->rrDrive->Set(0);
                myRobot->lfDrive->Set(0);
                myRobot->rfDrive->Set(0);
             }
            
                if(ArmPivotLimitUp->Get()) ArmPivot->Set(1);
                else ArmPivot->Set(0);
               
                if(ArmEncoder->Get() <= 70)
                {
                    if(ArmRetractLimit->Get()) ArmExtend->Set(-1);
                    else ArmExtend->Set(0);
                }
               
                else
                {
                    if(ArmExtend->Get() < 0.05f && stop == 10 && ExtendTarget != -1) stop = 11;
                    ExtendTarget = ARMEXTENDRACK + 3;
                    timesetting = 0;
                }
            }
           
            else if(stop == 11)
            {
            ExtendTarget = -1;
            TrackSteeringR(0);
            TrackSteeringL2(0);
            
            if(timesetting < RETRACTTIME)
            {
               timesetting++;
               myRobot->lfDrive->Set(0.2);
               myRobot->lrDrive->Set(0.2);
               myRobot->rfDrive->Set(0.2);
               myRobot->rrDrive->Set(0.2);
            }
            
            else if(timesetting < MANIPULATIONTIME)
            {
               myRobot->lfDrive->Set(0);
               myRobot->lrDrive->Set(0);
               myRobot->rfDrive->Set(0);
               myRobot->rrDrive->Set(0);
               myRobot->bottomclawmotor->Set(-CLAWROLLERSPEED);
               myRobot->topclawmotor->Set(-CLAWROLLERSPEED);
               timesetting++;
            }

            else
            {
               if (ArmRetractLimit->Get())
               {
                  myRobot->bottomclawmotor->Set(-CLAWROLLERSPEED);
                  myRobot->topclawmotor->Set(CLAWROLLERSPEED);
               }
               
               else
               {
                  myRobot->bottomclawmotor->Set(0);
                  myRobot->topclawmotor->Set(0);
               }
            }

            if (ArmRetractLimit->Get()) ArmExtend->Set(-1);
            else
            {
               ArmExtend->Set(0);
               stop = 12;
            }
            }
           
            else if(stop == 12)
            {
               TrackSteeringR(0);
               TrackSteeringL2(0);
               if(ArmPivotLimitDown->Get()) ArmPivot->Set(-1);
                else ArmPivot->Set(0);
                myRobot->lfDrive->Set(-0.4);
             myRobot->lrDrive->Set(-0.4);
             myRobot->rfDrive->Set(-0.4);
             myRobot->rrDrive->Set(-0.4);
            }

            // Until the arm has been fully pivoted up, and we haven't finished line-tracking,
         // move it up
         if (stop == 0)
         {
            if (ArmPivotLimitUp->Get())
            {
               if(ArmEncoderValue > 65) ArmPivot->Set(0.28);
               else ArmPivot->Set(1);
            }
            else ArmPivot->Set(0);
         }
      } // End of loop while in autonomous mode
   } // End of Autonomous
   
   // Use the encoders, with regard to the limit switches, turn the steering to the
   // goal value passed (used by autonomous function)
   int TrackSteeringL(int Target)
   {
      int leftSteeringEncoderValue;
      float speedincrement = 1/STEERING_APPROACH_DISTANCE;
      
      // Get the current steering positions
      leftSteeringEncoderValue = myRobot->leftSteeringEncoder->Get();
      
      // Turn the steering
      if(Abs(leftSteeringEncoderValue - Target) < (STEERING_DEADBAND * PULSES_PER_STEER)) myRobot->leftSteer->Set(0);
      else if(leftSteeringEncoderValue < Target)
      {
         if(!myRobot->backrightSteerLimit->Get()) myRobot->leftSteer->Set(0);
         else if(leftSteeringEncoderValue + STEERING_APPROACH_DISTANCE >= Target) myRobot->leftSteer->Set(TRACK_STEERING_SPEED_LIMIT * speedincrement * Abs(Target - leftSteeringEncoderValue));
         else myRobot->leftSteer->Set(0.9 * TRACK_STEERING_SPEED_LIMIT);
      }
      
      else
      {
         if(!myRobot->backleftSteerLimit->Get()) myRobot->leftSteer->Set(0);
         else if(leftSteeringEncoderValue - STEERING_APPROACH_DISTANCE <= Target) myRobot->leftSteer->Set(-1 * TRACK_STEERING_SPEED_LIMIT * speedincrement * Abs(Target - leftSteeringEncoderValue));
         else myRobot->leftSteer->Set(-0.9 * TRACK_STEERING_SPEED_LIMIT);
      }
      
      if(Abs(myRobot->leftSteer->Get()) < 0.2f) return 1;
      else return 0;
   }
   
   int TrackSteeringR(int Target)
   {
      int rightSteeringEncoderValue;
      float speedincrement = 1/STEERING_APPROACH_DISTANCE;
      
      rightSteeringEncoderValue = myRobot->rightSteeringEncoder->Get();
      
      Target *= -1;
      
      // Turn the steering
      if(Abs(rightSteeringEncoderValue - Target) < (STEERING_DEADBAND * PULSES_PER_STEER)) myRobot->rightSteer->Set(0);
      else if(rightSteeringEncoderValue < Target)
      {
         if(!myRobot->frontleftSteerLimit->Get()) myRobot->rightSteer->Set(0);
         else if(rightSteeringEncoderValue + STEERING_APPROACH_DISTANCE >= Target) myRobot->rightSteer->Set(TRACK_STEERING_SPEED_LIMIT * speedincrement * Abs(Target - rightSteeringEncoderValue));
         else myRobot->rightSteer->Set(0.9 * TRACK_STEERING_SPEED_LIMIT);
      }
         
      else
      {
         if(!myRobot->frontrightSteerLimit->Get()) myRobot->rightSteer->Set(0);
         else if(rightSteeringEncoderValue - STEERING_APPROACH_DISTANCE <= Target) myRobot->rightSteer->Set(-1 * TRACK_STEERING_SPEED_LIMIT * speedincrement * Abs(Target - rightSteeringEncoderValue));
         else myRobot->rightSteer->Set(-0.9 * TRACK_STEERING_SPEED_LIMIT);
      }
      
      if(Abs(myRobot->rightSteer->Get()) < 0.2f) return 1;
      else return 0;
   }
   
   /* There are inconsistencies between this code and the actual steering code.
   Both sections should be revised. Important to note: STEERING_DEADBAND is
   measured as a percentage and MUST be multiplied by PULSES_PER_STEER to be
   of any consequence. Second, STEERING_APPROACH_DISTANCE is actually in PULSES,
   not a percentage and thus should NOT be multiplied by pulses per steer. Finally,
   the ramping code seems to be yielding improper values that prevent the
   steering from unlocking and allowing the drive motors to turn. */
   int TrackSteeringL2(int Target)
   {
      int leftSteeringEncoderValue;
      int remainingDistance; // Consider making this a float and re-evaluating my type-casting
      float speed;
      
      leftSteeringEncoderValue = myRobot->leftSteeringEncoder->Get();
      remainingDistance = (int)Abs((double)(leftSteeringEncoderValue - Target));
      
      if(remainingDistance < (STEERING_DEADBAND * PULSES_PER_STEER)) speed = 0.0f;
      else if(leftSteeringEncoderValue < Target)
      {
         if(!myRobot->backrightSteerLimit->Get()) speed = 0.0f;
         else if(remainingDistance < (int)STEERING_APPROACH_DISTANCE) speed = ((float)(remainingDistance / STEERING_APPROACH_DISTANCE));
         else speed = 1.0f;
      }
      
      else
      {
         if(!myRobot->backleftSteerLimit->Get()) speed = 0.0f;
         else if(remainingDistance < (int)STEERING_APPROACH_DISTANCE) speed = ((float)(remainingDistance / STEERING_APPROACH_DISTANCE * -1.0f));
         else speed = -1.0f;
      }
      
      myRobot->leftSteer->Set(speed);
      CLEARMESSAGE;
      DISPLAYMESSAGE(4, remainingDistance);
      DISPLAYMESSAGE(5, (int)(speed * 100.0f));
      
      if(Abs(speed) < 0.05f) return 1;
      else return 0;
   }
}; // End of SimpleTracker class

// Return absolute value of argument (distance from zero)
float Abs(float x)
{
   if (x < 0)
      x *= -1;
   return x;
}

// Entry point is FRC_UserProgram_StartupLibraryInit
START_ROBOT_CLASS(SimpleTracker)
;

Brendan van Ryn

Posts : 95
Join date : 2010-09-30
Age : 24

Back to top Go down

Re: Code Samples

Post  Sponsored content


Sponsored content


Back to top Go down

Back to top

- Similar topics

 
Permissions in this forum:
You cannot reply to topics in this forum