#include "HAL/Digital.hpp" #include "Port.h" #include "HAL/HAL.hpp" #include "ChipObject.h" #include "HAL/cpp/Synchronized.hpp" #include "HAL/cpp/Resource.hpp" #include "NetworkCommunication/LoadOut.h" #include #include #include "i2clib/i2c-lib.h" static const uint32_t kExpectedLoopTiming = 40; static const uint32_t kDigitalPins = 20; static const uint32_t kPwmPins = 20; static const uint32_t kRelayPins = 8; static const uint32_t kNumHeaders = 10; // Number of non-MXP pins /** * kDefaultPwmPeriod is in ms * * - 20ms periods (50 Hz) are the "safest" setting in that this works for all devices * - 20ms periods seem to be desirable for Vex Motors * - 20ms periods are the specified period for HS-322HD servos, but work reliably down * to 10.0 ms; starting at about 8.5ms, the servo sometimes hums and get hot; * by 5.0ms the hum is nearly continuous * - 10ms periods work well for Victor 884 * - 5ms periods allows higher update rates for Luminary Micro Jaguar speed controllers. * Due to the shipping firmware on the Jaguar, we can't run the update period less * than 5.05 ms. * * kDefaultPwmPeriod is the 1x period (5.05 ms). In hardware, the period scaling is implemented as an * output squelch to get longer periods for old devices. */ static const float kDefaultPwmPeriod = 5.05; /** * kDefaultPwmCenter is the PWM range center in ms */ static const float kDefaultPwmCenter = 1.5; /** * kDefaultPWMStepsDown is the number of PWM steps below the centerpoint */ static const int32_t kDefaultPwmStepsDown = 1000; static const int32_t kPwmDisabled = 0; struct DigitalPort { Port port; uint32_t PWMGeneratorID; }; // XXX: Set these back to static once we figure out the memory clobbering issue MUTEX_ID digitalDIOSemaphore = NULL; MUTEX_ID digitalRelaySemaphore = NULL; MUTEX_ID digitalPwmSemaphore = NULL; MUTEX_ID digitalI2COnBoardSemaphore = NULL; MUTEX_ID digitalI2CMXPSemaphore = NULL; tDIO* digitalSystem = NULL; tRelay* relaySystem = NULL; tPWM* pwmSystem = NULL; Resource *DIOChannels = NULL; Resource *DO_PWMGenerators = NULL; bool digitalSystemsInitialized = false; uint8_t i2COnboardObjCount = 0; uint8_t i2CMXPObjCount = 0; uint8_t i2COnBoardHandle = 0; uint8_t i2CMXPHandle = 0; /** * Initialize the digital modules. */ void initializeDigital(int32_t *status) { if (digitalSystemsInitialized) return; // Create a semaphore to protect changes to the digital output values digitalDIOSemaphore = initializeMutexRecursive(); // Create a semaphore to protect changes to the relay values digitalRelaySemaphore = initializeMutexRecursive(); // Create a semaphore to protect changes to the DO PWM config digitalPwmSemaphore = initializeMutexRecursive(); digitalI2COnBoardSemaphore = initializeMutexRecursive(); digitalI2CMXPSemaphore = initializeMutexRecursive(); Resource::CreateResourceObject(&DIOChannels, tDIO::kNumSystems * kDigitalPins); Resource::CreateResourceObject(&DO_PWMGenerators, tDIO::kNumPWMDutyCycleAElements + tDIO::kNumPWMDutyCycleBElements); digitalSystem = tDIO::create(status); // Relay Setup relaySystem = tRelay::create(status); // Turn off all relay outputs. relaySystem->writeValue_Forward(0, status); relaySystem->writeValue_Reverse(0, status); // PWM Setup pwmSystem = tPWM::create(status); // Make sure that the 9403 IONode has had a chance to initialize before continuing. while(pwmSystem->readLoopTiming(status) == 0) delayTicks(1); if (pwmSystem->readLoopTiming(status) != kExpectedLoopTiming) { // TODO: char err[128]; // TODO: sprintf(err, "DIO LoopTiming: %d, expecting: %lu\n", digitalModules[port->module-1]->readLoopTiming(status), kExpectedLoopTiming); *status = LOOP_TIMING_ERROR; // NOTE: Doesn't display the error } //Calculate the length, in ms, of one DIO loop double loopTime = pwmSystem->readLoopTiming(status)/(kSystemClockTicksPerMicrosecond*1e3); pwmSystem->writeConfig_Period((uint16_t) (kDefaultPwmPeriod/loopTime + .5), status); uint16_t minHigh = (uint16_t) ((kDefaultPwmCenter-kDefaultPwmStepsDown*loopTime)/loopTime + .5); pwmSystem->writeConfig_MinHigh(minHigh, status); // printf("MinHigh: %d\n", minHigh); // Ensure that PWM output values are set to OFF for (uint32_t pwm_index = 0; pwm_index < kPwmPins; pwm_index++) { // Initialize port structure DigitalPort* digital_port = new DigitalPort(); digital_port->port.pin = pwm_index; setPWM(digital_port, kPwmDisabled, status); setPWMPeriodScale(digital_port, 3, status); // Set all to 4x by default. } digitalSystemsInitialized = true; } /** * 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). */ void* initializeDigitalPort(void* port_pointer, int32_t *status) { initializeDigital(status); Port* port = (Port*) port_pointer; // Initialize port structure DigitalPort* digital_port = new DigitalPort(); digital_port->port = *port; return digital_port; } bool checkDigitalModule(uint8_t module) { return nLoadOut::getModulePresence(nLoadOut::kModuleType_Digital, module - 1); } bool checkPWMChannel(void* digital_port_pointer) { DigitalPort* port = (DigitalPort*) digital_port_pointer; return port->port.pin < kPwmPins; } bool checkRelayChannel(void* digital_port_pointer) { DigitalPort* port = (DigitalPort*) digital_port_pointer; return port->port.pin < kRelayPins; } /** * Map DIO pin numbers from their physical number (10 to 19) to their position * in the bit field. */ uint32_t remapMXPChannel(uint32_t pin) { if(pin < 14) { return pin - 10; // First four digital headers } else { return pin - 6; // Last 8 digital headers, not counting the SPI pins } } /** * Set a PWM channel to the desired value. The values range from 0 to 255 and the period is controlled * by the PWM Period and MinHigh registers. * * @param channel The PWM channel to set. * @param value The PWM value to set. */ void setPWM(void* digital_port_pointer, unsigned short value, int32_t *status) { DigitalPort* port = (DigitalPort*) digital_port_pointer; checkPWMChannel(port); if(port->port.pin < tPWM::kNumHdrRegisters) { pwmSystem->writeHdr(port->port.pin, value, status); } else { pwmSystem->writeMXP(port->port.pin - tPWM::kNumHdrRegisters, value, status); // Enable special functions on this pin uint32_t bitToSet = 1 << remapMXPChannel(port->port.pin); short specialFunctions = digitalSystem->readEnableMXPSpecialFunction(status); digitalSystem->writeEnableMXPSpecialFunction(specialFunctions | bitToSet, status); } } /** * Get a value from a PWM channel. The values range from 0 to 255. * * @param channel The PWM channel to read from. * @return The raw PWM value. */ unsigned short getPWM(void* digital_port_pointer, int32_t *status) { DigitalPort* port = (DigitalPort*) digital_port_pointer; checkPWMChannel(port); if(port->port.pin < tPWM::kNumHdrRegisters) { return pwmSystem->readHdr(port->port.pin, status); } else { return pwmSystem->readMXP(port->port.pin - tPWM::kNumHdrRegisters, status); } } /** * Set how how often the PWM signal is squelched, thus scaling the period. * * @param channel The PWM channel to configure. * @param squelchMask The 2-bit mask of outputs to squelch. */ void setPWMPeriodScale(void* digital_port_pointer, uint32_t squelchMask, int32_t *status) { DigitalPort* port = (DigitalPort*) digital_port_pointer; checkPWMChannel(port); if(port->port.pin < tPWM::kNumPeriodScaleHdrElements) { pwmSystem->writePeriodScaleHdr(port->port.pin, squelchMask, status); } else { pwmSystem->writePeriodScaleMXP(port->port.pin - tPWM::kNumPeriodScaleHdrElements, squelchMask, status); } } /** * Allocate a DO PWM Generator. * Allocate PWM generators so that they are not accidently reused. * * @return PWM Generator refnum */ void* allocatePWM(int32_t *status) { return allocatePWMWithModule(0, status); } /** * Allocate a DO PWM Generator. * Allocate PWM generators so that they are not accidently reused. * * @return PWM Generator refnum */ void* allocatePWMWithModule(uint8_t module, int32_t *status) { char buf[64]; snprintf(buf, 64, "DO_PWM (Module: %d)", module); uint32_t* val = NULL; *val = DO_PWMGenerators->Allocate(buf); return val; } /** * Free the resource associated with a DO PWM generator. * * @param pwmGenerator The pwmGen to free that was allocated with AllocateDO_PWM() */ void freePWM(void* pwmGenerator, int32_t *status) { freePWMWithModule(0, pwmGenerator, status); } /** * Free the resource associated with a DO PWM generator. * * @param pwmGenerator The pwmGen to free that was allocated with AllocateDO_PWM() */ void freePWMWithModule(uint8_t module, void* pwmGenerator, int32_t *status) { uint32_t id = *((uint32_t*) pwmGenerator); if (id == ~0ul) return; DO_PWMGenerators->Free(id); } /** * Change the frequency of the DO PWM generator. * * The valid range is from 0.6 Hz to 19 kHz. The frequency resolution is logarithmic. * * @param rate The frequency to output all digital output PWM signals on this module. */ void setPWMRate(double rate, int32_t *status) { setPWMRateWithModule(0, rate, status); } /** * Change the frequency of the DO PWM generator. * * The valid range is from 0.6 Hz to 19 kHz. The frequency resolution is logarithmic. * * @param rate The frequency to output all digital output PWM signals on this module. */ void setPWMRateWithModule(uint8_t module, double rate, int32_t *status) { // Currently rounding in the log rate domain... heavy weight toward picking a higher freq. // TODO: Round in the linear rate domain. uint8_t pwmPeriodPower = (uint8_t)(log(1.0 / (pwmSystem->readLoopTiming(status) * 0.25E-6 * rate))/log(2.0) + 0.5); digitalSystem->writePWMPeriodPower(pwmPeriodPower, status); } /** * Configure the duty-cycle of the PWM generator * * @param pwmGenerator The generator index reserved by AllocateDO_PWM() * @param dutyCycle The percent duty cycle to output [0..1]. */ void setPWMDutyCycle(void* pwmGenerator, double dutyCycle, int32_t *status) { setPWMDutyCycleWithModule(0, pwmGenerator, dutyCycle, status); } /** * Configure the duty-cycle of the PWM generator * * @param pwmGenerator The generator index reserved by AllocateDO_PWM() * @param dutyCycle The percent duty cycle to output [0..1]. */ void setPWMDutyCycleWithModule(uint8_t module, void* pwmGenerator, double dutyCycle, int32_t *status) { uint32_t id = *((uint32_t*) pwmGenerator); if (id == ~0ul) return; if (dutyCycle > 1.0) dutyCycle = 1.0; if (dutyCycle < 0.0) dutyCycle = 0.0; float rawDutyCycle = 256.0 * dutyCycle; if (rawDutyCycle > 255.5) rawDutyCycle = 255.5; { Synchronized sync(digitalPwmSemaphore); uint8_t pwmPeriodPower = digitalSystem->readPWMPeriodPower(status); if (pwmPeriodPower < 4) { // The resolution of the duty cycle drops close to the highest frequencies. rawDutyCycle = rawDutyCycle / pow(2.0, 4 - pwmPeriodPower); } digitalSystem->writePWMDutyCycleA(id, (uint8_t)rawDutyCycle, status); digitalSystem->writePWMDutyCycleB(id, (uint8_t)rawDutyCycle, status); } } /** * Configure which DO channel the PWM siganl is output on * * @param pwmGenerator The generator index reserved by AllocateDO_PWM() * @param channel The Digital Output channel to output on */ void setPWMOutputChannel(void* pwmGenerator, uint32_t pin, int32_t *status) { setPWMOutputChannelWithModule(0, pwmGenerator, pin, status); } /** * Configure which DO channel the PWM siganl is output on * * @param pwmGenerator The generator index reserved by AllocateDO_PWM() * @param channel The Digital Output channel to output on */ void setPWMOutputChannelWithModule(uint8_t module, void* pwmGenerator, uint32_t pin, int32_t *status) { uint32_t id = *((uint32_t*) pwmGenerator); if (id == ~0ul) return; switch(id) { case 0: digitalSystem->writePWMOutputSelect(0, pin, status); break; case 1: digitalSystem->writePWMOutputSelect(1, pin, status); break; case 2: digitalSystem->writePWMOutputSelect(2, pin, status); break; case 3: digitalSystem->writePWMOutputSelect(3, pin, status); break; case 4: digitalSystem->writePWMOutputSelect(4, pin, status); break; case 5: digitalSystem->writePWMOutputSelect(5, pin, status); break; } } /** * Set the state of a relay. * Set the state of a relay output to be forward. Relays have two outputs and each is * independently set to 0v or 12v. */ void setRelayForward(void* digital_port_pointer, bool on, int32_t *status) { DigitalPort* port = (DigitalPort*) digital_port_pointer; checkRelayChannel(port); { Synchronized sync(digitalRelaySemaphore); uint8_t forwardRelays = relaySystem->readValue_Forward(status); if (on) forwardRelays |= 1 << port->port.pin; else forwardRelays &= ~(1 << port->port.pin); relaySystem->writeValue_Forward(forwardRelays, status); } } /** * Set the state of a relay. * Set the state of a relay output to be reverse. Relays have two outputs and each is * independently set to 0v or 12v. */ void setRelayReverse(void* digital_port_pointer, bool on, int32_t *status) { DigitalPort* port = (DigitalPort*) digital_port_pointer; checkRelayChannel(port); { Synchronized sync(digitalRelaySemaphore); uint8_t reverseRelays = relaySystem->readValue_Reverse(status); if (on) reverseRelays |= 1 << port->port.pin; else reverseRelays &= ~(1 << port->port.pin); relaySystem->writeValue_Reverse(reverseRelays, status); } } /** * Get the current state of the forward relay channel */ bool getRelayForward(void* digital_port_pointer, int32_t *status) { DigitalPort* port = (DigitalPort*) digital_port_pointer; uint8_t forwardRelays = relaySystem->readValue_Forward(status); return (forwardRelays & (1 << port->port.pin)) != 0; } /** * Get the current state of the reverse relay channel */ bool getRelayReverse(void* digital_port_pointer, int32_t *status) { DigitalPort* port = (DigitalPort*) digital_port_pointer; uint8_t reverseRelays = relaySystem->readValue_Reverse(status); return (reverseRelays & (1 << port->port.pin)) != 0; } /** * Allocate Digital I/O channels. * Allocate channels so that they are not accidently reused. Also the direction is set at the * time of the allocation. * * @param channel The Digital I/O channel * @param input If true open as input; if false open as output * @return Was successfully allocated */ bool allocateDIO(void* digital_port_pointer, bool input, int32_t *status) { DigitalPort* port = (DigitalPort*) digital_port_pointer; char buf[64]; snprintf(buf, 64, "DIO %d (Module %d)", port->port.pin, port->port.module); if (DIOChannels->Allocate(kDigitalPins * (port->port.module - 1) + port->port.pin, buf) == ~0ul) return false; { Synchronized sync(digitalDIOSemaphore); tDIO::tOutputEnable outputEnable = digitalSystem->readOutputEnable(status); if(port->port.pin < kNumHeaders) { uint32_t bitToSet = 1 << port->port.pin; if (input) { outputEnable.Headers = outputEnable.Headers & (~bitToSet); // clear the bit for read } else { outputEnable.Headers = outputEnable.Headers | bitToSet; // set the bit for write } } else { uint32_t bitToSet = 1 << remapMXPChannel(port->port.pin); // Disable special functions on this pin short specialFunctions = digitalSystem->readEnableMXPSpecialFunction(status); digitalSystem->writeEnableMXPSpecialFunction(specialFunctions & ~bitToSet, status); if (input) { outputEnable.MXP = outputEnable.MXP & (~bitToSet); // clear the bit for read } else { outputEnable.MXP = outputEnable.MXP | bitToSet; // set the bit for write } } digitalSystem->writeOutputEnable(outputEnable, status); } return true; } /** * Free the resource associated with a digital I/O channel. * * @param channel The Digital I/O channel to free */ void freeDIO(void* digital_port_pointer, int32_t *status) { DigitalPort* port = (DigitalPort*) digital_port_pointer; DIOChannels->Free(kDigitalPins * (port->port.module - 1) + port->port.pin); } /** * Write a digital I/O bit to the FPGA. * Set a single value on a digital I/O channel. * * @param channel The Digital I/O channel * @param value The state to set the digital channel (if it is configured as an output) */ void setDIO(void* digital_port_pointer, short value, int32_t *status) { DigitalPort* port = (DigitalPort*) digital_port_pointer; if (value != 0 && value != 1) { if (value != 0) value = 1; } { Synchronized sync(digitalDIOSemaphore); tDIO::tDO currentDIO = digitalSystem->readDO(status); if(port->port.pin < kNumHeaders) { if(value == 0) { currentDIO.Headers = currentDIO.Headers & ~(1 << port->port.pin); } else if (value == 1) { currentDIO.Headers = currentDIO.Headers | (1 << port->port.pin); } } else { if(value == 0) { currentDIO.MXP = currentDIO.MXP & ~(1 << remapMXPChannel(port->port.pin)); } else if (value == 1) { currentDIO.MXP = currentDIO.MXP | (1 << remapMXPChannel(port->port.pin)); } uint32_t bitToSet = 1 << remapMXPChannel(port->port.pin); short specialFunctions = digitalSystem->readEnableMXPSpecialFunction(status); digitalSystem->writeEnableMXPSpecialFunction(specialFunctions & ~bitToSet, status); } digitalSystem->writeDO(currentDIO, status); } } /** * Read a digital I/O bit from the FPGA. * Get a single value from a digital I/O channel. * * @param channel The digital I/O channel * @return The state of the specified channel */ bool getDIO(void* digital_port_pointer, int32_t *status) { DigitalPort* port = (DigitalPort*) digital_port_pointer; tDIO::tDI currentDIO = digitalSystem->readDI(status); //Shift 00000001 over channel-1 places. //AND it against the currentDIO //if it == 0, then return false //else return true if(port->port.pin < kNumHeaders) { return ((currentDIO.Headers >> port->port.pin) & 1) != 0; } else { // Disable special functions uint32_t bitToSet = 1 << remapMXPChannel(port->port.pin); short specialFunctions = digitalSystem->readEnableMXPSpecialFunction(status); digitalSystem->writeEnableMXPSpecialFunction(specialFunctions & ~bitToSet, status); return ((currentDIO.MXP >> remapMXPChannel(port->port.pin)) & 1) != 0; } } /** * Read the direction of a the Digital I/O lines * A 1 bit means output and a 0 bit means input. * * @param channel The digital I/O channel * @return The direction of the specified channel */ bool getDIODirection(void* digital_port_pointer, int32_t *status) { DigitalPort* port = (DigitalPort*) digital_port_pointer; tDIO::tOutputEnable currentOutputEnable = digitalSystem->readOutputEnable(status); //Shift 00000001 over port->port.pin-1 places. //AND it against the currentOutputEnable //if it == 0, then return false //else return true if(port->port.pin < kNumHeaders) { return ((currentOutputEnable.Headers >> port->port.pin) & 1) != 0; } else { return ((currentOutputEnable.MXP >> remapMXPChannel(port->port.pin)) & 1) != 0; } } /** * Generate a single pulse. * Write a pulse to the specified digital output channel. There can only be a single pulse going at any time. * * @param channel The Digital Output channel that the pulse should be output on * @param pulseLength The active length of the pulse (in seconds) */ void pulse(void* digital_port_pointer, double pulseLength, int32_t *status) { DigitalPort* port = (DigitalPort*) digital_port_pointer; tDIO::tPulse pulse; if(port->port.pin < kNumHeaders) { pulse.Headers = 1 << port->port.pin; } else { pulse.MXP = 1 << remapMXPChannel(port->port.pin); } digitalSystem->writePulseLength((uint8_t)(1.0e9 * pulseLength / (pwmSystem->readLoopTiming(status) * 25)), status); digitalSystem->writePulse(pulse, status); } /** * Check a DIO line to see if it is currently generating a pulse. * * @return A pulse is in progress */ bool isPulsing(void* digital_port_pointer, int32_t *status) { DigitalPort* port = (DigitalPort*) digital_port_pointer; tDIO::tPulse pulseRegister = digitalSystem->readPulse(status); if(port->port.pin < kNumHeaders) { return (pulseRegister.Headers & (1 << port->port.pin)) != 0; } else { return (pulseRegister.MXP & (1 << remapMXPChannel(port->port.pin))) != 0; } } /** * Check if any DIO line is currently generating a pulse. * * @return A pulse on some line is in progress */ bool isAnyPulsing(int32_t *status) { return isAnyPulsingWithModule(1, status) || isAnyPulsingWithModule(2, status); } /** * Check if any DIO line is currently generating a pulse. * * @return A pulse on some line is in progress */ bool isAnyPulsingWithModule(uint8_t module, int32_t *status) { tDIO::tPulse pulseRegister = digitalSystem->readPulse(status); return pulseRegister.Headers != 0 && pulseRegister.MXP != 0; } struct counter_t { tCounter* counter; uint32_t index; }; typedef struct counter_t Counter; static Resource *counters = NULL; void* initializeCounter(Mode mode, uint32_t *index, int32_t *status) { Resource::CreateResourceObject(&counters, tCounter::kNumSystems); *index = counters->Allocate("Counter"); if (*index == ~0ul) { *status = NO_AVAILABLE_RESOURCES; return NULL; } Counter* counter = new Counter(); counter->counter = tCounter::create(*index, status); counter->counter->writeConfig_Mode(mode, status); counter->counter->writeTimerConfig_AverageSize(1, status); counter->index = *index; return counter; } void freeCounter(void* counter_pointer, int32_t *status) { if (counter_pointer != NULL) { Counter* counter = (Counter*) counter_pointer; delete counter->counter; counters->Free(counter->index); } else { *status = NULL_PARAMETER; } } void setCounterAverageSize(void* counter_pointer, int32_t size, int32_t *status) { Counter* counter = (Counter*) counter_pointer; counter->counter->writeTimerConfig_AverageSize(size, status); } /** * Set the source object that causes the counter to count up. * Set the up counting DigitalSource. */ void setCounterUpSourceWithModule(void* counter_pointer, uint8_t module, uint32_t pin, bool analogTrigger, int32_t *status) { Counter* counter = (Counter*) counter_pointer; counter->counter->writeConfig_UpSource_Module(module, status); counter->counter->writeConfig_UpSource_Channel(pin, status); counter->counter->writeConfig_UpSource_AnalogTrigger(analogTrigger, status); if(counter->counter->readConfig_Mode(status) == kTwoPulse || counter->counter->readConfig_Mode(status) == kExternalDirection) { setCounterUpSourceEdge(counter_pointer, true, false, status); } counter->counter->strobeReset(status); } /** * Set the edge sensitivity on an up counting source. * Set the up source to either detect rising edges or falling edges. */ void setCounterUpSourceEdge(void* counter_pointer, bool risingEdge, bool fallingEdge, int32_t *status) { Counter* counter = (Counter*) counter_pointer; counter->counter->writeConfig_UpRisingEdge(risingEdge, status); counter->counter->writeConfig_UpFallingEdge(fallingEdge, status); } /** * Disable the up counting source to the counter. */ void clearCounterUpSource(void* counter_pointer, int32_t *status) { Counter* counter = (Counter*) counter_pointer; bool state = counter->counter->readConfig_Enable(status); counter->counter->writeConfig_Enable(false, status); counter->counter->writeConfig_UpFallingEdge(false, status); counter->counter->writeConfig_UpRisingEdge(false, status); // Index 0 of digital is always 0. counter->counter->writeConfig_UpSource_Channel(0, status); counter->counter->writeConfig_UpSource_AnalogTrigger(false, status); counter->counter->writeConfig_Enable(state, status); } /** * Set the source object that causes the counter to count down. * Set the down counting DigitalSource. */ void setCounterDownSourceWithModule(void* counter_pointer, uint8_t module, uint32_t pin, bool analogTrigger, int32_t *status) { Counter* counter = (Counter*) counter_pointer; unsigned char mode = counter->counter->readConfig_Mode(status); if (mode != kTwoPulse && mode != kExternalDirection) { // TODO: wpi_setWPIErrorWithContext(ParameterOutOfRange, "Counter only supports DownSource in TwoPulse and ExternalDirection modes."); *status = PARAMETER_OUT_OF_RANGE; return; } counter->counter->writeConfig_DownSource_Module(module, status); counter->counter->writeConfig_DownSource_Channel(pin, status); counter->counter->writeConfig_DownSource_AnalogTrigger(analogTrigger, status); setCounterDownSourceEdge(counter_pointer, true, false, status); counter->counter->strobeReset(status); } /** * Set the edge sensitivity on a down counting source. * Set the down source to either detect rising edges or falling edges. */ void setCounterDownSourceEdge(void* counter_pointer, bool risingEdge, bool fallingEdge, int32_t *status) { Counter* counter = (Counter*) counter_pointer; counter->counter->writeConfig_DownRisingEdge(risingEdge, status); counter->counter->writeConfig_DownFallingEdge(fallingEdge, status); } /** * Disable the down counting source to the counter. */ void clearCounterDownSource(void* counter_pointer, int32_t *status) { Counter* counter = (Counter*) counter_pointer; bool state = counter->counter->readConfig_Enable(status); counter->counter->writeConfig_Enable(false, status); counter->counter->writeConfig_DownFallingEdge(false, status); counter->counter->writeConfig_DownRisingEdge(false, status); // Index 0 of digital is always 0. counter->counter->writeConfig_DownSource_Channel(0, status); counter->counter->writeConfig_DownSource_AnalogTrigger(false, status); counter->counter->writeConfig_Enable(state, status); } /** * Set standard up / down counting mode on this counter. * Up and down counts are sourced independently from two inputs. */ void setCounterUpDownMode(void* counter_pointer, int32_t *status) { Counter* counter = (Counter*) counter_pointer; counter->counter->writeConfig_Mode(kTwoPulse, status); } /** * Set external direction mode on this counter. * Counts are sourced on the Up counter input. * The Down counter input represents the direction to count. */ void setCounterExternalDirectionMode(void* counter_pointer, int32_t *status) { Counter* counter = (Counter*) counter_pointer; counter->counter->writeConfig_Mode(kExternalDirection, status); } /** * Set Semi-period mode on this counter. * Counts up on both rising and falling edges. */ void setCounterSemiPeriodMode(void* counter_pointer, bool highSemiPeriod, int32_t *status) { Counter* counter = (Counter*) counter_pointer; counter->counter->writeConfig_Mode(kSemiperiod, status); counter->counter->writeConfig_UpRisingEdge(highSemiPeriod, status); setCounterUpdateWhenEmpty(counter_pointer, false, status); } /** * Configure the counter to count in up or down based on the length of the input pulse. * This mode is most useful for direction sensitive gear tooth sensors. * @param threshold The pulse length beyond which the counter counts the opposite direction. Units are seconds. */ void setCounterPulseLengthMode(void* counter_pointer, double threshold, int32_t *status) { Counter* counter = (Counter*) counter_pointer; counter->counter->writeConfig_Mode(kPulseLength, status); counter->counter->writeConfig_PulseLengthThreshold((uint32_t)(threshold * 1.0e6) * kSystemClockTicksPerMicrosecond, status); } /** * Get the Samples to Average which specifies the number of samples of the timer to * average when calculating the period. Perform averaging to account for * mechanical imperfections or as oversampling to increase resolution. * @return SamplesToAverage The number of samples being averaged (from 1 to 127) */ int32_t getCounterSamplesToAverage(void* counter_pointer, int32_t *status) { Counter* counter = (Counter*) counter_pointer; return counter->counter->readTimerConfig_AverageSize(status); } /** * Set the Samples to Average which specifies the number of samples of the timer to * average when calculating the period. Perform averaging to account for * mechanical imperfections or as oversampling to increase resolution. * @param samplesToAverage The number of samples to average from 1 to 127. */ void setCounterSamplesToAverage(void* counter_pointer, int samplesToAverage, int32_t *status) { Counter* counter = (Counter*) counter_pointer; if (samplesToAverage < 1 || samplesToAverage > 127) { *status = PARAMETER_OUT_OF_RANGE; } counter->counter->writeTimerConfig_AverageSize(samplesToAverage, status); } /** * Start the Counter counting. * This enables the counter and it starts accumulating counts from the associated * input channel. The counter value is not reset on starting, and still has the previous value. */ void startCounter(void* counter_pointer, int32_t *status) { Counter* counter = (Counter*) counter_pointer; counter->counter->writeConfig_Enable(1, status); } /** * Stop the Counter. * Stops the counting but doesn't effect the current value. */ void stopCounter(void* counter_pointer, int32_t *status) { Counter* counter = (Counter*) counter_pointer; counter->counter->writeConfig_Enable(0, status); } /** * Reset the Counter to zero. * Set the counter value to zero. This doesn't effect the running state of the counter, just sets * the current value to zero. */ void resetCounter(void* counter_pointer, int32_t *status) { Counter* counter = (Counter*) counter_pointer; counter->counter->strobeReset(status); } /** * Read the current counter value. * Read the value at this instant. It may still be running, so it reflects the current value. Next * time it is read, it might have a different value. */ int32_t getCounter(void* counter_pointer, int32_t *status) { Counter* counter = (Counter*) counter_pointer; int32_t value = counter->counter->readOutput_Value(status); return value; } /* * Get the Period of the most recent count. * Returns the time interval of the most recent count. This can be used for velocity calculations * to determine shaft speed. * @returns The period of the last two pulses in units of seconds. */ double getCounterPeriod(void* counter_pointer, int32_t *status) { Counter* counter = (Counter*) counter_pointer; tCounter::tTimerOutput output = counter->counter->readTimerOutput(status); double period; if (output.Stalled) { // Return infinity double zero = 0.0; period = 1.0 / zero; } else { // output.Period is a fixed point number that counts by 2 (24 bits, 25 integer bits) period = (double)(output.Period << 1) / (double)output.Count; } return period * 1.0e-6; } /** * Set the maximum period where the device is still considered "moving". * Sets the maximum period where the device is considered moving. This value is used to determine * the "stopped" state of the counter using the GetStopped method. * @param maxPeriod The maximum period where the counted device is considered moving in * seconds. */ void setCounterMaxPeriod(void* counter_pointer, double maxPeriod, int32_t *status) { Counter* counter = (Counter*) counter_pointer; counter->counter->writeTimerConfig_StallPeriod((uint32_t)(maxPeriod * 1.0e6), status); } /** * Select whether you want to continue updating the event timer output when there are no samples captured. * The output of the event timer has a buffer of periods that are averaged and posted to * a register on the FPGA. When the timer detects that the event source has stopped * (based on the MaxPeriod) the buffer of samples to be averaged is emptied. If you * enable the update when empty, you will be notified of the stopped source and the event * time will report 0 samples. If you disable update when empty, the most recent average * will remain on the output until a new sample is acquired. You will never see 0 samples * output (except when there have been no events since an FPGA reset) and you will likely not * see the stopped bit become true (since it is updated at the end of an average and there are * no samples to average). */ void setCounterUpdateWhenEmpty(void* counter_pointer, bool enabled, int32_t *status) { Counter* counter = (Counter*) counter_pointer; counter->counter->writeTimerConfig_UpdateWhenEmpty(enabled, status); } /** * Determine if the clock is stopped. * Determine if the clocked input is stopped based on the MaxPeriod value set using the * SetMaxPeriod method. If the clock exceeds the MaxPeriod, then the device (and counter) are * assumed to be stopped and it returns true. * @return Returns true if the most recent counter period exceeds the MaxPeriod value set by * SetMaxPeriod. */ bool getCounterStopped(void* counter_pointer, int32_t *status) { Counter* counter = (Counter*) counter_pointer; return counter->counter->readTimerOutput_Stalled(status); } /** * The last direction the counter value changed. * @return The last direction the counter value changed. */ bool getCounterDirection(void* counter_pointer, int32_t *status) { Counter* counter = (Counter*) counter_pointer; bool value = counter->counter->readOutput_Direction(status); return value; } /** * Set the Counter to return reversed sensing on the direction. * This allows counters to change the direction they are counting in the case of 1X and 2X * quadrature encoding only. Any other counter mode isn't supported. * @param reverseDirection true if the value counted should be negated. */ void setCounterReverseDirection(void* counter_pointer, bool reverseDirection, int32_t *status) { Counter* counter = (Counter*) counter_pointer; if (counter->counter->readConfig_Mode(status) == kExternalDirection) { if (reverseDirection) setCounterDownSourceEdge(counter_pointer, true, true, status); else setCounterDownSourceEdge(counter_pointer, false, true, status); } } struct encoder_t { tEncoder* encoder; uint32_t index; }; typedef struct encoder_t Encoder; static const double DECODING_SCALING_FACTOR = 0.25; static Resource *quadEncoders = NULL; void* initializeEncoder(uint8_t port_a_module, uint32_t port_a_pin, bool port_a_analog_trigger, uint8_t port_b_module, uint32_t port_b_pin, bool port_b_analog_trigger, bool reverseDirection, int32_t *index, int32_t *status) { // Initialize encoder structure Encoder* encoder = new Encoder(); Resource::CreateResourceObject(&quadEncoders, tEncoder::kNumSystems); encoder->index = quadEncoders->Allocate("4X Encoder"); *index = encoder->index; // TODO: if (index == ~0ul) { CloneError(quadEncoders); return; } encoder->encoder = tEncoder::create(encoder->index, status); encoder->encoder->writeConfig_ASource_Module(port_a_module, status); encoder->encoder->writeConfig_ASource_Channel(port_a_pin, status); encoder->encoder->writeConfig_ASource_AnalogTrigger(port_a_analog_trigger, status); encoder->encoder->writeConfig_BSource_Module(port_b_module, status); encoder->encoder->writeConfig_BSource_Channel(port_b_pin, status); encoder->encoder->writeConfig_BSource_AnalogTrigger(port_b_analog_trigger, status); encoder->encoder->strobeReset(status); encoder->encoder->writeConfig_Reverse(reverseDirection, status); encoder->encoder->writeTimerConfig_AverageSize(4, status); return encoder; } void freeEncoder(void* encoder_pointer, int32_t *status) { Encoder* encoder = (Encoder*) encoder_pointer; quadEncoders->Free(encoder->index); delete encoder->encoder; } /** * Start the Encoder. * Starts counting pulses on the Encoder device. */ void startEncoder(void* encoder_pointer, int32_t *status) { Encoder* encoder = (Encoder*) encoder_pointer; encoder->encoder->writeConfig_Enable(1, status); } /** * Stops counting pulses on the Encoder device. The value is not changed. */ void stopEncoder(void* encoder_pointer, int32_t *status) { Encoder* encoder = (Encoder*) encoder_pointer; encoder->encoder->writeConfig_Enable(0, status); } /** * Reset the Encoder distance to zero. * Resets the current count to zero on the encoder. */ void resetEncoder(void* encoder_pointer, int32_t *status) { Encoder* encoder = (Encoder*) encoder_pointer; encoder->encoder->strobeReset(status); } /** * Gets the raw value from the encoder. * The raw value is the actual count unscaled by the 1x, 2x, or 4x scale * factor. * @return Current raw count from the encoder */ int32_t getEncoder(void* encoder_pointer, int32_t *status) { Encoder* encoder = (Encoder*) encoder_pointer; return encoder->encoder->readOutput_Value(status); } /** * Returns the period of the most recent pulse. * Returns the period of the most recent Encoder pulse in seconds. * This method compenstates for the decoding type. * * @deprecated Use GetRate() in favor of this method. This returns unscaled periods and GetRate() scales using value from SetDistancePerPulse(). * * @return Period in seconds of the most recent pulse. */ double getEncoderPeriod(void* encoder_pointer, int32_t *status) { Encoder* encoder = (Encoder*) encoder_pointer; tEncoder::tTimerOutput output = encoder->encoder->readTimerOutput(status); double value; if (output.Stalled) { // Return infinity double zero = 0.0; value = 1.0 / zero; } else { // output.Period is a fixed point number that counts by 2 (24 bits, 25 integer bits) value = (double)(output.Period << 1) / (double)output.Count; } double measuredPeriod = value * 1.0e-6; return measuredPeriod / DECODING_SCALING_FACTOR; } /** * Sets the maximum period for stopped detection. * Sets the value that represents the maximum period of the Encoder before it will assume * that the attached device is stopped. This timeout allows users to determine if the wheels or * other shaft has stopped rotating. * This method compensates for the decoding type. * * @deprecated Use SetMinRate() in favor of this method. This takes unscaled periods and SetMinRate() scales using value from SetDistancePerPulse(). * * @param maxPeriod The maximum time between rising and falling edges before the FPGA will * report the device stopped. This is expressed in seconds. */ void setEncoderMaxPeriod(void* encoder_pointer, double maxPeriod, int32_t *status) { Encoder* encoder = (Encoder*) encoder_pointer; encoder->encoder->writeTimerConfig_StallPeriod((uint32_t)(maxPeriod * 1.0e6 * DECODING_SCALING_FACTOR), status); } /** * Determine if the encoder is stopped. * Using the MaxPeriod value, a boolean is returned that is true if the encoder is considered * stopped and false if it is still moving. A stopped encoder is one where the most recent pulse * width exceeds the MaxPeriod. * @return True if the encoder is considered stopped. */ bool getEncoderStopped(void* encoder_pointer, int32_t *status) { Encoder* encoder = (Encoder*) encoder_pointer; return encoder->encoder->readTimerOutput_Stalled(status) != 0; } /** * The last direction the encoder value changed. * @return The last direction the encoder value changed. */ bool getEncoderDirection(void* encoder_pointer, int32_t *status) { Encoder* encoder = (Encoder*) encoder_pointer; return encoder->encoder->readOutput_Direction(status); } /** * Set the direction sensing for this encoder. * This sets the direction sensing on the encoder so that it could count in the correct * software direction regardless of the mounting. * @param reverseDirection true if the encoder direction should be reversed */ void setEncoderReverseDirection(void* encoder_pointer, bool reverseDirection, int32_t *status) { Encoder* encoder = (Encoder*) encoder_pointer; encoder->encoder->writeConfig_Reverse(reverseDirection, status); } /** * Set the Samples to Average which specifies the number of samples of the timer to * average when calculating the period. Perform averaging to account for * mechanical imperfections or as oversampling to increase resolution. * @param samplesToAverage The number of samples to average from 1 to 127. */ void setEncoderSamplesToAverage(void* encoder_pointer, uint32_t samplesToAverage, int32_t *status) { Encoder* encoder = (Encoder*) encoder_pointer; if (samplesToAverage < 1 || samplesToAverage > 127) { *status = PARAMETER_OUT_OF_RANGE; } encoder->encoder->writeTimerConfig_AverageSize(samplesToAverage, status); } /** * Get the Samples to Average which specifies the number of samples of the timer to * average when calculating the period. Perform averaging to account for * mechanical imperfections or as oversampling to increase resolution. * @return SamplesToAverage The number of samples being averaged (from 1 to 127) */ uint32_t getEncoderSamplesToAverage(void* encoder_pointer, int32_t *status) { Encoder* encoder = (Encoder*) encoder_pointer; return encoder->encoder->readTimerConfig_AverageSize(status); } /** * Get the loop timing of the Digital Module * * @return The loop time */ uint16_t getLoopTiming(int32_t *status) { return getLoopTimingWithModule(1, status); } /** * Get the loop timing of the Digital Module * * @return The loop time */ uint16_t getLoopTimingWithModule(uint8_t module, int32_t *status) { return pwmSystem->readLoopTiming(status); } // XXX: What happened to SPI? // struct spi_t { // tSPI* spi; // tSPI::tConfig config; // tSPI::tChannels channels; // MUTEX_ID semaphore; // }; // typedef struct spi_t SPI; // void* initializeSPI(uint8_t sclk_routing_module, uint32_t sclk_routing_pin, // uint8_t mosi_routing_module, uint32_t mosi_routing_pin, // uint8_t miso_routing_module, uint32_t miso_routing_pin, int32_t *status) { // SPI* spi = new SPI(); // spi->semaphore = initializeMutex(SEMAPHORE_Q_PRIORITY | SEMAPHORE_DELETE_SAFE | SEMAPHORE_INVERSION_SAFE); // spi->spi = tSPI::create(status); // spi->config.BusBitWidth = 8; // spi->config.ClockHalfPeriodDelay = 0; // spi->config.MSBfirst = 0; // spi->config.DataOnFalling = 0; // spi->config.LatchFirst = 0; // spi->config.LatchLast = 0; // spi->config.FramePolarity = 0; // spi->config.WriteOnly = miso_routing_pin ? 0 : 1; // spi->config.ClockPolarity = 0; // spi->channels.SCLK_Channel = sclk_routing_pin; // spi->channels.SCLK_Module = sclk_routing_module; // spi->channels.SS_Channel = 0; // spi->channels.SS_Module = 0; // if (mosi_routing_pin) { // spi->channels.MOSI_Channel = mosi_routing_pin; // spi->channels.MOSI_Module = mosi_routing_module; // } else { // spi->channels.MOSI_Channel = 0; // spi->channels.MOSI_Module = 0; // } // if (miso_routing_pin) { // spi->channels.MISO_Channel = miso_routing_pin; // spi->channels.MISO_Module = miso_routing_module; // } else { // spi->channels.MISO_Channel = 0; // spi->channels.MISO_Module = 0; // } // return spi; // } // void cleanSPI(void* spi_pointer, int32_t *status) { // SPI* spi = (SPI*) spi_pointer; // delete spi->spi; // delete spi; // } // /** // * Configure the number of bits from each word that the slave transmits // * or receives. // * // * @param bits The number of bits in one frame (1 to 32 bits). // */ // void setSPIBitsPerWord(void* spi_pointer, uint32_t bits, int32_t *status) { // SPI* spi = (SPI*) spi_pointer; // spi->config.BusBitWidth = bits; // } // /** // * Get the number of bits from each word that the slave transmits // * or receives. // * // * @return The number of bits in one frame (1 to 32 bits). // */ // uint32_t getSPIBitsPerWord(void* spi_pointer, int32_t *status) { // SPI* spi = (SPI*) spi_pointer; // return spi->config.BusBitWidth; // } // /** // * Configure the rate of the generated clock signal. // * The default and maximum value is 76,628.4 Hz. // * // * @param hz The clock rate in Hertz. // */ // void setSPIClockRate(void* spi_pointer, double hz, int32_t *status) { // SPI* spi = (SPI*) spi_pointer; // int delay = 0; // int loopTiming = getLoopTimingWithModule(spi->spi->readChannels_SCLK_Module(status), status); // double v = (1.0 / hz) / (2 * loopTiming / (kSystemClockTicksPerMicrosecond * 1e6)); // if (v < 1) { // // TODO: wpi_setWPIErrorWithContext(ParameterOutOfRange, "SPI Clock too high"); // } // delay = (int) (v + .5); // if (delay > 255) { // // TODO: wpi_setWPIErrorWithContext(ParameterOutOfRange, "SPI Clock too low"); // } // spi->config.ClockHalfPeriodDelay = delay; // } // /** // * Configure the order that bits are sent and received on the wire // * to be most significant bit first. // */ // void setSPIMSBFirst(void* spi_pointer, int32_t *status) { // SPI* spi = (SPI*) spi_pointer; // spi->config.MSBfirst = 1; // } // /** // * Configure the order that bits are sent and received on the wire // * to be least significant bit first. // */ // void setSPILSBFirst(void* spi_pointer, int32_t *status) { // SPI* spi = (SPI*) spi_pointer; // spi->config.MSBfirst = 0; // } // /** // * Configure that the data is stable on the falling edge and the data // * changes on the rising edge. // */ // void setSPISampleDataOnFalling(void* spi_pointer, int32_t *status) { // SPI* spi = (SPI*) spi_pointer; // spi->config.DataOnFalling = 1; // } // /** // * Configure that the data is stable on the rising edge and the data // * changes on the falling edge. // */ // void setSPISampleDataOnRising(void* spi_pointer, int32_t *status) { // SPI* spi = (SPI*) spi_pointer; // spi->config.DataOnFalling = 0; // } // void setSPISlaveSelect(void* spi_pointer, uint8_t ss_routing_module, uint32_t ss_routing_pin, // int32_t *status) { // SPI* spi = (SPI*) spi_pointer; // spi->channels.SS_Channel = ss_routing_pin; // spi->channels.SS_Module = ss_routing_module; // } // void setSPILatchMode(void* spi_pointer, tFrameMode mode, int32_t *status) { // SPI* spi = (SPI*) spi_pointer; // switch (mode) { // case kChipSelect: // spi->config.LatchFirst = 0; // spi->config.LatchLast = 0; // break; // case kPreLatchPulse: // spi->config.LatchFirst = 1; // spi->config.LatchLast = 0; // break; // case kPostLatchPulse: // spi->config.LatchFirst = 0; // spi->config.LatchLast = 1; // break; // case kPreAndPostLatchPulse: // spi->config.LatchFirst = 1; // spi->config.LatchLast = 1; // break; // } // } // tFrameMode getSPILatchMode(void* spi_pointer, int32_t *status) { // SPI* spi = (SPI*) spi_pointer; // return (tFrameMode) (spi->config.LatchFirst | (spi->config.LatchLast << 1)); // } // void setSPIFramePolarity(void* spi_pointer, bool activeLow, int32_t *status) { // SPI* spi = (SPI*) spi_pointer; // spi->config.FramePolarity = activeLow ? 1 : 0; // } // bool getSPIFramePolarity(void* spi_pointer, int32_t *status) { // SPI* spi = (SPI*) spi_pointer; // return spi->config.FramePolarity != 0; // } // /** // * Configure the clock output line to be active low. // * This is sometimes called clock polarity high. // */ // void setSPIClockActiveLow(void* spi_pointer, int32_t *status) { // SPI* spi = (SPI*) spi_pointer; // spi->config.ClockPolarity = 1; // } // /** // * Configure the clock output line to be active high. // * This is sometimes called clock polarity low. // */ // void setSPIClockActiveHigh(void* spi_pointer, int32_t *status) { // SPI* spi = (SPI*) spi_pointer; // spi->config.ClockPolarity = 0; // } // /** // * Apply configuration settings and reset the SPI logic. // */ // void applySPIConfig(void* spi_pointer, int32_t *status) { // SPI* spi = (SPI*) spi_pointer; // Synchronized sync(spi->semaphore); // spi->spi->writeConfig(spi->config, status); // spi->spi->writeChannels(spi->channels, status); // spi->spi->strobeReset(status); // } // /** // * Get the number of words that can currently be stored before being // * transmitted to the device. // * // * @return The number of words available to be written. // */ // uint16_t getSPIOutputFIFOAvailable(void* spi_pointer, int32_t *status) { // SPI* spi = (SPI*) spi_pointer; // uint16_t result = spi->spi->readAvailableToLoad(status); // return result; // } // /** // * Get the number of words received and currently available to be read from // * the receive FIFO. // * // * @return The number of words available to read. // */ // uint16_t getSPINumReceived(void* spi_pointer, int32_t *status) { // SPI* spi = (SPI*) spi_pointer; // uint16_t result = spi->spi->readReceivedElements(status); // return result; // } // /** // * Have all pending transfers completed? // * // * @return True if no transfers are pending. // */ // bool isSPIDone(void* spi_pointer, int32_t *status) { // SPI* spi = (SPI*) spi_pointer; // bool result = spi->spi->readStatus_Idle(status); // return result; // } // /** // * Determine if the receive FIFO was full when attempting to add new data at // * end of a transfer. // * // * @return True if the receive FIFO overflowed. // */ // bool hadSPIReceiveOverflow(void* spi_pointer, int32_t *status) { // SPI* spi = (SPI*) spi_pointer; // bool result = spi->spi->readStatus_ReceivedDataOverflow(status); // return result; // } // /** // * Write a word to the slave device. Blocks until there is space in the // * output FIFO. // * // * If not running in output only mode, also saves the data received // * on the MISO input during the transfer into the receive FIFO. // */ // void writeSPI(void* spi_pointer, uint32_t data, int32_t *status) { // SPI* spi = (SPI*) spi_pointer; // if (spi->channels.MOSI_Channel == 0 && spi->channels.MOSI_Module == 0) { // *status = SPI_WRITE_NO_MOSI; // return; // } // Synchronized sync(spi->semaphore); // while (getSPIOutputFIFOAvailable(spi_pointer, status) == 0) // delayTicks(HAL_NO_WAIT); // spi->spi->writeDataToLoad(data, status); // spi->spi->strobeLoad(status); // } // /** // * Read a word from the receive FIFO. // * // * Waits for the current transfer to complete if the receive FIFO is empty. // * // * If the receive FIFO is empty, there is no active transfer, and initiate // * is false, errors. // * // * @param initiate If true, this function pushes "0" into the // * transmit buffer and initiates a transfer. // * If false, this function assumes that data is // * already in the receive FIFO from a previous write. // */ // uint32_t readSPI(void* spi_pointer, bool initiate, int32_t *status) { // SPI* spi = (SPI*) spi_pointer; // if (spi->channels.MISO_Channel == 0 && spi->channels.MISO_Module == 0) { // *status = SPI_READ_NO_MISO; // return 0; // } // uint32_t data; // { // Synchronized sync(spi->semaphore); // if (initiate) { // spi->spi->writeDataToLoad(0, status); // spi->spi->strobeLoad(status); // } // // Do we have anything ready to read? // if (getSPINumReceived(spi_pointer, status) == 0) { // if (!initiate && isSPIDone(spi_pointer, status) // && getSPIOutputFIFOAvailable(spi_pointer, status) == kTransmitFIFODepth) { // // Nothing to read: error out // *status = SPI_READ_NO_DATA; // return 0; // } // // Wait for the transaction to complete // while (getSPINumReceived(spi_pointer, status) == 0) // delayTicks(HAL_NO_WAIT); // } // spi->spi->strobeReadReceivedData(status); // data = spi->spi->readReceivedData(status); // } // return data; // } // /** // * Stop any transfer in progress and empty the transmit FIFO. // */ // void resetSPI(void* spi_pointer, int32_t *status) { // SPI* spi = (SPI*) spi_pointer; // spi->spi->strobeReset(status); // } // /** // * Empty the receive FIFO. // */ // void clearSPIReceivedData(void* spi_pointer, int32_t *status) { // SPI* spi = (SPI*) spi_pointer; // spi->spi->strobeClearReceivedData(status); // } void* initializeSPI(uint8_t sclk_routing_module, uint32_t sclk_routing_pin, uint8_t mosi_routing_module, uint32_t mosi_routing_pin, uint8_t miso_routing_module, uint32_t miso_routing_pin, int32_t *status) { return NULL; } void cleanSPI(void* spi_pointer, int32_t *status) {} void setSPIBitsPerWord(void* spi_pointer, uint32_t bits, int32_t *status) {} uint32_t getSPIBitsPerWord(void* spi_pointer, int32_t *status) { return 0; } void setSPIClockRate(void* spi_pointer, double hz, int32_t *status) {} void setSPIMSBFirst(void* spi_pointer, int32_t *status) {} void setSPILSBFirst(void* spi_pointer, int32_t *status) {} void setSPISampleDataOnFalling(void* spi_pointer, int32_t *status) {} void setSPISampleDataOnRising(void* spi_pointer, int32_t *status) {} void setSPISlaveSelect(void* spi_pointer, uint8_t ss_routing_module, uint32_t ss_routing_pin, int32_t *status) {} void setSPILatchMode(void* spi_pointer, tFrameMode mode, int32_t *status) {} tFrameMode getSPILatchMode(void* spi_pointer, int32_t *status) { return (tFrameMode) 0; } void setSPIFramePolarity(void* spi_pointer, bool activeLow, int32_t *status) {} bool getSPIFramePolarity(void* spi_pointer, int32_t *status) { return false; } void setSPIClockActiveLow(void* spi_pointer, int32_t *status) {} void setSPIClockActiveHigh(void* spi_pointer, int32_t *status) {} void applySPIConfig(void* spi_pointer, int32_t *status) {} uint16_t getSPIOutputFIFOAvailable(void* spi_pointer, int32_t *status) { return 0; } uint16_t getSPINumReceived(void* spi_pointer, int32_t *status) { return 0; } bool isSPIDone(void* spi_pointer, int32_t *status) { return false; } bool hadSPIReceiveOverflow(void* spi_pointer, int32_t *status) { return false; } void writeSPI(void* spi_pointer, uint32_t data, int32_t *status) {} uint32_t readSPI(void* spi_pointer, bool initiate, int32_t *status) {return 0;} void resetSPI(void* spi_pointer, int32_t *status) {} void clearSPIReceivedData(void* spi_pointer, int32_t *status) {} /* * Initialize the I2C port. Opens the port if necessary and saves the handle. * If opening the MXP port, also sets up the pin functions appropriately * @param port The port to open, 0 for the on-board, 1 for the MXP. */ void i2CInitialize(uint8_t port, int32_t *status) { initializeDigital(status); if(port > 1) { //Set port out of range error here return; } MUTEX_ID lock = port == 0 ? digitalI2COnBoardSemaphore:digitalI2CMXPSemaphore; { Synchronized sync(lock); if(port == 0) { i2COnboardObjCount++; if (i2COnBoardHandle > 0) return; i2COnBoardHandle = i2clib_open("/dev/i2c-2"); } else if(port == 1) { i2CMXPObjCount++; if (i2CMXPHandle > 0) return; digitalSystem->writeEnableMXPSpecialFunction(digitalSystem->readEnableMXPSpecialFunction(status)|0xC000, status); i2CMXPHandle = i2clib_open("/dev/i2c-1"); } return; } } /** * Generic transaction. * * This is a lower-level interface to the I2C hardware giving you more control over each transaction. * * @param dataToSend Buffer of data to send as part of the transaction. * @param sendSize Number of bytes to send as part of the transaction. [0..6] * @param dataReceived Buffer to read data into. * @param receiveSize Number of bytes to read from the device. [0..7] * @return Transfer Aborted... false for success, true for aborted. */ int i2CTransaction(uint8_t port, uint8_t deviceAddress, uint8_t *dataToSend, uint8_t sendSize, uint8_t *dataReceived, uint8_t receiveSize) { if(port > 1) { //Set port out of range error here return -1; } /*if (sendSize > 6) // Optional, provides better error message. TODO: Are these limits still right? Implement error. Check for null buffer { wpi_setWPIErrorWithContext(ParameterOutOfRange, "sendSize"); return true; } if (receiveSize > 7) // Optional, provides better error message. { wpi_setWPIErrorWithContext(ParameterOutOfRange, "receiveSize"); return true; }*/ int32_t handle = port == 0 ? i2COnBoardHandle:i2CMXPHandle; MUTEX_ID lock = port == 0 ? digitalI2COnBoardSemaphore:digitalI2CMXPSemaphore; { Synchronized sync(lock); return i2clib_writeread(handle, deviceAddress, (const char*) dataToSend, (int32_t) sendSize, (char*) dataReceived, (int32_t) receiveSize); } } /** * Execute a write transaction with the device. * * Write a single byte to a register on a device and wait until the * transaction is complete. * * @param registerAddress The address of the register on the device to be written. * @param data The byte to write to the register on the device. * @return Transfer Aborted... false for success, true for aborted. */ int i2CWrite(uint8_t port, uint8_t deviceAddress, uint8_t* dataToSend, uint8_t sendSize) { if(port > 1) { //Set port out of range error here return -1; } /*if (sendSize > 6) // Optional, provides better error message. TODO: Are these limits still right? Implement error. Check for null buffer { wpi_setWPIErrorWithContext(ParameterOutOfRange, "sendSize"); return true; }*/ int32_t handle = port == 0 ? i2COnBoardHandle:i2CMXPHandle; MUTEX_ID lock = port == 0 ? digitalI2COnBoardSemaphore:digitalI2CMXPSemaphore; { Synchronized sync(lock); return i2clib_write(handle, deviceAddress, (const char*) dataToSend, (int32_t) sendSize); } } /** * Execute a read transaction with the device. * * Read 1 to 7 bytes from a device. * Most I2C devices will auto-increment the register pointer internally * allowing you to read up to 7 consecutive registers on a device in a * single transaction. * * @param registerAddress The register to read first in the transaction. * @param count The number of bytes to read in the transaction. [1..7] * @param buffer A pointer to the array of bytes to store the data read from the device. * @return Transfer Aborted... false for success, true for aborted. */ int i2CRead(uint8_t port, uint8_t deviceAddress, uint8_t *buffer, uint8_t count) { if(port > 1) { //Set port out of range error here return -1; } /* if (count < 1 || count > 7) Todo: Are these limits still right? Implement error { wpi_setWPIErrorWithContext(ParameterOutOfRange, "count"); return true; } if (buffer == NULL) { wpi_setWPIErrorWithContext(NullParameter, "buffer"); return true; }*/ int32_t handle = port == 0 ? i2COnBoardHandle:i2CMXPHandle; MUTEX_ID lock = port == 0 ? digitalI2COnBoardSemaphore:digitalI2CMXPSemaphore; { Synchronized sync(lock); return i2clib_read(handle, deviceAddress, (char*) buffer, (int32_t) count); } } void i2CClose(uint8_t port) { if(port > 1) { //Set port out of range error here return; } MUTEX_ID lock = port == 0 ? digitalI2COnBoardSemaphore:digitalI2CMXPSemaphore; { Synchronized sync(lock); if((port == 0 ? i2COnboardObjCount--:i2CMXPObjCount--) == 0) { int32_t handle = port == 0 ? i2COnBoardHandle:i2CMXPHandle; i2clib_close(handle); } } return; } //// Float JNA Hack // double void setPWMRateIntHack(int32_t rate, int32_t *status) { setPWMRate(intToFloat(rate), status); } void setPWMRateWithModuleIntHack(uint8_t module, int32_t rate, int32_t *status) { setPWMRateWithModule(module, intToFloat(rate), status); } void setPWMDutyCycleIntHack(void* pwmGenerator, int32_t dutyCycle, int32_t *status) { setPWMDutyCycle(pwmGenerator, intToFloat(dutyCycle), status); } void setPWMDutyCycleWithModuleIntHack(uint8_t module, void* pwmGenerator, int32_t dutyCycle, int32_t *status) { setPWMDutyCycleWithModule(module, pwmGenerator, dutyCycle, status); }