inline void grabberPositionTaskFunc(uint32_t joystickPtr, uint32_t grabTalonPtr, uint32_t grabInnerLimitPtr, uint32_t pdpPtr, uint32_t backOutPtr, uint32_t grabPowerPtr, uint32_t isGrabbingPtr...) {//uint is a pointer and not an integer
Joystick *joystick = (Joystick *) joystickPtr;//initializes objects from pointers
Talon *grabTalon = (Talon *) grabTalonPtr;
Switch *grabInnerLimit = (Switch *) grabInnerLimitPtr;
PowerDistributionPanel *pdp = (PowerDistributionPanel *) pdpPtr;
bool *isGrabbing = (bool *) isGrabbingPtr;
bool *backOut = (bool *) backOutPtr;
double *grabPower = (double *) grabPowerPtr;
Timer timer;
timer.Start();
*isGrabbing = true;//tells robot.cpp that thread is running
while (grabInnerLimit->Get() && timer.Get() < Constants::grabDelay) {//starts to spin motor to pass startup current
grabTalon->Set(1);//move in
}
while (pdp->GetCurrent(Constants::grabPdpChannel) < *grabPower && grabInnerLimit->Get() && joystick->GetRawButton(Constants::pickupCancelButton) == false) {//while it hasn't reached the current cutoff, hit a limit switch, or been cancelled
grabTalon->Set(1);
SmartDashboard::PutNumber("Current",pdp->GetCurrent(Constants::grabPdpChannel));//displays current on SmartDashboard
}
if (*backOut) {
grabTalon->Set(0);//stop moving
timer.Reset();
while (timer.Get() < Constants::liftBackoutTime && joystick->GetRawButton(Constants::pickupCancelButton) == false) {
grabTalon->Set(-.75);
}
}
grabTalon->Set(0);//stop moving
timer.Stop();
*isGrabbing = false;//tells that thread is over
}
void UpdateDashboard() {
float r = 0.00001 * i;
SmartDashboard::PutNumber("State", currentState + r);
SmartDashboard::PutNumber("PID Turn Error",
turnController->GetError() + r);
SmartDashboard::PutNumber("PID Target",
turnController->GetSetpoint() + r);
// SmartDashboard::PutBoolean("Straight", straight);
SmartDashboard::PutData("test", turnController);
SmartDashboard::PutNumber("Yaw:", ahrs->GetYaw() + r);
SmartDashboard::PutNumber("Roll:", ahrs->GetRoll() + r);
SmartDashboard::PutNumber("Pitch", ahrs->GetPitch() + r);
SmartDashboard::PutNumber("Scissor 1", pdp->GetCurrent(1) + r);
SmartDashboard::PutNumber("Scissor 2", pdp->GetCurrent(2) + r);
SmartDashboard::PutNumber("Left Drive 1", pdp->GetCurrent(12) + r);
SmartDashboard::PutNumber("Left Drive 2", pdp->GetCurrent(13) + r);
SmartDashboard::PutNumber("Right Drive 1", pdp->GetCurrent(14) + r);
SmartDashboard::PutNumber("Right Drive 2", pdp->GetCurrent(15) + r);
SmartDashboard::PutNumber("Constant Lift", constantLift);
SmartDashboard::PutNumber("Rotate Rate", rotateRate + r);
i = (i + 1) % 2;
printf("2.1");
// .PutLong("test1.2", 1337);
printf("2.2");
// mqServer.PutDouble("test",DriverStation::GetInstance().GetMatchTime());
printf("2.3");
// mqServer.PutString("test1.1","YOLO_SWAGINS");
printf("2.4");
// SmartDashboard::PutString("test1.2", mqServer.GetString("test1.1"));
// SmartDashboard::PutNumber("test1", mqServer.GetDouble("test"));
// SmartDashboard::PutNumber("test1.3", mqServer.GetLong("test1.2"));
// SmartDashboard::PutNumber("test2", DriverStation::GetInstance().GetMatchTime());
}
/**
* Retrieve information from the PDP over CAN and
* displays it on the SmartDashboard interface.
* SmartDashboard::PutNumber takes a string (for a label) and a double;
* GetCurrent takes a channel number and returns a double for current,
* in Amperes. Channel numbers are printed on the PDP and range from 0-15.
*/
void OperatorControl()
{
while (IsOperatorControl() && IsEnabled())
{
// Get the current going through channel 7, in Amperes.
// The PDP returns the current in increments of 0.125A.
// At low currents the current readings tend to be less accurate.
SmartDashboard::PutNumber("Current Channel 7", m_pdp.GetCurrent(7));
// Get the voltage going into the PDP, in Volts.
// The PDP returns the voltage in increments of 0.05 Volts.
SmartDashboard::PutNumber("Voltage", m_pdp.GetVoltage());
// Retrieves the temperature of the PDP, in degrees Celsius.
SmartDashboard::PutNumber("Temperature", m_pdp.GetTemperature());
Wait(kUpdatePeriod);
}
}
void TeleopPeriodic()
{
drive_mode_t new_mode = drive_mode_chooser.GetSelected();
SmartDashboard::PutString("current mode", new_mode == TANK_DRIVE ? "Tank" : "Arcade");
if (new_mode != drive_mode)
SetDriveMode(new_mode);
if (drive_mode == TANK_DRIVE) {
left_speed = accel(left_speed, pilot->LeftY(), TICKS_TO_ACCEL);
right_speed = accel(right_speed, pilot->RightY(), TICKS_TO_ACCEL);
drive->TankDrive(left_speed, right_speed);
}
else {
rot_speed = accel(rot_speed, pilot->RightX(), TICKS_TO_ACCEL);
SmartDashboard::PutNumber("rotation speed", rot_speed);
rot_speed = pilot->RightX();
move_speed = accel(move_speed, pilot->LeftY(), TICKS_TO_ACCEL);
drive->ArcadeDrive(move_speed * MOVE_SPEED_LIMIT, -rot_speed * MOVE_SPEED_LIMIT, false);
}
SmartDashboard::PutBoolean("clamp open", clamp->isOpen());
SmartDashboard::PutBoolean("sword in", clamp->isSwordIn());
SmartDashboard::PutData("gyro", gyro);
// for (uint8 i = 0; i <= 15; ++i)
// SmartDashboard::PutNumber(std::string("current #") + std::to_string(i), pdp->GetCurrent(i));
SmartDashboard::PutNumber("Current", pdp->GetTotalCurrent());
if (pilot->ButtonState(GamepadF310::BUTTON_A)) {
clamp->open();
}
else if (pilot->ButtonState(GamepadF310::BUTTON_B)){
clamp->close();
}
clamp->update();
SmartDashboard::PutNumber("accelerometer Z", acceler->GetZ());
SmartDashboard::PutNumber("Encoder", encoder->Get());
flywheel->Set(pilot->RightTrigger());
if (pilot->LeftTrigger() != 0)
flywheel->Set(-pilot->LeftTrigger());
SmartDashboard::PutNumber("Left Trigger:", pilot->LeftTrigger());
if (pilot->ButtonState(GamepadF310::BUTTON_X)) {
cameraFeeds-> changeCam(cameraFeeds->kBtCamFront);
}
if (pilot->ButtonState(GamepadF310::BUTTON_Y)){
cameraFeeds-> changeCam(cameraFeeds->kBtCamBack);
}
cameraFeeds->run();
}
inline void grabberChuteTaskFunc(uint32_t joystickPtr, uint32_t grabTalonPtr, uint32_t grabOuterLimitPtr, uint32_t grabInnerLimitPtr, uint32_t pdpPtr, uint32_t isGrabbingPtr...) {//uint is a pointer and not an integer
SmartDashboard::PutBoolean("Breakpoint 0", true);
Wait(.5);
Joystick *joystick = (Joystick *) joystickPtr;//initializes objects from pointers
Talon *grabTalon = (Talon *) grabTalonPtr;
Switch *grabInnerLimit = (Switch *) grabInnerLimitPtr;
Switch *grabOuterLimit = (Switch *) grabOuterLimitPtr;
PowerDistributionPanel *pdp = (PowerDistributionPanel *) pdpPtr;
bool *isGrabbing = (bool *) isGrabbingPtr;
Timer timer;
timer.Start();
SmartDashboard::PutBoolean("Breakpoint 1", true);
Wait(.5);
*isGrabbing = true;//tells robot.cpp that thread is running
SmartDashboard::PutBoolean("Breakpoint 2", true);
Wait(.5);
while (grabOuterLimit->Get() && joystick->GetRawButton(Constants::pickupCancelButton) == false) {//starts to spin motor to pass startup current
//grabTalon->Set(1);//move in
}
SmartDashboard::PutBoolean("Breakpoint 3", true);
Wait(.5);
timer.Reset();
while (timer.Get() < Constants::grabChuteTime && grabInnerLimit->Get() && joystick->GetRawButton(Constants::pickupCancelButton) == false) {//while it hasn't reached the current cutoff, hit a limit switch, or been cancelled
//grabTalon->Set(1);
SmartDashboard::PutNumber("Current",pdp->GetCurrent(Constants::grabPdpChannel));//displays current on SmartDashboard
Wait(.5);
}
SmartDashboard::PutBoolean("Breakpoint 4", true);
Wait(.5);
grabTalon->Set(0);//stop moving
timer.Stop();
*isGrabbing = false;//tells that thread is over
}
inline void lifterPositionTaskFunc(uint32_t joystickPtr, uint32_t liftTalonPtr, uint32_t liftEncoderPtr, uint32_t liftUpperLimitPtr, uint32_t liftLowerLimitPtr, uint32_t pdpPtr, uint32_t heightPtr, uint32_t isLiftingPtr ...) {//uint is a pointer and not an integer
double *height = (double *) heightPtr;//initializes double
Joystick *joystick = (Joystick *) joystickPtr;
Talon *liftTalon = (Talon *) liftTalonPtr;
Encoder *liftEncoder = (Encoder *) liftEncoderPtr;
Switch *liftLowerLimit = (Switch *) liftLowerLimitPtr;
Switch *liftUpperLimit = (Switch *) liftUpperLimitPtr;
PowerDistributionPanel *pdp = (PowerDistributionPanel *) pdpPtr;
bool *isLifting = (bool *) isLiftingPtr;
*isLifting = true;//tells robot.cpp that thread is running
if (Constants::encoderToDistance(liftEncoder->Get(),Constants::liftEncoderTicks, Constants::liftEncoderBase, Constants::liftEncoderRadius) > *height) {//checks to see if encoder is higher than it's supposed to be
if (liftLowerLimit->Get() == false) {//starts to spin motor to pass startup current
liftTalon->Set(1);//move down
Wait(Constants::liftDelay);
}
while (Constants::encoderToDistance(liftEncoder->Get(),Constants::liftEncoderTicks, Constants::liftEncoderBase, Constants::liftEncoderRadius) > *height && pdp->GetCurrent(Constants::liftPdpChannel) < Constants::liftCurrent && liftLowerLimit->Get() == false && joystick->GetRawButton(Constants::pickupCancelButton) == false) {//while it is too high and hasn't hit a limit switch or been cancelled
SmartDashboard::PutNumber("Pretend Encoder",liftEncoder->Get());//displays number of ticks of encoder in SmartDashboard
liftTalon->Set(.7);//move down
}
}
else {
if (liftUpperLimit->Get() == false) {//starts to spin motor to pass startup current
liftTalon->Set(-1);//move up
Wait(Constants::liftDelay);
}
while (Constants::encoderToDistance(liftEncoder->Get(),Constants::liftEncoderTicks, Constants::liftEncoderBase, Constants::liftEncoderRadius) < *height && pdp->GetCurrent(Constants::liftPdpChannel) < Constants::liftCurrent && liftUpperLimit->Get() == false && joystick->GetRawButton(Constants::pickupCancelButton) == false) {//while it is too low and hasn't hit a limit switch or been cancelled
SmartDashboard::PutNumber("Pretend Encoder",liftEncoder->Get());//displays number of ticks of encoder on SmartDashboard
liftTalon->Set(-1);//move up
}
}
liftTalon->Set(0);//stop
*isLifting = false;//tells robot.cpp that thread is finished
}
/**
* 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);
}
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;
}
}
