RobotC Code

#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     gyro,           sensorI2CHiTechnicGyro)
#pragma config(Sensor, S3,     light,          sensorLightActive)
#pragma config(Sensor, S4,     sonar,          sensorSONAR)
#pragma config(Motor,  mtr_S1_C1_1,     rightMotor,    tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     leftMotor,     tmotorTetrix, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     liftMotor,     tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C2_2,     armMotor,      tmotorTetrix, openLoop, reversed)
#pragma config(Servo,  srvo_S1_C3_1,    LeftHookServo,        tServoStandard)
#pragma config(Servo,  srvo_S1_C3_2,    RightHookServo,       tServoStandard)
#pragma config(Servo,  srvo_S1_C3_3,    GateServo,            tServoStandard)
#pragma config(Servo,  srvo_S1_C3_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma DebuggerWindows("Globals")
#pragma DebuggerWindows("Locals")
#pragma debuggerWindows("joystickSimple");

/////////////////////////////////////////////////////////////////////////////////////////////////////
// FTC Team ID: 9246
// FTC Name: Cedarcrest Timberbots
// Robot configuration
//
//
// Physical Connection Key
//===================================================================================================
//  [       A             ] [       B       ] [       C       ] [      USB      ]
//  [                     NXT Brick                                             ]
//  [       1             ] [       2       ] [       3       ] [       4       ]
//  [Tetrix Motor Ctl     ] [ HiTechnicGyro ] [ Active Light  ] [ Sonar         ]
//  [rightMotor, leftMotor]
//  [Tetrix Motor Ctl     ]
//  [liftMotor, armMotor  ]
//  [Tetrix Servo Ctl     ]
//  [LeftHookServo, RightHookServo, GateServo, empty, empty, empty]
//===================================================================================================
// This code was developed by Team 9246 with advisor/coach Rob Hollis technical questions can be directed to RobJLHollis@hotmail.com
// Also, you may visit our blog at http://cedarcrestftc.blogspot.com
// Basic Tele-Op template used from usfirst.com under Team Resources.  Game, Scoring, FAQ and other valuable resources can be found here.
// NOTE: For best visibility load this using an editor which DOES NOT wrap lines like ultra edit, notepad++, or of course RobotC
// First time config errors: If you don't have the motors and sensors setup according to the pragma above, and/or don't have the Platform Type configured correctly you may get errors:
// First Time Config Error Examples: Info is good, Warnings are OK but not good, Errors will stop compile and must be fixed.
// **Error**:Invalid '#pragma config(Hubs, ...)' syntax: "#pragma config(Hubs, ...)" requires IDE feature "External Motor/Servo Controllers" enabled
// **Info***:Include file 'C:\PROGRA~2\ROBOMA~1\ROBOTC~1.X\Includes\JoystickDriver.c' has already been opened.
// *Warning*:'enum' is anonymous declaration. The 'typedef' for 'TDriveType' has no impact!
// **Error**:Undefined variable 'rightMotor'. 'short' assumed.
// **Error**:Undefined variable 'leftMotor'. 'short' assumed.
// **Error**:Undefined variable 'LeftHookServo'. 'short' assumed.
// **Error**:Undefined variable 'RightHookServo'. 'short' assumed.
// **Error**:Undefined variable 'GateServo'. 'short' assumed.
// **Error**:Undefined variable 'armMotor'. 'short' assumed.
// **Error**:Undefined variable 'liftMotor'. 'short' assumed.
// To overcome First time config errors, Check the following in your RobotC compiler:
// Robot -> Platform Type -> Click to Check allowing Natural Language
// Robot -> Platform Type -> Click to Check allowing external controllers
// Normal error: *Warning*:'enum' is anonymous declaration. The 'typedef' for 'TDriveType' has no impact!  This should be a yellow X and does not stop the program from running
// I am open to and request anyone to be able to help me understand why this warns and how to clear the warning without decreasing functionality of the code. Direct comments to RobJLHollis@hotmail.com

