/** * Create a new instance of an analog module. * * Create an instance of the analog module object. Initialize all the parameters * to reasonable values on start. * Setting a global value on an analog module can be done only once unless subsequent * values are set the previously set value. * Analog modules are a singleton, so the constructor is never called outside of this class. * * @param slot The slot in the chassis that the module is plugged into. */ AnalogModule::AnalogModule(UINT32 slot) : Module(slot) , m_module (NULL) , m_sampleRateSet (false) , m_numChannelsToActivate (0) { status = 0; AddToSingletonList(); m_module = new tAI(SlotToIndex(slot), &status); SetNumChannelsToActivate(kAnalogChannels); SetSampleRate(kDefaultSampleRate); for (UINT32 i = 0; i < kAnalogChannels; i++) { m_module->writeScanList(i, i, &status); SetAverageBits(i + 1, kDefaultAverageBits); SetOversampleBits(i + 1, kDefaultOversampleBits); } if (m_registerWindowSemaphore == NULL) { // Needs to be global since the protected resource spans both module singletons. m_registerWindowSemaphore = semMCreate(SEM_Q_PRIORITY | SEM_DELETE_SAFE | SEM_INVERSION_SAFE); } wpi_assertCleanStatus(status); }
/** * DriverStation contructor. * * This is only called once the first time GetInstance() is called */ DriverStation::DriverStation() { state = msgs::DriverStationPtr(new msgs::DriverStation()); stateSub = MainNode::Subscribe("~/ds/state", &DriverStation::stateCallback, this); // TODO: for loop + boost bind joysticks[0] = msgs::FRCJoystickPtr(new msgs::FRCJoystick()); joysticksSub[0] = MainNode::Subscribe("~/ds/joysticks/0", &DriverStation::joystickCallback0, this); joysticks[1] = msgs::FRCJoystickPtr(new msgs::FRCJoystick()); joysticksSub[1] = MainNode::Subscribe("~/ds/joysticks/1", &DriverStation::joystickCallback1, this); joysticks[2] = msgs::FRCJoystickPtr(new msgs::FRCJoystick()); joysticksSub[2] = MainNode::Subscribe("~/ds/joysticks/2", &DriverStation::joystickCallback2, this); joysticks[3] = msgs::FRCJoystickPtr(new msgs::FRCJoystick()); joysticksSub[3] = MainNode::Subscribe("~/ds/joysticks/5", &DriverStation::joystickCallback3, this); joysticks[4] = msgs::FRCJoystickPtr(new msgs::FRCJoystick()); joysticksSub[4] = MainNode::Subscribe("~/ds/joysticks/4", &DriverStation::joystickCallback4, this); joysticks[5] = msgs::FRCJoystickPtr(new msgs::FRCJoystick()); joysticksSub[5] = MainNode::Subscribe("~/ds/joysticks/5", &DriverStation::joystickCallback5, this); AddToSingletonList(); }
/** * Create a new instance of an analog module. * * Create an instance of the analog module object. Initialize all the parameters * to reasonable values on start. * Setting a global value on an analog module can be done only once unless subsequent * values are set the previously set value. * Analog modules are a singleton, so the constructor is never called outside of this class. * * @param moduleNumber The analog module to create (1 or 2). */ AnalogModule::AnalogModule(UINT8 moduleNumber) : Module(nLoadOut::kModuleType_Analog, moduleNumber) , m_module (NULL) , m_sampleRateSet (false) , m_numChannelsToActivate (0) { AddToSingletonList(); tRioStatusCode localStatus = NiFpga_Status_Success; m_module = tAI::create(m_moduleNumber - 1, &localStatus); wpi_setError(localStatus); SetNumChannelsToActivate(kAnalogChannels); SetSampleRate(kDefaultSampleRate); for (UINT32 i = 0; i < kAnalogChannels; i++) { m_module->writeScanList(i, i, &localStatus); wpi_setError(localStatus); SetAverageBits(i + 1, kDefaultAverageBits); SetOversampleBits(i + 1, kDefaultOversampleBits); } if (m_registerWindowSemaphore == NULL) { // Needs to be global since the protected resource spans both module singletons. m_registerWindowSemaphore = semMCreate(SEM_Q_PRIORITY | SEM_DELETE_SAFE | SEM_INVERSION_SAFE); } }
/** * Create a new instance of an digital module. * Create an instance of the digital module object. Initialize all the parameters * to reasonable values on start. * Setting a global value on an digital module can be done only once unless subsequent * values are set the previously set value. * Digital modules are a singleton, so the constructor is never called outside of this class. */ DigitalModule::DigitalModule(UINT32 slot) : Module(slot) , m_fpgaDIO (NULL) { Resource::CreateResourceObject(&DIOChannels, tDIO::kNumSystems * kDigitalChannels); m_fpgaDIO = new tDIO(SlotToIndex(m_slot), &status); // Make sure that the 9403 IONode has had a chance to initialize before continuing. while(m_fpgaDIO->readLoopTiming(&status) == 0) taskDelay(1); if (m_fpgaDIO->readLoopTiming(&status) != kExpectedLoopTiming) { wpi_fatal(LoopTimingError); printf("DIO LoopTiming: %d, expecting: %d\n", m_fpgaDIO->readLoopTiming(&status), kExpectedLoopTiming); } m_fpgaDIO->writePWMConfig_Period(PWM::kDefaultPwmPeriod, &status); m_fpgaDIO->writePWMConfig_MinHigh(PWM::kDefaultMinPwmHigh, &status); // Ensure that PWM output values are set to OFF for (UINT32 pwm_index = 1; pwm_index <= kPwmChannels; pwm_index++) { SetPWM(pwm_index, PWM::kPwmDisabled); SetPWMPeriodScale(pwm_index, 3); // Set all to 4x by default. } // Turn off all relay outputs. m_fpgaDIO->writeSlowValue_RelayFwd(0, &status); m_fpgaDIO->writeSlowValue_RelayRev(0, &status); // Create a semaphore to protect changes to the relay values m_relaySemaphore = semMCreate(SEM_Q_PRIORITY | SEM_DELETE_SAFE | SEM_INVERSION_SAFE); AddToSingletonList(); }
Kinect::Kinect() : m_recentPacketNumber(0), m_numberOfPlayers(0) { AddToSingletonList(); m_dataLock = semMCreate(SEM_Q_PRIORITY | SEM_INVERSION_SAFE | SEM_DELETE_SAFE); nUsageReporting::report(nUsageReporting::kResourceType_Kinect, 0); }
/** * DriverStation contructor. * * This is only called once the first time GetInstance() is called */ DriverStation::DriverStation() : m_controlData(NULL) , m_userControl(NULL) , m_userStatus(NULL) , m_digitalOut(0) , m_batteryChannel(NULL) , m_task("DriverStation",(FUNCPTR) DriverStation::InitTask) , m_dashboard(&m_userStatus) { m_controlData = new FRCControlData; m_userControl = new char[USER_CONTROL_DATA_SIZE]; m_userStatus = new char[USER_STATUS_DATA_SIZE]; bzero(m_userStatus, USER_STATUS_DATA_SIZE); // initialize packet number and control words to zero; m_controlData->packetIndex = 0; m_controlData->control = 0; // set all joystick axis values to neutral; buttons to OFF m_controlData->stick0Axis1 = m_controlData->stick0Axis2 = m_controlData->stick0Axis3 = 0; m_controlData->stick1Axis1 = m_controlData->stick1Axis2 = m_controlData->stick1Axis3 = 0; m_controlData->stick2Axis1 = m_controlData->stick2Axis2 = m_controlData->stick2Axis3 = 0; m_controlData->stick3Axis1 = m_controlData->stick3Axis2 = m_controlData->stick3Axis3 = 0; m_controlData->stick0Axis4 = m_controlData->stick0Axis5 = m_controlData->stick0Axis6 = 0; m_controlData->stick1Axis4 = m_controlData->stick1Axis5 = m_controlData->stick1Axis6 = 0; m_controlData->stick2Axis4 = m_controlData->stick2Axis5 = m_controlData->stick2Axis6 = 0; m_controlData->stick3Axis4 = m_controlData->stick3Axis5 = m_controlData->stick3Axis6 = 0; m_controlData->stick0Buttons = 0; m_controlData->stick1Buttons = 0; m_controlData->stick2Buttons = 0; m_controlData->stick3Buttons = 0; // initialize the analog and digital data. m_controlData->analog1 = 0; m_controlData->analog2 = 0; m_controlData->analog3 = 0; m_controlData->analog4 = 0; m_controlData->dsDigitalIn = 0; m_batteryChannel = new AnalogChannel(kBatterySlot, kBatteryChannel); AddToSingletonList(); if (!m_task.Start((INT32) this)) { wpi_fatal(DriverStationTaskError); } }
/** * DriverStationLCD contructor. * * This is only called once the first time GetInstance() is called */ DriverStationLCD::DriverStationLCD() : m_textBuffer (NULL) , m_textBufferSemaphore (NULL) { m_textBuffer = new char[USER_DS_LCD_DATA_SIZE]; memset(m_textBuffer, ' ', USER_DS_LCD_DATA_SIZE); *((UINT16 *)m_textBuffer) = kFullDisplayTextCommand; m_textBufferSemaphore = semMCreate(SEM_DELETE_SAFE | SEM_INVERSION_SAFE); AddToSingletonList(); }
/** * Create a new instance of an digital module. * Create an instance of the digital module object. Initialize all the parameters * to reasonable values on start. * Setting a global value on an digital module can be done only once unless subsequent * values are set the previously set value. * Digital modules are a singleton, so the constructor is never called outside of this class. * * @param moduleNumber The digital module to create (1 or 2). */ DigitalModule::DigitalModule(uint8_t moduleNumber) : Module(nLoadOut::kModuleType_Digital, moduleNumber) , m_fpgaDIO (NULL) { Resource::CreateResourceObject(&DIOChannels, tDIO::kNumSystems * kDigitalChannels); Resource::CreateResourceObject(&DO_PWMGenerators[m_moduleNumber - 1], tDIO::kNumDO_PWMDutyCycleElements); tRioStatusCode localStatus = NiFpga_Status_Success; m_fpgaDIO = tDIO::create(m_moduleNumber - 1, &localStatus); wpi_setError(localStatus); // Make sure that the 9403 IONode has had a chance to initialize before continuing. while(m_fpgaDIO->readLoopTiming(&localStatus) == 0) sleep_ms(1);//taskDelay(1); if (m_fpgaDIO->readLoopTiming(&localStatus) != kExpectedLoopTiming) { char err[128]; sprintf(err, "DIO LoopTiming: %d, expecting: %u\n", m_fpgaDIO->readLoopTiming(&localStatus), kExpectedLoopTiming); wpi_setWPIErrorWithContext(LoopTimingError, err); } //Calculate the length, in ms, of one DIO loop double loopTime = m_fpgaDIO->readLoopTiming(&localStatus)/(kSystemClockTicksPerMicrosecond*1e3); m_fpgaDIO->writePWMConfig_Period((uint16_t) (PWM::kDefaultPwmPeriod/loopTime + .5), &localStatus); m_fpgaDIO->writePWMConfig_MinHigh((uint16_t) ((PWM::kDefaultPwmCenter-PWM::kDefaultPwmStepsDown*loopTime)/loopTime + .5), &localStatus); // Ensure that PWM output values are set to OFF for (uint32_t pwm_index = 1; pwm_index <= kPwmChannels; pwm_index++) { SetPWM(pwm_index, PWM::kPwmDisabled); SetPWMPeriodScale(pwm_index, 3); // Set all to 4x by default. } // Turn off all relay outputs. m_fpgaDIO->writeSlowValue_RelayFwd(0, &localStatus); m_fpgaDIO->writeSlowValue_RelayRev(0, &localStatus); wpi_setError(localStatus); // Create a semaphore to protect changes to the digital output values // m_digitalSemaphore = semMCreate(SEM_Q_PRIORITY | SEM_DELETE_SAFE | SEM_INVERSION_SAFE); // Create a semaphore to protect changes to the relay values // m_relaySemaphore = semMCreate(SEM_Q_PRIORITY | SEM_DELETE_SAFE | SEM_INVERSION_SAFE); // Create a semaphore to protect changes to the DO PWM config // m_doPwmSemaphore = semMCreate(SEM_Q_PRIORITY | SEM_DELETE_SAFE | SEM_INVERSION_SAFE); AddToSingletonList(); }
/** * DriverStation constructor. * * This is only called once the first time GetInstance() is called */ DriverStation::DriverStation() : m_task ("DriverStation", (FUNCPTR)DriverStation::InitTask) , m_newControlData(0) , m_packetDataAvailableMultiWait(0) , m_waitForDataSem(0) , m_userInDisabled(false) , m_userInAutonomous(false) , m_userInTeleop(false) , m_userInTest(false) , m_nextMessageTime(0) { // All joysticks should default to having zero axes, povs and buttons, so // uninitialized memory doesn't get sent to speed controllers. for(unsigned int i = 0; i < kJoystickPorts; i++) { m_joystickAxes[i].count = 0; m_joystickPOVs[i].count = 0; m_joystickButtons[i].count = 0; m_joystickDescriptor[i].isXbox = 0; m_joystickDescriptor[i].type = -1; m_joystickDescriptor[i].name[0] = '\0'; } // Create a new semaphore m_packetDataAvailableMultiWait = initializeMultiWait(); m_newControlData = initializeSemaphore(SEMAPHORE_EMPTY); m_waitForDataSem = initializeMultiWait(); m_waitForDataMutex = initializeMutexNormal(); m_packetDataAvailableMultiWait = initializeMultiWait(); m_packetDataAvailableMutex = initializeMutexNormal(); // Register that semaphore with the network communications task. // It will signal when new packet data is available. HALSetNewDataSem(m_packetDataAvailableMultiWait); AddToSingletonList(); // They need to be identical or it could lead to runtime stack corruption if // the caller and callee push and pop different amounts of data on the stack. static_assert(sizeof(this) == sizeof(uint32_t), "We are passing a pointer through a uint32_t"); if (!m_task.Start((uint32_t)this)) { wpi_setWPIError(DriverStationTaskError); } }
/** * DriverStation constructor. * * This is only called once the first time GetInstance() is called */ DriverStation::DriverStation() { // All joysticks should default to having zero axes, povs and buttons, so // uninitialized memory doesn't get sent to speed controllers. for (unsigned int i = 0; i < kJoystickPorts; i++) { m_joystickAxes[i].count = 0; m_joystickPOVs[i].count = 0; m_joystickButtons[i].count = 0; m_joystickDescriptor[i].isXbox = 0; m_joystickDescriptor[i].type = -1; m_joystickDescriptor[i].name[0] = '\0'; } // Register that semaphore with the network communications task. // It will signal when new packet data is available. HALSetNewDataSem(m_packetDataAvailableCond.native_handle()); AddToSingletonList(); }
/** * DriverStation contructor. * * This is only called once the first time GetInstance() is called */ DriverStation::DriverStation() : m_digitalOut (0) , m_waitForDataSem(0) , m_approxMatchTimeOffset(-1.0) , m_userInDisabled(false) , m_userInAutonomous(false) , m_userInTeleop(false) , m_userInTest(false) { // Create a new semaphore m_waitForDataSem = initializeMultiWait(); m_waitForDataMutex = initializeMutexNormal(); m_stateSemaphore = initializeMutexRecursive(); m_joystickSemaphore = initializeMutexRecursive(); state = msgs::DriverStationPtr(new msgs::DriverStation()); stateSub = MainNode::Subscribe("~/ds/state", &DriverStation::stateCallback, this); // TODO: for loop + boost bind joysticks[0] = msgs::JoystickPtr(new msgs::Joystick()); joysticksSub[0] = MainNode::Subscribe("~/ds/joysticks/0", &DriverStation::joystickCallback0, this); joysticks[1] = msgs::JoystickPtr(new msgs::Joystick()); joysticksSub[1] = MainNode::Subscribe("~/ds/joysticks/1", &DriverStation::joystickCallback1, this); joysticks[2] = msgs::JoystickPtr(new msgs::Joystick()); joysticksSub[2] = MainNode::Subscribe("~/ds/joysticks/2", &DriverStation::joystickCallback2, this); joysticks[3] = msgs::JoystickPtr(new msgs::Joystick()); joysticksSub[3] = MainNode::Subscribe("~/ds/joysticks/5", &DriverStation::joystickCallback3, this); joysticks[4] = msgs::JoystickPtr(new msgs::Joystick()); joysticksSub[4] = MainNode::Subscribe("~/ds/joysticks/4", &DriverStation::joystickCallback4, this); joysticks[5] = msgs::JoystickPtr(new msgs::Joystick()); joysticksSub[5] = MainNode::Subscribe("~/ds/joysticks/5", &DriverStation::joystickCallback5, this); AddToSingletonList(); }
/** * DriverStation constructor. * * This is only called once the first time GetInstance() is called */ DriverStation::DriverStation() : m_task ("DriverStation", (FUNCPTR)DriverStation::InitTask) , m_newControlData(0) , m_packetDataAvailableMultiWait(0) , m_waitForDataSem(0) , m_userInDisabled(false) , m_userInAutonomous(false) , m_userInTeleop(false) , m_userInTest(false) , m_nextMessageTime(0) { // All joysticks should default to having zero axes, povs and buttons, so // uninitialized memory doesn't get sent to speed controllers. for(unsigned int i = 0; i < kJoystickPorts; i++) { m_joystickAxes[i].count = 0; m_joystickPOVs[i].count = 0; m_joystickButtons[i].count = 0; } // Create a new semaphore m_packetDataAvailableMultiWait = initializeMultiWait(); m_newControlData = initializeSemaphore(SEMAPHORE_EMPTY); m_waitForDataSem = initializeMultiWait(); m_waitForDataMutex = initializeMutexNormal(); m_packetDataAvailableMultiWait = initializeMultiWait(); m_packetDataAvailableMutex = initializeMutexNormal(); // Register that semaphore with the network communications task. // It will signal when new packet data is available. HALSetNewDataSem(m_packetDataAvailableMultiWait); AddToSingletonList(); if (!m_task.Start((int32_t)this)) { wpi_setWPIError(DriverStationTaskError); } }
Robot980::Robot980() : m_tcTraction(TC_OFF) , m_psdLeft(NULL) , m_psdRight(NULL) // left and right drive motors , m_pscLeft(new ReversableJaguar(SLOT_PWM_LEFT, CHAN_PWM_LEFT, true)) , m_pscRight(new Jaguar(SLOT_PWM_RIGHT,CHAN_PWM_RIGHT)) // lower and upper drive belts , m_pscLowerBelt(new Jaguar(SLOT_PWM_LOWER, CHAN_PWM_LOWER)) , m_pscUpperBelt(new Jaguar(SLOT_PWM_UPPER, CHAN_PWM_UPPER)) // ball release , m_pscFlap(new Jaguar(SLOT_PWM_FLAP, CHAN_PWM_FLAP)) // sensors , m_pGyro(new Gyro(SLOT_GYRO, CHAN_GYRO)) // encoders on the drive wheels , m_pEncDrvLeft(new Encoder(SLOT_ENC_DRV_LEFT, CHAN_ENC_DRV_LEFT_A, SLOT_ENC_DRV_LEFT, CHAN_ENC_DRV_LEFT_B, false, ENC_SCALE)) , m_pEncDrvRight(new Encoder(SLOT_ENC_DRV_RIGHT, CHAN_ENC_DRV_RIGHT_A, SLOT_ENC_DRV_RIGHT, CHAN_ENC_DRV_RIGHT_B, false, ENC_SCALE)) // encoders on the follow wheels , m_pEncFollowLeft(new Encoder(SLOT_ENC_FOLLOW_LEFT, CHAN_ENC_FOLLOW_LEFT_A, SLOT_ENC_FOLLOW_LEFT, CHAN_ENC_FOLLOW_LEFT_B, false, ENC_SCALE)) , m_pEncFollowRight(new Encoder(SLOT_ENC_FOLLOW_RIGHT, CHAN_ENC_FOLLOW_RIGHT_A, SLOT_ENC_FOLLOW_RIGHT, CHAN_ENC_FOLLOW_RIGHT_B, false, ENC_SCALE)) // Timer used for debugging , m_pTimer(new Timer) , m_pSrvPan(new Servo(CAMERA_SLOT_PAN, CAMERA_CHAN_PAN)) , m_pSrvTilt(new Servo(CAMERA_SLOT_TILT, CAMERA_CHAN_TILT)) , m_pVideoServer(NULL) // , m_pCamControlLoop(new Notifier(Robot980::CallCamUpdate, this)) , m_pCamControlLoop(NULL) , m_dSavedImageTimestamp(0.0) , m_trackColor(DriverStation::kInvalid) { // pi * diameter * gear ratio / encoder ticks / in/ft // 48:32 = 1.5 on drive wheels m_pEncDrvLeft->SetDistancePerPulse(M_PI * 6 * GEAR_RATIO / 250 / 12); m_pEncDrvRight->SetDistancePerPulse(M_PI * 6 * GEAR_RATIO / 250 / 12); m_pEncFollowLeft->SetDistancePerPulse(M_PI * 60/25.4 / 250 / 12); m_pEncFollowRight->SetDistancePerPulse(M_PI * 60/25.4 / 250 / 12); m_pEncDrvLeft->Start(); m_pEncDrvRight->Start(); m_pEncFollowLeft->Start(); m_pEncFollowRight->Start(); m_pTimer->Reset(); m_pTimer->Start(); #define SD_TIME 0.020 // // "Smart Drive" handles PID, slipping, etc // m_psdLeft = new SmartDrive(SD_ID_LEFT, // 0.6, 0.1, // velocity PID constants // 0.1, 0.1, // correction PID constants // 0.2, 0.1, // acceleration PID constants // true, // m_pscLeft, m_pEncDrvLeft, m_pEncFollowLeft, // SD_TIME); // Wait(SD_TIME / 2); // m_psdRight = new SmartDrive(SD_ID_RIGHT, // 0.6, 0.1, // velocity PID constants // 0.1, 0.1, // correction PID constants // 0.2, 0.1, // acceleration PID constants // true, // m_pscRight, m_pEncDrvRight, m_pEncFollowRight, // SD_TIME); // "Smart Drive" handles PID, slipping, etc m_psdLeft = new SmartDrive(SD_ID_LEFT, 0.6, 0.1, // velocity PID constants 1.0, 0.04, // correction PID constants 1.0, 0.0, false, // acceleration PID constants m_pscLeft, m_pEncDrvLeft, m_pEncFollowLeft, SD_TIME); Wait(SD_TIME / 2); m_psdRight = new SmartDrive(SD_ID_RIGHT, 0.6, 0.1, // velocity PID constants 1.0, 0.04, // correction PID constants 1.0, 0.0, false, // acceleration PID constants m_pscRight, m_pEncDrvRight, m_pEncFollowRight, SD_TIME); ParticleAnalysisReport par1, par2; // particle analysis reports memset(&par1, 0, sizeof(ParticleAnalysisReport)); memset(&par2, 0, sizeof(ParticleAnalysisReport)); /* image data for tracking - override default parameters if needed */ /* recommend making PINK the first color because GREEN is more * susceptible to hue variations due to lighting type so may * result in false positives */ // PINK sprintf(m_tdPink.name, "PINK"); m_tdPink.hue.minValue = 220; m_tdPink.hue.maxValue = 255; m_tdPink.saturation.minValue = 75; m_tdPink.saturation.maxValue = 255; m_tdPink.luminance.minValue = 85; m_tdPink.luminance.maxValue = 255; // GREEN sprintf(m_tdGreen.name, "GREEN"); m_tdGreen.hue.minValue = 55; m_tdGreen.hue.maxValue = 125; m_tdGreen.saturation.minValue = 58; m_tdGreen.saturation.maxValue = 255; m_tdGreen.luminance.minValue = 92; m_tdGreen.luminance.maxValue = 255; printf("Robot980 constructor\n"); m_trailerInfo.color = DriverStation::kInvalid; EnableTractionControl(TC_SMART_2); Wait(0.010); EnableTractionControl(TC_LOWPASS); /* start the CameraTask */ StartCameraTask(CAMERA_FPS, CAMERA_COMPRESSION, CAMERA_RESOLUTION, CAMERA_ROTATION); // m_pVideoServer = new PCVideoServer; m_pCamControlLoop->StartPeriodic((double)1.0 / (double)CAMERA_FPS); // tell SensorBase about us AddToSingletonList(); }
/** * The {@link SmartDashboard} class is the bridge between robot programs and the SmartDashboard on the * laptop. * * <p>When a value is put into the SmartDashboard here, it pops up on the SmartDashboard on the laptop. * Users can put values into and get values from the SmartDashboard</p> */ SmartDashboard::SmartDashboard() { AddToSingletonList(); m_table = NetworkTable::GetTable("SmartDashboard"); }