SlicedBaseChunkReader::SlicedBaseChunkReader(
const Metadata& m,
PointPool& pool,
const arbiter::Endpoint& endpoint)
: BaseChunkReader(m, pool)
{
const Structure& s(m.structure());
Pool t(s.baseDepthEnd() - s.baseDepthBegin());
std::mutex mutex;
for (std::size_t d(s.baseDepthBegin()); d < s.baseDepthEnd(); ++d)
{
t.add([this, &mutex, &m, &endpoint, d]()
{
const auto id(ChunkInfo::calcLevelIndex(2, d));
auto cells(m.storage().deserialize(endpoint, m_pool, id));
Climber climber(m);
std::lock_guard<std::mutex> lock(mutex);
for (const auto& cell : cells)
{
climber.reset();
climber.magnifyTo(cell.point(), d);
m_points.at(normalize(climber.index())).emplace_back(
cell.point(),
cell.uniqueData());
}
m_cells.push(std::move(cells));
});
}
t.join();
}
void MyTeleop::OperatorControl(void)
{
bool shootEnabled = false;
float magnitude = 0, direction = 0, rotation = 0;
MyDiscShooter shooter(myRobot);
MyClimber climber(myRobot);
JoystickButton leftTopButton(&myRobot->leftStick, Joystick::kDefaultTopButton);
JoystickButton leftTrigger(&myRobot->leftStick, Joystick::kDefaultTriggerButton);
//MyDiscShooterCmd *discShooterCmd;
LCD::ConsoleLog("MyTeleop.OperatorControl");
LCD::PrintLine(1, "Mode: OperatorControl");
LCD::PrintLine(4, "All: %d, Start=%d", myRobot->alliance, myRobot->startLocation);
// Sets command based disc shooter class to run when button pressed, as separate
// task from this one in the Command execution scheme. This depends on the command
// scheduler running and that is done below in the While loop.
// button class whenpressed requires a pointer to the command class instance we
// want to run so we use new to create the instance and return a pointer to
// that instance.
//discShooterCmd = new MyDiscShooterCmd(myRobot);
//leftTrigger.WhenPressed(discShooterCmd);
myRobot->robotDrive.SetSafetyEnabled(true);
// Driving loop runs until teleop is over or climbing phase started.
while (myRobot->IsEnabled() && myRobot->IsOperatorControl())
{
// Scheduler required to cause any 'commands' to run. If we don't use any commands,
// comment out following stmt.
//Scheduler::GetInstance()->Run();
// set driving parameters.
magnitude = myRobot->rightStick.GetMagnitude();
direction = myRobot->rightStick.GetDirectionDegrees();
rotation = myRobot->rotateStick.GetX();
LCD::PrintLine(3, "m=%f d=%f r=%f", magnitude, direction, rotation);
LCD::PrintLine(4, "r=%f", rotation);
myRobot->robotDrive.MecanumDrive_Polar(magnitude, direction, rotation);
// This logic shoots disk on trigger release.
if (myRobot->leftStick.GetTrigger(myRobot->leftStick.kLeftHand))
shootEnabled = true;
else if (shootEnabled)
{
shootEnabled = false;
shooter.Shoot();
}
// turn on/off climber Angular Adjustment (window) motor.
if (myRobot->leftStick.GetRawButton(JSB_TOP_LEFT))
climber.AngularAdjustmentMotorOn(true);
else
climber.AngularAdjustmentMotorOn(false);
// The design assumes operation of the climbing winches would be
// manually by JS buttons.
if (myRobot->leftStick.GetRawButton(JSB_LEFT_FRONT))
climber.Winch2Up();
else if (myRobot->leftStick.GetRawButton(JSB_LEFT_REAR))
climber.Winch2Down();
else
climber.Winch2Stop();
// control shooter ramp deployment.
if (myRobot->leftStick.GetRawButton(JSB_TOP_MIDDLE))
shooter.RampUp();
else if (myRobot->leftStick.GetRawButton(JSB_TOP_BACK))
shooter.RampDown();
else
shooter.RampStop();
// set shooter throttle value each loop.
shooter.SetThrottle(myRobot->leftStick.GetThrottle());
shooter.ShooterUpDown(myRobot->leftStick.GetY());
// Starts climbing process, drops out of driving loop when climb done.
// if (myRobot->rightStick.GetButton(myRobot->rightStick.kTopButton))
// {
// climber.Climb();
// break; // ends the While loop
// }
Wait(0.020); // wait for a motor update time
}
// wait for teleop period to end.
LCD::PrintLine(1, "Mode: End Wait");
LCD::ConsoleLog("MyTeleop.OperatorControl-endwait");
SmartDashboard::PutBoolean("End Wait", true);
myRobot->robotDrive.SetSafetyEnabled(false);
while (myRobot->IsEnabled() && myRobot->IsOperatorControl()) Wait(0.020);
// delete pointer to MyDiscShooterCmd object we created earlier.
// this releases the object instance.
//delete discShooterCmd;
LCD::ConsoleLog("MyTeleop.OperatorControl-end");
}
void MyTeleop::OperatorControl(void)
{
int encoderRaw = 0, encoderValue = 0;
bool checkBox1 = false, shootEnabled = false, arcadeDrive = false, arcadeEnabled = false;
MyDiscShooter shooter(myRobot);
MyClimber climber(myRobot);
JoystickButton topButton(&myRobot->stick, Joystick::kDefaultTopButton);
Encoder driveLeftEncoder(1, 2, false, Encoder::k4X);
int encoderDirection;
MyDiscShooterCmd *discShooterCmd;
LCD::ConsoleLog("MyTeleop.OperatorControl");
LCD::PrintLine(1, "Mode: OperatorControl");
LCD::PrintLine(4, "All: %d, Start=%d", myRobot->alliance, myRobot->startLocation);
// Sets command based disc shooter class to run when button pressed, as separate
// task from this one in the Command execution scheme. This depends on the command
// scheduler running and that is done below in the While loop.
// button class whenpressed requires a pointer to the command class instance we
// want to run so we use new to create the instance and return a pointer to
// that instance.
discShooterCmd = new MyDiscShooterCmd(myRobot);
topButton.WhenPressed(discShooterCmd);
//topButton.WhenPressed(new MyDiscShooterCmd(myRobot));
myRobot->robotDrive.SetSafetyEnabled(true);
driveLeftEncoder.Start();
// Driving loop runs until teleop is over or climbing phase started.
while (myRobot->IsEnabled() && myRobot->IsOperatorControl())
{
// Scheduler required to cause any 'commands' to run. If we don't use any commands,
// comment out following stmt.
Scheduler::GetInstance()->Run();
// the following code toggles the arcade drive flag on or off each time the right
// trigger is released.
if (myRobot->stick2.GetTrigger(myRobot->stick.kRightHand))
arcadeEnabled = true;
else if (arcadeEnabled)
//if (!myRobot->stick2.GetTrigger(myRobot->stick.kRightHand) && arcadeEnabled)
{
arcadeEnabled = false;
if (arcadeDrive)
arcadeDrive = false;
else
arcadeDrive = true;
SmartDashboard::PutBoolean("Arcade Drive", arcadeDrive);
}
if (arcadeDrive)
myRobot->robotDrive.ArcadeDrive(myRobot->stick2); // drive with arcade style (use right stick with trigger)
else
myRobot->robotDrive.TankDrive(myRobot->stick, myRobot->stick2); // drive tank style with both sticks
//LCD::ConsoleLog("left trigger=%d", myRobot->stick.GetTrigger(myRobot->stick.kLeftHand));
// This logic shoots disk on trigger release.
if (myRobot->stick.GetTrigger(myRobot->stick.kLeftHand))
shootEnabled = true;
else if (shootEnabled)
{
shootEnabled = false;
shooter.Shoot();
}
// Starts climbing process, drops out of driving loop when climb done.
if (myRobot->stick2.GetButton(myRobot->stick2.kTopButton))
{
climber.Climb();
break; // ends the While loop
}
Wait(0.020); // wait for a motor update time
// read encoder example.
encoderValue = driveLeftEncoder.Get();
encoderRaw = driveLeftEncoder.GetRaw();
if (driveLeftEncoder.GetDirection())
encoderDirection = 1; // forward
else
encoderDirection = 0; // backward
// example display info to dashboard lcd.
LCD::PrintLine(2, "Encoder Value: %d %d", encoderValue, encoderDirection);
LCD::PrintLine(3, "Encoder Raw: %d", encoderRaw);
// example on how to read a value from the dashboard.
checkBox1 = SmartDashboard::GetBoolean("Checkbox 1");
LCD::PrintLine(5, "Checkbox1 Value=%d", checkBox1);
}
// wait for teleop period to end.
LCD::PrintLine(1, "Mode: End Wait");
LCD::ConsoleLog("MyTeleop.OperatorControl-endwait");
SmartDashboard::PutBoolean("End Wait", true);
myRobot->robotDrive.SetSafetyEnabled(false);
while (myRobot->IsEnabled() && myRobot->IsOperatorControl()) Wait(0.020);
// delete pointer to MyDiscShooterCmd object we created earlier.
// this releases the object instance.
delete discShooterCmd;
LCD::ConsoleLog("MyTeleop.OperatorControl-end");
}