// Robot functional overview.
// Drivetrain: This is used on a frame with 3 wheels chain drive to one motor per side (left and right).
// Drive Styles: The robot code can be driven in tank or arcade mode selectable by pressing the Start --> button during runtime. (tones alert operator to current mode)
// Logarithmic Joystick scaling: Use of a Log table to scale joystick values to allow fine control at lower voltage levels
// Players: The code supports single or dual controller selectable by pressing the Back <-- button during runtime where in 2 Player mode
//          the second player controls the manipulators while player one controls the driving (tones alert operator to current mode)
// Arm: The robot has an arm which moves on an arc over its motor mount to pickup balls and when raised, deposits them into the hopper.
// Lift: The lift raises the hopper from load position to various heights from 8 inches to 60 inches by way of a motor running a lead screw which contracts the base of a scisor lift.
// Hopper: The hopper has a servo which operates a gate momentary open with default of shut.
// Tailhooks: The rear of the robot has two servos (Left and right) each which raise and lower to attach to the rolling goals to move them into scoring positions.
// IR Seeker: sensor is equipped but code is non-functional at this point
// Strategy:  In autonomous mode the robot should attempt to dislodge the kickstand by moving to 3 positions where the kickstand could be.
//            In Teleop mode the operators will attempt to:
//               1) Move the goals near the bottom of our ramp
//               2) Score into the goals until 45 seconds remain
//               3) Score into the high goal at 30 seconds
//               4) Move goals and robot onto ramp before game end
//
//                                                                                     [   scaleJoystick   ]
//                                                                                      ^^vv          ^^vv
//                                                                                     [Tank]       [Arcade]
//                                                                                      ^^             ^^      
// [Pragma Configs] -> [Includes] -> [Initialize Robot] -> [Main - w/ waitforstart and [   forever loop    ]] [Loop]
//                                                                                     ^-------------------------|
//
//



/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                           Tele-Operation Mode Code Template
//
// This file contains a template for simplified creation of an tele-op program for an FTC
// competition.
//
// You need to customize two functions with code unique to your specific robot.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////

#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.

typedef enum
{
  kDriveTank     = 1,
  kDriveArcade   = 2
} TDriveType;

////////////////////////////////////////////////////////////////////////////////
//                         scaleJoystick
// * Function to scale a joystick value using a logarithmic like scale with a dead
//   band at zero.
// * Most of the adjustment range is used for fine control over low power settings.
// * The extreme end of the range provide coarse control over high power.
// * Large dead band around center point.
// * Above makes it easier to control robot at slow speeds.
////////////////////////////////////////////////////////////////////////////////

const bool bLogarithmicScale = true;
const int kMaximumPowerLevel = 100;  // Adjust to set max power level to be used.

int scaleJoystick(int &nJoy1, int nMaxValue = kMaximumPowerLevel)
{
///////////////////////////////////////////////////////////////////////////////
// * This function scales the joystick settings to the appropriate range for
//   controlling a NXT motor.
//
// * Joystick values range from -128 to +127.
// * Speed/power settings for NXT motors range from -100 to +100
// * The physical range of motion of a joystick is quite small and it is sometimes
//   hard to control slow speeds. So another capability of this program to apply
//   a "logarithmic" scale to the joystick settings.
////////////////////////////////////////////////////////////////////////////////
static const int nLogScale[17] = {0, 5, 9, 10, 12, 15, 18, 24, 30, 36, 43, 50, 60, 72, 85, 100, 100};
int nScaled;

nScaled = nJoy1;
if (bLogarithmicScale)
{
 nScaled /= 8;
 if (nScaled >= 0)
   nScaled = nLogScale[nScaled];
 else
   nScaled = - nLogScale[ - nScaled];
  }
nScaled *= nMaxValue;
nScaled /= 100;
  return nScaled;
}

void SetMotors(int powLeft, int powRight)
{
  motor[rightMotor] =  powRight;
  motor[leftMotor]  =  powLeft;
}

// Tank mode uses both left and right joysticks to drive the robot.
//
void Tank(int y1, int y2)
{
int powLeft;
int powRight;
powLeft  = scaleJoystick(y1);   // Left  hand joystick, y value.
powRight = scaleJoystick(y2);   // Right hand joystick, y value.
SetMotors(powLeft, powRight);
}


