Code Samples

**Brendan van Ryn** 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** 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** 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** 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** 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** 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** 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** 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

**StuartSullivan** 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

**Brendan van Ryn** 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

William Surmak 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.

**Brendan van Ryn** 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** 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')

Jacob 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

**Brendan van Ryn** 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

Jacob Wed Mar 21, 2012 9:51 pm

Ok, thanks Very Happy

**Brendan van Ryn** 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

Jacob Thu Mar 22, 2012 8:41 pm

Thanks Brendan Very Happy

**Brendan van Ryn** 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** 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** 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** 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) ;

Sponsored content

Code Samples

Code Samples

Code Update

Common Syntax

New Cod as of February 4th

GetProcessImage

Optimization

Robot Configuration

Re: Code Samples

Today's Progress

Re: Code Samples

Re: Code Samples

Re: Code Samples

Day Before UOIT

Current Code?

Code Update

Current Code?

Code Update

code update

Final Version

The Chess Assignment

The 2011 Code

Part Two

Re: Code Samples