float Cypress::GetAnalogIn(int channel) { int getchan=abs(channel-4)+1; if (enhanced) return m_ds->GetEnhancedIO().GetAnalogIn(getchan); else return m_ds->GetAnalogIn(getchan); }
void DataSending::SendTheData(){ strIndex = 0; Dashboard &dash = DriverStation::GetInstance()->GetHighPriorityDashboardPacker(); DriverStation *drive = DriverStation::GetInstance(); Send(true);//yes this is the dataSending Code Send(drive->GetBatteryVoltage());//battery info Send((batteryCurrent->GetVoltage()-2.5)*AMPS_CONSTANT); GetDriveJoystickInfo();//joystick info GetOperatorJoystickInfo();//moar joystick info GetVicInfo();//victor info Send(RobotMap::launcherSolenoidLeft->Get());//solenoid info Send(RobotMap::launcherSolenoidRight->Get()); Send(RobotMap::shifterDoubleSolenoid->Get()); Send((bool)RobotMap::launcherCompressor->GetPressureSwitchValue());//sensor info Send(RobotMap::robotRangeFinderUltrasonicSensor->GetVoltage()/VoltsPerCM); Send(RobotMap::driveTrainGyro->GetAngle()); Send(RobotMap::launcherPressureSwitch->GetVoltage()*PSI_CONSTANT);//transducer1 Send(RobotMap::collectorLeftRoller->Get()*-1);//talon info Send(RobotMap::collectorRightRoller->Get()); Send(count++);//number of packets Send(timesPerSecond);//every nth number data is sent from a 50 hz source //a.k.a 50 / above number = Hz Send(drive->GetMatchTime()); Send(drive->IsEnabled()); dash.AddString(strBuffer); dash.Finalize(); UpdateUserLCD(); }
void Cypress::SetDigitalOut(int channel, bool value) { int getchan=abs(channel-8)+1; if (enhanced) { getchan+=8; m_ds->GetEnhancedIO().SetDigitalOutput(getchan, value); } else m_ds->SetDigitalOut(getchan, value); }
OI::OI() : pIO(NULL), driverStick(DRIVER_STICK), gunnerStick(GUNNER_STICK) { DriverStation *pDS = DriverStation::GetInstance(); pIO = &pDS->GetEnhancedIO(); }
MedicOperatorInterface::MedicOperatorInterface() { joyDrive = new Joystick(1);//TODO:real value joyManip = new Joystick(2);//TODO:real value DriverStation *dsSimple = DriverStation::GetInstance(); ds = &dsSimple->GetEnhancedIO(); dsLCD = DriverStationLCD::GetInstance(); dashboard->init(); }
bool Cypress::GetDigitalIn(int channel) { int getchan=abs(channel-8)+1; if (enhanced) return !m_ds->GetEnhancedIO().GetDigital(getchan); else return m_ds->GetDigitalIn(getchan); }
void DriverStation::MergeFrom(const DriverStation& from) { GOOGLE_CHECK_NE(&from, this); if (from._has_bits_[0 / 32] & (0xffu << (0 % 32))) { if (from.has_enabled()) { set_enabled(from.enabled()); } if (from.has_state()) { set_state(from.state()); } } mutable_unknown_fields()->MergeFrom(from.unknown_fields()); }
void DisabledInit() { //Config loading try { cameraLight->Set(Relay::kOff); if (!Config::LoadFromFile("config.txt")) { cout << "Something happened during the load." << endl; } Config::Dump(); myDrive->DisablePID(); myDrive->ResetPID(); if(fout.is_open() && !freshStart && !ds->IsFMSAttached()){ fout.close(); myShooter->ResetShooterProcess(); lc->holdState(false); } } catch (exception ex) { cout << "Disabled exception. Trying again." << endl; cout << "Exception: " << ex.what() << endl; } //ResetShooterMotors(); /* SmartDashboard::PutNumber("Target Info S",1741); cout<<SmartDashboard::GetNumber("Target Info S"); */ }
/** * Check if this motor has exceeded its timeout. * This method is called periodically to determine if this motor has exceeded its timeout * value. If it has, the stop method is called, and the motor is shut down until its value is * updated again. */ void MotorSafetyHelper::Check() { DriverStation *ds = DriverStation::GetInstance(); if (!m_enabled || ds->IsDisabled() || ds->IsTest()) return; ::std::unique_lock<ReentrantMutex> sync(m_syncMutex); if (m_stopTime < Timer::GetFPGATimestamp()) { char buf[128]; char desc[64]; m_safeObject->GetDescription(desc); snprintf(buf, 128, "%s... Output not updated often enough.", desc); wpi_setWPIErrorWithContext(Timeout, buf); m_safeObject->StopMotor(); } }
/** * Runs the motors under driver control with either tank or arcade steering selected * by a jumper in DS Digin 0. Also an arm will operate based on a joystick Y-axis. */ void OperatorControl(void) { Victor armMotor(5); // create arm motor instance while (IsOperatorControl()) { GetWatchdog().Feed(); // determine if tank or arcade mode; default with no jumper is for tank drive if (ds->GetDigitalIn(ARCADE_MODE) == 0) { myRobot->TankDrive(leftStick, rightStick); // drive with tank style } else{ myRobot->ArcadeDrive(rightStick); // drive with arcade style (use right stick) } // Control the movement of the arm using the joystick // Use the "Y" value of the arm joystick to control the movement of the arm float armStickDirection = armStick->GetY(); // if at a limit and telling the arm to move past the limit, don't drive the motor if ((armUpperLimit->Get() == 0) && (armStickDirection > 0.0)) { armStickDirection = 0; } else if ((armLowerLimit->Get() == 0) && (armStickDirection < 0.0)) { armStickDirection = 0; } // Set the motor value armMotor.Set(armStickDirection); } }
bool Cypress::GetDigitalIn(int channel) { return m_ds->GetEnhancedIO().GetDigital(channel); }
// Test Autonomous void Autonomous() { robotDrive.SetSafetyEnabled(false); // STEP 1: Set all of the states. // SAFETY AND SANITY - SET ALL TO ZERO loaded = winchSwitch.Get(); loading = false; intake.Set(0.0); rightWinch.Set(0.0); leftWinch.Set(0.0); // STEP 2: Move forward to optimum shooting position Drive(-AUTO_DRIVE_SPEED, SHOT_POSN_DIST); // STEP 3: Drop the arm for a clean shot arm.Set(DoubleSolenoid::kForward); Wait(1.0); // Ken // STEP 4: Launch the catapult LaunchCatapult(); Wait (1.0); // Ken if (ds->GetDigitalIn(1)) { // STEP 5: Start the intake motor and backup to our origin position to pick up another ball InitiateLoad(); intake.Set(-INTAKE_COLLECT); while (CheckLoad()); Drive(AUTO_DRIVE_SPEED, SHOT_POSN_DIST); Wait(1.0); // For the ball to collect // STEP 6: Shut off the intake, bring up the arm and move to shooting position intake.Set(0.0); arm.Set(DoubleSolenoid::kReverse); Wait (1.0); // "Settle down" Drive(-AUTO_DRIVE_SPEED, SHOT_POSN_DIST); // Step 7: drop the arm for a clean shot and shoot arm.Set(DoubleSolenoid::kForward); Drive(AUTO_DRIVE_SPEED, SHOT_POSN_DIST); // UNTESTED KICKED OFF FIELD Wait(1.0); // For arm to go down LaunchCatapult(); } // Get us fully into the zone for 5 points Drive(-AUTO_DRIVE_SPEED, INTO_ZONE_DIST - SHOT_POSN_DIST); // SAFETY AND SANITY - SET ALL TO ZERO intake.Set(0.0); rightWinch.Set(0.0); leftWinch.Set(0.0); }
/** * Drive left & right motors for 2 seconds, enabled by a jumper (jumper * must be in for autonomous to operate). */ void Autonomous(void) { myRobot->SetSafetyEnabled(false); if (ds->GetDigitalIn(ENABLE_AUTONOMOUS) == 1) // only run the autonomous program if jumper is in place { myRobot->Drive(0.5, 0.0); // drive forwards half speed Wait(2.0); // for 2 seconds myRobot->Drive(0.0, 0.0); // stop robot\ } myRobot->SetSafetyEnabled(true); }
void DashBoardInput() { int i = 0; GetWatchdog().SetEnabled(true); while (IsAutonomous() && IsEnabled()) { i++; GetWatchdog().Feed(); dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "%f, %i", driverStation->GetAnalogIn(1), i); dsLCD->UpdateLCD(); } }
void RA14Robot::logging() { if (fout.is_open()) { fout << missionTimer->Get() << ","; #ifndef DISABLE_SHOOTER myCam->log(fout); #endif //Ends DISABLE_SHOOTER myDrive->log(fout); CurrentSensorSlot * slots[4] = { camMotor1Slot, camMotor2Slot, driveLeftSlot, driveRightSlot }; DriverStation * ds = DriverStation::GetInstance(); for (int i = 0; i < 4; ++i) { fout << slots[i]->Get() << ","; } fout << auto_case << "," << gyro->GetAngle() << "," << dropSensor->GetPosition() << "," << ds->GetBatteryVoltage() << ","; fout << target->IsValid() << "," << target->IsHot() << "," << target->GetDistance() << "," << target->GetX() << ","; fout << target->GetY() << "," << target->IsLeft() << "," << target->IsRight() << ","; fout << ds->GetMatchTime() << "," << auto_state << ","; fout << endl; } }
/** * unchanged from SimpleDemo: * Runs the motors under driver control with either tank or arcade steering selected * by a jumper in DS Digin 0. */ void OperatorControl(void) { DPRINTF(LOG_DEBUG, "OperatorControl"); GetWatchdog().Feed(); while (1) { // determine if tank or arcade mode; default with no jumper is for tank drive if (ds->GetDigitalIn(ARCADE_MODE) == 0) { myRobot->TankDrive(leftStick, rightStick); // drive with tank style } else{ myRobot->ArcadeDrive(rightStick); // drive with arcade style (use right stick) } } } // end operator control
void logdata() { //log data fout << logTimer->Get() << ',' << CurrentMode() << ',' << ds->GetBatteryVoltage() << ','; fout << magicBox->getGyroAngle() << ','; autoLog(); myDrive->log(fout); myShooter->log(fout); //myClimber->Log(fout); log_gamepad(fout, driverGamepad); log_gamepad(fout, operatorGamepad); fout<<LEDTimer<<","; fout<<LEDPercent(LEDTimer)<<","; fout<<LEDPercent(LEDTimer + 30)<<","; fout<<endl; }
Robot(void) : dseio(ds->GetInstance()->GetEnhancedIO()) { this->SetPeriod(.05); FL = new Jaguar(PWM_PORT_5); RL = new Jaguar(PWM_PORT_7); FR = new Jaguar(PWM_PORT_4); RR = new Jaguar(PWM_PORT_6); mygyro = new Gyro(AI_PORT_1); drive = new RobotDrive(FL,RL,FR,RR); drive->SetInvertedMotor(RobotDrive::kFrontRightMotor, true); drive->SetInvertedMotor(RobotDrive::kRearRightMotor,true); drive->SetInvertedMotor(RobotDrive::kFrontLeftMotor, true);//inversed because motor physically spins in the rwrong direction drive->SetInvertedMotor(RobotDrive::kRearLeftMotor, false); drive->SetExpiration(15); left = new DigitalInput(DI_PORT_1); middle = new DigitalInput(DI_PORT_2); right = new DigitalInput(DI_PORT_3); StraightLineSwitch = new DigitalInput(DI_PORT_5); GoLeftSwitch = new DigitalInput(DI_PORT_6); robot_compressor = new Compressor(DI_PORT_4,DO_PORT_1); rightStick = new Joystick(1); leftStick = new Joystick(2); myarm = new DDCArm(); deploy = new Deployment(); autotimer = new Timer(); }
/** * Drive left & right motors for 2 seconds then stop */ void Autonomous(void) { Saftey->SetEnabled(false); //myRobot->SetSafetyEnabled(false); //myRobot->Drive(0.5, 0.0); // drive forwards half speed dsLCD = DriverStationLCD::GetInstance(); dsLCD->Clear(); //dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Hello World" ); //dsLCD->UpdateLCD(); Wait(0.5); ds = DriverStation::GetInstance(); switch(ds->GetLocation()) { case 1: //Execute Autonomous code #1 dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Executing Autonomous 1"); break; case 2: dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Executing Autonomous 2"); //Execute Autonomous code #2 break; case 3: dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Executing Autonomous 3"); //Execute Autonomous code #3 break; } dsLCD->UpdateLCD(); Saftey->SetEnabled(false); Wait(0.5); // for 2 seconds delete Jaguar1; delete Jaguar2; delete Saftey; delete dsLCD; delete ds; }
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; } }
/** * Robot-wide initialization code should go here. * * Use this method for default Robot-wide initialization which will * be called when the robot is first powered on. It will be called exactly 1 time. */ void RobotInit() { printf(">>> RobotInit\n"); LogInit(); #ifdef HAVE_COMPRESSOR compressor = new Compressor(1, 1); #endif #ifdef HAVE_TOP_WHEEL #ifdef HAVE_TOP_CAN1 topWheel1 = new CANJaguar(1); topWheel1->SetSafetyEnabled(false); // motor safety off while configuring topWheel1->SetSpeedReference( CANJaguar::kSpeedRef_Encoder ); topWheel1->ConfigEncoderCodesPerRev( 1 ); #endif #ifdef HAVE_TOP_PWM1 topWheel1 = new Victor(1); topWheel1->SetSafetyEnabled(false); // motor safety off while configuring #endif #ifdef HAVE_TOP_CAN2 topWheel2 = new CANJaguar(2); topWheel2->SetSafetyEnabled(false); // motor safety off while configuring topWheel2->SetSpeedReference( CANJaguar::kSpeedRef_Encoder ); topWheel2->ConfigEncoderCodesPerRev( 1 ); #endif topTach = new Tachometer(2); #endif #ifdef HAVE_BOTTOM_WHEEL #ifdef HAVE_BOTTOM_CAN1 bottomWheel1 = new CANJaguar(3); bottomWheel1->SetSafetyEnabled(false); // motor safety off while configuring bottomWheel1->SetSpeedReference( CANJaguar::kSpeedRef_Encoder ); bottomWheel1->ConfigEncoderCodesPerRev( 1 ); #endif #ifdef HAVE_BOTTOM_PWM1 bottomWheel1 = new Victor(2); bottomWheel1->SetSafetyEnabled(false); // motor safety off while configuring #endif #ifdef HAVE_BOTTOM_CAN2 bottomWheel2 = new CANJaguar(4); bottomWheel2->SetSafetyEnabled(false); // motor safety off while configuring bottomWheel2->SetSpeedReference( CANJaguar::kSpeedRef_Encoder ); bottomWheel2->ConfigEncoderCodesPerRev( 1 ); #endif bottomTach = new Tachometer(3); #endif #ifdef HAVE_ARM arm = new DoubleSolenoid(2, 1); #endif #ifdef HAVE_INJECTOR injectorL = new DoubleSolenoid(5, 3); injectorR = new DoubleSolenoid(6, 4); #endif #ifdef HAVE_EJECTOR ejector = new Solenoid(7); #endif #ifdef HAVE_LEGS legs = new Solenoid(8); #endif ds = DriverStation::GetInstance(); eio = &ds->GetEnhancedIO(); gamepad = new Joystick(1); LiveWindow *lw = LiveWindow::GetInstance(); #ifdef HAVE_COMPRESSOR lw->AddActuator("K9", "Compressor", compressor); #endif #ifdef HAVE_TOP_WHEEL #ifdef HAVE_TOP_CAN1 lw->AddActuator("K9", "Top1", topWheel1); #endif #ifdef HAVE_TOP_PWM1 lw->AddActuator("K9", "Top1", topWheel1); #endif #ifdef HAVE_TOP_CAN2 lw->AddActuator("K9", "Top2", topWheel2); #endif #endif #ifdef HAVE_BOTTOM_WHEEL #ifdef HAVE_BOTTOM_CAN1 lw->AddActuator("K9", "Bottom1", bottomWheel1); #endif #ifdef HAVE_BOTTOM_PWM1 lw->AddActuator("K9", "Bottom1", bottomWheel1); #endif #ifdef HAVE_BOTTOM_CAN2 lw->AddActuator("K9", "Bottom2", bottomWheel2); #endif #endif #ifdef HAVE_ARM lw->AddActuator("K9", "Arm", arm); #endif #ifdef HAVE_INJECTOR lw->AddActuator("K9", "InjectorL", injectorL); lw->AddActuator("K9", "InjectorR", injectorR); #endif #ifdef HAVE_EJECTOR lw->AddActuator("K9", "Ejector", ejector); #endif #ifdef HAVE_LEGS lw->AddActuator("K9", "Legs", legs); #endif SmartDashboard::PutNumber("Shooter P", kP); SmartDashboard::PutNumber("Shooter I", kI); SmartDashboard::PutNumber("Shooter D", kD); spinFastNow = false; #ifdef HAVE_TOP_WHEEL SmartDashboard::PutNumber("Top Set ", topSpeed); #ifdef HAVE_TOP_CAN1 SmartDashboard::PutNumber("Top Current 1", 0.0); #endif #ifdef HAVE_TOP_CAN2 SmartDashboard::PutNumber("Top Current 2", 0.0); SmartDashboard::PutNumber("Top Jag ", 0.0); #endif SmartDashboard::PutNumber("Top Tach ", 0.0); #endif #ifdef HAVE_BOTTOM_WHEEL SmartDashboard::PutNumber("Bottom Set ", bottomSpeed); #ifdef HAVE_BOTTOM_CAN1 SmartDashboard::PutNumber("Bottom Current 1", 0.0); #endif #ifdef HAVE_BOTTOM_CAN2 SmartDashboard::PutNumber("Bottom Current 2", 0.0); SmartDashboard::PutNumber("Bottom Jag ", 0.0); #endif SmartDashboard::PutNumber("Bottom Tach ", 0.0); #endif SetPeriod(0); //Set update period to sync with robot control packets (20ms nominal) printf("<<< RobotInit\n"); }
void DashboardSender::SendData(Robot1073 *p) { static unsigned int packetCt = 0; float tempFloat; DriverStation *ds = DriverStation::GetInstance(); Dashboard &dash = ds->GetHighPriorityDashboardPacker(); dash.AddCluster(); dash.AddU32(packetCt++); dash.AddU32(0xFFFFFFFF); unsigned short packData = 1; if(p->IsEnabled()){packData += 2;} if(false){packData += 4;} // has tube if(p->IsOperatorControl()){packData += 8;} else if(p->IsAutonomous()){packData += 16;} else {packData += 24;} if(false){packData += 32;} // has line if(p->arm->IsUpLimitSwitchActive()){packData += 64;} if(p->arm->IsDownLimitSwitchActive()){packData += 128;} if(p->pincer->IsClosedLimitSwitchActive()){packData += 256;} if(p->pincer->IsOpenLimitSwitchActive()){packData += 512;} if(p->leftLineSensor->Get()){ packData += 1024; } if(p->middleLineSensor->Get()){packData += 2048; } if(p->rightLineSensor->Get()){packData += 4096; } if(p->kraken->GetMode() != p->kraken->IdleMode ) { packData += 1 << 13; } dash.AddU16(packData); //printf("left = %d middle = %d right = %d\n", p->leftLineSensor->Get(),p->middleLineSensor->Get(),p->rightLineSensor->Get()); for (int i = 1; i <= 8; i++) { tempFloat = (float) AnalogModule::GetInstance(1)->GetAverageVoltage(i); dash.AddFloat(tempFloat); // analogs //printf("Float %d = %fV\n", i, tempFloat); } int module = 1; DigitalModule * digitalModule = DigitalModule::GetInstance(module); unsigned char relayForward = digitalModule->GetRelayForward(); dash.AddU8(relayForward); // relays (forward) dash.AddU8(DigitalModule::GetInstance(module)->GetRelayReverse()); // relays (reverse) dash.AddU16((short)DigitalModule::GetInstance(module)->GetDIO()); // state dash.AddU16(DigitalModule::GetInstance(module)->GetDIODirection());//direction for (int i = 1; i <= 10; i++) { dash.AddU8((unsigned char) DigitalModule::GetInstance(module)->GetPWM(i)); // pwm's //printf("PWM %d %02X\n" ,i, DigitalModule::GetInstance(module)->GetPWM(i)); } dash.AddFloat(p->matchTimer->GetTimeRemaining()); dash.AddFloat(ds->GetBatteryVoltage()); dash.AddFloat(p->gyro->GetAngle()); dash.AddFloat(p->leftJoystick->GetX()); dash.AddFloat(p->rightJoystick->GetX()); dash.AddFloat(p->leftJoystick->GetY()); dash.AddFloat(p->rightJoystick->GetY()); std::pair<double, double> lrDistance = p->encoders->GetDistance(); dash.AddFloat((float)lrDistance.first); dash.AddFloat((float)lrDistance.second); // Navigation Data dash.AddFloat(p->navigation->GetX()); dash.AddFloat(p->navigation->GetY()); dash.AddFloat(5);//accel temp dash.AddFloat(5);//accel temp dash.AddFloat(p->navigation->GetXVelocity()); dash.AddFloat(p->navigation->GetYVelocity()); dash.AddFloat(sqrt(p->navigation->GetXVar())); dash.AddFloat(sqrt(p->navigation->GetYVar())); dash.AddFloat(p->navigation->GetHeading()); dash.AddFloat(p->elevator->GetCurrentPositionFeet()); dash.AddFloat(p->elevator->GetTargetPositionFeet()); // jags++ //+++++++++++++++++++++++ dash.AddFloat((float)p->leftMotorJaguar->GetOutputCurrent()); dash.AddFloat((float)p->rightMotorJaguar->GetOutputCurrent()); dash.AddFloat((float)p->pincerJaguar->GetOutputCurrent()); dash.AddFloat((float)p->armJaguar->GetOutputCurrent()); dash.AddFloat((float)p->elevatorJaguarMotorA->GetOutputCurrent()); dash.AddFloat(35); // pincher % open dash.AddU32(dashboardIndex); dash.AddFloat(p->matchTimer->GetElapsedTime()); dash.AddFloat((float)p->systemTimer->Get()); int x = p->GetTargetPole(); int y = (p->GetTargetFoot()+1)/2; dash.AddU8(x); dash.AddU8(y); dash.AddFloat(p->navigation->GetHeadingToPeg(x)); dash.AddFloat(p->navigation->GetHeadingToBait(x)); dash.AddFloat(p->navigation->GetDistanceToPeg(x)); dash.AddFloat(p->navigation->GetDistanceToBait(x)); dash.AddFloat(p->magEncoder->GetVoltage()); // dash.AddFloat(p->arm->magEncoder->GetVoltage()); dash.AddFloat(42.0); dash.AddU32(0xDCBA25); // temporary test data to make sure data is aligned dash.FinalizeCluster(); dash.Finalize(); }
float Cypress::GetAnalogIn(int channel) { return m_ds->GetEnhancedIO().GetAnalogIn(channel); }
void Cypress::SetDigitalOut(int channel, bool value) { m_ds->GetEnhancedIO().SetDigitalOutput(channel, value); }
void Arm::DriveArm() { float claw_target= CLAW_OPEN; DriverStation *ds = DriverStation::GetInstance(); if(ds->GetDigitalIn(3) && !ds->GetDigitalIn(4)) { float tower_target = (ds->GetEnhancedIO().GetAnalogInRatio(1) * (TOWER_MAX - TOWER_MIN)) + TOWER_MIN; float wrist_target = (ds->GetEnhancedIO().GetAnalogInRatio(3) * (WRIST_MAX-WRIST_MIN)) + WRIST_MIN; if (tower_target > TOWER_MAX) tower_target = TOWER_MAX; else if (tower_target < TOWER_MIN) tower_target = TOWER_MIN; if (wrist_target < WRIST_MIN) wrist_target = WRIST_MIN; if (wrist_target > WRIST_MAX) wrist_target = WRIST_MAX; if (ds->GetDigitalIn(1)) { claw_target = CLAW_CLOSED; } float tower_voltage = towerPID->GetOutput(GetTowerPot(), tower_target, 0, .03) * -1; float claw_voltage = clawPID->GetOutput(GetClawPot(), claw_target, 0, .1); float wrist_voltage = wristPID->GetOutput(GetWristPot(), wrist_target, 0, .2) * -1; if(!towerLimit->Get() && tower_voltage < 0) { tower_voltage = 0; towerMotor->Set(0); } if(fabs(claw_voltage) > .5) wrist_voltage = 0; towerMotor->Set(tower_voltage); wristMotor->Set(wrist_voltage); clawMotor->Set(claw_voltage); } else if(ds->GetDigitalIn(3) && ds->GetDigitalIn(4)) { float tower_voltage = towerPID->GetOutput(GetTowerPot(), TOWER_AUTO_DOWN, 0, .03) * -1; float claw_voltage = clawPID->GetOutput(GetClawPot(), CLAW_OPEN, 0, .1); float wrist_voltage = wristPID->GetOutput(GetWristPot(), WRIST_AUTO_DOWN, 0, .2) * -1; towerMotor->Set(tower_voltage); wristMotor->Set(wrist_voltage); clawMotor->Set(claw_voltage); } else if(!ds->GetDigitalIn(3) && ds->GetDigitalIn(4)) { float tower_voltage = towerPID->GetOutput(GetTowerPot(), TOWER_AUTO_UP, 0, .03) * -1; float claw_voltage = clawPID->GetOutput(GetClawPot(), CLAW_CLOSED, 0, .1); float wrist_voltage = wristPID->GetOutput(GetWristPot(), WRIST_AUTO_UP, 0, .2) * -1; towerMotor->Set(tower_voltage); wristMotor->Set(wrist_voltage); clawMotor->Set(claw_voltage); } }
void OperatorControl(void) { autonomous = false; //myRobot.SetSafetyEnabled(false); //myRobot.SetInvertedMotor(kFrontLeftMotor, true); // myRobot.SetInvertedMotor(3, true); //variables for great pid double rightSpeed,leftSpeed; float rightSP, leftSP, liftSP, lastLiftSP, gripSP, tempLeftSP, tempRightSP; float stickY[2]; float stickYAbs[2]; bool lightOn = false; AxisCamera &camera = AxisCamera::GetInstance(); camera.WriteResolution(AxisCameraParams::kResolution_160x120); camera.WriteMaxFPS(5); camera.WriteBrightness(50); camera.WriteRotation(AxisCameraParams::kRotation_0); rightEncoder->Start(); leftEncoder->Start(); liftEncoder->Start(); rightEncoder->Reset(); leftEncoder->Reset(); liftEncoder->Reset(); bool fastest = false; //transmission mode float reduction = 1; //gear reduction from bool bDrivePID = false; //float maxSpeed = 500; float liftPower = 0; float liftPos = -10; bool disengageBrake = false; int count = 0; //int popCount = 0; double popStart = 0; double popTime = 0; int popStage = 0; int liftCount = 0; int liftCount2 = 0; const float LOG17 = log(17.61093344); float liftPowerAbs = 0; float gripError = 0; float gripErrorAbs = 0; float gripPropMod = 0; float gripIntMod = 0; bool shiftHigh = false; leftEncoder->SetDistancePerPulse((6 * PI / 1000)/reduction); //6-inch wheels, 1000 raw counts per revolution, rightEncoder->SetDistancePerPulse((6 * PI / 1000)/reduction); //about 1:1 gear ratio DriverStationEnhancedIO &myEIO = driverStation->GetEnhancedIO(); GetWatchdog().SetEnabled(true); GetWatchdog().SetExpiration(0.3); PIDDriveLeft->SetOutputRange(-1, 1); PIDDriveRight->SetOutputRange(-1, 1); //PIDDriveLeft->SetInputRange(-244,244); //PIDDriveRight->SetInputRange(-244,244); PIDDriveLeft->SetTolerance(5); PIDDriveRight->SetTolerance(5); PIDDriveLeft->SetContinuous(false); PIDDriveRight->SetContinuous(false); //PIDDriveLeft->Enable(); //PIDDriveRight->Enable(); PIDDriveRight->SetPID(DRIVEPROPGAIN, DRIVEINTGAIN, DRIVEDERIVGAIN); PIDDriveLeft->SetPID(DRIVEPROPGAIN, DRIVEINTGAIN, DRIVEDERIVGAIN); liftSP = 0; PIDLift->SetTolerance(10); PIDLift->SetContinuous(false); PIDLift->SetOutputRange(-0.75, 1.); //BUGBUG PIDLift->Enable(); gripSP = 0; float goalGripSP = 0; bool useGoalSP = true; bool gripPIDOn = true; float gripP[10]; float gripI[10]; float gripD[10]; PIDGrip->SetOutputRange(-0.5, 0.28); //Negative goes up PIDGrip->SetTolerance(5); PIDGrip->SetSetpoint(0); PIDGrip->Enable(); miniDep->Set(miniDep->kForward); int i = 0; while(i < 10) { gripP[i] = GRIPPROPGAIN; i++; } i = 0; while(i < 10) { gripI[i] = GRIPINTGAIN; i++; } i = 0; while(i < 10) { gripD[i] = GRIPDERIVGAIN; i++; } while (IsOperatorControl()) { GetWatchdog().Feed(); count++; #if !(SKELETON) sendVisionData(); #endif /* if(LIFTLOW1BUTTON && !(counts%10)) printf("LIFTLOW1BUTTON\n"); if(LIFTLOW2BUTTON && !(counts%10)) printf("LIFTLOW2BUTTON\n"); if(LIFTMID1BUTTON && !(counts%10)) printf("LIFTMID1BUTTON\n"); if(LIFTMID2BUTTON && !(counts%10)) printf("LIFTMID2BUTTON\n"); if(LIFTHIGH1BUTTON && !(counts%10)) printf("LIFTHIGH1BUTTON\n"); if(LIFTHIGH2BUTTON && !(counts%10)) printf("LIFTHIGH2BUTTON\n"); */ /* if(lsLeft->Get()) printf("LSLEFT\n"); if(lsMiddle->Get()) printf("LSMIDDLE\n"); if(lsRight->Get()) printf("LSRIGHT\n"); */ stickY[0] = stickL.GetY(); stickY[1] = stickR.GetY(); stickYAbs[0] = fabs(stickY[0]); stickYAbs[1] = fabs(stickY[1]); if(bDrivePID) { #if 0 frontLeftMotor->Set(stickY[0]); rearLeftMotor->Set(stickY[0]); frontRightMotor->Set(stickY[1]); rearRightMotor->Set(stickY[1]); if(!(count%5)) printf("Speeds: %4.2f %4.2f Outputs: %f %f \n", leftEncoder->GetRate(), rightEncoder->GetRate(), frontLeftMotor->Get(), frontRightMotor->Get()); #endif if(stickYAbs[0] <= 0.05 ) { leftSP = 0; if(!(count%3) && !BACKWARDBUTTON) { PIDDriveLeft->Reset(); PIDDriveLeft->Enable(); } } else leftSP = stickY[0] * stickY[0] * (stickY[0]/stickYAbs[0]); //set points for pid control if(stickYAbs[1] <= 0.05) { rightSP = 0; if(!(count%3) && !BACKWARDBUTTON) { PIDDriveRight->Reset(); PIDDriveRight->Enable(); } } else rightSP = stickY[1] * stickY[1] * (stickY[1]/stickYAbs[1]); if (BACKWARDBUTTON) { tempRightSP = rightSP; tempLeftSP = leftSP; rightSP = -tempLeftSP; leftSP = -tempRightSP; //This line and above line sets opposite values for the controller. ...Theoretically. } PIDDriveLeft->SetSetpoint(leftSP); PIDDriveRight->SetSetpoint(rightSP); leftSpeed = leftEncoder->GetRate(); rightSpeed = rightEncoder->GetRate(); if(!(count++ % 5)) { printf("rate L: %2.2f R: %2.2f SP %2.4f %2.4f ERR %2.2f %2.2f Pow: %1.2f %1.2f\n", leftPIDSource->PIDGet(), rightPIDSource->PIDGet(), leftSP, rightSP, PIDDriveLeft->GetError(), PIDDriveRight->GetError(), frontLeftMotor->Get(), frontRightMotor->Get()); //printf("Throttle value: %f", stickR.GetThrottle()); if(PIDDriveRight->OnTarget()) printf("Right on \n"); if(PIDDriveLeft->OnTarget()) printf("Left on \n"); } if(PIDRESETBUTTON) { //PIDDriveRight->SetPID(stickR.GetThrottle()+1,DRIVEINTGAIN, DRIVEDERIVGAIN); //PIDDriveLeft->SetPID(stickR.GetThrottle()+1,DRIVEINTGAIN, DRIVEDERIVGAIN); PIDDriveLeft->Reset(); PIDDriveRight->Reset(); PIDDriveLeft->Enable(); PIDDriveRight->Enable(); } } else { if(PIDDriveLeft->IsEnabled()) PIDDriveLeft->Reset(); if(PIDDriveRight->IsEnabled()) PIDDriveRight->Reset(); if(DEMOSWITCH) { stickY[0] = stickY[0]*(1 - lift->getPosition()); //reduces power based on lift height stickY[1] = stickY[0]*(1 - lift->getPosition()); } if(stickYAbs[0] > 0.05) { frontLeftMotor->Set(stickY[0]); rearLeftMotor->Set(stickY[0]); } else { frontLeftMotor->Set(0); rearLeftMotor->Set(0); } if(stickYAbs[1] > 0.05) { frontRightMotor->Set(-stickY[1]); rearRightMotor->Set(-stickY[1]); } else { frontRightMotor->Set(0); rearRightMotor->Set(0); } } if(stickL.GetRawButton(2) && stickL.GetRawButton(3) && stickR.GetRawButton(2) && stickR.GetRawButton(3) && BACKWARDBUTTON && !(count%5)) bDrivePID = !bDrivePID; if((SHIFTBUTTON && shiftHigh) || DEMOSWITCH) { shifter->Set(shifter->kReverse); shiftHigh = false; maxSpeed = 12; } else if(!SHIFTBUTTON && !shiftHigh && !DEMOSWITCH) { shifter->Set(shifter->kForward); shiftHigh = true; maxSpeed = 25; //last value 35 } sendIOPortData(); #if !(SKELETON) /* if(LIGHTBUTTON) lightOn = true; else lightOn = false; if(!lightOn) light->Set(light->kOff); if(lightOn) light->Set(light->kOn); */ if(!MODESWITCH) { lastLiftSP = liftSP; if(!PIDLift->IsEnabled()) PIDLift->Enable(); if(LIFTLOW1BUTTON) liftSP = LIFTLOW1; if(LIFTLOW2BUTTON) liftSP = LIFTLOW2; if(LIFTMID1BUTTON) liftSP = LIFTMID1; if(LIFTMID2BUTTON) liftSP = LIFTMID2; if(LIFTHIGH1BUTTON) liftSP = LIFTHIGH1; if(LIFTHIGH2BUTTON && !(DEMOSWITCH && (stickYAbs[0] > 0.05 || stickYAbs[1] > 0.05))) liftSP = LIFTHIGH2; if(!lift->isBraking() && !disengageBrake) { PIDLift->SetSetpoint(liftSP); if(liftSP == 0 && liftPIDSource->PIDGet() < 0.1) //BUGBUG { //PIDLift->SetPID(LIFTPROPGAIN, LIFTINTGAIN, 3*LIFTDERIVGAIN); PIDLift->SetOutputRange(-liftPIDSource->PIDGet() - 0.1, 1.); //BUGBUG } else PIDLift->SetOutputRange(-0.75, 1.); //BUGBUG } if(lift->isBraking() && lastLiftSP != liftSP) { PIDLift->SetSetpoint(lastLiftSP + 0.06); PIDLift->SetPID(12.5 + 1.5*lift->getPosition(), LIFTINTGAIN + 0.6 + 3*lift->getPosition()/10, 0); lift->brakeOff(); disengageBrake = true; if(!liftCount) liftCount = count; } //set brake (because near) if(fabs(PIDLift->GetError()) < 0.01 && !lift->isBraking() && !disengageBrake) { if(liftCount == 0) liftCount = count; if(count - liftCount > 3) { PIDLift->Reset(); liftMotor1->Set(LIFTNEUTRALPOWER); liftMotor2->Set(LIFTNEUTRALPOWER); Wait(0.02); lift->brakeOn(); Wait(0.02); liftMotor1->Set(0.0); liftMotor2->Set(0.0); PIDLift->Enable(); PIDLift->SetSetpoint(lift->getPosition()); liftCount = 0; } //if(!(count%50)) printf("Braking/Not PID \n"); } if(lift->isBraking() && !(count%10)) PIDLift->SetSetpoint(lift->getPosition()); if(fabs(PIDLift->GetError()) < 0.01 && disengageBrake && count - liftCount > 3) { disengageBrake = false; if(liftEncoder->PIDGet() < liftSP) PIDLift->SetPID(LIFTPROPGAIN, LIFTINTGAIN, LIFTDERIVGAIN - 0.015); else PIDLift->SetPID(LIFTPROPGAIN, LIFTINTGAIN, LIFTDERIVGAIN + 0.015); liftCount = 0; } //pid /* else if(!(fabs(PIDLift->GetError()) < 0.04) && !lift->isBraking() && liftPos == -20) { PIDLift->Enable(); liftPos = -10; printf("PID GO PID GO PID GO PID GO PID GO \n"); } */ //when liftPos is positive, measures position //when liftPos = -10, is pidding //when liftPos = -20, has just released brake } else //(MODESWITCH) { if(PIDLift->IsEnabled()) PIDLift->Reset(); liftPower = .8*pow(2*((log(LIFTSLIDER + 0.003/.0208116511)/LOG17) + 0.116), 2)*(2*((log(LIFTSLIDER + 0.003/.0208116511)/LOG17) + 0.116)/fabs(2*((log(LIFTSLIDER + 0.003/.0208116511)/LOG17) + 0.116))); liftPowerAbs = fabs(liftPower); if(liftPowerAbs > 1) liftPower /= liftPowerAbs; //if(!(count%5)) printf("Slider: %f", liftPower); if(lift->isBraking() && liftPowerAbs > 0.05) lift->brakeOff(); else if(!lift->isBraking() && liftPowerAbs <= 0.05 && !(count%5)) lift->brakeOn(); if (liftPowerAbs > 0.05) { liftMotor1->Set(liftPower); liftMotor2->Set(liftPower); } else { liftMotor1->Set(0); liftMotor2->Set(0); } } if(MODESWITCH && LIFTLOW1BUTTON && LIFTMID1BUTTON && LIFTHIGH1BUTTON) liftEncoder->Reset(); /* if(!(count%5)) { printf("Lift pos: %f Lift error: %f Lift SP: %f \n", liftPIDSource->PIDGet(), PIDLift->GetError(), PIDLift->GetSetpoint()); } */ if(!(count%5) && MODESWITCH && GRIPPERPOSUP && GRIPPERPOSDOWN && GRIPPERPOP) { gripPIDOn = !gripPIDOn; printf("Switch'd\n"); } if(gripPIDOn) { if(!PIDGrip->IsEnabled()) PIDGrip->Enable(); if(GRIPPERPOSUP && !GRIPPERPOSDOWN) { goalGripSP = 0; } else if(GRIPPERPOSDOWN && !GRIPPERPOSUP && lift->getPosition() < 0.5) { goalGripSP = 1; } /* else if(!GRIPPERPOSDOWN && !GRIPPERPOSUP) { goalGripSP = 0.5; } */ gripError = PIDGrip->GetError(); gripErrorAbs = fabs(gripError); PIDGrip->SetSetpoint(goalGripSP); if(gripErrorAbs < 0.4) PIDGrip->SetOutputRange(-0.4, 0.6); //negative is up else PIDGrip->SetOutputRange(-0.9, 0.8); //negative is up if(gripErrorAbs > 0.0 && gripErrorAbs < 0.2) { PIDGrip->SetPID(GRIPPROPGAIN - 1.25*(1 - gripErrorAbs) + gripPropMod, GRIPINTGAIN + gripIntMod, 0.3 + 0.1*(1 - gripPIDSource->PIDGet())); } else { PIDGrip->SetPID(GRIPPROPGAIN - 1.*(1 - gripErrorAbs) + gripPropMod, 0, 0.02); } if(fabs(gripPIDSource->PIDGet()) < 0.03 && PIDGrip->GetSetpoint() == 0) { gripLatch1->Set(true); gripLatch2->Set(false); } else if(!(gripLatch1->Get() && PIDGrip->GetSetpoint() == 0) || gripPIDSource->PIDGet() < 0) { gripLatch1->Set(false); gripLatch2->Set(true); } if(gripLatch1->Get() && !(count%20)) { PIDGrip->Reset(); PIDGrip->Enable(); } /* if(stickL.GetRawButton(1) && !(count%5)){ gripIntMod -= 0.001; } if(stickR.GetRawButton(1) &&!(count%5)) { gripIntMod += 0.001; } if(stickL.GetRawButton(2) && !(count%5)) { gripPropMod -= 0.1; } if(stickL.GetRawButton(3) && !(count%5)) { gripPropMod += 0.1; } */ /* if(LIFTBOTTOMBUTTON) { PIDGrip->Reset(); PIDGrip->Enable(); } */ if(!(count%5)) { printf("Gripper pos: %f Gripper error: %f Grip power: %f \n", gripPIDSource->PIDGet(), PIDGrip->GetError(), gripMotor1->Get()); } } //Calibration routine else { if(gripLatch1->Get() == true) { gripLatch1->Set(false); gripLatch2->Set(true); } if(PIDGrip->IsEnabled()) PIDGrip->Reset(); if(GRIPPERPOSUP) { gripMotor1->Set(-0.5); //gripMotor2->Set(0.5); } else if(GRIPPERPOSDOWN) { gripMotor1->Set(0.5); //gripMotor2->Set(-0.5); } else { gripMotor1->Set(0); //gripMotor2->Set(0); } } //if(!(count%5)) printf("Grip volts: %f \n", gripPot->GetVoltage()); //if(!(count%5)) printf("Grip 1 voltage: %f \n", gripMotor1->Get()); if(GRIPPEROPEN && !popStage) { #if !(INNERGRIP) gripOpen1->Set(true); gripOpen2->Set(false); #else gripOpen1->Set(false); gripOpen2->Set(true); #endif } else if(!popStage) { #if !(INNERGRIP) gripOpen1->Set(false); gripOpen2->Set(true); #else gripOpen1->Set(true); gripOpen2->Set(false); #endif } if(GRIPPERPOP && !popStage && goalGripSP == 0 && !(GRIPPEROPEN && GRIPPERCLOSE)) popStage = 1; if(popStage) popTime = GetClock(); if(popStage == 1) { //popCount = count; popStart = GetClock(); popStage++; //printf("POP START POP START POP START \n"); } if(popStage == 2) { #if !(INNERGRIP) gripOpen1->Set(true); gripOpen2->Set(false); #else gripOpen1->Set(false); gripOpen2->Set(true); #endif popStage++; //printf("GRIP OPEN GRIP OPEN GRIP OPEN \n"); } if(popStage == 3 && popTime - popStart > 0.0) //used to be 0.15 { gripPop1->Set(true); gripPop2->Set(false); popStage++; //printf("POP OUT POP OUT POP OUT \n"); } if(popStage == 4 && popTime - popStart > .75) //time was 0.9 { gripPop1->Set(false); gripPop2->Set(true); popStage++; //printf("POP IN POP IN POP IN \n"); } if(popStage == 5 && popTime - popStart > 0.9) popStage = 0; //time was 1.05 if(MINIBOTSWITCH && !(MODESWITCH && stickR.GetRawButton(1) && stickL.GetRawButton(1))) miniDep->Set(miniDep->kReverse); else if(MINIBOTSWITCH && MODESWITCH && stickR.GetRawButton(1) && stickL.GetRawButton(1)) miniDep->Set(miniDep->kOn); #endif if(!compSwitch->Get()) compressor->Set(compressor->kReverse); else compressor->Set(compressor->kOff); /* if(stickR.GetRawButton(1)) compressor->Set(compressor->kReverse); else compressor->Set(compressor->kForward); */ Wait(0.02); // wait for a motor update time } }
/** * 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(); } }
/** * unchanged from SimpleDemo: * * Runs the motors under driver control with either tank or arcade * steering selected by a jumper in DS Digin 0. * * added for vision: * * Adjusts the servo gimbal based on the color tracked. Driving the * robot or operating an arm based on color input from gimbal-mounted * camera is currently left as an exercise for the teams. */ void OperatorControl(void) { char funcName[] = "OperatorControl"; DPRINTF(LOG_DEBUG, "OperatorControl"); //GetWatchdog().Feed(); TrackingThreshold td = GetTrackingData(GREEN, FLUORESCENT); /* for controlling loop execution time */ float loopTime = 0.05; double currentTime = GetTime(); double lastTime = currentTime; double savedImageTimestamp = 0.0; bool foundColor = false; bool staleImage = false; while (IsOperatorControl()) { setServoPositions(rightStick->GetX(), rightStick->GetY()); /* calculate gimbal position based on color found */ if (FindColor (IMAQ_HSL, &td.hue, &td.saturation, &td.luminance, &par, &cReport)) { foundColor = true; if (par.imageTimestamp == savedImageTimestamp) { // This image has been processed already, // so don't do anything for this loop staleImage = true; } else { staleImage = false; savedImageTimestamp = par.imageTimestamp; // compute final H & V destination horizontalDestination = par.center_mass_x_normalized; verticalDestination = par.center_mass_y_normalized; } } else { foundColor = false; } PrintReport(&cReport); if (!staleImage) { if (foundColor) { /* Move the servo a bit each loop toward the * destination. Alternative ways to task servos are * to move immediately vs. incrementally toward the * final destination. Incremental method reduces the * need for calibration of the servo movement while * moving toward the target. */ ShowActivity ("** %s found: Servo: x: %f y: %f", td.name, horizontalDestination, verticalDestination); } else { ShowActivity("** %s not found", td.name); } } dashboardData.UpdateAndSend(); // sleep to keep loop at constant rate // elapsed time can vary significantly due to debug printout currentTime = GetTime(); lastTime = currentTime; if (loopTime > ElapsedTime(lastTime)) { Wait(loopTime - ElapsedTime(lastTime)); } } while (IsOperatorControl()) { // determine if tank or arcade mode; default with no jumper is // for tank drive if (ds->GetDigitalIn(ARCADE_MODE) == 0) { // drive with tank style myRobot->TankDrive(leftStick, rightStick); } else { // drive with arcade style (use right stick) myRobot->ArcadeDrive(rightStick); } } } // end operator control
void RhsRobotBase::StartCompetition() //Robot's main function { DriverStation *pDS = DriverStation::GetInstance(); Init(); //Initialize the robot while(true) { if(!pDS->IsNewControlData()) { Wait(0.002); continue; } //Checks the current state of the robot if(IsDisabled()) { currentRobotState = ROBOT_STATE_DISABLED; } else if(IsEnabled() && IsAutonomous()) { currentRobotState = ROBOT_STATE_AUTONOMOUS; } else if(IsEnabled() && IsOperatorControl()) { currentRobotState = ROBOT_STATE_TELEOPERATED; } else if(IsEnabled() && IsTest()) { currentRobotState = ROBOT_STATE_TEST; } else { currentRobotState = ROBOT_STATE_UNKNOWN; } if(HasStateChanged()) //Checks for state changes { switch(GetCurrentRobotState()) { case ROBOT_STATE_DISABLED: printf("ROBOT_STATE_DISABLED\n"); robotMessage.command = COMMAND_ROBOT_STATE_DISABLED; //robotMessage.robotMode = ROBOT_STATE_DISABLED; break; case ROBOT_STATE_AUTONOMOUS: printf("ROBOT_STATE_AUTONOMOUS\n"); robotMessage.command = COMMAND_ROBOT_STATE_AUTONOMOUS; //robotMessage.robotMode = ROBOT_STATE_AUTONOMOUS; break; case ROBOT_STATE_TELEOPERATED: printf("ROBOT_STATE_TELEOPERATED\n"); robotMessage.command = COMMAND_ROBOT_STATE_TELEOPERATED; //robotMessage.robotMode = ROBOT_STATE_TELEOPERATED; break; case ROBOT_STATE_TEST: printf("ROBOT_STATE_TEST\n"); robotMessage.command = COMMAND_ROBOT_STATE_TEST; //robotMessage.robotMode = ROBOT_STATE_TEST; break; case ROBOT_STATE_UNKNOWN: printf("ROBOT_STATE_UNKNOWN\n"); robotMessage.command = COMMAND_ROBOT_STATE_UNKNOWN; //robotMessage.robotMode = ROBOT_STATE_UNKNOWN; break; } OnStateChange(); //Handles the state change } if(IsEnabled()) { if((currentRobotState == ROBOT_STATE_TELEOPERATED) || (currentRobotState == ROBOT_STATE_TEST) || (currentRobotState == ROBOT_STATE_AUTONOMOUS)) { Run(); //Robot logic } } previousRobotState = currentRobotState; ++loop; //Increment the loop counter } }