//Arcade Mode uses one stick to control forward back, and directional control by Adjusting motors using math from single joystick X,Y values
void Arcade(int x, int y) // make joystick arcade style controller
{
  int powY;
  int powRightMotor;
  int powLeftMotor;

  // convert joystick -128 to 127 range to -100 to 100 for powering motors
  powY = scaleJoystick(y); // joystick y axis gives maximum power level
                            // reversed for a turn

  if (x < 0) //  if x negative, turning left; otherwise, turning right
  {
    powLeftMotor  = (powY * (128 + (2 * x))/128); // left motor reduced for right turn
    powRightMotor = powY;                         // right motor not changed
  }
  else
  {
    powRightMotor = (powY * (128 - (2 * x))/128); // right motor reduced for left turn
    powLeftMotor  = powY;                         // left motor not changed
  }

  SetMotors(powLeftMotor, powRightMotor);
}

/////////////////////////////////////////////////////////////////////////////////////////////////////
//                                    initializeRobot
//
// * Prior to the start of tele-op mode, you may want to perform some initialization on your robot
//   and the variables within your program.
// * In most cases, you may not have to add any code to this function and it will remain "empty".
/////////////////////////////////////////////////////////////////////////////////////////////////////

void initializeRobot()
{
  // Place code here to sinitialize servos to starting positions.
  // Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize.

  //motor[servoFingerA] = 75;     //...both hooks up
servo[LeftHookServo] = 0;  //Lift the left Tailhook up
servo[RightHookServo] = 250; //Lift the right Tailhook up
 servo[GateServo] = 20; //Close the gate to the 1230 o clock
  return;
}

/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                         Main Task
//
// The following is the main code for the tele-op robot operation. Customize as appropriate for
// your specific robot.
//
// Game controller / joystick information is sent periodically (about every 50 milliseconds) from
// the FMS (Field Management System) to the robot. Most tele-op programs will follow the following
// logic:
//   1. Loop forever repeating the following actions:
//   2. Get the latest game controller / joystick settings that have been received from the PC.
//   3. Perform appropriate actions based on the joystick + buttons settings. This is usually a
//      simple action:
//      *  Joystick values are usually directly translated into power levels for a motor or
//         position of a servo.
//      *  Buttons are usually used to start/stop a motor or cause a servo to move to a specific
//         position.
//   4. Repeat the loop.
//
// Your program needs to continuously loop because you need to continuously respond to changes in
// the game controller settings.
//
// At the end of the tele-op period, the FMS will autonmatically abort (stop) execution of the program.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////

