void EnhancedRobotDrive::doControls() {
if((std::fabs(driver -> GetRawAxis(DRIVER_LEFT_DRIVE_AXIS)) > EnhancedJoystick::JOYSTICK_ZERO_TOLERANCE) || (std::fabs(driver -> GetRawAxis(DRIVER_RIGHT_DRIVE_AXIS)) > EnhancedJoystick::JOYSTICK_ZERO_TOLERANCE))
{
//Skyler Driving
TankDrive((driver -> GetRawAxis(DRIVER_LEFT_DRIVE_AXIS) * drivePower),(driver -> GetRawAxis(DRIVER_RIGHT_DRIVE_AXIS) * drivePower));
}
else if((gunner -> GetRawButton(GUNNER_SWIVEL_RIGHT) || gunner -> GetRawButton(GUNNER_SWIVEL_LEFT)) && *robotState == robot_class::NORMAL)
{
//Ben Swivel
if(gunner -> GetRawButton(GUNNER_SWIVEL_RIGHT))
{
swivel(RIGHT);
}
else
{
swivel(LEFT);
}
}
else if(driver -> GetRawButton(DRIVER_SWIVEL_RIGHT) || driver -> GetRawButton(DRIVER_SWIVEL_LEFT))
{
//Skyler Swivel
if(driver -> GetRawButton(DRIVER_SWIVEL_RIGHT))
{
swivel(RIGHT);
}
else
{
swivel(LEFT);
}
}
else
TankDrive(0.0f,0.0f);
}
void SimpleDrive::TankDrive(float leftValue, float rightValue, bool squaredInputs) {
if (squaredInputs) {
leftValue = Tools::SquareMagnitude(leftValue);
rightValue = Tools::SquareMagnitude(rightValue);
}
TankDrive(leftValue, rightValue);
}
void OperatorControl(void)
{
OperatorControlInit();
compressor.Start();
testActuator.Start();
while (IsOperatorControl())
{
ProgramIsAlive();
//No need to do waits because ProgramIsAlive function does a wait. //Wait(0.005);
bool isButtonPressed = stick.GetRawButton(3);
SmartDashboard::PutNumber("Actuator Button Status",isButtonPressed);
if (isButtonPressed)
{
testActuator.Go();
}
float leftYaxis = stick.GetY();
float rightYaxis = stick.GetRawAxis(5); //RawAxis(5);
TankDrive(leftYaxis,rightYaxis); // drive with arcade style (use right stick)for joystick 1
SmartDashboard::PutNumber("Left Axis",leftYaxis);
SmartDashboard::PutNumber("Right Axis",rightYaxis);
}
}
/**
* Provide tank steering using the stored robot configuration.
* Drive the robot using two joystick inputs. The Y-axis will be selected from
* each Joystick object.
* @param leftStick The joystick to control the left side of the robot.
* @param rightStick The joystick to control the right side of the robot.
*/
void RobotDrive::TankDrive(GenericHID *leftStick, GenericHID *rightStick,
bool squaredInputs) {
if (leftStick == nullptr || rightStick == nullptr) {
wpi_setWPIError(NullParameter);
return;
}
TankDrive(leftStick->GetY(), rightStick->GetY(), squaredInputs);
}
/**
* Provide tank steering using the stored robot configuration.
* Drive the robot using two joystick inputs. The Y-axis will be selected from
* each Joystick object.
* @param leftStick The joystick to control the left side of the robot.
* @param rightStick The joystick to control the right side of the robot.
*/
void RobotDrive::TankDrive(GenericHID *leftStick, GenericHID *rightStick)
{
if (leftStick == NULL || rightStick == NULL)
{
wpi_fatal(NullParameter);
return;
}
TankDrive(leftStick->GetY(), rightStick->GetY());
}
/**
* Provide tank steering using the stored robot configuration.
* This function lets you pick the axis to be used on each Joystick object for the left
* and right sides of the robot.
* @param leftStick The Joystick object to use for the left side of the robot.
* @param leftAxis The axis to select on the left side Joystick object.
* @param rightStick The Joystick object to use for the right side of the robot.
* @param rightAxis The axis to select on the right side Joystick object.
*/
void RobotDrive::TankDrive(GenericHID *leftStick, uint32_t leftAxis,
GenericHID *rightStick, uint32_t rightAxis, bool squaredInputs)
{
if (leftStick == NULL || rightStick == NULL)
{
wpi_setWPIError(NullParameter);
return;
}
TankDrive(leftStick->GetRawAxis(leftAxis), rightStick->GetRawAxis(rightAxis), squaredInputs);
}
/**
* Provide tank steering using the stored robot configuration.
* This function lets you pick the axis to be used on each Joystick object for the left
* and right sides of the robot.
* @param leftStick The Joystick object to use for the left side of the robot.
* @param leftAxis The axis to select on the left side Joystick object.
* @param rightStick The Joystick object to use for the right side of the robot.
* @param rightAxis The axis to select on the right side Joystick object.
*/
void RobotDrive::TankDrive(GenericHID *leftStick, UINT32 leftAxis,
GenericHID *rightStick, UINT32 rightAxis)
{
if (leftStick == NULL || rightStick == NULL)
{
wpi_fatal(NullParameter);
return;
}
TankDrive(leftStick->GetRawAxis(leftAxis), rightStick->GetRawAxis(rightAxis));
}
bool DriveTrain::NewSpinCW(double speed, double angle)
{
if(mGyro->IsWorking())
{
if(mIsDoneDriving)
{
mIsDoneDriving = false;
ResetGyro();
TankDrive(speed, -speed);
}
else if(fabs(mGyro->GetZ()) >= angle)
{
StopDriving();
mIsDoneDriving = true;
}
else
{
TankDrive(speed, -speed);
}
}
else
{
if (mIsDoneDriving)
{
mIsDoneDriving = false;
ResetEncoders();
TankDrive(speed, -speed);
}
else if (fabs(mLeftMaster->GetPosition()) >= (angle / 360.0) * WHEEL_BASE_CIRCUMFERENCE || fabs(mRightMaster->GetPosition()) >= (angle / 360.0) * WHEEL_BASE_CIRCUMFERENCE)
{
StopDriving();
ResetEncoders();
mIsDoneDriving = true;
}
else
{
TankDrive(speed, -speed);
}
}
return mIsDoneDriving;
}
bool DriveTrain::AutoTurn(double target, double kP){ // THIS IS WORKING!! DO NOT OVERWRITE!!!!!!!!!!!!!!!!!!!!!!!
SmartDashboard::PutString("Current Task", "auto turn");
int angleDiff = ((int)(GetGyro()-target)) % 360 ; //angle diff
double error = (1 - (( (double) abs( abs(angleDiff) - 180 ) / 180.0))) * kP;
if(((angleDiff > 0 && angleDiff <= 180) || (angleDiff < -180))){
TankDrive(((0.1 * error) + 0.9), -(0.1 * error + 0.9));
}
else if(((angleDiff < 0 && angleDiff >= -180) || (angleDiff > 180))){
TankDrive(-(0.1 * error + 0.9), (0.1 * error + 0.9));
}
else{
TankDrive(0.0f, 0.0f);
return true;
}
return false;
}
bool DriveTrain::GoUltraForward(double distance, float baseSpeed, double target, double kP, double source){ // THIS IS WORKING!! DO NOT OVERWRITE!!!!!!!!!!!!!!!!!!!!!!!
double absoluteTolerance = 5; //in cms
SmartDashboard::PutString("Current Task", "automove");
double currentDistance = source; //return value in meteres with temp compensation
double directionSpeed;
double goingBack; //i'm confused which way is front, but i'm glad that I'm not the only one
if(distance < currentDistance){
goingBack = 1;
directionSpeed = -baseSpeed;
}else{
directionSpeed = baseSpeed; //don't ask
goingBack = 0;
}
SmartDashboard::PutNumber("abs", abs(currentDistance*10 - distance*10));
SmartDashboard::PutNumber("absoluteTolerance", absoluteTolerance);
int angleDiff = ((int)(GetGyro() - target)) % 360;
double error = (((double)abs(abs(angleDiff) - goingBack * 180)/180.0) * 2 - 1.0) * kP; //1.0 - -1.0 //kP must be =< 1.0
if(((angleDiff > 0 && angleDiff <= 180) || (angleDiff < -180)) && abs(currentDistance*100 - distance*100)>=absoluteTolerance){
TankDrive(directionSpeed * error, directionSpeed);
}
else if(((angleDiff < 0 && angleDiff >= -180) || (angleDiff > 180)) && abs(currentDistance*100 - distance*100)>=absoluteTolerance){
TankDrive(directionSpeed, directionSpeed * error);
}
else if(angleDiff == 0 && abs(currentDistance*100 - distance*100)>=absoluteTolerance){
TankDrive(directionSpeed, directionSpeed);
}
else{
TankDrive(0.0f, 0.0f);
return true;
}
return false;
}
bool DriveTrain::GoForward(double distance, float baseSpeed, double target, double kP){ // THIS IS WORKING!! DO NOT OVERWRITE!!!!!!!!!!!!!!!!!!!!!!!
double absoluteTolerance = 10;
SmartDashboard::PutString("Current Task", "automove");
double currentDistance = GetRightEncoderValue();
double directionSpeed;
double goingBack; //i'm confused which way is front, but i'm glad that I'm not the only one
if(distance > currentDistance){
goingBack = 1;
directionSpeed = -baseSpeed;
}else{
directionSpeed = baseSpeed; //don't ask
goingBack = 0;
}
SmartDashboard::PutNumber("EncoderReading", currentDistance);
SmartDashboard::PutNumber("Goalz", distance);
int angleDiff = ((int)(GetGyro() - target)) % 360;
double error = (((double)abs(abs(angleDiff) - goingBack * 180)/180.0) * 2 - 1.0) * kP; //1.0 - -1.0 //kP must be =< 1.0
if(((angleDiff > 0 && angleDiff <= 180) || (angleDiff < -180)) && abs(currentDistance - distance)>=absoluteTolerance){
TankDrive(directionSpeed * error, directionSpeed);
}
else if(((angleDiff < 0 && angleDiff >= -180) || (angleDiff > 180)) && abs(currentDistance - distance)>=absoluteTolerance){
TankDrive(directionSpeed, directionSpeed * error);
}
else if(angleDiff == 0 && abs(currentDistance - distance)>=absoluteTolerance){
TankDrive(directionSpeed, directionSpeed);
}
else{
TankDrive(0.0f, 0.0f);
return true;
}
return false;
}
bool DriveTrain::NewDriveDistance(double speed, double distance)
{
if (mIsDoneDriving)
{
mIsDoneDriving = false;
ResetEncoders();
TankDrive(speed, speed);
std::cout << distance << '\n';
}
else if (fabs(mLeftMaster->GetPosition()) >= distance || fabs(mRightMaster->GetPosition()) >= distance)
{
StopDriving();
ResetEncoders();
mIsDoneDriving = true;
}
else
{
TankDrive(speed, speed);
}
return mIsDoneDriving;
}
// Spin clockwise
bool DriveTrain::SpinCW(double speed, double angle)
{
if (mIsDoneDriving)
{
mIsDoneDriving = false;
ResetEncoders();
Wait(0.1);
TankDrive(speed, -speed);
}
else if (GetDistance() >= (angle / 360.0) * WHEEL_BASE_CIRCUMFERENCE)
{
StopDriving();
ResetEncoders();
Wait(0.1);
mIsDoneDriving = true;
}
return mIsDoneDriving;
}
task main()
{
//false=off
servo[basketServo]=basketServoUp;
while(true)
{
getJoystickSettings(joystick);
//Calls the tank drive function
TankDrive();
//Calls the sweeper function
Sweeper();
//Moving the arm up or down
if(ARMJOYSTICK>deadZone && armEnabled==true)
{
motor[armMotor]=armMotorUp;
}
else if(ARMJOYSTICK<-deadZone && armEnabled==true)
{
//Checks if touchsensor is pressed before moving the arm
if(SensorValue[touchSensor]==touchUnpressed)
{
motor[armMotor]=-armMotorUp;
}
else
{
motor[armMotor]=off;
}
}
else
{
motor[armMotor]=off;
}
//Calls the functions
BasketStateMachine();
FlagMotor();
}
}
// Drives certain distance
bool DriveTrain::DriveDistance(double speed, double distance)
{
if (mIsDoneDriving)
{
mIsDoneDriving = false;
ResetEncoders();
Wait(0.1);
TankDrive(speed, speed);
}
else if (GetDistance() >= distance)
{
StopDriving();
ResetEncoders();
Wait(0.1);
mIsDoneDriving = true;
}
return mIsDoneDriving;
}
/*******************************************************************************
*
* FUNCTION: Teleop()
*
* PURPOSE: This is where you put code that you want to execute while
* your robot is in teleoperation mode. While in teleoperation
* mode, this function is called every 26.2ms after new data
* is received from the master processor.
*
* CALLED FROM: main() in ifi_frc.c
*
* PARAMETERS: None
*
* RETURNS: Nothing
*
* COMMENTS: While in this mode, all operator interface data is valid
* and all robot controller outputs are enabled.
*
*******************************************************************************/
void Teleop(void)
{
static int launch_stick = 0;
static unsigned char out=127;
//printf("in loop\r\n");
TankDrive();
if(p2_sw_top) ShiftHigh();
if(p2_sw_trig) ShiftLow();
if(p1_sw_top) OrientationFwd();
if(p1_sw_trig) OrientationRev();
DriveHandler();
Compressor_Handler();
// Launcher Control
/*launch_stick = (int)(p4_y) - 127;
if( launch_stick >=0) launch_stick = launch_stick;
else launch_stick = launch_stick;
out = (unsigned char)LimitChar(launch_stick);
//LAUNCHER_PWM = out;
*/
//
//printf("Top: %d | Trig: %d | aux1: %d | aux2: %d | y: %d \r\n",p3_sw_top,p3_sw_trig,p3_sw_aux1,p3_sw_aux2,p3_y);
Launcher_Handler();
// update the state of the LEDs on the operator interface
Update_OI_LEDs(); // located in ifi_code.c
}
// Stops Driving
void DriveTrain::StopDriving()
{
TankDrive(0, 0);
}
void RobotDrive::TankDrive(GenericHID &leftStick, uint32_t leftAxis,
GenericHID &rightStick, uint32_t rightAxis, bool squaredInputs)
{
TankDrive(leftStick.GetRawAxis(leftAxis), rightStick.GetRawAxis(rightAxis), squaredInputs);
}
void PidSimpleDrive::TankDrive(float leftValue, float rightValue) {
TryToggling(Rate);
TankDrive(leftValue, rightValue, false);
}
void RobotDrive::TankDrive(GenericHID &leftStick, GenericHID &rightStick, bool squaredInputs)
{
TankDrive(leftStick.GetY(), rightStick.GetY(), squaredInputs);
}
void EnhancedRobotDrive::swivel(dir d) {
if(d == LEFT)
TankDrive(DRIVE_TURN_SPEED,-1.0 * DRIVE_TURN_SPEED);
else
TankDrive(-1.0 *DRIVE_TURN_SPEED,DRIVE_TURN_SPEED);
}
/**
* Tells the robot to stop moving, accounting for things like
* slipping and sliding.
*
* On the SimpleDrive, this is equivalent to calling
* SimpleDrive::StopMoving.
*/
void SimpleDrive::Brake() {
TankDrive(0.0, 0.0);
}
void RobotDrive::TankDrive(GenericHID &leftStick, UINT32 leftAxis,
GenericHID &rightStick, UINT32 rightAxis)
{
TankDrive(leftStick.GetRawAxis(leftAxis), rightStick.GetRawAxis(rightAxis));
}
void RobotDrive::TankDrive(GenericHID &leftStick, GenericHID &rightStick)
{
TankDrive(leftStick.GetY(), rightStick.GetY());
}
