DEWBOT IV Control Software
From DEW Robotics
This is the main file for DEWBOT IV control system. A full file set (code, headers, etc can be found at DEWBOT_IV_Control#Control_System_Files.
/*******************************************************************************
* FILE NAME: 1640.c
*
* DESCRIPTION:
* This file contains the Team 1640 Robot Routines -- DEWBOT IV - 2008 FRC
*
* Revision History
* Initial Coding - FAS 2/4/2008
* Released - FAS 2/22/2008 Version: 1.0.0
* Added IR_Ramp() 2/24/2008 Version: 1.1.0
* Helps prevent wheelies in IR mode by ramping up/dn drive motors
* Added runIRMode() 3/19/2008 Version: 1.2.0
* Added runAutoMode()
* These routines were added to facilitate using IR and Full Autonomous functionality
* Either mode is choosen by the Manual Arm Switch position
* IR = Manual and Autonomous = Auto
*******************************************************************************/
#include <stdio.h>
#include "ifi_aliases.h"
#include "ifi_default.h"
#include "ifi_utilities.h"
#include "user_routines.h"
#include "user_Serialdrv.h"
#include "encoder.h"
#include "adc.h"
#include "gyro.h"
#include "pid.h"
#include "1640.h"
// Start of 1640 Routines
//Pid Structure for PID Routines
extern Pid_Config_Data_Type Pid_Config_Data[];
Pid_Config_Data_Type *arm_pid = &Pid_Config_Data[ARM];
Pid_Config_Data_Type *drive_pid = &Pid_Config_Data[DRIVE];
//Set default conditions
unsigned char armed_enabled = 0;
unsigned char capture_enabled = 0;
unsigned char op_mode = RESET;
unsigned char armMotion = NEUTRAL;
#define FULL 0
#define IR 1
#define GYRO_90_LEFT -900
#define DEFAULT_IR_SPEED 48
#define FULL_IR_SPEED 100
#define LOW_DEADBAND -35 // was -7,
#define HIGH_DEADBAND 35 // was 7
// the dead band is pretty high with the way the floor runs
// with 7 there was lots of slow movements because of the deadband
// 25 worked well, minimal adjustments
unsigned char motion_state = INITIAL_SEGMENT;
//This should represent
// 536" - 20.68 wheel rotations
// 162" - 6.25 wheel rotations
// 486" - 18.75 wheel rotations
// 162" - 6.25 wheel rotations
// Full Encoder Revolution = 128 Ticks
//long const seg_len[] = {2647,800,2400,800}; // on drive shaft
long const seg_len[] = {7400,2200,6700,2200}; // with small VEX wheel
//For Gyro
unsigned char gyro_biased = 0;
int foster_wait = 0;
/*
*****************************************************
* a r m p o s i t i o n *
*****************************************************
* Uses PID motion or Manual Joystick to move Arm into Position
* Position Constants are set using Manual Mode and Reading Encoder Output
* PID Error is current Encoder Position - Position Constant for op_mode
* PID will constanly adjust motors to keep arm in op_mode position
* Called from Default_Routine
*
* Initial Coding - FAS 2/4/2008
*/
void armPosition (unsigned char arm_position, unsigned char over_ride)
{
long encoder_pos;
long pos_error;
char speed;
unsigned char reset_condition = 0;
//Get Current Arm Position
encoder_pos = Get_Encoder_1_Count();
printf("Arm Position: %d\r", (int) encoder_pos);
//Check for Manual Arm Over Ride
if (over_ride == MANUAL_MODE)
{
//Manual Arm Indicator
Pwm2_green = 0;
//Manual Arm Movement using Joystick Y Axis Only
ArmMotor1 = p4_y ;
printf("Arm Joystick: %d\r", p4_y);
//Output Encoder Position
//printf("Arm Position: %d\r", (int) encoder_pos);
if (top_limit == ON)
if (ArmMotor1 > 127)
ArmMotor1 = 127;
if (bot_limit == ON)
{
if (ArmMotor1 < 127)
{
Reset_Encoder_1_Count(BOTTOM_LIMIT);
ArmMotor1 = 127;
reset_condition = 0;
}
}
}
else
{
//Auto Arm Mode Indicator
Pwm2_green = 1;
//Calculuate distance to arm_position
switch (arm_position)
{
// RESET is called before any other arm_position can be set
// Can be reset if the OI reset button is pushed.
// Drives arm to bottom limit and resets arm encoder to ZERO.
case RESET:
//Move Arm Downward, continue until Bottom Limit is tripped
reset_condition = 1;
break;
case PARKED:
//Calculate distance from PARKED POSITION
//pos_error = encoder_pos - CATCH_POSITION;
pos_error = PARKED_POSITION - encoder_pos;
break;
case CATCH:
//Calculate distance from CATCH POSITION
//pos_error = encoder_pos - CATCH_POSITION;
pos_error = CATCH_POSITION - encoder_pos;
break;
case CAPTURE:
//Calculate distance from CAPTURE POSITION
//pos_error = encoder_pos - CAPTURE_POSITION;
pos_error = CAPTURE_POSITION - encoder_pos;
break;
case POSSESS:
//Calculate distance from POSSESS POSITION
//pos_error = encoder_pos - POSSESS_POSITION;
pos_error = POSSESS_POSITION - encoder_pos;
break;
case ARMED:
//Calculate distance from ARMED POSITION
//pos_error = encoder_pos - ARMED_POSITION;
pos_error = ARMED_POSITION - encoder_pos;
break;
case HURDLE:
//Calculate distance from HURDLE POSITION
//pos_error = encoder_pos - HURDLE_POSITION;
pos_error = HURDLE_POSITION - encoder_pos;
break;
case PTP:
//Calculate distance from PTP POSITION
//pos_error = encoder_pos - PTP_POSITION;
pos_error = PTP_POSITION - encoder_pos;
break;
case PLACE:
//Calculate distance from PLACE POSITION
//pos_error = encoder_pos - PLACE_POSITION;
pos_error = PLACE_POSITION - encoder_pos;
break;
}
//Look for Limit Trip
if (top_limit == ON && armMotion == UP)
{
//Reset Arm to Hurdle Position
//Calculate distance from "Prepare To Place" POSITION
//pos_error = encoder_pos - PTP;
pos_error = PTP - encoder_pos;
op_mode = PTP;
//Set Arm Motors to Downward Speed
ArmMotor1 = 100;
//Don't do a reset
reset_condition = 0;
}
// Arm motion is Neutral or Trying to move down.
//Or in a RESET Condition moving towards bottom and hits the limit
else if (bot_limit == ON && armMotion != UP)
{
//Set Encoder to ZERO
Reset_Encoder_1_Count(BOTTOM_LIMIT);
//Now Position Arm to CAPTURE Position
//Calculate distance from CAPTURE POSITION
//pos_error = encoder_pos - CAPTURE_POSITION;
//op_mode = CAPTURE_POSITION;
if (autonomous_mode && (manual_toggle == MANUAL_MODE))
{
pos_error = HURDLE_POSITION - encoder_pos;
op_mode = HURDLE;
}
else
{
pos_error = CAPTURE_POSITION - encoder_pos;
op_mode = CAPTURE;
}
//Set Arm Motors to Upward Speed so that CAPTURE/HURDLE position can be reached
//after a RESET
ArmMotor1 = 200;
//Turn OFF Reset
reset_condition = 0;
}
else if (reset_condition == 1)
{
//RESET keeps arm moving downward until limit is reached
//Move Downward
ArmMotor1 = 20;
}
else
{
//Everthing is ok to move to or keep at op_mode POSITION
//Calculate PID Speed
speed = UpdatePID (arm_pid, pos_error, encoder_pos);
//printf("Speed: %d ", (int) speed);
//printf("Encoder Pos: %d pos_error: %d\r", (int) encoder_pos, (int) pos_error);
//Set Arm Motors to PID Speed
ArmMotor1 = 127 + speed;
reset_condition = 0;
}
}
//Check for "in-range" to op_mode position
//Arm is in position when Encoder = a range of +-ARM_RANGE from POSITION CONSTANT
//POSITION #4 (PTP and Place) - Set LED's
if ((encoder_pos > (POSITION_4 - ARM_RANGE)) && (encoder_pos < (POSITION_4 + ARM_RANGE)))
Relay2_green = 1;
else
Relay2_green = 0;
//POSITION #3 (ARMED and HURDLE) - Set LED's and enable/disable ARMED Mode
if ((encoder_pos > (POSITION_3 - ARM_RANGE -5)) && (encoder_pos < (POSITION_3 + ARM_RANGE + 5)))
{
//Only ENABLE ARMED when arm is in position for ARMED
armed_enabled = 1;
Switch1_LED = 1;
}
else
{
armed_enabled = 0;
Switch1_LED = 0;
}
//POSITION #2 (PARKED and POSSES) - Set LED's
if ((encoder_pos > (POSITION_2 - ARM_RANGE)) && (encoder_pos < (POSITION_2 + ARM_RANGE)))
Switch2_LED = 1;
else
Switch2_LED = 0;
//POSITION #1 (CATCH and CAPTURE) - Set LED's and consider Sonar only if capture_enabled is true,
//which means the ball is in CAPTURE POSITION.
if ((encoder_pos > (POSITION_1 - ARM_RANGE-5)) && (encoder_pos < (POSITION_1 + ARM_RANGE+5)))
{
//Auto capture if Sonar Sensed Ball @ programmed distance
if (capture_enabled)
{
//Set op_mode to CAPTURE so forks stay UP
op_mode = CAPTURE;
Relay1_green = 0;
}
Switch3_LED = 1;
}
else
Switch3_LED = 0;
}
/*
******************************************************
* c h e c k C O M P R E S S O R *
******************************************************
* Turns compressor on/off depending on digital input from Pressure Switch
* Called From Default_Routine and User_Initialization
*
* Revision History
* Initial Coding - FAS 2/4/2008
*/
void checkCompressor (void)
{
if (PressureSwitch == ON)
{
//Turn Compressor ON
Compressor_fwd = TRUE;
Compressor_rev = FALSE;
}
else
{
//Turn Compressor OFF
Compressor_fwd = FALSE;
Compressor_rev = FALSE;
}
}
/*
******************************************************
* f o r k _ 8 *
******************************************************
* retracts/extends 8" Fork Cylinder based on op_mode
* Called from Default_Routine
*
* Revision History
* Initial Coding - FAS 2/4/2008
*/
void fork_8 (unsigned char position)
{
if (position == RETRACTED)
{
//Retract 8" Fork Cylinder
Fork_8_fwd = FALSE;
Fork_8_rev = TRUE;
}
else
{
//Extend 8" Fork Cylinder
Fork_8_fwd = TRUE;
Fork_8_rev = FALSE;
}
}
/*
*****************************************************
* f o r k _ 1_5 *
*****************************************************
* retracts/extends 1.5" Fork Cylinder based on op_mode
* Called from Default_Routine
*
* Revision History
* Initial Coding - FAS 2/4/2008
*/
void fork_1_5 (unsigned char position)
{
if (position == RETRACTED)
{
//Retract 1 1/2" Fork Cylinder
Fork_1_5_fwd = FALSE;
Fork_1_5_rev = TRUE;
}
else
{
//Extend 1 1/2" Fork Cylinder
Fork_1_5_fwd = TRUE;
Fork_1_5_rev = FALSE;
}
}
/*
*****************************************************
* c a t a p u l t *
*****************************************************
* retracts/extends Catapult Cylinder based on op_mode
* Called from Default_Routine
*
* Revision History
* Initial Coding - FAS 2/4/2008
*/
void catapult (unsigned char position)
{
if (position == RETRACTED)
{
//Retract Catapult 1
Catapult_1_fwd = FALSE;
Catapult_1_rev = TRUE;
//Retract Catapult 2
Catapult_2_fwd = FALSE;
Catapult_2_rev = TRUE;
}
else
{
//Extend Catapult 1
Catapult_1_fwd = TRUE;
Catapult_1_rev = FALSE;
//Extend Catapult 2
Catapult_2_fwd = TRUE;
Catapult_2_rev = FALSE;
}
}
/*
*****************************************************
* s h i f t e r *
*****************************************************
* retracts/extends Shifter Cylinder based on shifter_Toggle (High/Low Gear)
* Called from Default_Routine
*
* Initial Coding - FAS 2/4/2008
*/
void shifter (unsigned char gear)
{
if (gear == HIGH_GEAR)
{
//Set High Gear
Shifter_fwd = FALSE;
Shifter_rev = TRUE;
Pwm1_green = 1;
}
else
{
//Set Low Gear
Shifter_fwd = TRUE;
Shifter_rev = FALSE;
Pwm1_green = 0;
}
}
/*
*****************************************************
* g e t O p e r a t i o n M o d e *
*****************************************************
* Determines op_mode based on Push Button condition
* Called from Default_Routine
*
* Initial Coding - FAS 2/4/2008
*/
unsigned char getOperationMode(unsigned char current_op_mode)
{
unsigned char which_button;
//printf("Button: %d\r", current_op_mode);
//Set which_button to Operation MODE
if (RESET_button)
which_button = RESET;
else if (catch_button)
which_button = CATCH;
else if (capture_button)
which_button = CAPTURE;
else if (park_button)
which_button = PARKED;
else if (possess_button)
which_button = POSSESS;
else if (armed_button)
which_button = ARMED;
else if (hurdle_button)
which_button = HURDLE;
else if (ptp_button)
which_button = PTP;
else if (place_button)
which_button = PLACE;
else
{
//No New Button Pressed, so keep last
which_button = current_op_mode;
}
//Return which_button got pressed
return (which_button);
}
/*
******************************************************
* r a m p i n g *
******************************************************
* Joystick / PWM ramping function found on ChiefDelphi
* Called From Default_Routine
*
* Revision History
*/
long ramping (unsigned char pwmIN)
{
long pwmOUT = 0;
pwmOUT = ((long)pwmIN - 127);
pwmOUT = ((pwmOUT) * (pwmOUT) * (pwmOUT));
pwmOUT = ((pwmOUT) / (128 * 128));
pwmOUT = (pwmOUT) + (127);
return pwmOUT;
}
// init_Gyro -- called from auto mode and master startup
void init_Gyro(void)
{
static unsigned int i;
static unsigned int j;
// GYRO
i++;
j++; // this will rollover every ~1000 seconds
if(j == 10)
{
printf("\rCalculating Gyro Bias...");
}
if(j == 60)
{
// start a gyro bias calculation
Start_Gyro_Bias_Calc();
}
if(j == 150)
{
// terminate the gyro bias calculation
Stop_Gyro_Bias_Calc();
// reset the gyro heading angle
Reset_Gyro_Angle();
printf("Done\r");
j = 0;
i = 0;
gyro_biased = 1;
}
}
/* End of init_Gyro
*/
/*
******************************************************
* Initialize_1640 *
******************************************************
* Initialize Routines for 1640.c
* Called From Default_Routine
*
* Revision History
*/
void Initialize_1640(void)
{
Initialize_Gyro();
Initialize_ADC();
Initialize_Encoders();
Initialize_Arm_PID();
Initialize_Drive_PID();
}
/*
******************************************************
* s e t A r m D i r e c t i o n *
******************************************************
* Determine direction of Arm Travel
* Called From Default_Routine
*
* Revision History
*/
void setArmDirection(void)
{
if (ArmMotor1 > 134)
armMotion = UP;
else if (ArmMotor1 < 120)
armMotion = DN;
else
armMotion = NEUTRAL;
}
/*
******************************************************
* m o v e R o b o t *
******************************************************
* Set Drive Motors in motion
* Called From Default_Routine
*
* Revision History
* Added Y and X parameters 3/19/2008 Version: 1.2.0
* to make this routine more generic
*
*/
void moveRobot(unsigned char Y, unsigned char X)
{
//Arcade Driving Mode
//pwm03
//with Ramping
// RightDriveMotor = ramping(Limit_Mix(2000 + Y - X + 127));
//without Ramping
RightDriveMotor = Limit_Mix(2000 + Y - X + 127);
//Tank Mode - Need second Joystick on PORT3
//RightDriveMotor = ramping(Y);
//pwm04
//with Ramping
// LeftDriveMotor = ramping(Limit_Mix(2000 + Y + X - 127));
//without Ramping
LeftDriveMotor = Limit_Mix(2000 + Y + X - 127);
//Tank Mode - Need second Joystick on PORT3
//LeftDriveMotor = ramping(Y);
//Tank Mode using XBOX Controller and Chicklet
//Aux = x
//Wheel = y
//pwm03 = p1_aux;
//pwm04 = p1_wheel;
}
/*
******************************************************
* I R _ R a m p *
******************************************************
* Increment/Decrement Drive Motors to prevent Wheelies
* Called From User_Autonomous_Code()
*
* Revision History
* Initial Coding by FAS on 02/24/2008
*/
void IR_Ramp (unsigned char direction, unsigned char leftDrive, unsigned rightDrive)
{
//See 1640.h IR Section for IR_RAMP constant
switch (direction)
{
case IR_FORWARD:
//Increment by IR_RAMP (1 will cause about 3.3 second ramp to top Speed)
leftDrive = leftDrive + IR_RAMP;
rightDrive = rightDrive + IR_RAMP;
break;
case IR_REVERSE:
//Decrement by IR_RAMP (1 will cause about 3.3 second ramp to top Speed)
leftDrive = leftDrive - IR_RAMP;
rightDrive = rightDrive - IR_RAMP;
break;
}
//Limits for Victor Speed Controllers - Don' go above 254 or below 0
//Right Drive
if (rightDrive > 254)
rightDrive = 254;
if (rightDrive < 0)
rightDrive = 0;
//Left Drive
if (leftDrive > 254)
leftDrive = 254;
if (leftDrive < 0)
leftDrive = 0;
}
void runAutoMode(void)
{
// for GYRO
static unsigned int i = 0;
static unsigned int j = 0;
long heading;
long pos_error;
char speed;
static long current_gyro_target = 0;
static unsigned int seg_pos = 0;
static long gyro_target = 0;
// for Encoder
long drive_tick_cnt;
static long last_drive_tick_cnt;
//Get Current Gyro Heading
//heading = Get_Gyro_Angle()/10L;
heading = Get_Gyro_Angle();
pos_error = heading - current_gyro_target;
printf("Heading: %d Error: %d\r", (int) heading, (int) pos_error);
//printf("Heading: %ld Error: %ld\r", heading, pos_error);
//Get Encoder #3 tick count
drive_tick_cnt = Get_Encoder_3_Count();
printf("Drive Count: %ld Segment Len: %d\r", drive_tick_cnt, (unsigned int) seg_len[seg_pos]);
//printf("P:%d I:%d D:%d Speed: %d\r", (int)(pid->pTerm*100), (int)(pid->iTerm*100), (int)(pid->dTerm*100), (int)pid->pwmSpeed);
switch (motion_state)
{
//Just in case we want to do something before we begin to move
case INITIAL_SEGMENT:
motion_state = MOVE_SEGMENT;
//printf("INITIAL_SEGMENT\r");
break;
case MOVE_SEGMENT:
//printf("MOVE_SEGMENT\r");
// Move Robot Straight With Motor Error Correction
//Position Error comes from GYRO
speed = UpdatePID (drive_pid, pos_error, heading);
printf("Speed: %d ", (int) speed);
//Set Drive Motors to PID Speed
if ((speed > LOW_DEADBAND) && (speed < HIGH_DEADBAND))
//Full Forward (actually reverse in Autononmous mode)
pwm03 = pwm04 = 254;
else
{
/* PID Adjustment Commentary
The PID returns a single value, this is how much the speed
should change. The PID is really set up to do a single motor,
which works well on the lifting arm.
When we get a negative speed we should subtract it from the
right motor, postive speed subtract from the left
if ( speed > 0 )
pwm03 = 254 - speed;
if ( speed < 0)
pwm04 = 254 + speed;
*/
//Use Correction Speed Factor from PID (Forward robot direction)
//pwm03 = 127 + speed;
//pwm04 = 127 - speed;
//Use Correction Speed Factor from PID (Frank - reverse robot direction)
pwm03 = 127 - speed;
pwm04 = 127 + speed;
}
if (drive_tick_cnt >= seg_len[seg_pos])
{
motion_state = QTR_TURN_LEFT;
seg_pos++;
//After one round, only use the 3rd and forth segment distances
if (seg_pos > 3)
seg_pos = 2;
pwm03 = pwm04 = 127;
}
break;
case QTR_TURN_LEFT:
//Turn Robot Left
printf("QTR_TURN_LEFT\r");
//Set motors for Turning Speed
//Forks are the front
LeftDriveMotor = 160; // 160;
RightDriveMotor = 32; //32;
//Update for next Gyro heading
//current_gyro_target += GYRO_90_LEFT;
current_gyro_target = GYRO_90_LEFT;
//gyro_target = GYRO_90_LEFT + current_gyro_target;
//printf("Target: %d Heading: %d\r", (int)gyro_target, (int) heading);
motion_state = TURNING;
//Reset PID State
Initialize_Drive_PID();
break;
case TURNING:
//printf("TURNING\r");
//Are we at GYRO TARGET Heading?
//Only checking for when we hit +??? to target position
//Even if we carry over we should be ready to move straight
if (pos_error < 50)
{
printf("TURNed\r");
motion_state = STOP;
// gyro_biased = 0;
// was -- motion_state = MOVE_SEGMENT;
}
else
Reset_Encoder_3_Count();
break;
case STOP:
if ( last_drive_tick_cnt != drive_tick_cnt )
{
printf("Motor Stopped\r");
pwm03 = pwm04 = 127; // Stop
}
else
{
motion_state = MOVE_SEGMENT;
printf("Starting moving again\r");
//Reset Gyro Angle
Reset_Gyro_Angle();
//Reset Target Angle
current_gyro_target = 0;
//Reset Encoder for next segment
Reset_Encoder_3_Count();
}
break;
}
last_drive_tick_cnt = drive_tick_cnt;
}
void runIRMode(void)
{
static unsigned char j=0;
static unsigned char k=0;
static unsigned char ir_startup=1;
//IR Mode - Start out going Forward which is really Reverse on the Robot
static unsigned char ir_cmd = CMD3;
//IR Command Mode
if (CMD0_IN) //IR Command #0 Active (FORWARD)
{
Pwm1_red = 1;
Pwm2_red = Relay1_red = Relay2_red = 0;
j = 0;
//LeftDriveMotor = 127;
//RightDriveMotor = 127;
ir_cmd = CMD0;
}
else if (CMD1_IN) //IR Command #1 Active (RIGHT JOG)
{
Pwm2_red = 1;
Pwm1_red = Relay1_red = Relay2_red = 0;
ir_cmd = CMD1;
j = 0;
}
else if (CMD2_IN) //IR Command #2 Active (LEFT JOG)
{
Relay1_red = 1;
Pwm1_red = Pwm2_red = Relay2_red = 0;
ir_cmd = CMD2;
j = 0;
}
else if (CMD3_IN) //IR Command #3 Active (REVERSE)
{
Relay2_red = 1;
Pwm1_red = Pwm2_red = Relay1_red = 0;
j = 0;
//LeftDriveMotor = 127;
//RightDriveMotor = 127;
ir_cmd = CMD3;
}
if ((CMD0_IN + CMD1_IN + CMD2_IN + CMD3_IN) > 1)
{
//If no commands are active set IR command Mode to #0 - FORWARD
ir_cmd = CMD3;
}
printf("IR COMMAND: %d\r", ir_cmd);
//IR State
switch (ir_cmd)
{
case CMD0: //Forward
LeftDriveMotor = 127 + DEFAULT_IR_SPEED;
RightDriveMotor = 127 + DEFAULT_IR_SPEED;
//printf("IR COMMAND 0\r");
///IR_Ramp(IR_FORWARD, LeftDriveMotor, RightDriveMotor);
break;
case CMD1: //Turn Left for (j * 26.2 ms))
//NOTE: Maybe turn 90 using GYRO instead of Timing JOG
if (j++ < 5)
{
LeftDriveMotor = 32;
RightDriveMotor = 160;
}
else
{
//LeftDriveMotor = 127 + DEFAULT_IR_SPEED;
//RightDriveMotor = 127 + DEFAULT_IR_SPEED;
LeftDriveMotor = 127;
RightDriveMotor = 127;
ir_cmd = CMD0;
}
break;
case CMD2: //Turn Right for (j * 26.2 ms))
//NOTE: Maybe turn 90 using GYRO instead of Timing JOG
if (j++ < 5)
{
LeftDriveMotor = 160;
RightDriveMotor = 32;
}
else
{
LeftDriveMotor = 127 + DEFAULT_IR_SPEED;
RightDriveMotor = 127 + DEFAULT_IR_SPEED;
}
break;
case CMD3: //Reverse
if (k++ < 40 && ir_startup)
{
LeftDriveMotor = 127; //- DEFAULT_IR_SPEED;
RightDriveMotor = 127; //- DEFAULT_IR_SPEED;
}
else
{
ir_startup = 0;
LeftDriveMotor = 127 - DEFAULT_IR_SPEED; //FULL_IR_SPEED;
RightDriveMotor = 127 - DEFAULT_IR_SPEED; //FULL_IR_SPEED;
}
//IR_Ramp(IR_REVERSE, LeftDriveMotor, RightDriveMotor);
break;
}
}See our other robots at FRC Team 1640