task main()
{ //Open Task Main Loop
  initializeRobot();
  int Players; //Set the program to have 1 player or 2 player operation  In 1 player, all inputs are from Joy1, in 2 Player, Joy1 drives, while Joy2 manipulates arm and lift
Players = 1;
int DriveStyle; //Set the program to have Tank or Arcade Drive.  Tank Drive each stick relates to a motor.  Arcade Drive both drive motors run from the Left Stick.
DriveStyle = 1; //1 is Tank Mode
//DriveStyle = 2; //2 is Arcade Mode
float redline;
redline = 0.4;
int threshold;
threshold = 10;

//Built-in Soundlist:
//playSound(soundBeepBeep);  
   //playSound(soundBlip);
   //playSound(soundDownwardTones);
//playSound(soundException);
//playSound(soundFastUpwardTones);
//playSound(soundLowBuzz);
//playSound(soundLowBuzzShort);
//playSound(soundShortBlip);
//playSound(soundUpwardTones);


  //waitForStart();   // wait for start of tele-op phase

  while (true)
  { //Open While True Loop
 /////////////////////////////////////////////////////////
 //                                                     //
 //      Add your robot specific tele-op code here.     //
 //                                                     //
 /////////////////////////////////////////////////////////

    // Insert code to have servos and motors respond to joystick and button values.
    switch (DriveStyle)
    {
      case kDriveTank:
        Tank(joystick.joy1_y1, joystick.joy1_y2);
        break;
      case kDriveArcade:
        Arcade(joystick.joy1_x2, joystick.joy1_y2); //right stick arcade control
        //Arcade(joystick.joy1_x1, joystick.joy1_y1); //left stick arcade control
        break;
    } //Close DriveStyle switch
 
 
    //Threshold for line tracking
 int threshold = 10;
 int y1;
 int y2;
 int x1;
 int x2;

    while(1 == 1) //Open while 1==1 Loop Forever
 {
  //Get the Latest joystick values
 getJoystickSettings(joystick);
 y1 = joystick.joy1_y1;
 y2 = joystick.joy1_y2;
 x1 = joystick.joy1_x1;
 x2 = joystick.joy1_x2;
 if(y1<= threshold) y1 = 0;
 if(y2<= threshold) y2 = 0;
 if(x1<= threshold) x1 = 0;
 if(x2<= threshold) x2 = 0;

 //When Pressing Start Switch DriveStyle
 if(DriveStyle == 1) //If currently in Tank Mode
 {
  if(joy1Btn(10) == 1) //And Start --> is pressed
 {
 DriveStyle = 2; //Switch to Arcade Mode
 playSound(soundException);
 playSound(soundFastUpwardTones);
     wait1Msec(500);
   
   } //Close Start Button Code
 } //Close Tank Mode DriveStyle
 else if(DriveStyle == 2) //If currently in Arcade Mode
 {
  if(joy1Btn(10) == 1) //And Start --> is pressed
 {
  DriveStyle = 1; //Switch to Tank Mode
 playSound(soundBeepBeep);
 wait1Msec(500);
   
 } //Close Start Button Code
 } //Close Arcade DriveStyle
 
 //When Back <-- is pressed, Switch Players - Toggle to allow multiple or single joysticks
 if(Players == 1)
 {
  if(joy1Btn(9) == 1)  //If the Back <-- button is pressed
 {
  Players = 2; //Set Players to 2
     playSound(soundUpwardTones);
     wait1Msec(500);
 }
 } //Close toggle from 1 to 2 players
 else if(Players == 2)
 {
  if(joy1Btn(9) == 1)  //If the Back <-- button is pressed
 {
  Players = 1; //Set Players to 1
     playSound(soundDownwardTones);
     wait1Msec(500);
} //Close if back is pressed
 } // Close toggle from 2 to 1 player
 
  //Get the Latest joystick values after waiting following mode switch
 getJoystickSettings(joystick);
 y1 = joystick.joy1_y1;
 y2 = joystick.joy1_y2;
 x1 = joystick.joy1_x1;
 x2 = joystick.joy1_x2;
 if(y1<= threshold) y1 = 0;
 if(y2<= threshold) y2 = 0;
 if(x1<= threshold) x1 = 0;
 if(x2<= threshold) x2 = 0;
   
  //Drive Motor Control  Drive motor control is always assumed to go to Controller 1,
 //                     Player numbers shifts manipulators from Con1 to Con2 and back.
      switch (DriveStyle) //Check the Drivestyle to determine how to handle the joystick inputs
      {
      case kDriveTank: //In the case that the drivestyle is Tank Drive, send to the Tank subroutine
        Tank(joystick.joy1_y1, joystick.joy1_y2);
        break;

      case kDriveArcade: //In the case that the drivestyle is Arcade Drive, send the joystick values to the Arcade subroutine
        Arcade(joystick.joy1_x2, joystick.joy1_y2); //right stick arcade control -- use only 1 of these 2 statements
        //Arcade(joystick.joy1_x1, joystick.joy1_y1); //left stick arcade control-- use only 1 of these 2 statements
        break;
      } //Close the DriveStyle Switch
   
      //Manipulator Motor Control
      switch (Players) //Check the Number of players and if 1 Sets Gate, Tailhooks, Arm and Lift to first controller
      {
      case 1: //If one player, then put drive and manipulator controls on Controller 1
  //Arm Motor Control
 if(joy1Btn(6) == 1)       // If button 6 (RB) is pressed...
 {
  motor[armMotor] = 35;     // ...raise the arm.
 }
 else if(joy1Btn(8) == 1)  // Else, if button 8 (RT) is pressed...
 {
 motor[armMotor] = -35;    // ...lower the arm.
 }
 else                      // Else (neither button is pressed)...
 {
 motor[armMotor] = 0;      // ...stop the arm.
 }
 
 //Lift Motor Control
 if(joy1Btn(5) == 1)       // If button 5 (LB) is pressed...
 {
  motor[liftMotor] = 120;     //...raise the lift.
 }
 else if(joy1Btn(7) == 1)  // Else, if button 7 (LT) is pressed...
 {
  motor[liftMotor] = -100;    // ...lower the lift.
 }
 else                      // Else (neither button is pressed)...
 {
 motor[liftMotor] = 0;      // ...stop moving the lift.
 }
 
 //TailHook Controls
 if(joy1Btn(4) == 1)       // If button 4 (Y) is pressed...
 {
  //motor[servoFingerA] = 75;     //...both hooks up
 servo[LeftHookServo] = 0;
   servo[RightHookServo] = 250;
 }
 else if(joy1Btn(3) == 1)  // Else, if button 3 (B) is pressed...Right
 {
  servo[RightHookServo] = 10; //Right Hook Down
 }
 else if (joy1Btn(1) == 1)//Folds down Jaws button 1 (X) is pressed...Left
 {
  servo[LeftHookServo] = 190; //Left Hook Down
 }
 
 //Gate Servo Control
   else if(joy1Btn(2) == 1)  // Else, if button 2 (A) is pressed...
servo[GateServo] = 190; //Open the Gate
     else if(joy1Btn(2) != 1)  // Else, if button 2 (A) is not pressed...
//servo[GateServo] = 70; //Close the gate to the 10 o clock
//servo[GateServo] = 60; //Close the gate to the 1030 o clock
    //servo[GateServo] = 50; //Close the gate to the 11 o clock
//servo[GateServo] = 40; //Close the gate to the 1130 o clock
//servo[GateServo] = 30; //Close the gate to the 1200 o clock
servo[GateServo] = 20; //Close the gate to the 1230 o clock
//servo[GateServo] = 10; //Close the gate to the 10 o clock
//servo[GateServo] = 0; //Close the gate to the 10 o clock
 break; //Close Case 1 Player

 case 2:  //Check the Number of players and if 2 Sets Gate, Tailhooks, Arm and Lift to second controller
 //Arm Motor Control
   if(joy2Btn(6) == 1)       // If button 6 (RB) is pressed...
 {
  motor[armMotor] = 35;     // ...raise the arm.
 }
 else if(joy2Btn(8) == 1)  // Else, if button 8 (RT) is pressed...
 {
 motor[armMotor] = -35;    // ...lower the arm.
 }
 else                      // Else (neither button is pressed)...
 {
 motor[armMotor] = 0;      // ...stop the arm.
 }
 
 //Lift Motor Control
 if(joy2Btn(5) == 1)       // If button 5 (LB) is pressed...
 {
  motor[liftMotor] = 120;     //...run the Lift Raised.
 }
 else if(joy2Btn(7) == 1)  // Else, if button 7 (LT) is pressed...
 {
  motor[liftMotor] = -100;    // ...run the Lift Lowered.
 }
 else                      // Else (neither button is pressed)...
 {
 motor[liftMotor] = 0;      // ...stop moving the Lift.
 }
 
 //TailHook Controls
 if(joy2Btn(4) == 1)       // If button 4 (Y) is pressed...
 {
  //motor[servoFingerA] = 75;     //...both hooks up
 servo[LeftHookServo] = 0;
 servo[RightHookServo] = 250;
 }
 else if(joy2Btn(3) == 1)  // Else, if button 3 (B) is pressed...Right
 {
  servo[RightHookServo] = 10; //Right Hook Down
 }
 else if (joy2Btn(1) == 1)//Folds down Jaws button 1 (X) is pressed...Left
 {
  servo[LeftHookServo] = 190; //Left Hook Down
 }
 //Gate Servo Control
   else if(joy2Btn(2) == 1)  // Else, if button 2 (A) is pressed...
servo[GateServo] = 190; //Open the Gate
     else if(joy2Btn(2) != 1)  // Else, if button 2 (A) is not pressed...
//servo[GateServo] = 70; //Close the gate to the 10 o clock
//servo[GateServo] = 60; //Close the gate to the 1030 o clock
    //servo[GateServo] = 50; //Close the gate to the 11 o clock
//servo[GateServo] = 40; //Close the gate to the 1130 o clock
//servo[GateServo] = 30; //Close the gate to the 1200 o clock
servo[GateServo] = 20; //Close the gate to the 1230 o clock
//servo[GateServo] = 10; //Close the gate to the 10 o clock
//servo[GateServo] = 0; //Close the gate to the 10 o clock
 break; //Close Case 1 Player
 } //Close Player Switch
    } //Close while 1==1 Loop
} //Close While True Loop
} //Close Main