void DisabledInit(){
printf("finished %s at %f",mode.c_str(),GetTime());
leftMotor->SetControlMode(defaultMode);
rightMotor->SetControlMode(defaultMode);
leftMotor->Set(0);
rightMotor->Set(0);
}
void TeleopPeriodic()
{
while(1)
{
fps->SetLeftRightMotorOutputs(0.5*-drivercontroller->GetRawAxis(1) + drivercontroller->GetRawAxis(2), 0.5*-drivercontroller->GetRawAxis(1) - drivercontroller->GetRawAxis(2));
if(operatorcontroller->GetRawAxis(1) > 0)
{
armminipluator->Set(0.6*operatorcontroller->GetRawAxis(1));
}
else if(operatorcontroller->GetRawAxis(1) < 0)
{
armminipluator->Set(0.3*operatorcontroller->GetRawAxis(1));
}
if(operatorcontroller->GetRawButton(5))
{
shro->Set(1);
}
else if(operatorcontroller->GetRawAxis(6))
{
shro->Set(-1);
}
else
{
shro->Set(0);
}
}
}
void SetLiftMotor(float val)
{
liftMotor->Set(val);
#if BUILD_VERSION == COMPETITION
liftMotor2->Set(val);
#endif
}
//void StartAutomaticCapture(std::shared_ptr<USBCamera>cam0);
void TeleopPeriodic() {
// ui.GetData(&wui);
// m_tank.Drive(wui.LeftSpeed, wui.RightSpeed);
//
// m_shooter.Rotate(wui.RotateSpeed*3); //70 degrees per second at full value
// m_shooter.Lift(wui.LiftSpeed*1.193); //4 seconds for 180 degree revolution
// if(wui.SpinUp) {
// m_shooter.Spinup(1);
// }
// if(wui.Shoot) {
// m_shooter.Shoot();
// }
// if(wui.Pickup) {
// m_shooter.Pickup();
// }
//
// m_suspension.SetFrontLeft(wui.DropFL);
// m_suspension.SetBackLeft(wui.DropBL);
// m_suspension.SetFrontRight(wui.DropFR);
// m_suspension.SetBackRight(wui.DropBR);
// m_leddar.GetDetections();
// m_shooter.Update();
//float RTrigger = m_lStick->GetRawAxis(3);
//float LTrigger = m_lStick->GetRawAxis(2);
//if (m_PWMTalonLeftFrontTop == .5)
//if (abs(RTrigger) < 0.2)
//RTrigger = 0;
//if (abs(LTrigger) < 0.2)
//LTrigger = 0;
float leftSpeed = m_lStick->GetRawAxis(1);
float rightSpeed = m_lStick->GetRawAxis(5);
if (abs(leftSpeed) < 0.2)
leftSpeed = 0;
if (abs(rightSpeed) < 0.2)
rightSpeed = 0;
//float LTrigger = m_lStick->GetRawAxis(3);
//float RTrigger = m_lStick->GetRawAxis(2);
SmartDashboard::PutNumber("Left Stick", leftSpeed);
SmartDashboard::PutNumber("Right Stick", rightSpeed);
//SmartDashboard::PutNumber("L Trigger", LTrigger);
//SmartDashboard::PutNumber("R Trigger", RTrigger);
SmartDashboard::PutNumber("Left Encoder", leftEncoder->Get());
SmartDashboard::PutNumber("Right Encoder", rightEncoder->Get());
drive->TankDrive(leftSpeed, rightSpeed, true);
//drive->TankDrive(RTrigger, LTrigger, true);
LEFTDRIVE1->Set(leftSpeed);
LEFTDRIVE2->Set(leftSpeed);
RIGHTDRIVE1->Set(rightSpeed);
RIGHTDRIVE2->Set(rightSpeed);
//m_PWMTalonLeftFrontTop->Set(RTrigger);
//m_PWMTalonRightFrontTop->Set(RTrigger);
//m_PWMTalonRightRearTop->Set(LTrigger);
//m_PWMTalonLeftRearTop->Set(LTrigger);
}
void TeleopInit()
{
//Set slaves
l_slave->SetControlMode(CANTalon::ControlMode::kFollower);
r_slave->SetControlMode(CANTalon::ControlMode::kFollower);
l_slave->Set(1);
r_slave->Set(2);
//l_slave->SetClosedLoopOutputDirection(true);
//r_slave->SetClosedLoopOutputDirection(true);
zeroAll();
}
void toggleIntakeMode(){
if (intakeMode == 1){
intake_Spin_Motor.Set(1);
}
if (intakeMode == 0){
intake_Spin_Motor.Set(0);
}
if (intakeMode == -1){
intake_Spin_Motor.Set(-1);
}
}
void stateLaunching() {
stateTimer++;
if (stateTimer == 1) {
if (shootingHigh) {
rShooter->Set(kRightHighRPM);
lShooter->Set(kLeftHighRPM);
} else {
rShooter->Set(kRightLowRPM);
lShooter->Set(kLeftLowRPM);
}
} else if (stateTimer > 50 && stateTimer <= 65) {
shooterInOut->Set(-1.0);
LOGGER(DEBUG) << "[stateLaunching] Timer: " << stateTimer << " Angle: " << launchPIDSource.PIDGet()
<< " Right RPM: " << rShooter->GetSpeed() << " Left RPM: " << lShooter->GetSpeed();
} else if (stateTimer > 65 && stateTimer <= 70) {
shooterInOut->Set(0.0);
} else if (stateTimer > 70 && stateTimer <= 75) {
shooterInOut->Set(0.6);
} else if (stateTimer > 75) {
shootingHigh = false;
stateTimer = 0;
rShooter->Set(0.0);
lShooter->Set(0.0);
shooterInOut->Set(0.0);
robotState = kOperatorControl;
}
}
void zeroAll() {
//Zero all speed controllers
std::cout << "Stopping all speed controllers..." << std::endl;
sc_left->StopMotor();
sc_right->StopMotor();
intake ->StopMotor();
l_shoot ->StopMotor();
r_shoot ->StopMotor();
l_arm ->StopMotor();
r_arm->StopMotor();
//Turn off fleshlight
std::cout << "Turning off fleshlight..." << std::endl;
fleshlight -> Set(Relay::Value::kOff);
}
void TeleopPeriodic()
{
while(IsOperatorControl() && IsEnabled())
{
// FrontL->Set(.5*Driver->GetRawAxis(0)+.5*Driver->GetRawAxis(2));
//FrontR->Set(.5*Driver->GetRawAxis(0)-.5*Driver->GetRawAxis(2));
RearL->Set(.5*Driver->GetRawAxis(0)+.5*Driver->GetRawAxis(2));
RearR->Set(.5*Driver->GetRawAxis(0)-.5*Driver->GetRawAxis(2));
/**Manipulator->Set(.5*Operator->GetRawAxis(1));*/
/** Takes axes value of driver's and operator's joysticks to recieve values that the motors will be set to.
* Pushing left stick forward and backwards on driver controller causes all motors to move forward and backwards, respectively.
* Pushing right stick left on driver controller causes right motors to speed up making the robot turn left
* Pushing right stick right on driver controller causes left motors to speed up making the robot turn right
* Pushing left stick forward and backwards causes the arm to mover up and down, respectively.
*/
}
}
void AutonomousLowBar() {
// Strategy 2 - start in a normal position lined up with low bar, go through low bars and score boulder in lower goal.
// -------------------------------------------------------------------------------------------------------------------
// backUp straight for a little bit
// drop arm
// backup more under lowbar
// stop (we might add going to lowgoal later)
switch(currentState)
{
case 1:
timer->Reset();
timer->Start();
currentState = 2;
break;
case 2:
drive->TankDrive(autoSpeed,autoSpeed);
if(timer->Get() >= .4)
{
drive->TankDrive(0.0,0.0);
currentState = 3;
timer->Reset();
timer->Start();
}
break;
case 3:
intakeLever->Set(autoIntakeSpeed);
if(timer->Get() >= .5)
{
intakeLever->Set(0.0);
currentState = 4;
timer->Reset();
timer->Start();
}
break;
case 4:
drive->TankDrive(autoSpeed,autoSpeed);
if(timer->Get() >= autoLength)
{
drive->TankDrive(0.0,0.0);
currentState = 5;
timer->Reset();
timer->Stop();
}
break;
}
}
void AutonomousStraightSpy() {
switch (currentState) {
case 1:
timer->Reset();
timer->Start();
turnController->Reset();
turnController->SetSetpoint(ahrs->GetYaw());
turnController->Enable();
currentState = 2;
break;
case 2:
intakeLever->Set(0.25);
if (timer->Get() >= 1) {
intakeLever->Set(0);
currentState = 3;
timer->Reset();
timer->Start();
}
break;
case 3:
drive->TankDrive(0.5, 0.5);
if (timer->Get() >= 5) {
drive->TankDrive(0.0, 0.0);
currentState = 4;
timer->Reset();
timer->Start();
}
break;
case 4:
intake->Set(0.5);
if (timer->Get() >= 2) {
currentState = 5;
}
break;
case 5:
intake->Set(0.0);
drive->TankDrive(0.0, 0.0);
break;
}
}
void TestPeriodic()
{
bool nowButton4 = stick->GetRawButton(4);
bool nowButton1 = stick->GetRawButton(1);
if (nowButton4 && !lastButton4) {
testingLeft = !testingLeft;
}
if (nowButton1 && !lastButton1) {
testingRight = !testingRight;
}
lastButton4 = nowButton4;
lastButton1 = nowButton1;
if (testingRight) {
rShooter->Set(kRightHighRPM);
std::cout << rShooter->GetSpeed() << std::endl;
} else {
rShooter->Set(0.0);
}
if (testingLeft) {
lShooter->Set(kLeftHighRPM);
std::cout << lShooter->GetSpeed() << std::endl;
} else {
lShooter->Set(0.0);
}
}
void RobotInit()
{
prefs = Preferences::GetInstance();
stick = new Joystick(0);
frontLeft = new TalonSRX(1);
backLeft = new TalonSRX(0);
frontRight = new TalonSRX(8);
backRight = new TalonSRX(9);
shooterInOut = new TalonSRX(6);
gatherer = new TalonSRX(5); // Gatherer up/down
gathererWheels = new TalonSRX(4); // Gatherer in and out wheels
elevator = new TalonSRX(7); // Actuator for shooter
lShooter = new CANTalon(1); // Left shooter wheels
rShooter = new CANTalon(0); // Right shooter wheels
rShooter->SetTalonControlMode(CANTalon::kSpeedMode);
rShooter->SetSensorDirection(false);
rShooter->SetFeedbackDevice(CANTalon::CtreMagEncoder_Relative);
rShooter->ConfigNominalOutputVoltage(0.0, 0.0);
rShooter->ConfigPeakOutputVoltage(12.0, -12.0);
lShooter->SetTalonControlMode(CANTalon::kSpeedMode);
lShooter->SetSensorDirection(false);
lShooter->SetFeedbackDevice(CANTalon::CtreMagEncoder_Relative);
lShooter->ConfigNominalOutputVoltage(0.0, 0.0);
lShooter->ConfigPeakOutputVoltage(12.0, -12.0);
cameraTilt = new Servo(3);
drive = new RobotDrive(backLeft, frontLeft, backRight, frontRight);
drive->SetSafetyEnabled(false);
m_pixy = new PixyTracker();
m_pixy->startVideo();
turnPIDSource = new TurnPIDSource(m_pixy, m_signature, m_targets);
}
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;
}
}
void TeleopInit()
{
getPreferences();
rShooter->SetF(kRightF);
rShooter->SetP(kRightP);
rShooter->SetI(kRightI);
rShooter->SetD(kRightD);
lShooter->SetF(kLeftF);
lShooter->SetP(kLeftP);
lShooter->SetI(kLeftI);
lShooter->SetD(kLeftD);
resetPIDControllers();
}
void TestInit()
{
getPreferences();
testingLeft = false;
testingRight = false;
rShooter->SetF(kRightF);
rShooter->SetP(kRightP);
rShooter->SetI(kRightI);
rShooter->SetD(kRightD);
lShooter->SetF(kLeftF);
lShooter->SetP(kLeftP);
lShooter->SetI(kLeftI);
lShooter->SetD(kLeftD);
}
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);
}
}
}
void AutonomousAdjustableStraight() {
switch (currentState) {
case 1:
timer->Reset();
timer->Start();
turnController->Reset();
turnController->SetSetpoint(ahrs->GetYaw());
turnController->Enable();
currentState = 2;
break;
case 2:
intakeLever->Set(autoIntakeSpeed);
if (timer->Get() >= 1) {
intakeLever->Set(0);
currentState = 3;
timer->Reset();
timer->Start();
}
break;
case 3:
drive->TankDrive(autoSpeed, autoSpeed);
intakeLever->Set(-0.1);
if (timer->Get() >= autoLength) {
intakeLever->Set(0.0);
drive->TankDrive(0.0, 0.0);
currentState = 4;
timer->Reset();
timer->Start();
}
break;
case 4:
intake->Set(0.5);
shooter->Set(-0.5);
if (timer->Get() >= 2) {
currentState = 5;
}
break;
case 5:
intake->Set(0.0);
shooter->Set(0.0);
drive->TankDrive(0.0, 0.0);
break;
}
}
void RobotInit()
{
// Auto chooser
chooser = new SendableChooser();
chooser->AddDefault(autoNameDefault, (void*)&autoNameDefault);
chooser->AddObject(autoNameCustom, (void*)&autoNameCustom);
SmartDashboard::PutData("Auto Modes", chooser);
// Drive declarations
frontLeftTalon = new CANTalon(frontLeftChannel);
rearLeftTalon = new CANTalon(rearLeftChannel);
frontRightTalon = new Victor(frontRightChannel);
rearRightTalon = new CANTalon(rearRightChannel);
//frontLeftTalon->SetInverted(true);
rearLeftTalon->SetInverted(true);
yawGyro = new ADXRS450_Gyro();
robotDrive = new RobotDrive(frontLeftTalon, rearLeftTalon, frontRightTalon, rearRightTalon);
flightStick = new Joystick(flightstickChannel);
robotDrive -> SetSafetyEnabled(false);
// Shooter variable declarations MAKE SURE YOU PLUG THEM INTO THE RIGHT PORTS WENDY
motor1 = new Talon(8); // loook above
motor2 = new Jaguar(4);
motor3 = new Jaguar(5);
shootStick = new Joystick(0);
shooterSwitch = new DigitalInput(0);
// Distance sensor declarations
distanceSensor = new AnalogInput(0);
getVcc = new AnalogInput(2);
// Servo
doorLift = new Servo(9);
// Camera stuff
camera1 = new USBCamera("cam0", false);
camera2 = new USBCamera("cam1", false);
camera1->OpenCamera();
camera2->OpenCamera();
camera1->StartCapture();
camera2->StartCapture();
//Camera servos
frontCamServo = new Servo(10); // WHAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAT
backCamServo = new Servo(11); // WHAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAT
}
void TeleopPeriodic()
{
char myString [STAT_STR_LEN];
if (initialZeroing) // moving to the inner limit
{
sprintf(myString, "initZero ip\n"); //in progress
SmartDashboard::PutString("DB/String 0", myString);
if (GetForkLimitSwitchMin())
{
SetForkMotor(MOTOR_SPEED_STOP);
gearToothCounter->Reset();
initialZeroing = false;
sprintf(myString, "initZero comp\n"); //complete
SmartDashboard::PutString("DB/String 0", myString);
}
}
else //manual control
{
//motor control
if (joystick->GetRawButton(BUT_JS_OUT) && !GetForkLimitSwitchMax()) // move outwards
SetForkMotor(MOTOR_SPEED_GO);
else if(joystick->GetRawButton(BUT_JS_IN) && !GetForkLimitSwitchMin()) // move inwards
SetForkMotor(-MOTOR_SPEED_GO);
else
SetForkMotor(MOTOR_SPEED_STOP); //stop
//counter control
if (joystick->GetRawButton(BUT_JS_RES_GTC)) // reset the gear tooth counter
gearToothCounter->Reset();
}
//status
sprintf(myString, "jsOut %d\n", joystick->GetRawButton(BUT_JS_OUT));
SmartDashboard::PutString("DB/String 1", myString);
sprintf(myString, "jsIn %d\n", joystick->GetRawButton(BUT_JS_IN));
SmartDashboard::PutString("DB/String 2", myString);
sprintf(myString, "jsResGtc %d\n", joystick->GetRawButton(BUT_JS_RES_GTC));
SmartDashboard::PutString("DB/String 3", myString);
sprintf(myString, "curr: %f\n", forkMotor->GetOutputCurrent());
SmartDashboard::PutString("DB/String 4", myString);
sprintf(myString, "gtc #: %d\n", gearToothCounter->Get()); //gtc count
SmartDashboard::PutString("DB/String 5", myString);
}
void TeleopPeriodic()
{
char myString [STATUS_STR_LEN];
if (running)
{
switch (forkState)
{
case closing:
UpdateGearCount (); //update whenever the forks are moving
if (GetForkLimitSwitchMin())
{
sprintf(myString, "min gear count: %d\n", absGearToothCount);
SmartDashboard::PutString("DB/String 8", myString);
SetForkMotor(MOTOR_STOP);
SetForkMotor(MOTOR_SPEED);
dsBox->SetOutputs(ALL_LEDS_ON);
forkState = opening;
}
break;
case opening:
UpdateGearCount (); //update whenever the forks are moving
if (GetForkLimitSwitchMax())
{
sprintf(myString, "max gear count: %d\n", absGearToothCount);
SmartDashboard::PutString("DB/String 9", myString);
SetForkMotor(MOTOR_STOP);
SetForkMotor(-MOTOR_SPEED);
dsBox->SetOutputs(ALL_LEDS_OFF);
forkState = closing;
}
break;
}
}
//current monitor check for safety
if (forkMotor->GetOutputCurrent() > MAX_CUR_TH)
{
SetForkMotor(MOTOR_STOP);
running = false;
}
sprintf(myString, "curr: %f\n", forkMotor->GetOutputCurrent());
SmartDashboard::PutString("DB/String 0", myString);
sprintf(myString, "State: %d\n", forkState);
SmartDashboard::PutString("DB/String 1", myString);
sprintf(myString, "running: %d\n", running);
SmartDashboard::PutString("DB/String 2", myString);
sprintf(myString, "forkDir: %d\n", forkDirection);
SmartDashboard::PutString("DB/String 3", myString);
sprintf(myString, "abs gear count: %d\n", absGearToothCount);
SmartDashboard::PutString("DB/String 4", myString);
#if 0
if (minAVal > aIn->GetValue())
minAVal = aIn->GetValue();
if (maxAVal < aIn->GetValue())
maxAVal = aIn->GetValue();
sprintf(myString, "min A val: %d\n", minAVal);
SmartDashboard::PutString("DB/String 5", myString);
sprintf(myString, "max A val: %d\n", maxAVal);
SmartDashboard::PutString("DB/String 6", myString);
#endif
}
void AutonomousSpy() {
// Strategy 1 - start as spy with a boulder, score in lower goal. Starts with intake facing low goal
// -------------------------------------------------------------------------------------------------------------------
switch (currentState) {
case 1:
// -State: stopped
timer->Reset();
timer->Start();
ahrs->ZeroYaw();
currentState = 2;
break;
// --transition: state Driving Forward
case 2:
// -State: Driving Forward
// --wait until lined up with low goal
// --transition: State stopped
drive->TankDrive(0.5, 0.5);
if (timer->Get() >= 1) { // NEEDS TO BE SET
// -State: stopped
// --wait until stopped
drive->TankDrive(0.0, 0.0);
currentState = 3;
timer->Reset();
timer->Start();
}
break;
// --transition: State Shooting
case 3:
// -State: Shooting
// --wait until shooting complete
intake->Set(-.5);
if (timer->Get() >= .7) { //Find Out Actual Time
intake->Set(0);
timer->Reset();
timer->Start();
currentState = 4;
}
break;
// --transition: State Backing Up
case 4:
// -State: Backing Up
// --wait until off tower ramp
drive->TankDrive(-0.5, -0.5);
if (timer->Get() > 1) {
drive->TankDrive(0.0, 0.0);
ahrs->ZeroYaw();
ahrs->Reset();
currentState = 5;
turnController->SetSetpoint(-65.5);
turnController->Enable();
}
break;
// --transition: Turning
case 5:
// -State: Turning Left
// --wait until 65 degrees has been reached to line up with low bar
drive->TankDrive(-0.5, 0.5);
if (turnController->OnTarget()) {
drive->TankDrive(0.0, 0.0);
timer->Reset();
timer->Start();
currentState = 6;
}
break;
// --transition: Backing Up
case 6:
// -State backing Up
// --wait until near guard wall
drive->TankDrive(-0.5, -0.5);
if (timer->Get() >= 1) {
drive->TankDrive(0.0, 0.0);
ahrs->ZeroYaw();
ahrs->Reset();
currentState = 7;
turnController->SetSetpoint(-24.5);
turnController->Enable();
}
break;
// --transition: Turn Left
case 7:
// -State: Turn Right
// --wait until 25 degree turn has been made to line with low bar
drive->TankDrive(-0.5, 0.5);
if (turnController->OnTarget()) {
drive->TankDrive(0.0, 0.0);
timer->Reset();
timer->Start();
currentState = 8;
}
break;
// --transition: Back Up
case 8:
// -State: Backing Up
// --wait until backed through low bar
drive->TankDrive(-0.5, -0.5);
if (timer->Get() >= 1) { // NeedTo Update Value
timer->Stop();
currentState = 9;
}
break;
// --transition: Stopped
case 9:
// -State: Stopped
drive->TankDrive(0.0, 0.0);
break;
}
}
void SetForkMotor(float val)
{
forkMotor->Set(FORK_MOTOR_DIR*val);
}
Robot() :
robotDrive(Motor1, Motor2), // these must be initialized in the same order
stick(5), // as they are declared above.
lw(LiveWindow::GetInstance()),
autoLoopCounter(0),
Motor1(21),
Motor2(12),
Slave1(20),
Slave2(14),
t_motor(13),
arm_Motor(23),
finger_Motor(22),
intake_Spin_Motor(11),
intake_Winch_Motor(13),
stick2(4),
autoLoopCounter2(0)
{
robotDrive.SetExpiration(0.1);
robotDrive.SetSafetyEnabled(false);
Slave1.SetControlMode(CANSpeedController::kFollower);
Slave1.Set(21);
Slave2.SetControlMode(CANSpeedController::kFollower);
Slave2.Set(12);
Motor2.SetInverted(true); //12
Slave2.SetInverted(true);//14
arm_Motor.SetInverted(false);//23
t_motor.SetInverted(true);//23
// t_motor.SetControlMode(CANSpeedController::kVoltage);
// t_motor.Set(0);
// CameraServer::GetInstance()->SetQuality(50);
// CameraServer::GetInstance()->SetSize(2);
// //the camera name (ex "cam0") can be found through the roborio web interface
// CameraServer::GetInstance()->StartAutomaticCapture("cam0");
t_motor.SetControlMode(CANSpeedController::kPercentVbus);
// t_motor.SetVoltageCompensationRampRate(24.0);
t_motor.SetFeedbackDevice(CANTalon::QuadEncoder);
t_motor.SetPosition(0);
// t_motor.SetPID(1, 0, 0);
arm_Motor.SetControlMode(CANSpeedController::kPercentVbus);
finger_Motor.SetControlMode(CANSpeedController::kPercentVbus);
// ourRangefinder = new AnalogInput(0);
}
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 motor from the output of a Joystick.
*/
void OperatorControl() {
LifterEncoder.StartLiveWindowMode();
LifterEncoder.Reset();
while (IsOperatorControl() && IsEnabled()) {
// Set the motor controller's output.
// This takes a number from -1 (100% speed in reverse) to +1 (100% speed forwards).
// lifterA_motor.Set(joy.GetY());
// lifterB_motor.Set(joy.GetY());
if ( stick.GetRawButton(1))
{
}
else if (stick.GetRawButton(2))
{
}
else if (stick.GetRawButton(3))
{
// Elevator down
if (BottomLimitSwitch.Get() == 0) {
lifterA_motor.Set(0.5);
lifterB_motor.Set(0.5);
} else {
lifterA_motor.Set(0);
lifterB_motor.Set(0);
}
}
else if (stick.GetRawButton(4))
{
}
else if (stick.GetRawButton(5))
{
// Elevator up
if (TopLimitSwitch.Get() == 0) {
lifterA_motor.Set(-0.5);
lifterB_motor.Set(-0.5);
} else {
lifterA_motor.Set(0);
lifterB_motor.Set(0);
}
}
else if (stick.GetRawButton(6))
{
}
else if (stick.GetRawButton(7))
{
}
else if (stick.GetRawButton(8))
{
}
else if (stick.GetRawButton(9))
{
}
else if (stick.GetRawButton(10))
{
}
else if (stick.GetRawButton(11))
{
}
else if (stick.GetRawButton(12))
{
}
else
{
lifterA_motor.Set(0.0);
lifterB_motor.Set(0.0);
}
if(BottomLimitSwitch.Get()==true)
{
LifterEncoder.Reset();
}
// Send some stuff to the dashboard
SmartDashboard::PutBoolean("Top Limit Switch", TopLimitSwitch.Get());
SmartDashboard::PutBoolean("Bottom Limit Switch", BottomLimitSwitch.Get());
SmartDashboard::PutNumber("Encoder Position",LifterEncoder.GetRaw());
Wait(kUpdatePeriod); // Wait 5ms for the next update.
}
}
DriveTrain(Vision* visionTracking) :
IComponent(new string("DriveTrain")),
leftDriveMaster(new CANTalon(1)),
leftDriveSlave1(new CANTalon(3)),
leftDriveSlave2(new CANTalon(5)),
rightDriveMaster(new CANTalon(2)),
rightDriveSlave1(new CANTalon(4)),
rightDriveSlave2(new CANTalon(6)),
shift(new Solenoid(4)),
vision(visionTracking),
visionPIDSource(new DrivePIDSource()),
visionPIDOutput(new DrivePIDOutput()),
visionPID(new PIDController(0.70f, 0, 0, visionPIDSource, visionPIDOutput)),
angleETC(new ErrorTimeCubed(DRIVE_ANGLE_TOLERANCE, 45.0f, -180.0f, 180.0f)),
crossTime(new Timer()),
hasCrossed(false),
crossState(0),
isClimbing(true),
driveTime(new Timer()),
timedDriveState(0),
shiftHigh(new RobotButton(RobotButton::JoystickType::PRIMARY, RobotButton::ControlTypes::KEY, JOYSTICK_BUMPER_RIGHT)),
shiftLow(new RobotButton(RobotButton::JoystickType::PRIMARY, RobotButton::ControlTypes::KEY, JOYSTICK_BUMPER_LEFT)),
stateUntoggle(new RobotButton(RobotButton::JoystickType::PRIMARY, RobotButton::ControlTypes::KEY, JOYSTICK_BACK)),
autoCrossToggle(new RobotButton(RobotButton::JoystickType::PRIMARY, NEW_JOYSTICK ? RobotButton::ControlTypes::AXIS : RobotButton::ControlTypes::KEY, JOYSTICK_TRIGGER_RIGHT)),
reverseToggle(new RobotButton(RobotButton::JoystickType::PRIMARY, RobotButton::ControlTypes::KEY, JOYSTICK_X)),
crossSpeedMultiplier(1.0f),
crossingForward(true),
leftSpeedCurrent(0),
rightSpeedCurrent(0),
targetDistance(0),
crossReverse(false),
reverse(true),
primaryDriving(false),
state(DriveState::NONE)
{
leftDriveMaster->SetControlMode(CANTalon::ControlMode::kPosition);
leftDriveMaster->SetFeedbackDevice(CANTalon::FeedbackDevice::QuadEncoder);
leftDriveMaster->ConfigEncoderCodesPerRev(1024);
leftDriveMaster->Enable();
leftDriveSlave1->SetControlMode(CANTalon::ControlMode::kFollower);
leftDriveSlave1->Enable();
leftDriveSlave1->Set(1);
leftDriveSlave2->SetControlMode(CANTalon::ControlMode::kFollower);
leftDriveSlave2->Enable();
leftDriveSlave2->Set(1);
rightDriveMaster->SetControlMode(CANTalon::ControlMode::kPosition);
rightDriveMaster->SetFeedbackDevice(CANTalon::FeedbackDevice::QuadEncoder);
leftDriveMaster->ConfigEncoderCodesPerRev(1024);
rightDriveMaster->Enable();
rightDriveSlave1->SetControlMode(CANTalon::ControlMode::kFollower);
rightDriveSlave1->Enable();
rightDriveSlave1->Set(2);
rightDriveSlave2->SetControlMode(CANTalon::ControlMode::kFollower);
rightDriveSlave2->Enable();
rightDriveSlave2->Set(2);
visionPID->SetInputRange(-1, 1);
visionPID->SetOutputRange(-1, 1);
visionPID->SetContinuous(true);
visionPID->SetAbsoluteTolerance(0.05);
visionPID->Disable();
}
void SetForkMotor(float val)
{
curForkSetSpeed = FORK_MOTOR_REV_STATE*val;
forkMotor->Set(curForkSetSpeed);
}
void TeleopPeriodic() override {
float leftPower, rightPower; // Get the values for the main drive train joystick controllers
leftPower = -leftjoystick->GetY();
rightPower = -rightjoystick->GetY();
float multiplier; // TURBO mode
if (rightjoystick->GetRawButton(1))
{
multiplier = 1;
} else {
multiplier = 0.5;
}
// wtf is a setpoint - it's an angle to turn to
if (leftjoystick->GetRawButton(6)) {
turnController->Reset();
turnController->SetSetpoint(0);
turnController->Enable();
ahrs->ZeroYaw();
//ahrs->Reset();
}
// Press button to auto calculate angle to rotate bot to nearest ball
// if(leftjoystick->GetRawButton(99))
// {
// ahrs->ZeroYaw();
// turnController->Reset();
// turnController->SetSetpoint(mqServer.GetDouble("angle"));
// turnController->Enable();
// aimState = 1;
// }
switch(aimState)
{
default:
case 0: // No camera assisted turning
//Drive straight with one controller, else: drive with two controllers
if (leftjoystick->GetRawButton(1)) {
drive->TankDrive(leftPower * multiplier, leftPower * multiplier,
false);
} else if (leftjoystick->GetRawButton(2)) {
drive->TankDrive(leftPower * multiplier + rotateRate,
leftPower * multiplier + -rotateRate, false);
} else {
drive->TankDrive(leftPower * multiplier, rightPower * multiplier,
false);
}
break;
case 1: // Camera assisted turning, deny input from controllers
drive->TankDrive(rotateRate, -rotateRate, false);
if(turnController->OnTarget() || leftjoystick->GetRawButton(97)) {
aimState = 0; // Finished turning, auto assist off
turnController->Disable();
turnController->Reset();
}
break;
}
// That little flap at the bottom of the joystick
float scaleIntake = (1 - (controlstick->GetThrottle() + 1) / 2);
// Depending on the button, our intake will eat or shoot the ball
if (controlstick->GetRawButton(2)) {
intake->Set(-scaleIntake);
shooter->Set(scaleIntake);
} else if (controlstick->GetRawButton(1)) {
intake->Set(scaleIntake);
shooter->Set(-scaleIntake);
} else {
intake->Set(0);
shooter->Set(0);
}
// Control the motor that lifts and descends the intake bar
float intake_lever_power = 0;
if (controlstick->GetRawButton(6)) {
manual = true;
intake_lever_power = .3;
// intakeLever->Set(.30); // close
} else if (controlstick->GetRawButton(4)) {
manual = true;
intake_lever_power = -.4;
// intakeLever->Set(-.40); // open
} else if (controlstick->GetRawButton(3)){
manual = true;
intake_lever_power = -scaleIntake;
// intakeLever->Set(-scaleIntake);
} else if (controlstick->GetRawButton(5)) {
manual = true;
intake_lever_power = scaleIntake;
// intakeLever->Set(scaleIntake);
} else {
if (manual) {
manual = false;
lastLiftPos = intakeLever->GetEncPosition();
intakeLever->SetControlMode(CANTalon::ControlMode::kPosition);
intakeLever->SetFeedbackDevice(CANTalon::FeedbackDevice::QuadEncoder);
intakeLever->SetPID(1, 0.001, 0.0);
intakeLever->EnableControl();
}
intake_hold = true;
intakeLever->Set(lastLiftPos);
}
if (manual) {
intake_hold = false;
intakeLever->SetControlMode(CANTalon::ControlMode::kPercentVbus);
intakeLever->Set(intake_lever_power);
}
if (controlstick->GetRawButton(11)) {
lift->Set(true);
liftdown->Set(false);
} else if (controlstick->GetRawButton(12)){
lift->Set(false);
liftdown->Set(true);
} else if (controlstick->GetRawButton(7)) {
liftdown->Set(false);
}
if (controlstick->GetRawButton(9)) {
winch->Set(scaleIntake);
} else if (controlstick->GetRawButton(10)) {
winch->Set(-scaleIntake);
} else {
winch->Set(0);
}
if (controlstick->GetPOV() == 0 && !bounce ) {
constantLift -= 0.05;
bounce = true;
} else if (controlstick->GetPOV() == 180 && !bounce) {
constantLift += 0.05;
bounce = true;
} else if (controlstick->GetPOV() == 270 && !bounce) {
constantLift = 0;
bounce = true;
} else {
bounce = false;
}
UpdateDashboard();
}
void TeleopPeriodic()
{
//Drive
if(SmartDashboard::GetBoolean("DB/Button 0", false))
{
sc_left -> Set(-evan -> GetRawAxis(1));
sc_right -> Set(evan -> GetRawAxis(5));
}
else
{
sc_left -> Set(-evan->GetRawAxis(1) + .5*evan->GetRawAxis(0));
sc_right -> Set(evan->GetRawAxis(1) + .5*evan->GetRawAxis(0));
}
//Intake
if (i_limit->Get()) {
if(hunter->GetRawButton(1))
{
l_shoot->Set(-1);
r_shoot->Set(1);
if(count == 0)
{
timer1->Start();
count = 1;
}
}
else if(evan->GetRawAxis(2) > 0.2)
{
intake->Set(-evan->GetRawAxis(2));
}
else{
intake->Set(0);
}
}
else
{
if(evan->GetRawAxis(3) > 0.2)
{
intake->Set(.40*evan->GetRawAxis(3));
}
else if(evan->GetRawAxis(2) > 0.2)
{
intake->Set(-evan->GetRawAxis(2));
}
else {
intake->Set(0);
}
}
if(timer1->Get()>2){
intake->Set(1);
timer2->Start();
if(timer2->Get()>.5){
l_shoot->Set(0);
r_shoot->Set(0);
intake->Set(0);
timer1->Stop();
timer1->Reset();
timer2->Stop();
timer2->Reset();
count = 0;
}
}
//Fleshlight
fleshlight->Set(evan->GetRawButton(1) ? Relay::Value::kForward : Relay::Value::kOff);;
/*
//DPAD Stuff
switch (evan -> GetPOV())
{
case (0):
l_shoot -> Set(-.25*evan -> GetRawAxis(3));
r_shoot -> Set(.25*evan -> GetRawAxis(3));
break;
case (90):
l_shoot -> Set(-.5*evan -> GetRawAxis(3));
r_shoot -> Set(.5*evan -> GetRawAxis(3));
break;
case (180):
l_shoot -> Set(-.75*evan -> GetRawAxis(3));
r_shoot -> Set(.75*evan -> GetRawAxis(3));
break;
case (270):
l_shoot -> Set(evan-> GetRawAxis(3));
r_shoot ->Set(-evan->GetRawAxis(3));
break;
default:
l_shoot -> Set(-evan -> GetRawAxis(3));
r_shoot -> Set(evan -> GetRawAxis(3));
break;
}*/
//Hunter controls Arm
if ((hunter->GetRawAxis(5) < 0.1 and hunter->GetRawAxis(5) > -0.1) or (a_limit->Get() and hunter->GetRawAxis(5) > 0))
{
l_arm->Set(0);
r_arm->Set(0);
}
else
{
l_arm -> Set(-hunter-> GetRawAxis(5));
r_arm -> Set(hunter-> GetRawAxis(5));
}
//let evan control arm
/*if ((evan->GetRawAxis(5) < 0.1 and evan->GetRawAxis(5) > -0.1) or (a_limit->Get() and evan->GetRawAxis(5) > 0))
{
l_arm->Set(0);
r_arm->Set(0);
}
else
{
l_arm -> Set(evan-> GetRawAxis(5));
r_arm -> Set(-evan-> GetRawAxis(5));
}*/
//FORCEFEEDBACK
evan -> SetRumble(Joystick::RumbleType::kLeftRumble, hunter -> GetRawButton(2));
evan -> SetRumble(Joystick::RumbleType::kRightRumble, hunter -> GetRawButton(2));
//Message
SmartDashboard::PutNumber("DB/String 0", evan->GetPOV() );
//std::cout << evan->GetPOV() << std::endl;
//Wait
Wait(0.001);
}
