// Sets the sabertooth address
void FPGA_setSabertoothAddress(uint8_t address) {
NiFpga_MergeStatus(&FPGA_Status,NiFpga_WriteU8(FPGA_Session,NiFpga_mainFPGA_ControlU8_SerialAddress, address));
if (NiFpga_IsError(FPGA_Status))
{
LOG.ERR("Failed to set Sabertooth Address");
}
}
void Encoder::enable(bool quadPhase) {
uint8_t mode = quadPhase ? Encoder_QuadPhase : Encoder_StepDirection;
configure(Encoder_ConfigureMask(Encoder_Enable | Encoder_SignalMode),
Encoder_ConfigureSettings(Encoder_Enabled | mode));
uint8_t selectReg;
/*
* Encoder inputs are on pins shared with other onboard devices. To input
* from a physical pin, select the encoder on the appropriate SELECT
* register.
*
* Read the value of the SYSSELECTB register.
*/
NiFpga_ReadU8(MRio.session, sysSelect, &selectReg);
/*
* Set bit 5 of the SYSSELECTB register to enable ENCB functionality.
* The functionality of these bits is specified in the documentation.
*/
selectReg = selectReg | (1 << bitNumber);
/*
* Write the updated value of the SYSSELECTB register.
*/
NiFpga_WriteU8(MRio.session, sysSelect, selectReg);
}
/**
* Sets options for the encoder configuration register.
*
* @param[in] channel A struct containing the registers on the encoder channel
* to modify.
* @param[in] mask Array of flags that indicate which of the configure
* settings are valid.
* @param[in] settings Array of flags that indicate the configuration settings.
*/
void Encoder_Configure(MyRio_Encoder* channel, Encoder_ConfigureMask mask,
Encoder_ConfigureSettings settings)
{
NiFpga_Status status;
uint8_t cnfgValue;
/*
* Get the current value of the configure register.
*
* The returned NiFpga_Status value is stored for error checking.
*/
status = NiFpga_ReadU8(myrio_session, channel->cnfg, &cnfgValue);
/*
* Check if there was an error reading from the encoder registers.
*
* If there was an error then the rest of the function cannot complete
* correctly so print an error message to stdout and return from the
* function early.
*/
MyRio_ReturnIfNotSuccess(status,
"Could not read from the encoder configure registers!")
/*
* Clear the value of the masked bits in the configure register. This is
* done so that the correct value can be set later on.
*/
cnfgValue = cnfgValue & (~mask);
/*
* Set the value of the settings in the configure register. If the
* value to set is 0 this operation would not work unless the bit was
* previously cleared.
*/
cnfgValue = cnfgValue | settings;
/*
* Write the new value of the configure register to the device.
*/
NiFpga_MergeStatus(&status,
NiFpga_WriteU8(myrio_session, channel->cnfg, cnfgValue));
/*
* Check if there was an error writing to encoder configure registers.
*
* If there was an error then print an error message to stdout.
*/
MyRio_ReturnIfNotSuccess(status,
"Could not write to the encoder configure registers!")
}
/**
* Reserve the interrupt from FPGA and configure DI IRQ.
*
* @param[in] irqChannel A structure containing the registers and settings
for a particular analog IRQ I/O to modify.
* @param[in] irqContext IRQ context under which you need to reserve.
* @param[in] irqNumber The IRQ number (IRQNO_MIN-IRQNO_MAX).
* @param[in] count The incremental times that you use to trigger the interrupt.
* @param[in] type The trigger type that you use to increment the count.
* @return the configuration status.
*/
int32_t Irq_RegisterDiIrq(MyRio_IrqDi* irqChannel,
NiFpga_IrqContext* irqContext,
uint8_t irqNumber,
uint32_t count,
Irq_Dio_Type type)
{
int32_t status;
uint8_t cnfgValue;
uint16_t typeValue;
/*
* Reserve an IRQ context. IRQ contexts are single-threaded; only one thread
* can wait with a particular context at any given time. To minimize jitter
* when first waiting on IRQs, reserve as many contexts as the application requires.
* If a context is successfully reserved, you must unreserve it later.
* Otherwise a memory leak will occur.
*/
status = NiFpga_ReserveIrqContext(myrio_session, irqContext);
/*
* Check if there was an error when you reserved an IRQ.
*
* If there was an error, print an error message to stdout and return the configuration status.
*/
MyRio_ReturnStatusIfNotSuccess(status,
"A required NiFpga_IrqContext was not reserved.")
/*
* Limit the IRQ number within a range,
* if the entered value is out of range, print an error message.
*/
if (irqNumber > IRQNO_MAX || irqNumber < IRQNO_MIN)
{
printf("The specified IRQ Number is out of range.\n");
return NiMyrio_Status_IrqNumberNotUsable;
}
/*
* Check if the IRQ number or channel value already exists in the resource list,
* return the configuration status and print error message.
*/
status = Irq_CheckReserved(irqChannel->dioChannel, irqNumber);
if (status == NiMyrio_Status_IrqNumberNotUsable)
{
printf("You have already registered an interrupt with the same interrupt number.\n");
return status;
}
else if (status == NiMyrio_Status_IrqChannelNotUsable)
{
printf("You have already registered an interrupt with the same channel name.\n");
return status;
}
/*
* Write the value to the DI IRQ number register.
*/
status = NiFpga_WriteU8(myrio_session, irqChannel->dioIrqNumber, irqNumber);
/*
* Check if there was an error when your wrote to the DI IRQ number register.
*
* If there was an error, print an error message to stdout and return the configuration status.
*/
MyRio_ReturnStatusIfNotSuccess(status,
"Could not write to DI IRQ number register!")
/*
* Write the value to the DI IRQ count register.
*/
status = NiFpga_WriteU32(myrio_session, irqChannel->dioCount, count);
/*
* Check if there was an error when you reserved an IRQ.
*
* If there was an error, print an error message to stdout and return the configuration status.
*/
MyRio_ReturnStatusIfNotSuccess(status,
"Could not write to DI IRQ count register!")
/*
* Get the current value of the DI rising-configure register.
*/
status = NiFpga_ReadU8(myrio_session, irqChannel->dioIrqRisingEdge, &cnfgValue);
typeValue = (uint16_t) cnfgValue;
/*
* Get the current value of the DI falling-configure register.
* Merge it with the rising-configure register and write to typeValue.
*/
NiFpga_MergeStatus(&status,
NiFpga_ReadU8(myrio_session, irqChannel->dioIrqFallingEdge, &cnfgValue));
typeValue = typeValue | (cnfgValue << 8);
/*
* Check if there was an error reading from the DI rising/falling configure registers.
*
* If there was an error, the rest of the function cannot execute successfully, print
* an error message to stdout and return the configuration status from the earlier
* execution of the function.
*/
MyRio_ReturnStatusIfNotSuccess(status,
"Could not read from the DI rise/fall configure registers!")
/*
* Get the current value of the DI configure register.
*/
status = NiFpga_ReadU8(myrio_session, irqChannel->dioIrqEnable, &cnfgValue);
/*
* Check if there was an error when you reserved an IRQ.
*
* If there was an error, print an error message to stdout and return the configuration status.
*/
MyRio_ReturnStatusIfNotSuccess(status,
"Could not read from the DI configure register!")
/*
* Configure the IRQ triggered-type for the particular digital IRQ I、O.
*/
if (irqChannel->dioChannel == Irq_Dio_A0)
{
/*
* Clear the value of the masked bits in the DI configure register, then
* set which IO is enabled.
*/
cnfgValue = cnfgValue & (~Irq_Dio_A0_Enable);
cnfgValue = cnfgValue | Irq_Dio_A0_Enable;
/*
* Clear the value of the masked bits in the DI configure register and set the I/O to enable.
*/
typeValue = typeValue & (~Irq_Dio_A0_Edge);
if (type == Irq_Dio_RisingEdge)
{
typeValue = typeValue | Irq_Dio_A0_RisingEdge;
}
else if (type == Irq_Dio_FallingEdge)
{
typeValue = typeValue | Irq_Dio_A0_FallingEdge;
}
else if (type == Irq_Dio_Edge)
{
typeValue = typeValue | Irq_Dio_A0_Edge;
}
}
else if (irqChannel->dioChannel == Irq_Dio_A1)
{
/*
* Clear the value of the masked bits in the DI configure register, then
* set which IO is enabled.
*/
cnfgValue = cnfgValue & (~Irq_Dio_A1_Enable);
cnfgValue = cnfgValue | Irq_Dio_A1_Enable;
/*
* Clear the value of the masked bits in the DI configure register, then
* set the triggered type.
*/
typeValue = typeValue & (~Irq_Dio_A1_Edge);
if (type == Irq_Dio_RisingEdge)
{
typeValue = typeValue | Irq_Dio_A1_RisingEdge;
}
else if (type == Irq_Dio_FallingEdge)
{
typeValue = typeValue | Irq_Dio_A1_FallingEdge;
}
else if (type == Irq_Dio_Edge)
{
typeValue = typeValue | Irq_Dio_A1_Edge;
}
}
else if (irqChannel->dioChannel == Irq_Dio_A2)
{
/*
* Clear the value of the masked bits in the DI configure register, then
* set which IO is enabled.
*/
cnfgValue = cnfgValue & (~Irq_Dio_A2_Enable);
cnfgValue = cnfgValue | Irq_Dio_A2_Enable;
/*
* Clear the value of the masked bits in the DI configure register, then
* set the triggered type.
*/
typeValue = typeValue & (~Irq_Dio_A2_Edge);
if (type == Irq_Dio_RisingEdge)
{
typeValue = typeValue | Irq_Dio_A2_RisingEdge;
}
else if (type == Irq_Dio_FallingEdge)
{
typeValue = typeValue | Irq_Dio_A2_FallingEdge;
}
else if (type == Irq_Dio_Edge)
{
typeValue = typeValue | Irq_Dio_A2_Edge;
}
}
else if (irqChannel->dioChannel == Irq_Dio_A3)
{
/*
* Clear the value of the masked bits in the DI configure register, then
* set which IO is enabled.
*/
cnfgValue = cnfgValue & (~Irq_Dio_A3_Enable);
cnfgValue = cnfgValue | Irq_Dio_A3_Enable;
/*
* Clear the value of the masked bits in the DI configure register, then
* set the triggered type.
*/
typeValue = typeValue & (~Irq_Dio_A3_Edge);
if (type == Irq_Dio_RisingEdge)
{
typeValue = typeValue | Irq_Dio_A3_RisingEdge;
}
else if (type == Irq_Dio_FallingEdge)
{
typeValue = typeValue | Irq_Dio_A3_FallingEdge;
}
else if (type == Irq_Dio_Edge)
{
typeValue = typeValue | Irq_Dio_A3_Edge;
}
}
/*
* Write the new value of the DI enable configure register to the device.
*/
status = NiFpga_WriteU8(myrio_session, irqChannel->dioIrqEnable, cnfgValue);
/*
* Check if there was an error writing to DI enable configure registers.
*
* If there was an error then print an error message to stdout and return configuration status.
*/
MyRio_ReturnStatusIfNotSuccess(status,
"Could not write to the DI enable configure registers!")
/*
* Write the new value of the DI rise-configure register to the device.
*/
status = NiFpga_WriteU8(myrio_session, irqChannel->dioIrqRisingEdge, (uint8_t) typeValue);
/*
* Write the new value of the DI fall-configure register to the device.
*/
NiFpga_MergeStatus(&status,
NiFpga_WriteU8(myrio_session, irqChannel->dioIrqFallingEdge, (uint8_t)(typeValue >> 8)));
/*
* Check if there was an error writing to DI rise/fall configure registers.
*
* If there was an error then print an error message to stdout.
*/
MyRio_ReturnStatusIfNotSuccess(status,
"Could not write to the DI rise/fall configure registers!")
/*
* Add the channel value and IRQ number in the list.
*/
Irq_AddReserved(irqChannel->dioChannel, irqNumber);
return NiMyrio_Status_Success;
}
/**
* Unreserve the interrupt from FPGA, and disable the particular digital IRQ IO,
* clear according channel value and IRQ number in the resource list.
* So the IO can be configured in the next time.
*
* @param[in] irqChannel A struct containing the registers and settings
* for a particular analog IRQ IO to modify.
* @param[in] irqContext IRQ context with to unreserve.
* @return the configuration status.
*/
int32_t Irq_UnregisterDiIrq(MyRio_IrqDi* irqChannel,
NiFpga_IrqContext irqContext,
uint8_t irqNumber)
{
int32_t status;
uint8_t cnfgValue;
/*
* Limit the IRQ number within a range,
* if the entered value is out of range, print an error message.
*/
if (irqNumber > IRQNO_MAX || irqNumber < IRQNO_MIN)
{
printf("The specified IRQ Number is out of range.\n");
return NiMyrio_Status_IrqNumberNotUsable;
}
/*
* Check if the specified IRQ resource is registered.
*/
status = Irq_CheckReserved(irqChannel->dioChannel, irqNumber);
if (status == NiMyrio_Status_Success)
{
/*
* Did not find the resource in the list
*/
printf("You didn't register an interrupt with this IRQ number.\n");
return NiMyrio_Status_Success;
}
/*
* Get the current value of the DI configure register.
*/
status = NiFpga_ReadU8(myrio_session, irqChannel->dioIrqEnable, &cnfgValue);
/*
* Check if there was an error reading from the DI configure register.
*
* If there was an error then print an error message to stdout and return configuration status.
*/
MyRio_ReturnStatusIfNotSuccess(status,
"Could not read from the DI configure register!")
/*
* Disable the specified channel.
*/
if (irqChannel->dioChannel == Irq_Dio_A0)
{
/*
* Clear the value of the masked bits in the DI configure register. This is
* done so DI0 is disabled.
*/
cnfgValue = cnfgValue & (~Irq_Dio_A0_Enable);
}
else if (irqChannel->dioChannel == Irq_Dio_A1)
{
/*
* Clear the value of the masked bits in the DI configure register. This is
* done so DI1 is disabled.
*/
cnfgValue = cnfgValue & (~Irq_Dio_A1_Enable);
}
else if (irqChannel->dioChannel == Irq_Dio_A2)
{
/*
* Clear the value of the masked bits in the DI configure register. This is
* done so DI2 is disabled.
*/
cnfgValue = cnfgValue & (~Irq_Dio_A2_Enable);
}
else if (irqChannel->dioChannel == Irq_Dio_A3)
{
/*
* Clear the value of the masked bits in the DI configure register. This is
* done so DI3 is disabled.
*/
cnfgValue = cnfgValue & (~Irq_Dio_A3_Enable);
}
/*
* Write the new value of the DI configure register to the device.
*/
status = NiFpga_WriteU8(myrio_session, irqChannel->dioIrqEnable, cnfgValue);
/*
* Check if there was an error writing to DI configure register.
*
* If there was an error then print an error message to stdout and return configuration status.
*/
MyRio_ReturnStatusIfNotSuccess(status,
"Could not write to the AI configure register!")
/*
* Remove the reserved resource in the list.
*/
status = Irq_RemoveReserved(irqNumber);
/*
* Check if there was an error releasing the resource from list.
*
* If there was an error then print an error message to stdout.
*/
MyRio_ReturnStatusIfNotSuccess(status,
"Could not release the irq resource!");
/*
* Unreserve an IRQ context obtained from Irq_ReserveIrqContext.
* The returned NiFpga_Status value is stored for error checking.
*/
status = NiFpga_UnreserveIrqContext(myrio_session, irqContext);
/*
* Check if there was an error when unreserve an IRQ.
*
* If there was an error then print an error message to stdout and return configuration status.
*/
MyRio_ReturnStatusIfNotSuccess(status,
"A required NiFpga_IrqContext was not unreserved.")
return NiMyrio_Status_Success;
}
// Initializes the FPGA, the sabertooth and the mototrs
void FPGA_Boot(void)
{
LOG.INFO("Initializing FPGA...");
FPGA_Status = NiFpga_Initialize();
if (NiFpga_IsNotError(FPGA_Status))
{
// opens a session, downloads the bitstream, and runs the FPGA.
LOG.INFO("Opening a session FPGA...");
NiFpga_MergeStatus(&FPGA_Status, NiFpga_Open(NiFpga_mainFPGA_Bitfile,
NiFpga_mainFPGA_Signature,
"RIO0",
NiFpga_OpenAttribute_NoRun,
&FPGA_Session));
if (NiFpga_IsNotError(FPGA_Status))
{
LOG.INFO("ReDownloading the FPGA");
NiFpga_MergeStatus(&FPGA_Status,NiFpga_Download(FPGA_Session));
if (NiFpga_IsNotError(FPGA_Status))
{
LOG.INFO("Restarting the FPGA");
NiFpga_MergeStatus(&FPGA_Status,NiFpga_Reset(FPGA_Session));
if (NiFpga_IsNotError(FPGA_Status))
{
LOG.INFO("Running the FPGA");
NiFpga_MergeStatus(&FPGA_Status,NiFpga_Run(FPGA_Session, 0));
if (NiFpga_IsNotError(FPGA_Status))
{
}
else
{
LOG.ERR("FPGA Fail to run fpga %d ",FPGA_Status);
}
}
else
{
LOG.ERR("FPGA Fail to redownload fpga %d ",FPGA_Status);
}
}
else
{
LOG.ERR("FPGA Fail to redownload fpga %d ",FPGA_Status);
}
}
else
{
LOG.ERR("FPGA Fail to open a session. Error Code %d ",FPGA_Status);
}
}
LOG.VERBOSE("Reading Constants for the fpga");
if(fileExists("config/odometry.ini"))
{
dictionary* config = iniparser_load("config/odometry.ini");
leftWheelConversionConstant = 1/(iniparser_getdouble(config,"MotorEncoderConstant:MeterPerTickLeft",0)) * PIDUpdateRateInMs/1000;
rightWheelConversionConstant = 1/(iniparser_getdouble(config,"MotorEncoderConstant:MeterPerTickRight",0)) * PIDUpdateRateInMs/1000;
iniparser_freedict(config);
LOG.VERBOSE("Odometry Ini file loaded for fpga!");
}
else
{
LOG.ERR("!!!!!!!!!!!!!!!!odomentry.ini was not found!!!!!!!!!!!!!!!!!!!!!!");
}
if(fileExists("config/pid.ini")) {
dictionary* config = iniparser_load("config/pid.ini");
max_pid_speed = (uint16_t) iniparser_getint(config, "Both:MaxSpeedTicksPerDt", 0);
left_pid_pro_gain = iniparser_getint(config, "LeftPID:ProportionalGain",0);
left_pid_int_gain = iniparser_getint(config, "LeftPID:IntegralGain",0);
left_pid_der_gain = iniparser_getint(config, "LeftPID:DerivativeGain", 0);
right_pid_pro_gain = iniparser_getint(config, "RightPID:ProportionalGain",0);
right_pid_int_gain = iniparser_getint(config, "RightPID:IntegralGain",0);
right_pid_der_gain = iniparser_getint(config, "RightPID:DerivativeGain",0);
iniparser_freedict(config);
LOG.VERBOSE("PID INI file loaded for FPGA");
}
else {
LOG.ERR("PID.ini was not found");
}
if(fileExists("config/CRIO.ini")) {
dictionary* config = iniparser_load("config/CRIO.ini");
sabertooth_address = (uint8_t) iniparser_getint(config, "Sabertooth:Address", 130);
iniparser_freedict(config);
LOG.VERBOSE("Sabertooth address loaded for FPGA");
}
else {
LOG.ERR("Unable to load Sabertooth address");
}
NiFpga_MergeStatus(&FPGA_Status,NiFpga_WriteU8(FPGA_Session,NiFpga_mainFPGA_ControlU8_SlewRateControl, 10));
if (NiFpga_IsError(FPGA_Status))
{
LOG.ERR("Failed to set Sabertooth slew rate");
}
FPGA_SetPIDdt(PIDUpdateRateInMs * 1000);
FPGA_setMaxPIDSpeed(max_pid_speed);
FPGA_setLPIDProGain(left_pid_pro_gain);
FPGA_setLPIDIntGain(left_pid_int_gain);
FPGA_setLPIDDerGain(left_pid_der_gain);
FPGA_setRPIDProGain(right_pid_pro_gain);
FPGA_setRPIDIntGain(right_pid_int_gain);
FPGA_setRPIDDerGain(right_pid_der_gain);
FPGA_setSabertoothAddress(sabertooth_address);
// This logs the version number
LOG.INFO("FPGA VERSION = %d",FPGA_GetVersion());
LOG.INFO("Turning on the motors");
FPGA_SetMotorStatus(1);
}
/**
* Overview:
* Demonstrates using the PWM. Generates a PWM signal from PWM 0 on
* connector A.
*
* Instructions:
* 1. Connect an oscilloscope to the PWM 0 pin on connector A.
* 2. Run this program.
*
* Output:
* The program generates a 25% duty cycle signal at 10 kHz for 60 s.
*
* Note:
* The Eclipse project defines the preprocessor symbol for the NI myRIO-1900.
* Change the preprocessor symbol to use this example with the NI myRIO-1950.
*/
int main(int argc, char **argv)
{
NiFpga_Status status;
MyRio_Pwm pwmA0;
uint8_t selectReg;
time_t currentTime;
time_t finalTime;
printf("PWM\n");
/*
* Initialize the PWM struct with registers from the FPGA personality.
*/
pwmA0.cnfg = PWMA_0CNFG;
pwmA0.cs = PWMA_0CS;
pwmA0.max = PWMA_0MAX;
pwmA0.cmp = PWMA_0CMP;
pwmA0.cntr = PWMA_0CNTR;
/*
* Open the myRIO NiFpga Session.
* This function MUST be called before all other functions. After this call
* is complete the myRIO target will be ready to be used.
*/
status = MyRio_Open();
if (MyRio_IsNotSuccess(status))
{
return status;
}
/*
* Set the waveform, enabling the PWM onboard device.
*/
Pwm_Configure(&pwmA0, Pwm_Invert | Pwm_Mode,
Pwm_NotInverted | Pwm_Enabled);
/*
* Set the clock divider. The internal PWM counter will increments at
* f_clk / 4
*
* where:
* f_clk = the frequency of the myRIO FPGA clock (40 MHz default)
*/
Pwm_ClockSelect(&pwmA0, Pwm_4x);
/*
* Set the maximum counter value. The counter counts from 0 to 1000.
*
* The counter increments at 40 MHz / 4 = 10 MHz and the counter counts
* from 0 to 1000. The frequency of the PWM waveform is 10 MHz / 1000
* = 10 kHz.
*/
Pwm_CounterMaximum(&pwmA0, 1000);
/*
* Set the comparison counter value. The PWM counter counts from 0 to 1000
* and outputs from 0 to the comparison value (250).
*
* The duty cycle is 250 / 1000 = 25%.
*/
Pwm_CounterCompare(&pwmA0, 250);
/*
* PWM outputs are on pins shared with other onboard devices. To output on
* a physical pin, select the PWM on the appropriate SELECT register. See
* the MUX example for simplified code to enable-disable onboard devices.
*
* Read the value of the SYSSELECTA register.
*/
status = NiFpga_ReadU8(myrio_session, SYSSELECTA, &selectReg);
MyRio_ReturnValueIfNotSuccess(status, status,
"Could not read from the SYSSELECTA register!")
/*
* Set bit2 of the SYSSELECTA register to enable PWMA_0 functionality.
* The functionality of the bit is specified in the documentation.
*/
selectReg = selectReg | (1 << 2);
/*
* Write the updated value of the SYSSELECTA register.
*/
status = NiFpga_WriteU8(myrio_session, SYSSELECTA, selectReg);
MyRio_ReturnValueIfNotSuccess(status, status,
"Could not write to the SYSSELECTA register!")
/*
* Normally, the main function runs a long running or infinite loop.
* Keep the program running for 60 seconds so that the PWM output can be
* observed using an external instrument.
*/
time(¤tTime);
finalTime = currentTime + LoopDuration;
while (currentTime < finalTime)
{
time(¤tTime);
}
/*
* Close the myRIO NiFpga Session.
* This function MUST be called after all other functions.
*/
status = MyRio_Close();
/*
* Returns 0 if successful.
*/
return status;
}
bool nifpga::WriteU8(uint32_t control, uint8_t value) {
if (sessionOpen) return HandleStatus(NiFpga_WriteU8(sessionHandle, control, value));
return false;
}
