void TestRamLock()
{
if (fabs(m_driver->GetRawAxis(TRIGGERS)) < .2)
m_ramServo->SetAngle(0);
else if (fabs(m_driver->GetRawAxis(TRIGGERS)) > .2)
m_ramServo->SetAngle(90);
}
void updateShooter(){
SmartDashboard::PutBoolean("stateDisarmed", stateDisarmed);
SmartDashboard::PutBoolean("stateArming1", stateArming1);
SmartDashboard::PutBoolean("stateArming2", stateArming2);
SmartDashboard::PutBoolean("stateArmed", stateArmed);
SmartDashboard::PutBoolean("stateFiring1", stateFiring1);
float winchLocked = SmartDashboard::GetNumber("winchLocked",-4500.00);
// float winchUnlocked = SmartDashboard::GetNumber("winchUnlocked",-3000.00);
float winchMidway = SmartDashboard::GetNumber("winchMidway",-0.00);
// float winchServoTrigger = SmartDashboard::GetNumber("winchServoTrigger",-0.00);
// float winchOverUnlocked = SmartDashboard::GetNumber("winchOverUnlocked",0.00);
float servoLocked = SmartDashboard::GetNumber("servoLocked",5.00);
float servoUnlocked = SmartDashboard::GetNumber("servoUnlocked",175.00);
float motorStopped = 0.00;
float motorWinding = SmartDashboard::GetNumber("motorWinding",-.70);
float motorUnwinding = SmartDashboard::GetNumber("motorUnwinding",.50);
// float motorFastUnwinding = SmartDashboard::GetNumber("motorFastUnwinding",.80);
if (stateDisarmed == true){
// t_motor.Set(0);
}
if (stick2.GetRawButton(buttonA) == true and stateDisarmed == true){
myServo->SetAngle(servoLocked);
stateDisarmed = false;
stateArming1 = true;
}
if (stateArming1 == true){
t_motor.Set(motorWinding);
myServo->SetAngle(servoLocked);
if (t_motor.GetEncPosition() < winchLocked){
t_motor.Set(motorStopped);
stateArming1 = false;
stateArming2 = true;
}
}
if (stateArming2 == true){
t_motor.Set(motorUnwinding);
if (t_motor.GetEncPosition() >= winchMidway){
t_motor.Set(motorStopped);
stateArming2 = false;
stateArmed = true;
}
}
if (stick.GetRawButton(1) == true and stateArmed == true){
stateArmed = false;
stateFiring1 = true;
}
if (stateFiring1 == true){
t_motor.Set(0);
myServo->SetAngle(servoUnlocked);
stateFiring1 = false;
stateDisarmed = true;
}
}
/**
* Gets calibrated parameters from previously calibrated gyro, allocates a new
* gyro with the given parameters for center and offset, and re-runs tests on
* the new gyro.
*/
void TiltPanCameraTest::GyroCalibratedParameters() {
uint32_t cCenter = m_gyro->GetCenter();
float cOffset = m_gyro->GetOffset();
delete m_gyro;
m_gyro = new AnalogGyro(TestBench::kCameraGyroChannel, cCenter, cOffset);
m_gyro->SetSensitivity(kSensitivity);
// Default gyro angle test
// Accumulator needs a small amount of time to reset before being tested
m_gyro->Reset();
Wait(.001);
EXPECT_NEAR(0.0f, m_gyro->GetAngle(), 1.0f);
// Gyro angle test
// Make sure that the gyro doesn't get jerked when the servo goes to zero.
m_pan->SetAngle(0.0);
Wait(0.5);
m_gyro->Reset();
for (int i = 0; i < 600; i++) {
m_pan->Set(i / 1000.0);
Wait(0.001);
}
double gyroAngle = m_gyro->GetAngle();
EXPECT_NEAR(gyroAngle, kTestAngle, 10.0)
<< "Gyro measured " << gyroAngle << " degrees, servo should have turned "
<< kTestAngle << " degrees";
}
// WHen robot is enabled
void init() {
log->info("initializing");
log->print();
climber = NULL;
leftDriveEncoder->Reset();
leftDriveEncoder->SetDistancePerPulse(DriveDistancePerPulse);
leftDriveEncoder->Start();
rightDriveEncoder->Reset();
rightDriveEncoder->SetDistancePerPulse(DriveDistancePerPulse);
rightDriveEncoder->Start();
//leftClimber->motorController->Disable();
leftClimber->encoder->Reset();
leftClimber->encoder->Start();
//rightClimber->motorController->Disable();
rightClimber->encoder->Reset();
rightClimber->encoder->Start();
bcdValue = bcd->value();
loadSwitchOldState = loaderSwitch->Get();
#if 0
bool leftDone = false;
bool rightDone = false;
// Only do this for some BCD values?
while (!leftDone || !rightDone){
if (!leftDone)
leftDone = leftClimber->UpdateState(-1.0, -30);
if (!rightDone)
rightDone = rightClimber->UpdateState(-1.0, -30);
log->info("Wait: Ll Rl: %d %d",
leftClimber->lowerLimitSwitch->Get(),
rightClimber->lowerLimitSwitch->Get());
log->print();
}
#endif
climbState = NotInitialized;
cameraElevateAngle =
(cameraElevateMotor->GetMaxAngle()-cameraElevateMotor->GetMinAngle()) * 2/3;
cameraPivotAngle = 0;
cameraPivotMotor->SetAngle(cameraPivotAngle);
cameraElevateMotor->SetAngle(cameraElevateAngle);
loading = false;
loaderDisengageDetected = false;
//This is a rough guess of motor power it should be based on voltage
shooterMotorVolts = 8.0; // volts as a fraction of 12V
loadCount = 0;
}
virtual void SetUp() override {
m_tilt = new Servo(TestBench::kCameraTiltChannel);
m_pan = new Servo(TestBench::kCameraPanChannel);
m_spiAccel = new ADXL345_SPI(SPI::kOnboardCS0);
m_tilt->Set(kTiltSetpoint45);
m_pan->SetAngle(0.0f);
Wait(kServoResetTime);
m_gyro->Reset();
}
void shootingModes(){
if (manShootMode == 0){
updateShooter();
}
if (manShootMode == 1){
if (stick2.GetRawAxis(triggerR) > 0){
t_motor.Set(scaler(-1*(stick2.GetRawAxis(triggerR))));
}
else if (stick2.GetRawAxis(triggerL) > 0){
t_motor.Set(scaler(stick2.GetRawAxis(triggerL)));
}
else if (stick2.GetRawAxis(triggerL) == 0 and stick2.GetRawAxis(triggerR) == 0){
t_motor.Set(0);
}
if (stick2.GetRawButton(buttonX) == true){
myServo->SetAngle(175);
}
if (stick2.GetRawButton(buttonX) == false){
myServo->SetAngle(5);
}
}
}
/**
* Test if the servo turns 90 degrees and the gyroscope measures this angle
* Note servo on TestBench is not the same type of servo that servo class
* was designed for so setAngle is significantly off. This has been calibrated
* for the servo on the rig.
*/
void TiltPanCameraTest::GyroAngle() {
// Make sure that the gyro doesn't get jerked when the servo goes to zero.
m_pan->SetAngle(0.0);
Wait(0.5);
m_gyro->Reset();
for (int i = 0; i < 600; i++) {
m_pan->Set(i / 1000.0);
Wait(0.001);
}
double gyroAngle = m_gyro->GetAngle();
EXPECT_NEAR(gyroAngle, kTestAngle, 10.0)
<< "Gyro measured " << gyroAngle << " degrees, servo should have turned "
<< kTestAngle << " degrees";
}
bool servoSetPos(bool s){
if (s == true){
myServo->SetAngle(SmartDashboard::GetNumber("servoMin",5));
myServo2->SetAngle(SmartDashboard::GetNumber("servoMin",5));
myServo3->SetAngle(SmartDashboard::GetNumber("servoMin",5));
// myServo4->SetAngle(SmartDashboard::GetNumber("servoMin",5));
}
if (s == false){
myServo->SetAngle(SmartDashboard::GetNumber("servoMax",165));
myServo2->SetAngle(SmartDashboard::GetNumber("servoMax",165));
myServo3->SetAngle(SmartDashboard::GetNumber("servoMax",165));
// myServo4->SetAngle(SmartDashboard::GetNumber("servoMax",165));
}
return true;
}
/*
* OPERATOR CONTROL using a arcade style drive
*/
void OperatorControl(void) {
GetWatchdog().SetEnabled(true);
while (IsOperatorControl()) //This code will loop continuously as long it is operator control mode
{
GetWatchdog().Feed(); // Feed the watchdog
shoulderPotentiometerReading = (5-(shoulderPotentiometerChannel->GetVoltage())); // reads the potentiometer at channel 1
/* COMMENT GRIPPER CODE BELOW
if (shoulderPotentiometerReading == 0) {
gripperMotor->SetAngle(0); //opens gripper
}
else if (shoulderPotentiometerReading > 3) {
gripperMotor->SetAngle(55); //closes gripper
}
*/
if (leftstick->GetRawButton(4) == true) {
shoulderMotor->Drive(-kMaxShoulderMotorSpeed, 0); //moves shoulderMotor down
shoulderDestinationVoltage = shoulderPotentiometerReading;
} else if (leftstick->GetRawButton(5) == true) {
shoulderMotor->Drive(kMaxShoulderMotorSpeed, 0); //moves shoulderMotor up
shoulderDestinationVoltage = shoulderPotentiometerReading;
} else {
//LEFTSTICK PRESETS
if (leftstick->GetRawButton(2) == true) {
//shoulderDestinationVoltage = kPickUpPosition;
} else if (leftstick->GetRawButton(6) == true) {
shoulderDestinationVoltage = kTopRow;
} else if (leftstick->GetRawButton(7) == true) {
shoulderDestinationVoltage = kMiddleRow;
} else if (leftstick->GetRawButton(8) == true) {
shoulderDestinationVoltage = kBottomRow;
} else if (leftstick->GetRawButton(9) == true) {
shoulderDestinationVoltage = kBottomRowSecondColumn;
} else if (leftstick->GetRawButton(10) == true) {
shoulderDestinationVoltage = kMiddleRowSecondColumn;
} else if (leftstick->GetRawButton(11) == true) {
shoulderDestinationVoltage = kTopRowSecondColumn;
}
MoveShoulderTo(shoulderDestinationVoltage);
}
//----------------------------------------------------------------
//GRIPPER OPEN AND CLOSE
if (leftstick->GetRawButton(1) == true) {
gripperMotor->SetAngle(0); //opens gripper
} else {
gripperMotor->SetAngle(55); //closes gripper
}
//MINIBOT DEPLOYMENT
if (leftstick->GetRawButton(3) == true) {
minibotDeployMotor->SetAngle(60); //releases four-bar
myRobot->Drive(0.0, 0); // stop robot since controls are going to be inverted momentarily - don't want to go from full forward to full reverse instantly
}
//RIGHTSTICK PRESETS
if (rightstick->GetRawButton(1) == true) {
//back of the robot is moved forward by pushing forward on the joystick
myRobot->ArcadeDrive((rightstick->GetY()),
(rightstick->GetX()), false); // inverted drive control
} else {
//front of the robot is moved forward by pushing forward on the joystick
myRobot->ArcadeDrive(-(rightstick->GetY()),
(rightstick->GetX()), false); // normal drive control
}
//MINIBOT CLOSING/CLIMBING POLE
if (rightstick->GetRawButton(3) == true) {
//minibot closes only after its four-bar is dropped
minibotCloseMotor1->SetAngle(45); //closes minibot so it starts climbing pole
minibotCloseMotor2->SetAngle(45); //closes minibot so it starts climbing pole
} else if (rightstick->GetRawButton(4) == true) {
//MANUAL ARM CONTROL
armMotor->Drive(-kMaxArmMotorSpeed/2.0, 0); //turn armMotor counter clockwise
} else if (rightstick->GetRawButton(5) == true) {
//MANUAL ARM CONTROL
armMotor->Drive(kMaxArmMotorSpeed/1.5, 0); //turn armMotor cw
}
}
}
void TeleopPeriodic() {
displayCount++;
//tbs change button number
if ((control->gamepadButton(9) && control->gamepadButton(10)) || // start
climbState != NotInitialized) { // continue
// Do we want a manual abort here?
ClimbPeriodic();
return;
}
// drive
drive->setLeft(control->left());
// control->setRightScale(.95);
drive->setRight(control->right());
drive->setScale(control->throttle());
//drive->setLowShift(control->button(1)); // right trigger
drive->setReversed(control->toggleButton(11)); // right JS button 11
//turn light on or off
//lightRing->Set(control->gamepadToggleButton(4) ? Relay::kForward : Relay::kOff );
blowerMotor->Set(control->gamepadButton(6) ? 1.0 : 0.0 );
// For the loader, if we are rotating, wait for at least 100 counts before
// checking the switch or adjusting motor power
if (loading) {
//if (loaderSwitch->Get()) {
//loaderDisengageDetected = true;
//}
loadSwitchDelay++;
// If already rotating, and the switch trips, power down the motor
if (/*loaderDisengageDetected*/ loaderSwitch->Get() != loadSwitchOldState) {
loaderMotor->Set(Relay::kOff);
loading = false;
loadSwitchOldState = loaderSwitch->Get();
}
} else {
// If not rotating and the gamepad button is set, start rotating
if (control->gamepadButton(7)) {
loadSwitchDelay = 0;
loadCount++;
loaderMotor->Set(Relay::kForward);
loading = true;
loaderDisengageDetected = false;
}
}
if (control->gamepadToggleButton(4)){
if (control->gamepadRightVertical() > 0.05 ||
control->gamepadRightVertical() < -0.05) {
shooterMotorVolts += control->gamepadRightVertical();
if (shooterMotorVolts < 6.0)
shooterMotorVolts = 6.0;
if (shooterMotorVolts > 12.0)
shooterMotorVolts = 12.0;
}
}
// For the shooter, spin it up or down based on the toggle
//
if (control->gamepadToggleButton(8)) {
shooterMotor->Set(-shooterMotorVolts);// negative because motor is wired backwerds
log->info("shooter on");
} else {
shooterMotor->Set(0.0);
log->info("shooter off");
}
if (control->gamepadLeftVertical() > 0.05 ||
control->gamepadLeftVertical() < -0.05) {
cameraElevateAngle += control->gamepadLeftVertical()*5;
if (cameraElevateAngle < cameraElevateMotor->GetMinAngle())
cameraElevateAngle = cameraElevateMotor->GetMinAngle();
if (cameraElevateAngle > cameraElevateMotor->GetMaxAngle())
cameraElevateAngle = cameraElevateMotor->GetMaxAngle();
}
if (control->gamepadLeftHorizontal() > 0.05 ||
control->gamepadLeftHorizontal() < -0.05) {
cameraPivotAngle += control->gamepadLeftHorizontal()*5;
if (cameraPivotAngle < cameraPivotMotor->GetMinAngle())
cameraPivotAngle = cameraPivotMotor->GetMinAngle();
if (cameraPivotAngle > cameraPivotMotor->GetMaxAngle())
cameraPivotAngle = cameraPivotMotor->GetMaxAngle();
}
cameraPivotMotor->SetAngle(cameraPivotAngle);
cameraElevateMotor->SetAngle(cameraElevateAngle);
if (control->gamepadToggleButton(1)) {
// If the climber lower limit switch is set, and the distance is >0, set it to 0
if (!leftClimber->lowerLimitSwitch->Get()) {
leftClimber->encoder->Reset();
leftClimber->encoder->Start();
// Only allow forward motion
if (control->gamepadRightVertical() > 0) {
leftClimber->motor->Set(control->gamepadRightVertical());
} else {
leftClimber->motor->Set(0.0);
}
} else {
leftClimber->motor->Set(control->gamepadRightVertical());
}
}
if (control->gamepadToggleButton(3)) {
// If the climber lower limit switch is set, and the distance is >0, set it to 0
if (!rightClimber->lowerLimitSwitch->Get()) {
rightClimber->encoder->Reset();
rightClimber->encoder->Start();
// Only allow forward motion
if (control->gamepadRightVertical() > 0) {
rightClimber->motor->Set(control->gamepadRightVertical());
} else {
rightClimber->motor->Set(0.0);
}
} else {
rightClimber->motor->Set(control->gamepadRightVertical());
}
}
// assorted debug
//log->info("Shift %s", control->toggleButton(8)
// ? "low" : "high");
//log->info("ArmPot: %.0f", arm->encoderValue());
//log->info("pidf: %.2f", arm->pidFactor());
//log->info("piden: %s", arm->isPidEnabled() ? "true" : "false");
//double myTest = drive->rightPosition();
//log->info("renc: %f", myTest);
//myTest = drive->leftPosition();
//log->info("lenc: %f", myTest);
//log->info("gplh %f %f", control->gamepadLeftHorizontal(), cameraPivotAngle);
//log->info("gplv %f %f", control->gamepadLeftVertical(), cameraElevateAngle);
//log->info("gprh %f", control->gamepadRightHorizontal());
//log->info("gprv %f", control->gamepadRightVertical());
if (display()) {
//log->info("lg %d ldd %d", loading, loaderDisengageDetected);
//log->info("BCD: %d", bcd->value());
log->info("SHV: %f", shooterMotorVolts);
//log->info("LdCDS: %d %d %d", loadCount, loaderDisengageDetected, loaderSwitch->Get());
log->info("ena: %c%c%c%c",
" L"[control->gamepadToggleButton(1)],
" R"[control->gamepadToggleButton(3)],
" J"[control->gamepadToggleButton(2)],
" S"[control->gamepadToggleButton(4)]);
log->info("LRC %f %f",
leftClimber->encoder->GetDistance(),
rightClimber->encoder->GetDistance());
log->info("LRD %f %f",
leftDriveEncoder->GetDistance(),
rightDriveEncoder->GetDistance());
log->info("LRL %d %d %d %d %d %d %d",
leftClimber->lowerLimitSwitch->Get(),
leftClimber->lowerHookSwitch->Get(),
leftClimber->upperHookSwitch->Get(),
rightClimber->lowerLimitSwitch->Get(),
rightClimber->lowerHookSwitch->Get(),
rightClimber->upperHookSwitch->Get(),
loaderSwitch->Get());
log->print();
}
}
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 OperatorControl()
{
while (IsOperatorControl() && IsEnabled())
{
robotDrive.ArcadeDrive(scaler(stick.GetZ()),scaler(stick.GetY()));
SmartDashboard::PutNumber("StickZ",stick.GetZ());
SmartDashboard::PutNumber("StickZscaled",scaler(stick.GetZ()));
finger_Motor.Set(scaler(stick2.GetRawAxis(thumbpadL_Y)));
// scalerValue = stick.GetRawAxis(3);
arm_Motor.Set(scaler(stick2.GetRawAxis(thumbpadR_Y)));
manualShooter();
shootingModes();
toggleIntake();
toggleIntakeMode();
setScalerValue();
// double volts = ourRangefinder->GetVoltage();
// SmartDashboard::PutNumber("Voltage",volts);
//t_motor.Set(stick2.GetZ());
//t_motor.Set(stick.GetAxis(Joystick::kDefaultThrottleAxis));
// t_motor.Set(stick.GetAxis(Joystick::kThrottleAxis));
// finger_Motor.Set(stick2.GetAxis(Joystick::kThrottleAxis));
// arm_Motor.Set(stick2.GetY());
// Current Control mode Debug
SmartDashboard::PutNumber("Motor30 Current",t_motor.GetOutputCurrent());
SmartDashboard::PutNumber("Position",t_motor.GetPosition());
// t_motor.Set(stick.GetAxis(Joystick::Slider));
// if (stick.GetRawButton(3) == true){
// t_motor.SetPosition(10000);
// }
// if (stick2.GetRawButton(7) == true and buttonpress == false){
//
// }
// else if (stick2.GetRawButton(3) == false and buttonpress == true){
// buttonpress = false;// drive with arcade style (use right stick)
//
// }
SmartDashboard::PutBoolean("buttonpress state",buttonpress);
if (stick2.GetRawButton(buttonBack) == true){
stateDisarmed = true;
stateArming1 = false;
stateArming2 = false;
stateArmed = false;
stateFiring1 = false;
stateFiring2 = false;
t_motor.SetPosition(0);
}
toggleIntake();
// if (stick2.GetRawButton(5) == true){
// intake_Spin_Motor.Set(1);
// }
// if (stick2.GetRawButton(5) == false){
// intake_Spin_Motor.Set(0);
// }
if (stick.GetRawButton(2) == true and buttonpress2 == false){
myServo->SetAngle(175);
}
if (stick.GetRawButton(2) == false and buttonpress2 == true){
myServo->SetAngle(5);
}
// servoSetPos(servoState);
// myServo->Set(.5);
Wait(0.005);// wait for a motor update time
// motorSetPos(launcherState);
}
}
/**
* Runs the motors with arcade steering.
*/
void OperatorControl(void)
{
myRobot->SetSafetyEnabled(true);
while (IsOperatorControl())
{
bool setLimit;
double cimValue1= -scaleThrottle(-(stick->GetZ())); //Set desired speed from the Throttle, assuming from -1 to 1, also invert the cim, since we want it to rotate coutnerclockwise/clockwise
double cimValue2= scaleThrottle(-(stick->GetZ())); //Set desired speed from the Throttle, assuming from -1 to 1
//For shooter
/*if (stick.GetRawButton(1) == true) {
//back of the robot is moved forward by pushing forward on the joystick
myRobot.ArcadeDrive((stick.GetY()),
(stick.GetX()), false); // inverted drive control
} else {
//front of the robot is moved forward by pushing forward on the joystick
myRobot.ArcadeDrive(-(stick.GetY()),
(stick.GetX()), false); // normal drive control
}
*/
//For manual button speed control, this sets the speed
if (stick->GetRawButton(4) == true) {
cim1->Set(cimValue1); //use the value from the throttle to set cim speed
cim2->Set(cimValue2);//Get speed from throttle, and then scale it
setLimit = true;
//Open Ball Stopper
}
else {
cim1->Set(0.0);
cim2->Set(0.0);
setLimit = false;
//Close Ball Stopper
}
//For precisebelt pickup
if (stick->GetRawButton(1) == true) {
//back of the robot is moved forward by pushing forward on the joystick
if (setLimit == true) {
JagBelt->ArcadeDrive(0.1,
(stick->GetX()), false); // inverted drive control
} else {
JagBelt->ArcadeDrive((stick->GetY()),
(stick->GetX()), false); // inverted drive control
}
} else {
//front of the robot is moved forward by pushing forward on the joystick
if (setLimit == true) {
JagBelt->ArcadeDrive(-0.1,
(stick->GetX()), false); // inverted drive control
} else {
JagBelt->ArcadeDrive(-(stick->GetY()),
(stick->GetX()), false); // inverted drive control
}
}
//For normal belt pickup
/*if (stick->GetRawButton(6) == true) {
JagBelt->Drive(1.0, 0); //opens gripper
} else {
JagBelt->Drive(0.0, 0); //closes gripper
}
*/
//For drive
if (rightstick->GetRawButton(1) == true) {
//back of the robot is moved forward by pushing forward on the joystick
if (rightstick->GetRawButton(10) == true) {
precisionMode= true;
}
else {
precisionMode= false;
}
myRobot->ArcadeDrive((rightstick->GetY()), (rightstick->GetX()), precisionMode); // inverted drive control
} else
{
if (rightstick->GetRawButton(10) == true) {
precisionMode= true;
}
else {
precisionMode= false;
}
//front of the robot is moved forward by pushing forward on the joystick
myRobot->ArcadeDrive(-(rightstick->GetY()), (rightstick->GetX()), precisionMode); // normal drive control
}
/*if (stick->GetRawButton(8) == true) {
cim1->Set(-0.37); //run cim 1 at 50% speed counterclockwise??
cim2->Set(0.37); // run cim at 50% speed clockwise
check = true; //indicate if motors are running
} else if (check == true){ // if motors are running a
Wait(2.0);
belt->Set(1); // run the belt
Wait(2.0); // one sec delay
belt->Set(0.0); // turn belt off
check = false; // put new check
}
else if (check == false){ //if false
// 2 sec delay to wait for the first ball to shoot
cim1->Set(0.0); //stop cims
cim2->Set(0.0);
}
else { // Stop everything
cim1->Set(0.0); //stop cims
cim2->Set(0.0);
belt->Set(0.0);
}
*/
//Code for using window motor
if (rightstick->GetRawButton(4)) {
window->Set(Relay::kOn);
window->Set(Relay::kForward); // tell window motor to go forward
} else if (rightstick->GetRawButton(5) == true){
window->Set(Relay::kOn);
window->Set(Relay::kReverse); //tell window motor to go backward
}
else {
//Wait(1.0);
window->Set(Relay::kOff); //turn it off, if the relays aint being used
}
//Code for Banebot Motor for stopping ballz
if (stick->GetRawButton(2) == true) { // press button TWO to close
pickStop->Set(-0.5); //closes ball stopper
Wait(1);
pickStop->Set(0.0);
} else if (stick->GetRawButton(3) == true){ //press button three to open
pickStop->Set(0.5); //opens ball stopper
Wait(1.2); // too slow, so needs more time
pickStop->Set(0.0);
}
else if (stick->GetRawButton(5)== true){ //press 5 to stop imediately, useful for adjusting angles...
//Wait(1.0);
pickStop->Set(0.0);
}
//Code for ... servoooo
if (stick->GetRawButton(10) == true) { //press 10 on the left stick...
bar->SetAngle(60); // set the angles to 60...clockwise?
bar2->SetAngle(60);
} else if (stick->GetRawButton(11) == true) // press 11 on the left stick
{
bar->SetAngle(-60); //set the angles to -60...counterclockwise?
bar2->SetAngle(-60);
}
// Initialize functions...
// RelayServo();
//PreciseBelt();
//UltrasonicRange();
// Accelerometer();
//dash->GetPacketNumber();
// send data back to dashboard
dash->GetPacketNumber(); //not sure why this is here 0_0
//int limitValue= limitSwitch->Get() ? 1 : 0; // retrieve 1 and 0 only.../ look for 0 and 1
float servoAngle = bar->GetAngle();
//dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Limit State: %d", limitValue); //send data back to driver station...
// dsLCD->Printf(DriverStationLCD::kUser_Line2, 2, "Servo Angle: %f", servoAngle); //send data back to driver station...
float gyroVal = gyro -> GetVoltage();//Gets voltage from gyro
float ultraVal = rangeFinder -> GetVoltage(); //Get voltage from ultrasonic sensor
float tempVal = Temperature -> GetVoltage();//Gets temperature
//Do the math to convert data received from the ultrasonic volts->miliVolts->milivolts per inch->inches
float Vm = (ultraVal*1000);
float Ri = (Vm/9.765625);
// Print data back to dashboard
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Ultrasonic Range: %f",Ri);
dsLCD->Printf(DriverStationLCD::kUser_Line2, 1, "Gyro: %f", gyroVal);
dsLCD->Printf(DriverStationLCD::kUser_Line3, 1, "Temperature: %f", tempVal);
dsLCD->Printf(DriverStationLCD::kUser_Line4, 1, "Cim1 Speed: %f%%", (cimValue1*100)); //display speed that the mototrs are reunning at different percentages...
dsLCD->Printf(DriverStationLCD::kUser_Line5, 1, "Cim2 Speed: %f%%", (cimValue2*100));
//Update dashboard
dsLCD->UpdateLCD();
}
}
