void OperatorControl(void)
{
NetTest();
return;
myRobot.SetSafetyEnabled(true);
digEncoder.Start();
const double ppsTOrpm = 60.0/250.0; //Convert from Pos per Second to Rotations per Minute by multiplication
// (See the second number on the back of the encoder to replace 250 for different encoders)
const float VoltsToIn = 41.0; // Convert from volts to cm by multiplication (volts from ultrasonic).
// This value worked for distances between 1' and 10'.
while (IsOperatorControl())
{
if (stick.GetRawButton(4)) {
myRobot.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(), -1);
}
else if (stick.GetRawButton(5))
{
myRobot.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(), 1);
}
else
{
myRobot.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(), 0);
}
myRobot.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(), 0);
SmartDashboard::PutNumber("Digital Encoder RPM", abs(digEncoder.GetRate()*ppsTOrpm));
SmartDashboard::PutNumber("Ultrasonic Distance inch", (double) ultra.GetAverageVoltage()*VoltsPerInch);
SmartDashboard::PutNumber("Ultrasonic Voltage", (double) ultra.GetAverageVoltage());
Wait(0.1);
}
digEncoder.Stop();
}
void FunctionBot::driveX(int x)
{
m_RawBot->resetEncoders();
correct=m_RawBot->m_Gyro->GetAngle();
correct/=30;
correct*=-1;
float traveled;
float traveledmode;
traveled=m_RawBot->averageEncoders();
traveledmode=m_RawBot->averageEncoders()+abs(x);
//traveledmode=(traveledmode * 8 * 3.14) + abs(x);
if(x<0)
{
while(abs(x)-m_RawBot->averageEncoders()>1&&!IsOperatorControl())
{
drive->MecanumDrive_Cartesian(0,-.15,0,0);//numbers might be wrong get checked
traveled=m_RawBot->averageEncoders();
}
}
else
{
while(abs(x)-m_RawBot->averageEncoders()>1&&!IsOperatorControl())
{
drive->MecanumDrive_Cartesian(0,.15,0,0);//numbers might be wrong get checked
traveled=m_RawBot->averageEncoders();
}
}
drive->MecanumDrive_Cartesian(0,0,0,0);
}
void TeleopPeriodic(void)
{
myarm->prepareSignal();
// Call the drive routine to drive the robot.
if(rightStick->GetRawButton(1)|| leftStick->GetRawButton(1))
drive->MecanumDrive_Cartesian(rightStick->GetX()/2,leftStick->GetY()/2,leftStick->GetX()/2,0.00);
else
drive->MecanumDrive_Cartesian(rightStick->GetX(),leftStick->GetY(),leftStick->GetX(),0.00);
GetStateForArm();
//Wait(.1);
if(modeArm==DDCArm::kManualOveride)
{
myarm->OperateArm(0,0,0,modeArm);
//Shoulder movement
if(leftStick->GetRawButton(3))
myarm->MoveShoulder(1);
else if(leftStick->GetRawButton(2))
myarm->MoveShoulder(-1);
else
myarm->MoveShoulder(0);
//Elbow Movement
if(rightStick->GetRawButton(3))
myarm->MoveElbow(1);
else if (rightStick->GetRawButton(2))
myarm->MoveElbow(-1);
else
myarm->MoveElbow(0);
//Wrist Movement
if(rightStick->GetRawButton(4))
myarm->MoveWrist(-1);
else if(rightStick->GetRawButton(5))
myarm->MoveWrist(1);
else
myarm->MoveWrist(0);
}
else
myarm->OperateArm(0.0,0.0,peg,modeArm);
if(leftStick->GetRawButton(4))
myarm->MoveClaw(-1);
else if(leftStick->GetRawButton(5))
myarm->MoveClaw(1);
else
myarm->MoveClaw(0);
if(rightStick->GetRawButton(10))
{
printf("S: %f \n E: %f \n W: %f \n \n",myarm->GetShoulderVoltage(),myarm->GetElbowVoltage(), myarm->GetWristVoltage());
}
deploy->OperateDeployment(fire,pull);
// Send Data to the Driver Station for Monitoring (w/in .
//sendIOPortData();
//Wait(.1);
}
void AutonomousType1() { //Just drive forward
SmartDashboard::PutString("STATUS:", "STARTING AUTO 1");
//<TODO>-0.2 y for 3 seconds =43 inches --> 1 second at full speed is 71.66667 inches
robotDrive.MecanumDrive_Cartesian(0, -0.36, 0);
if (WaitF(1.75))
return;
robotDrive.MecanumDrive_Cartesian(0, 0, 0);
SmartDashboard::PutString("STATUS:", "AUTO 1 COMPLETE");
}
//Grab first yellow, back up to auto zone, DON'T DROP
void AutonomousType12() {
SmartDashboard::PutString("STATUS:", "STARTING AUTO 12");
chainLift.SetSpeed(0.5);
while (IsAutonomous() && IsEnabled() && maxUp.Get() && midPoint.Get()) {
}
chainLift.SetSpeed(0);
robotDrive.MecanumDrive_Cartesian(0, 0.4, 0);
if (WaitF(3))
return;
robotDrive.MecanumDrive_Cartesian(0, 0, 0);
SmartDashboard::PutString("STATUS:", "AUTO 12 COMPLETE");
}
//Steal cans
void AutonomousType13() {
SmartDashboard::PutString("STATUS:", "STARTING AUTO 13");
robotDrive.MecanumDrive_Cartesian(0, 0.2, 0);
if (WaitF(1.2))
return;
robotDrive.MecanumDrive_Cartesian(0, 0, 0);
canGrabber.SetSpeed(1);
if (WaitF(4))
return;
canGrabber.SetSpeed(0);
LinearAcceleration(1, 0, 1, 0);
SmartDashboard::PutString("STATUS:", "AUTO 13 COMPLETE");
}
void AutomaticLineup() {
double leftVolts = leftIR.GetAverageVoltage() - leftIRZero;
double rightVolts = rightIR.GetAverageVoltage() - leftIRZero;
if (leftVolts <= VOLTAGE_TO_PICK && rightVolts <= VOLTAGE_TO_PICK) {
robotDrive.MecanumDrive_Cartesian(0, 0, 0);
} else if (leftVolts > VOLTAGE_TO_PICK
&& rightVolts > VOLTAGE_TO_PICK) {
robotDrive.MecanumDrive_Cartesian(0, -0.3, 0);
} else if (leftVolts > VOLTAGE_TO_PICK) {
robotDrive.MecanumDrive_Cartesian(0, 0, 0.2);
} else if (rightVolts > VOLTAGE_TO_PICK) {
robotDrive.MecanumDrive_Cartesian(0, 0, -0.2);
}
}
void AutonomousType7() {
SmartDashboard::PutString("STATUS:", "STARTING AUTO 7");
robotDrive.MecanumDrive_Cartesian(0, -0.2, 0);
if (WaitF(1.2))
return;
robotDrive.MecanumDrive_Cartesian(0, 0, 0);
chainLift.SetSpeed(0.5);
while (IsAutonomous() && maxUp.Get() && midPoint.Get()) {
}
chainLift.SetSpeed(0);
robotDrive.MecanumDrive_Cartesian(0, 0.2, 0);
if (WaitF(1.6))
return;
SmartDashboard::PutString("STATUS:", "AUTO 7 COMPLETE");
}
void DriveControl() { //Drives la wheels of the robot
flightX = flightStick->GetRawAxis(0); //Pull joystick side motion for later use
flightY = flightStick->GetRawAxis(1); //Pull joystick forward motion for later use (forward is -, backwards is +)
flightZ = flightStick->GetRawAxis(4); //Pull joystick twist motion for later use
flightThrottle = ((((shootStick->GetThrottle() - 1)*-1)/2) * .8 + .2); //Pull throttle to modify drive variables
//Throttle value is between .2 and 1.0
if (fabs(flightX) < deadZone) { //Deaden x
flightX = 0;
}
if (fabs(flightY) < deadZone) { //Deaden y
flightY = 0;
}
if (fabs(flightZ) < deadZone) { //Deaden z
flightZ = 0;
}
if (flightStick->GetRawButton(strafeButtonChannel)){ //Set drive to strafe mode
driveMode = 0;
}
if (flightStick->GetRawButton(arcadeButtonChannel)){ //Set drive to arcade mode
driveMode = 1;
}
if (flightStick->GetRawButton(fieldButtonChannel)){ //Set drive to field-centric mode
driveMode = 2;
}
if (shootStick->GetRawButton(gyroResetChannel)){ //Reset gyro with the trigger
yawGyro->Reset();
}
flightX = flightX * flightThrottle;
flightY = flightY * flightThrottle;
flightZ = flightZ * flightThrottle;
if(driveMode == 1){
robotDrive->MecanumDrive_Cartesian(flightZ, flightY, flightX, 0);
SmartDashboard::PutString("DriveMode", "Arcade");
}
else if(driveMode == 2){
robotDrive->MecanumDrive_Cartesian(flightX, flightY, flightZ, yawGyro->GetAngle());
SmartDashboard::PutString("DriveMode", "Field");
}
else{
robotDrive->MecanumDrive_Cartesian(flightX, flightY, flightZ, 0);
SmartDashboard::PutString("DriveMode", "Strafe");
}
SmartDashboard::PutNumber("GyroAngle", yawGyro->GetAngle());
}
//Choose which auto to use
void AutonomousInit() {
chainLift.SetSpeed(0);
canGrabber.SetSpeed(0);
robotDrive.MecanumDrive_Cartesian(0, 0, 0);
SmartDashboard::PutString("STATUS:", "STARTING AUTO");
robotDrive.SetSafetyEnabled(false);
chainLift.SetSafetyEnabled(false);
SmartDashboard::PutBoolean("Auto switch A: ", autoSwitch1.Get());
SmartDashboard::PutBoolean("Auto switch B: ", autoSwitch2.Get());
//Select auto type
if (autoSwitch1.Get()) {
if (autoSwitch2.Get())
AutonomousType4();
else
//1 on 2 grab n back
AutonomousType8();
} else {
if (autoSwitch2.Get())
//1 off, 2 on: grab n turn
AutonomousType10();
else {
SmartAutoPicker();
}
//Do Nothing
}
}
void AutonomousType3() { //Grab a bin/trash bin, and move forward
SmartDashboard::PutString("STATUS:", "STARTING AUTO 3");
chainLift.SetSpeed(0.5);
while (midPoint.Get() && maxUp.Get()) {
}
chainLift.SetSpeed(0);
robotDrive.MecanumDrive_Cartesian(0, -0.75, 0);
if (WaitF(1.75))
return;
robotDrive.MecanumDrive_Cartesian(0, 0, 0);
chainLift.SetSpeed(-0.5);
while (maxDown.Get()) {
}
chainLift.SetSpeed(0);
SmartDashboard::PutString("STATUS:", "AUTO 3 COMPLETE");
}
/**
* Runs the motors with Mecanum drive.
*/
void OperatorControl()
{
robotDrive.SetSafetyEnabled(false);
while (IsOperatorControl() && IsEnabled())
{
bool collisionDetected = false;
double curr_world_linear_accel_x = ahrs->GetWorldLinearAccelX();
double currentJerkX = curr_world_linear_accel_x - last_world_linear_accel_x;
last_world_linear_accel_x = curr_world_linear_accel_x;
double curr_world_linear_accel_y = ahrs->GetWorldLinearAccelY();
double currentJerkY = curr_world_linear_accel_y - last_world_linear_accel_y;
last_world_linear_accel_y = curr_world_linear_accel_y;
if ( ( fabs(currentJerkX) > COLLISION_THRESHOLD_DELTA_G ) ||
( fabs(currentJerkY) > COLLISION_THRESHOLD_DELTA_G) ) {
collisionDetected = true;
}
SmartDashboard::PutBoolean( "CollisionDetected", collisionDetected);
try {
/* Use the joystick X axis for lateral movement, */
/* Y axis for forward movement, and Z axis for rotation. */
/* Use navX MXP yaw angle to define Field-centric transform */
robotDrive.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(),
stick.GetZ(),ahrs->GetAngle());
} catch (std::exception ex ) {
std::string err_string = "Error communicating with Drive System: ";
err_string += ex.what();
DriverStation::ReportError(err_string.c_str());
}
Wait(0.005); // wait 5ms to avoid hogging CPU cycles
}
}
/**
* Drive based upon joystick inputs, and automatically control
* motors if the robot begins tipping.
*/
void OperatorControl()
{
robotDrive.SetSafetyEnabled(false);
while (IsOperatorControl() && IsEnabled()) {
double xAxisRate = stick.GetX();
double yAxisRate = stick.GetY();
double pitchAngleDegrees = ahrs->GetPitch();
double rollAngleDegrees = ahrs->GetRoll();
if ( !autoBalanceXMode &&
(fabs(pitchAngleDegrees) >=
fabs(kOffBalanceThresholdDegrees))) {
autoBalanceXMode = true;
}
else if ( autoBalanceXMode &&
(fabs(pitchAngleDegrees) <=
fabs(kOnBalanceThresholdDegrees))) {
autoBalanceXMode = false;
}
if ( !autoBalanceYMode &&
(fabs(pitchAngleDegrees) >=
fabs(kOffBalanceThresholdDegrees))) {
autoBalanceYMode = true;
}
else if ( autoBalanceYMode &&
(fabs(pitchAngleDegrees) <=
fabs(kOnBalanceThresholdDegrees))) {
autoBalanceYMode = false;
}
// Control drive system automatically,
// driving in reverse direction of pitch/roll angle,
// with a magnitude based upon the angle
if ( autoBalanceXMode ) {
double pitchAngleRadians = pitchAngleDegrees * (M_PI / 180.0);
xAxisRate = sin(pitchAngleRadians) * -1;
}
if ( autoBalanceYMode ) {
double rollAngleRadians = rollAngleDegrees * (M_PI / 180.0);
yAxisRate = sin(rollAngleRadians) * -1;
}
try {
// Use the joystick X axis for lateral movement, Y axis for forward movement, and Z axis for rotation.
robotDrive.MecanumDrive_Cartesian(xAxisRate, yAxisRate,stick.GetZ());
} catch (std::exception ex ) {
std::string err_string = "Drive system error: ";
err_string += ex.what();
DriverStation::ReportError(err_string.c_str());
}
Wait(0.005); // wait 5ms to avoid hogging CPU cycles
}
}
/**
* Runs the motors with Mecanum drive.
*/
void OperatorControl()
{
robotDrive.SetSafetyEnabled(false);
while (IsOperatorControl() && IsEnabled())
{
// Use the joystick X axis for lateral movement, Y axis for forward movement, and Z axis for rotation.
// This sample does not use field-oriented drive, so the gyro input is set to zero.
robotDrive.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(), stick.GetZ());
Wait(0.005); // wait 5ms to avoid hogging CPU cycles
}
}
void OperatorControl()
{
myRobot.SetSafetyEnabled(true);
while (IsOperatorControl() && IsEnabled())
{
//MECANUM DRIVE
float yValue = stick->GetY();
float xValue = stick->GetX();
float rotation = stick->GetTwist();
myRobot.MecanumDrive_Cartesian(xValue, yValue, rotation, 0.0);
Wait(0.005);
}
}
void CupidShuffle() {
SmartDashboard::PutString("STATUS:", "TIME 2 GET DOWWWWWWN");
//Tempo of song
static double tempo = 0.41666666667;
//Repeat # of times
for (int j = 0; j < 10 && IsAutonomous() && IsEnabled(); j++) {
//to the left to the left to the left to the left
for (int k = 0; k < 4; k++) {
robotDrive.MecanumDrive_Cartesian(-0.2, 0, 0);
Wait(tempo);
robotDrive.MecanumDrive_Cartesian(0, 0, 0);
Wait(tempo);
}
//to the right to the right to the right
for (int k = 0; k < 4; k++) {
robotDrive.MecanumDrive_Cartesian(0.2, 0, 0);
Wait(tempo);
robotDrive.MecanumDrive_Cartesian(0, 0, 0);
Wait(tempo);
}
//kick kick kick kick
for (int k = 0; k < 4; k++) {
chainLift.SetSpeed(0.3);
Wait(tempo);
chainLift.SetSpeed(-0.3);
Wait(tempo);
}
chainLift.SetSpeed(0);
//walk it by uself (turn 90)
robotDrive.MecanumDrive_Polar(0, 0, 0.3);
Wait(tempo * 8);
}
SmartDashboard::PutString("STATUS:", "GIT GUD");
}
/**
* Runs the motors with Mecanum drive.
*/
void OperatorControl()
{
robotDrive.SetSafetyEnabled(false);
while (IsOperatorControl() && IsEnabled())
{
bool reset_yaw_button_pressed = stick.GetRawButton(1);
if ( reset_yaw_button_pressed ) {
ahrs->ZeroYaw();
}
bool rotateToAngle = false;
if ( stick.GetRawButton(2)) {
turnController->SetSetpoint(0.0f);
rotateToAngle = true;
} else if ( stick.GetRawButton(3)) {
turnController->SetSetpoint(90.0f);
rotateToAngle = true;
} else if ( stick.GetRawButton(4)) {
turnController->SetSetpoint(179.9f);
rotateToAngle = true;
} else if ( stick.GetRawButton(5)) {
turnController->SetSetpoint(-90.0f);
rotateToAngle = true;
}
double currentRotationRate;
if ( rotateToAngle ) {
turnController->Enable();
currentRotationRate = rotateToAngleRate;
} else {
turnController->Disable();
currentRotationRate = stick.GetTwist();
}
try {
/* Use the joystick X axis for lateral movement, */
/* Y axis for forward movement, and the current */
/* calculated rotation rate (or joystick Z axis), */
/* depending upon whether "rotate to angle" is active. */
robotDrive.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(),
currentRotationRate ,ahrs->GetAngle());
} catch (std::exception ex ) {
std::string err_string = "Error communicating with Drive System: ";
err_string += ex.what();
DriverStation::ReportError(err_string.c_str());
}
Wait(0.005); // wait 5ms to avoid hogging CPU cycles
}
}
Wheels_Out Run(Wheels_In input)
{
Wheels_Out output;
driveSafety->Feed(); // feeding motor safety
// X movement inverted for new wheel configuration
input.liftActive = false; // temporary override
// motors run at half power unless turbo is activated
// motors will be halved again if lift is being used
float xMove = -(input.xMovement / (input.turboMode ? 1 : 2) / (input.liftActive ? 2 : 1));
float yMove = (input.yMovement / (input.turboMode ? 1 : 2) / (input.liftActive ? 2 : 1));
float rotationSpeed = input.rotation;
drive->MecanumDrive_Cartesian(xMove, yMove, rotationSpeed);//, input.gyroAngle); // no gyro anymore
return output;
}
void FunctionBot::mecDrive(float cam) //tele-op function to drive using a joystick implementing RobotDrive->MecanumDrive_Cartesian function
{
float x, y, z;
m_RawBot->getStickValues(x, y, z);
if(m_RawBot->stick->GetRawButton(1))
{
z=z/3;
x=x/2;
y=y/2;
}
drive->MecanumDrive_Cartesian(-x, -y,-z,0);
//m_RawBot->camAng(-cam);
if(m_RawBot->stick->GetRawButton(2)) //button that outputs current encoder reading on electric arm
{
m_RawBot->resetJags();
}
m_RawBot->checkJag();
//cout<<m_RawBot->m_Gyro->GetAngle()<<"\n";
}
/**
* Runs the motors with Mecanum drive.
*/
void OperatorControl()
{
robotDrive.SetSafetyEnabled(false);
while (IsOperatorControl() && IsEnabled())
{
bool motionDetected = ahrs->IsMoving();
SmartDashboard::PutBoolean("MotionDetected", motionDetected);
try {
/* Use the joystick X axis for lateral movement, */
/* Y axis for forward movement, and Z axis for rotation. */
/* Use navX MXP yaw angle to define Field-centric transform */
robotDrive.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(),
stick.GetZ(),ahrs->GetAngle());
} catch (std::exception ex ) {
std::string err_string = "Error communicating with Drive System: ";
err_string += ex.what();
DriverStation::ReportError(err_string.c_str());
}
Wait(0.005); // wait 5ms to avoid hogging CPU cycles
}
}
void AutonomousType5() { //All 3 totes with correction constant
double CORRECTION = -0.021;
SmartDashboard::PutString("STATUS:", "STARTING AUTO 5");
chainLift.SetSpeed(0.8);
while (midPoint.Get() && maxUp.Get()) {
if (!IsAutonomous() || !IsEnabled())
return;
}
chainLift.SetSpeed(0);
//Move forward
robotDrive.MecanumDrive_Cartesian(0, -0.5, CORRECTION);
Wait(1.5);
if (!IsAutonomous() || !IsEnabled())
return;
//Drop
robotDrive.MecanumDrive_Cartesian(0, 0, 0);
chainLift.SetSpeed(-0.2);
Wait(0.35);
if (!IsAutonomous() || !IsEnabled())
return;
//back a little to unhook from stack
robotDrive.MecanumDrive_Cartesian(0, 0.2, 0);
Wait(0.5);
if (!IsAutonomous() || !IsEnabled())
return;
//down to grab stack
robotDrive.MecanumDrive_Cartesian(0, 0, 0);
chainLift.SetSpeed(-0.8);
while (maxDown.Get()) {
if (!IsAutonomous() || !IsEnabled())
return;
}
//forward a little
robotDrive.MecanumDrive_Cartesian(0, -0.3, 0);
Wait(0.4);
if (!IsAutonomous() || !IsEnabled())
return;
//pick up stack
robotDrive.MecanumDrive_Cartesian(0, 0, 0);
chainLift.SetSpeed(0.8);
while (midPoint.Get() && maxUp.Get()) {
if (!IsAutonomous() || !IsEnabled())
return;
}
chainLift.SetSpeed(0);
//forward
robotDrive.MecanumDrive_Cartesian(0, -0.5, CORRECTION);
Wait(1.5);
if (!IsAutonomous() || !IsEnabled())
return;
//down
robotDrive.MecanumDrive_Cartesian(0, 0, 0);
chainLift.SetSpeed(-0.2);
Wait(0.35);
if (!IsAutonomous() || !IsEnabled())
return;
//back
robotDrive.MecanumDrive_Cartesian(0, 0.2, 0);
Wait(0.5);
if (!IsAutonomous() || !IsEnabled())
return;
//down
robotDrive.MecanumDrive_Cartesian(0, 0, 0);
chainLift.SetSpeed(-0.8);
while (maxDown.Get()) {
if (!IsAutonomous() || !IsEnabled())
return;
}
//forward
chainLift.SetSpeed(0);
robotDrive.MecanumDrive_Cartesian(0, -0.3, CORRECTION);
Wait(0.4);
if (!IsAutonomous() || !IsEnabled())
return;
//up
robotDrive.MecanumDrive_Cartesian(0, 0, 0);
chainLift.SetSpeed(0.8);
while (midPoint.Get() && maxUp.Get()) {
if (!IsAutonomous() || !IsEnabled())
return;
}
chainLift.SetSpeed(0);
//sideways to zone
robotDrive.MecanumDrive_Cartesian(0.5, 0, 0);
if (WaitF(0.5))
return;
//stop
robotDrive.MecanumDrive_Cartesian(0, 0, 0);
SmartDashboard::PutString("STATUS:", "AUTO 5 COMPLETE");
}
/**
* Runs the motors with Mecanum drive.
*/
void OperatorControl()//teleop code
{
robotDrive.SetSafetyEnabled(false);
gyro.Reset();
grabEncoder.Reset();
timer.Start();
timer.Reset();
double liftHeight = 0; //variable for lifting thread
int liftHeightBoxes = 0; //another variable for lifting thread
int liftStep = 0; //height of step in inches
int liftRamp = 0; //height of ramp in inches
double grabPower;
bool backOut;
uint8_t toSend[10];//array of bytes to send over I2C
uint8_t toReceive[10];//array of bytes to receive over I2C
uint8_t numToSend = 1;//number of bytes to send
uint8_t numToReceive = 0;//number of bytes to receive
toSend[0] = 1;//set the byte to send to 1
i2c.Transaction(toSend, 1, toReceive, 0);//send over I2C
bool isGrabbing = false;//whether or not grabbing thread is running
bool isLifting = false;//whether or not lifting thread is running
bool isBraking = false;//whether or not braking thread is running
float driveX = 0;
float driveY = 0;
float driveZ = 0;
float driveGyro = 0;
bool liftLastState = false;
bool liftState = false; //button pressed
double liftLastTime = 0;
double liftTime = 0;
bool liftRan = true;
Timer switchTimer;
Timer grabTimer;
switchTimer.Start();
grabTimer.Start();
while (IsOperatorControl() && IsEnabled())
{
// Use the joystick X axis for lateral movement, Y axis for forward movement, and Z axis for rotation.
// This sample does not use field-oriented drive, so the gyro input is set to zero.
toSend[0] = 1;
numToSend = 1;
driveX = driveStick.GetRawAxis(Constants::driveXAxis);//starts driving code
driveY = driveStick.GetRawAxis(Constants::driveYAxis);
driveZ = driveStick.GetRawAxis(Constants::driveZAxis);
driveGyro = gyro.GetAngle() + Constants::driveGyroTeleopOffset;
if (driveStick.GetRawButton(Constants::driveOneAxisButton)) {//if X is greater than Y and Z, then it will only go in the direction of X
toSend[0] = 6;
numToSend = 1;
if (fabs(driveX) > fabs(driveY) && fabs(driveX) > fabs(driveZ)) {
driveY = 0;
driveZ = 0;
}
else if (fabs(driveY) > fabs(driveX) && fabs(driveY) > fabs(driveZ)) {//if Y is greater than X and Z, then it will only go in the direction of Y
driveX = 0;
driveZ = 0;
}
else {//if Z is greater than X and Y, then it will only go in the direction of Z
driveX = 0;
driveY = 0;
}
}
if (driveStick.GetRawButton(Constants::driveXYButton)) {//Z lock; only lets X an Y function
toSend[0] = 7;
driveZ = 0;//Stops Z while Z lock is pressed
}
if (!driveStick.GetRawButton(Constants::driveFieldLockButton)) {//robot moves based on the orientation of the field
driveGyro = 0;//gyro stops while field lock is enabled
}
driveX = Constants::scaleJoysticks(driveX, Constants::driveXDeadZone, Constants::driveXMax * (.5 - (driveStick.GetRawAxis(Constants::driveThrottleAxis) / 2)), Constants::driveXDegree);
driveY = Constants::scaleJoysticks(driveY, Constants::driveYDeadZone, Constants::driveYMax * (.5 - (driveStick.GetRawAxis(Constants::driveThrottleAxis) / 2)), Constants::driveYDegree);
driveZ = Constants::scaleJoysticks(driveZ, Constants::driveZDeadZone, Constants::driveZMax * (.5 - (driveStick.GetRawAxis(Constants::driveThrottleAxis) / 2)), Constants::driveZDegree);
robotDrive.MecanumDrive_Cartesian(driveX, driveY, driveZ, driveGyro);//makes the robot drive
if (pdp.GetCurrent(Constants::grabPdpChannel) < Constants::grabManualCurrent) {
pickup.setGrabber(Constants::scaleJoysticks(grabStick.GetX(), Constants::grabDeadZone, Constants::grabMax, Constants::grabDegree)); //defines the grabber
if(grabTimer.Get() < 1) {
toSend[0] = 6;
}
}
else {
pickup.setGrabber(0);
grabTimer.Reset();
toSend[0] = 6;
}
if (Constants::grabLiftInverted) {
pickup.setLifter(-Constants::scaleJoysticks(grabStick.GetY(), Constants::liftDeadZone, Constants::liftMax, Constants::liftDegree)); //defines the lifter
}
else {
pickup.setLifter(Constants::scaleJoysticks(grabStick.GetY(), Constants::liftDeadZone, Constants::liftMax, Constants::liftDegree)); //defines the lifter
}
SmartDashboard::PutNumber("Lift Power", Constants::scaleJoysticks(grabStick.GetY(), Constants::liftDeadZone, Constants::liftMax, Constants::liftDegree));
SmartDashboard::PutBoolean("Is Lifting", isLifting);
if (Constants::scaleJoysticks(grabStick.GetY(), Constants::liftDeadZone, Constants::liftMax, Constants::liftDegree) != 0 || isLifting) { //if the robot is lifting
isBraking = false; //stop braking thread
SmartDashboard::PutBoolean("Braking", false);
}
else if(!isBraking) {
isBraking = true; //run braking thread
pickup.lifterBrake(isBraking);//brake the pickup
}
if (grabStick.GetRawButton(Constants::liftFloorButton)) {
liftHeight = 0;
pickup.lifterPosition(liftHeight, isLifting, grabStick);//start lifting thread
liftRan = true;
}
liftTime = timer.Get();
liftState = grabStick.GetRawButton(Constants::liftButton);
if (liftState) { //if button is pressed
if (!liftLastState) {
if (liftTime - liftLastTime < Constants::liftMaxTime) {
if (liftHeightBoxes < Constants::liftMaxHeightBoxes) {
liftHeightBoxes++; //adds 1 to liftHeightBoxes
}
}
else {
liftHeightBoxes = 1;
liftRamp = 0;
liftStep = 0;
}
}
liftLastTime = liftTime;
liftLastState = true;
liftRan = false;
}
else if (grabStick.GetRawButton(Constants::liftRampButton)) {
if (liftTime - liftLastTime > Constants::liftMaxTime) {
liftHeight = 0;
liftStep = 0;
}
liftRamp = 1; //prepares to go up ramp
liftLastTime = liftTime;
liftRan = false;
}
else if (grabStick.GetRawButton(Constants::liftStepButton)) {
if (liftTime - liftLastTime > Constants::liftMaxTime) {
liftHeight = 0;
liftRamp = 0;
}
liftStep = 1; //prepares robot for step
liftLastTime = liftTime;
liftRan = false;
}
else {
if (liftTime - liftLastTime > Constants::liftMaxTime && !liftRan) {
liftHeight = liftHeightBoxes * Constants::liftBoxHeight + liftRamp * Constants::liftRampHeight + liftStep * Constants::liftStepHeight; //sets liftHeight
if (liftHeightBoxes > 0) {
liftHeight -= Constants::liftBoxLip;
}
pickup.lifterPosition(liftHeight, isLifting, grabStick);//start lifting thread
liftRan = true;
}
liftLastState = false;
}
if (grabStick.GetRawButton(Constants::grabToteButton)) {//if grab button is pressed
grabPower = Constants::grabToteCurrent;
backOut = true;
if (!isGrabbing) {
pickup.grabberGrab(isGrabbing, grabPower, backOut, grabStick);//start grabber thread
}
}
else if (grabStick.GetRawButton(Constants::grabBinButton)) {//if grab button is pressed
grabPower = Constants::grabBinCurrent;
backOut = false;
if (!isGrabbing) {
pickup.grabberGrab(isGrabbing, grabPower, backOut, grabStick);//start grabber thread
}
}
else if (grabStick.GetRawButton(Constants::grabChuteButton)) {//if grab button is presset
SmartDashboard::PutBoolean("Breakpoint -2", false);
SmartDashboard::PutBoolean("Breakpoint -1", false);
SmartDashboard::PutBoolean("Breakpoint 0", false);
SmartDashboard::PutBoolean("Breakpoint 1", false);
SmartDashboard::PutBoolean("Breakpoint 2", false);
SmartDashboard::PutBoolean("Breakpoint 3", false);
SmartDashboard::PutBoolean("Breakpoint 4", false);
//Wait(.5);
if (!isGrabbing) {
//pickup.grabberChute(isGrabbing, grabStick);//start grabber thread
}
}
//determines what the LED's look like based on what the Robot is doing
if (isGrabbing) {
toSend[0] = 5;
numToSend = 1;
}
if (isLifting) {//if the grabbing thread is running
if (Constants::encoderToDistance(liftEncoder.Get(),Constants::liftEncoderTicks, Constants::liftEncoderBase, Constants::liftEncoderRadius) < liftHeight) {
toSend[0] = 3;
}
else {
toSend[0] = 4;
}
numToSend = 1;//sends 1 byte to I2C
}
if(!grabOuterLimit.Get()) { //tells if outer limit is hit with lights
if(switchTimer.Get() < 1) {
toSend[0] = 6;
}
}
else {
switchTimer.Reset();
}
if (driveStick.GetRawButton(Constants::sneakyMoveButton)) {
toSend[0] = 0;
numToSend = 1;
}
float distance = prox.GetVoltage() * Constants::ultrasonicVoltageToInches / 12; // distance from ultrasonic sensor
float rotations = (float) liftEncoder.Get(); // rotations on encoder
SmartDashboard::PutNumber("Distance", distance); // write stuff to smart dash
SmartDashboard::PutNumber("Current", pdp.GetCurrent(Constants::grabPdpChannel));
SmartDashboard::PutNumber("LED Current", pdp.GetCurrent(Constants::ledPdpChannel));
SmartDashboard::PutNumber("Lift Encoder", rotations);
SmartDashboard::PutNumber("Lift Height", liftHeight);
SmartDashboard::PutNumber("Grab Encoder", grabEncoder.Get());
SmartDashboard::PutBoolean("Grab Inner", grabInnerLimit.Get());
SmartDashboard::PutBoolean("Grab Outer", grabOuterLimit.Get());
SmartDashboard::PutNumber("Drive Front Left Current", pdp.GetCurrent(Constants::driveFrontLeftPin));
SmartDashboard::PutNumber("Drive Front Right Current", pdp.GetCurrent(Constants::driveFrontRightPin));
SmartDashboard::PutNumber("Drive Rear Left Current", pdp.GetCurrent(Constants::driveRearLeftPin));
SmartDashboard::PutNumber("Drive Rear Right Current", pdp.GetCurrent(Constants::driveRearRightPin));
SmartDashboard::PutNumber("Throttle", grabStick.GetZ());
i2c.Transaction(toSend, 1, toReceive, 0);//send and receive information from arduino over I2C
Wait(0.005); // wait 5ms to avoid hogging CPU cycles
} //end of teleop
isBraking = false;
toSend[0] = 0;
i2c.Transaction(toSend, numToSend, toReceive, numToReceive);
}
void Autonomous()
{
Timer timer;
float power = 0;
bool isLifting = false;
bool isGrabbing = false;
double liftHeight = Constants::liftBoxHeight-Constants::liftBoxLip;
double grabPower = Constants::grabAutoCurrent;
bool backOut;
uint8_t toSend[1];//array of bytes to send over I2C
uint8_t toReceive[0];//array of bytes to receive over I2C
uint8_t numToSend = 1;//number of bytes to send
uint8_t numToReceive = 0;//number of bytes to receive
toSend[0] = 2;//set the byte to send to 1
i2c.Transaction(toSend, numToSend, toReceive, numToReceive);//send over I2C
bool isSettingUp = true;
//pickup.setGrabber(-1); //open grabber all the way
pickup.setLifter(0.8);
while (isSettingUp && IsEnabled() && IsAutonomous()) {
isSettingUp = false;
/*if (grabOuterLimit.Get() == false) {
pickup.setGrabber(0); //open until limit
}
else {
isSettingUp = true;
}*/
if (liftLowerLimit.Get()) {
pickup.setLifter(0); //down till bottom
}
else {
isSettingUp = true;
}
}
gyro.Reset();
liftEncoder.Reset();
grabEncoder.Reset();
if (grabStick.GetZ() > .8) {
timer.Reset();
timer.Start();
while (timer.Get() < 1) {
robotDrive.MecanumDrive_Cartesian(0, power, 0, gyro.GetAngle()); // drive back
if(power>-.4){
power-=0.005;
Wait(.005);
}
}
robotDrive.MecanumDrive_Cartesian(0, 0, 0, gyro.GetAngle()); // STOP!!!
timer.Stop();
timer.Reset();
Wait(1);
}
power = 0;
while (isLifting && IsEnabled() && IsAutonomous()) {
Wait(.005);
}
backOut = Constants::autoBackOut;
pickup.grabberGrab(isGrabbing, grabPower, backOut, grabStick);
Wait(.005);
while (isGrabbing && IsEnabled() && IsAutonomous()) {
Wait(.005);
}
liftHeight = 3*Constants::liftBoxHeight;
Wait(.005);
pickup.lifterPosition(liftHeight, isLifting, grabStick);
Wait(.005);
while (isLifting && IsEnabled() && IsAutonomous()) {
Wait(.005);
}
while(prox.GetVoltage() * Constants::ultrasonicVoltageToInches / 12 < 2 && IsEnabled() && IsAutonomous()); // while the nearest object is closer than 2 feet
timer.Start();
while(prox.GetVoltage() * Constants::ultrasonicVoltageToInches < Constants::autoBackupDistance && timer.Get() < Constants::autoMaxDriveTime && IsEnabled() && IsAutonomous()) { // while the nearest object is further than 12 feet
if (power < .45) { //ramp up the power slowly
power += .00375;
}
robotDrive.MecanumDrive_Cartesian(0, power, 0, gyro.GetAngle()); // drive back
float distance = prox.GetVoltage() * Constants::ultrasonicVoltageToInches / 12; // distance from ultrasonic sensor
SmartDashboard::PutNumber("Distance", distance); // write stuff to smart dash
SmartDashboard::PutNumber("Drive Front Left Current", pdp.GetCurrent(Constants::driveFrontLeftPin));
SmartDashboard::PutNumber("Drive Front Right Current", pdp.GetCurrent(Constants::driveFrontRightPin));
SmartDashboard::PutNumber("Drive Rear Left Current", pdp.GetCurrent(Constants::driveRearLeftPin));
SmartDashboard::PutNumber("Drive Rear Right Current", pdp.GetCurrent(Constants::driveRearRightPin));
SmartDashboard::PutNumber("Gyro Angle", gyro.GetAngle());
SmartDashboard::PutNumber("Distance (in)", prox.GetVoltage() * Constants:: ultrasonicVoltageToInches);
Wait(.005);
}
timer.Reset();
while(timer.Get() < Constants::autoBrakeTime && IsEnabled() && IsAutonomous()) { // while the nearest object is further than 12 feet
robotDrive.MecanumDrive_Cartesian(0,Constants::autoBrakePower,0); ///Brake
}
float turn = 0;
while (fabs(turn) < 85 && IsEnabled() && IsAutonomous()) { //turn 90(ish) degrees
robotDrive.MecanumDrive_Cartesian(0,0,.1);
turn = gyro.GetAngle();
if (turn > 180) {
turn -= 360;
}
}
robotDrive.MecanumDrive_Cartesian(0,0,0); ///STOP!!!
timer.Stop();
toSend[0] = 8;
i2c.Transaction(toSend, numToSend, toReceive, numToReceive);
while(IsAutonomous() && IsEnabled());
toSend[0] = 0;
i2c.Transaction(toSend, numToSend, toReceive, numToReceive);
}
/**
* Periodic code for teleop mode should go here.
*
* Use this method for code which will be called periodically at a regular
* rate while the robot is in teleop mode.
*/
void RobotDemo::TeleopPeriodic()
{
m_robotDrive.MecanumDrive_Cartesian(m_driveStick.GetX(), m_driveStick.GetY(), m_driveStick2.GetX(), m_gyro.GetAngle());
printf("rate: %d\n", (int) m_encoder.GetRaw());
}
/*
* Sample line tracking class for FIRST 2011 Competition
* Jumpers on driver station digital I/O pins select the operating mode:
* The Driver Station digital input 1 select whether the code tracks the straight
* line or the forked line. Driver station digital input 2 selects whether the
* code takes the left or right fork. You can set these inputs using jumpers on
* the USB I/O module or in the driver station I/O Configuration pane (if there
* is no Digital I/O module installed.
*
* Since there is the fork to contend with, the code tracks the edge of the line
* using a technique similar to that used with a single-sensor Lego robot.
*
* The two places to do tuning are:
*
* defaultSteeringGain - this is the amount of turning correction applied
* forkProfile & straightProfile - these are power profiles applied at various
* times (one power setting / second of travel) as the robot moves towards
* the wall.
*/
void AutonomousPeriodic(void)
{
// // loop until either we hit the "T" at the end or 8 seconds has
// // elapsed. The time to the end should be less than 7 seconds
// // for either path.
// time = autotimer->Get();
// //if(time < 8.0 && !atCross) {
// if(!atCross){
//
// int timeInSeconds = (int) time;
// int leftValue = left->Get() ? 0 : 1; // read the line tracking sensors
// int middleValue = middle->Get() ? 0 : 1;
// int rightValue = right->Get() ? 0 : 1;
//
// // compute the single value from the 3 sensors. Notice that the bits
// // for the outside sensors are flipped depending on left or right
// // fork. Also the sign of the steering direction is different for left/right.
// if (goLeft)//on fork and go left
// {
// binaryValue = leftValue * 4 + middleValue * 2 + rightValue;
// steeringGain = -defaultSteeringGain;
// }
// else //on straight, or on fork and go right
// {
// binaryValue = rightValue * 4 + middleValue * 2 + leftValue;
// steeringGain = defaultSteeringGain;
// }
// speed=-.2;
// turn = 0; // default to no turn
//
// switch (binaryValue) {
// case 1: // just the outside sensor - drive straight
// turn = 0;
// break;
// case 7: // all sensors - maybe at the "T"
// if (time > stopTime) {
// atCross = true;
// speed = 0;
// }
// break;
// case 0: // no sensors - apply previous correction
// if (previousValue == 0 || previousValue == 1) {
// turn = steeringGain;
// }
// else {
// turn = -steeringGain;
// }
// break;
// default: // anything else, steer back to the line
// turn = -steeringGain;
// }
//
//
// // useful debugging output for tuning your power profile and steering gain
// if(binaryValue != previousValue)
// {
// printf("Time: %2.2f sensor: %d speed: %1.2f turn: %1.2f atCross: %d\n", time, binaryValue, speed, turn, atCross);
// }
//
// // move the robot forward
// float angle = mygyro->GetAngle(); // get heading
// drive->MecanumDrive_Cartesian(turn, speed, 0, angle);
//
// if (binaryValue != 0)
// previousValue = binaryValue;
//
// oldTimeInSeconds = timeInSeconds;
// }
// // stop driving when finished
// else
// drive->MecanumDrive_Cartesian(0,0,0,mygyro->GetAngle());
// Wait(.1);
drive->MecanumDrive_Cartesian(0,0,0,0);
deploy->OperateDeployment(false,true);
}
void DisabledPeriodic(void) {
drive->MecanumDrive_Cartesian(0.0f,0.0f,0.0f);
}
void TeleopPeriodic(void) {
float x = gamepad->GetLeftX();
float y = gamepad->GetLeftY();
float rot = gamepad->GetRightX();
//small gamepad values are ignored
if (x < 0.1f && x > -0.1f)
{
x = 0;
}
if (y < 0.1f && y > -0.1f)
{
y = 0;
}
if (rot < 0.1f && rot > -0.1f)
{
rot = 0;
}
drive->MecanumDrive_Cartesian(SPEED_LIMIT * x, SPEED_LIMIT * y, SPEED_LIMIT * rot);
//shoot smoke if button is pressed
if (gamepad2->GetNumberedButton(FIRE_SMOKE_BUTTON)){
//SHOOT SMOKE!
//makingSmoke = !makingSmoke;
smoke_cannon->Set(SMOKE_CANNON_SPEED);
lcd->PrintfLine(DriverStationLCD::kUser_Line5, "Shooting");
firing_smoke_timer->Start(); //measure how long we've fired smoke, so we know if it's ok to make more
}
else
{
smoke_cannon->Set(0.0f);
lcd->PrintfLine(DriverStationLCD::kUser_Line5, "Not shooting");
firing_smoke_timer->Stop(); //stop the timer, since we're not firing smoke.
//don't reset, cuz we need to how much smoke we've fired.
}
//Eye Code
// float eye_pos = gamepad2->GetLeftX();
//
// right_eye_x->Set((eye_pos * 60) + default_eye_position);
// left_eye_x->Set((eye_pos * 60) + default_eye_position - LEFT_EYE_OFFSET);
//
// //button lock code
// if(gamepad2->GetNumberedButtonPressed(EYE_LOCK_BUTTON)){
// default_eye_position = eye_pos;
// }
//
//left eye control
//If A isn't pressed the value should stay the same as before
if (!gamepad2->GetNumberedButton(1)){
float left_joystick_x = gamepad2->GetLeftX();
float left_eye_x_axis = (1 - left_joystick_x)*60;
left_eye_val = left_eye_x_axis + 50;
float right_joystick_x = gamepad2->GetRawAxis(4);//right x axis
float right_eye_x_axis = (1-right_joystick_x)*60;
right_eye_val = right_eye_x_axis+20;
}
left_eye_x->SetAngle(left_eye_val);
right_eye_x->SetAngle(right_eye_val);
//float right_joystick_y = gamepad2->GetRawAxis(4);
//float right_eye_y_axis = (right_joystick_y+1)*60;
//right_eye_y->SetAngle(right_eye_y_axis);
/*
bool rbutton = gamepad2->GetNumberedButton(HEAD_UP_BUTTON);
bool lbutton = gamepad2->Ge tNumberedButton(HEAD_DOWN_BUTTON);
if (rbutton){
lcd->PrintfLine(DriverStationLCD::kUser_Line6, "rb pressed");
jaw_motor->Set(0.2f);
}else if(lbutton){
lcd->PrintfLine(DriverStationLCD::kUser_Line6, "lb pressed");
jaw_motor->Set(-0.15f);
}else{
lcd->PrintfLine(DriverStationLCD::kUser_Line6, "no buttons");
jaw_motor->Set(0.0f);
}
*/
//REAL head & jaw code
//move head down
if(gamepad2->GetRightX()<=-0.5f && can_move_head_down() && can_move_jaw_down()){
head_motor->Set(-0.3f);
jaw_motor->Set(0.3f);
}
//move head up
else if(gamepad2->GetNumberedButton(HEAD_UP_BUTTON) && can_move_head_up()){
head_motor->Set(0.3f);
jaw_motor->Set(-0.3f);
}
//move jaw down
else if(gamepad2->GetRightX()>=0.5f && can_move_jaw_down()){
jaw_motor->Set(0.3f);
}
//move jaw up
else if(gamepad2->GetNumberedButton(JAW_UP_BUTTON) && can_move_jaw_up()){
jaw_motor->Set(-0.3f);
}
//sets to zero if no buttons pressed
else {
jaw_motor->Set(0.0f);
head_motor->Set(0.0f);
}
lcd->PrintfLine(DriverStationLCD::kUser_Line6, "b:%d t:%d c:%d", bottomjaw_limit->Get(), tophead_limit->Get(), crash_limit->Get());
//Smoke code
if (gamepad2->GetNumberedButton(MAKE_SMOKE_BUTTON)){
//MAKE SMOKE!!!!!!!!!!!
//only if we don't have too much excess smoke
if (making_smoke_timer->Get() - firing_smoke_timer->Get() < MAX_EXCESS_SMOKE_TIME){
lcd->PrintfLine(DriverStationLCD::kUser_Line4, "smoke");
smoke_machine->Set(true);
} else {
lcd->PrintfLine(DriverStationLCD::kUser_Line4, "too much smoke");
smoke_machine->Set(false);
}
making_smoke_timer->Start(); //measure how long we've been making smoke, so we don't overflow the machine
//doesn't do anything if we've already started the timer
}
else
{
lcd->PrintfLine(DriverStationLCD::kUser_Line4, "not smoke");
smoke_machine->Set(false);
making_smoke_timer->Stop(); //stop the timer, since we're not making smoke
//don't reset it, cuz we need to know how much smoke we've made
}
//if both timers are the same, we can set them both to zero to ensure we don't overflow them or something
if (making_smoke_timer->Get() == firing_smoke_timer->Get()){
making_smoke_timer->Reset();
firing_smoke_timer->Reset();
}
lcd->PrintfLine(DriverStationLCD::kUser_Line1, "x:%f", x);
lcd->PrintfLine(DriverStationLCD::kUser_Line2, "y:%f", y);
lcd->PrintfLine(DriverStationLCD::kUser_Line3, "r:%f", rot);
lcd->UpdateLCD();
}
void TeleopPeriodic() {
if(tick==10) if (ds->IsSysBrownedOut()) {
ds->ReportError("[ERROR] BROWNOUT DETECTED!!");
}
if(tick == 15) if (!ds->IsNewControlData()) {
ds->ReportError(
"[ERROR] NO DATA FROM DRIVER STATION IN THIS TICK!");
}
if(tick==20) if (!ds->IsDSAttached()) {
ds->ReportError("[ERROR] DRIVER STATION NOT DETECTED!");
}
if (stick.GetRawButton(10))
zeroSanics();
if (stick.GetRawButton(8)) {
leftIRZero = 0;
rightIRZero = 0;
}
tick++;
if (liftStick.GetRawButton(2)) {
double canScale = liftStick.GetRawAxis(2);
canScale += 1;
canScale = 2 - canScale;
canScale /= 2;
canGrabber.SetSpeed(canScale);
} else if (liftStick.GetRawButton(3)) {
double canScale = liftStick.GetRawAxis(2);
canScale += 1;
canScale = 2 - canScale;
canScale /= 2;
canGrabber.SetSpeed(-canScale);
} else
canGrabber.SetSpeed(0);
double speed;
//Calculate scalar to use for POV/Adjusted drive
double scale = stick.GetRawAxis(3);
scale += 1;
scale = 2 - scale;
scale /= 2;
//Use pov/hat switch for movement if enabled
if (stick.GetRawButton(1) && stick.GetRawButton(2)) {
AutomaticLineup();
} else if (stick.GetRawButton(1)) {
double leftVolts = leftIR.GetAverageVoltage() - leftIRZero;
double rightVolts = rightIR.GetAverageVoltage() - leftIRZero;
if (rightVolts + VOLTAGE_TOLERANCE > leftVolts
&& rightVolts - VOLTAGE_TOLERANCE < leftVolts) {
robotDrive.MecanumDrive_Cartesian(0, 0, 0);
} else if (rightVolts > leftVolts)
robotDrive.MecanumDrive_Cartesian(0, 0, 0.2);
else if (leftVolts > rightVolts)
robotDrive.MecanumDrive_Cartesian(0, 0, -0.2);
} else if (stick.GetRawButton(6)) {
//Rotate
robotDrive.MecanumDrive_Polar(0, 0, scale);
} else if (stick.GetRawButton(5)) {
//Rotate
robotDrive.MecanumDrive_Polar(0, 0, -scale);
} else if (stick.GetPOV(0) != -1) {
//If POV moved, move polar (getPOV returns an angle in degrees)
robotDrive.MecanumDrive_Polar(scale, -stick.GetPOV(0), 0);
} else if (stick.GetRawButton(2)) {
//Drive with scalar
robotDrive.MecanumDrive_Cartesian(-stick.GetRawAxis(0) * scale,
stick.GetRawAxis(1) * scale, stick.GetRawAxis(2) * scale);
} else {
//Drive normally
robotDrive.MecanumDrive_Cartesian(-stick.GetX(), stick.GetY(),
stick.GetZ());
}
speed = -liftStick.GetY();
//bool canGoUp = maxUp.Get();
bool canGoUp = true;
//bool canGoDown = maxDown.Get();
bool canGoDown = true;
//If at a limit switch and moving in that direction, stop
if (speed > 0 && !canGoUp)
speed = 0;
if (speed < 0 && !canGoDown)
speed = 0;
chainLift.SetSpeed(speed);
if (tick >50) {
if (SmartDashboard::GetBoolean("Smart Dashboard Enabled")) {
//Smart Dash outputs
//SmartDashboard::PutNumber("X Acceleration: ", accel.GetX());
//SmartDashboard::PutNumber("Y Acceleration: ", accel.GetY());
//SmartDashboard::PutNumber("Z Acceleration: ", accel.GetZ());
SmartDashboard::PutBoolean("Switch 1: (up)", maxUp.Get());
SmartDashboard::PutBoolean("Switch 2: (down)", maxDown.Get());
SmartDashboard::PutBoolean("Switch 3: (mid)", midPoint.Get());
SmartDashboard::PutBoolean("Auto switch A: ",
autoSwitch1.Get());
SmartDashboard::PutBoolean("Auto switch B: ",
autoSwitch2.Get());
//SmartDashboard::PutBoolean("RobotDrive Alive?",
// robotDrive.IsAlive());
//SmartDashboard::PutBoolean("ChainLift Alive?",
// robotDrive.IsAlive());
SmartDashboard::PutNumber("Left Sensor",
leftIR.GetAverageVoltage());
SmartDashboard::PutNumber("Right Sensor",
rightIR.GetAverageVoltage());
SmartDashboard::PutNumber("Left w zero",
leftIR.GetAverageVoltage() - leftIRZero);
SmartDashboard::PutNumber("Rigt w zero",
rightIR.GetAverageVoltage() - rightIRZero);
SmartDashboard::PutNumber("PDP 14 Current", pdp.GetCurrent(14));
SmartDashboard::PutNumber("PDP 15 Current", pdp.GetCurrent(15));
}
tick = 0;
}
}
void AutonomousType6() { //All 3 totes with accelerometer
//Pick up 3 bins using gyroscope to correct corse
Wait(1);
SmartDashboard::PutString("STATUS:", "AUTO 6 (ACCEL)");
//Move forward
//robotDrive.MecanumDrive_Cartesian(0, -0.5, 0);
//Wait(0.1);
if (!IsAutonomous() || !IsEnabled())
return;
//Lift up first box
//robotDrive.MecanumDrive_Cartesian(0, 0, 0);
chainLift.SetSpeed(0.8);
while (midPoint.Get() && maxUp.Get()) {
if (!IsAutonomous() || !IsEnabled())
return;
}
chainLift.SetSpeed(0);
for (int j = 0; j < 1400; j++) {
if (!IsAutonomous() || !IsEnabled())
return;
//Move forward
//CORRECT WITH ACCEL
robotDrive.MecanumDrive_Cartesian(-accel.GetX(), 0, 0);
Wait(0.001);
}
//Drop
robotDrive.MecanumDrive_Cartesian(0, 0, 0);
chainLift.SetSpeed(-0.3);
Wait(1);
if (!IsAutonomous() || !IsEnabled())
return;
//back a little to unhook from stack
robotDrive.MecanumDrive_Cartesian(0, 0.5, 0);
Wait(0.3);
if (!IsAutonomous() || !IsEnabled())
return;
//down to grab stack
robotDrive.MecanumDrive_Cartesian(0, 0, 0);
chainLift.SetSpeed(-0.8);
while (maxDown.Get()) {
}
if (!IsAutonomous() || !IsEnabled())
return;
//forward a little
robotDrive.MecanumDrive_Cartesian(0, -0.4, 0);
Wait(1.2);
if (!IsAutonomous() || !IsEnabled())
return;
//pick up stack
robotDrive.MecanumDrive_Cartesian(0, 0, 0);
chainLift.SetSpeed(0.8);
while (midPoint.Get() && maxUp.Get()) {
if (!IsAutonomous() || !IsEnabled())
return;
}
chainLift.SetSpeed(0);
for (int j = 0; j < 1500; j++) {
if (!IsAutonomous() || !IsEnabled())
return;
//Move forward
robotDrive.MecanumDrive_Cartesian(-accel.GetX(), 0, 0);
Wait(0.001);
}
//down
robotDrive.MecanumDrive_Cartesian(0, 0, 0);
chainLift.SetSpeed(-0.3);
Wait(0.5);
if (!IsAutonomous() || !IsEnabled())
return;
//back
robotDrive.MecanumDrive_Cartesian(0, 0.7, 0);
Wait(0.5);
if (!IsAutonomous() || !IsEnabled())
return;
//down
robotDrive.MecanumDrive_Cartesian(0, 0, 0);
chainLift.SetSpeed(-0.8);
while (maxDown.Get()) {
if (!IsAutonomous() || !IsEnabled())
return;
}
//forward
chainLift.SetSpeed(0);
robotDrive.MecanumDrive_Cartesian(0, -0.3, 0);
Wait(2);
if (!IsAutonomous() || !IsEnabled())
return;
//up
robotDrive.MecanumDrive_Cartesian(0, 0, 0);
chainLift.SetSpeed(0.8);
while (midPoint.Get() && maxUp.Get()) {
if (!IsAutonomous() || !IsEnabled())
return;
}
chainLift.SetSpeed(0);
//turn 90 deg
robotDrive.MecanumDrive_Polar(0, 0, -0.3);
Wait(4);
robotDrive.MecanumDrive_Polar(0.5, 0, 0);
Wait(2.5);
robotDrive.MecanumDrive_Polar(0, 0, 0);
chainLift.SetSpeed(-0.4);
while (maxDown.Get() && IsAutonomous()) {
}
chainLift.SetSpeed(0);
SmartDashboard::PutString("STATUS:", "AUTO 6 COMPLETE");
}
void Disabled() {
robotDrive.MecanumDrive_Cartesian(0, 0, 0);
chainLift.SetSpeed(0);
SmartDashboard::PutString("STATUS:", "DISABLED");
}