void start_fpga(NiFpga_Session* session, NiFpga_Status* status)
{
// must be called before any other calls
*status = NiFpga_Initialize();
printf("initializing FPGA read AI AO\n");
printf("bitfile expected at:\n");
printf(NiFpga_FPGA_read_ai_ao_Bitfile);
printf("\n");
//printf("Current working dir: %s\n", strcat(cwd, '../labview_fpga_lib/read_ai_ao/'));
if (NiFpga_IsNotError(*status))
{
// opens a session, downloads the bitstream, and runs the FPGA
NiFpga_MergeStatus(status, NiFpga_Open(NiFpga_FPGA_read_ai_ao_Bitfile,
NiFpga_FPGA_read_ai_ao_Signature,
"RIO0",
NiFpga_OpenAttribute_NoRun,
session));
if (NiFpga_IsNotError(*status))
{
// run the FPGA application
NiFpga_MergeStatus(status, NiFpga_Run(*session, 0));
}
else{
// print warning
printf("error occurred at FPGA open");
printf("%d", *status);
}
}
fflush(stdout);
}
void lia_readI32(double control[], int32_t value[])
{
/* Create status, initialize to good */
NiFpga_Status status = NiFpga_Status_Success;
/* Create a session variable */
NiFpga_Session session;
/* Load the NiFpga library */
NiFpga_Initialize();
/* Load bitfile */
NiFpga_Open(NiFpga_lockin_fpga_Bitfile, NiFpga_lockin_fpga_Signature, "RIO0", NiFpga_OpenAttribute_NoRun,&session);
/* Read value from control */
NiFpga_ReadI32(session, control[0], value);
/* Close session */
NiFpga_Close(session,0);
NiFpga_Finalize();
}
// 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);
}
/**
* Opens a session to the myRIO FPGA Personality.
*
* This function ensures that the NiFpga library is loaded and that the correct
* myRIO personality bitfile is programmed to the FPGA, started, and is
* running. The myRIO personality bitfile (.lvbitx file) for your target
* must be in the same directory as where the executable is run otherwise the
* function will fail.
*
* @warning This function is not thread-safe.
* It should be called before all other function calls and only once
* per application.
*
* @return NiFpga_Status which indicates if the operation was successful.
*/
NiFpga_Status MyRio_Open()
{
const uint32_t timeoutDelay = 5; /* 5 seconds */
NiFpga_Status status;
NiFpga_Bool sysReady;
time_t currentTime;
time_t finalTime;
/*
* Initialize the NiFpga Library.
*/
status = NiFpga_Initialize();
if (MyRio_IsNotSuccess(status))
{
MyRio_PrintStatus(status);
printf("Could not load NiFpga library!\n");
return status;
}
/*
* Open the appropriate FPGA bitfile.
*/
status = NiFpga_Open(MyRio_BitfilePath,
MyRio_Signature, "RIO0", NiFpga_OpenAttribute_NoRun,
&myrio_session);
if (MyRio_IsNotSuccess(status))
{
MyRio_PrintStatus(status);
printf("Could not Open FPGA!\n");
if (status == NiFpga_Status_BitfileReadError)
{
printf("Ensure the bitfile %s exists\n", MyRio_BitfilePath);
}
return status;
}
/*
* Ensure that the FPGA code is reset and in a known state.
*/
status = NiFpga_Reset(myrio_session);
if (MyRio_IsNotSuccess(status))
{
MyRio_PrintStatus(status);
printf("Could not Reset FPGA!\n");
return status;
}
/*
* Start the FPGA code.
*/
status = NiFpga_Run(myrio_session, 0);
if (MyRio_IsNotSuccess(status))
{
MyRio_PrintStatus(status);
printf("Could not Run FPGA!\n");
return status;
}
/*
* Wait for the FPGA to signal ready
*/
time(¤tTime);
finalTime = currentTime + timeoutDelay;
sysReady = NiFpga_False;
while (currentTime < finalTime && !MyRio_IsNotSuccess(status) && !sysReady)
{
time(¤tTime);
NiFpga_MergeStatus(&status,
NiFpga_ReadBool(myrio_session, SYSRDY, &sysReady));
}
if (MyRio_IsNotSuccess(status))
{
MyRio_PrintStatus(status);
printf("Could not read the system ready register!\n");
return status;
}
if (!sysReady)
{
printf("Problem waiting for system ready!\n");
printf("Timed out\n");
return NiFpga_Status_InternalError;
}
return NiFpga_Status_Success;
}
int main()
{
double desired_angle;
int32_t desired_tick;
int32_t Kd = .5 * ACCURACY;
int32_t Kp = 5 * ACCURACY;
int32_t Ki = .0001 * ACCURACY;
int32_t error=0xFFFF;
/* must be called before any other calls */
printf("Initializing...\n");
NiFpga_Status status = NiFpga_Initialize();
if (NiFpga_IsNotError(status))
{
NiFpga_Session session;
/* opens a session, downloads the bitstream, and runs the FPGA */
printf("Opening a session...\n");
NiFpga_MergeStatus(&status,
NiFpga_Open(NiFpga_Math_Pid_Bitfile, NiFpga_Math_Pid_Signature,
"rio://146.6.84.251/RIO0",
NiFpga_OpenAttribute_NoRun,&session));
if (NiFpga_IsNotError(status))
{
/* run the FPGA application */
printf("Running the FPGA...\n");
NiFpga_MergeStatus(&status, NiFpga_Run(session, 0));
/********************************************************************/
/* Code goes here */
NiFpga_MergeStatus(&status,
NiFpga_WriteBool(session,NiFpga_Encoder_Reset,0));
NiFpga_MergeStatus(&status,
NiFpga_WriteI32(session,NiFpga_Kd,Kd));
NiFpga_MergeStatus(&status,
NiFpga_WriteI32(session,NiFpga_Kp,Kp));
NiFpga_MergeStatus(&status,
NiFpga_WriteI32(session,NiFpga_Ki,Ki));
NiFpga_MergeStatus(&status,
NiFpga_WriteI32(session,NiFpga_Accuracy,ACCURACY));
printf("Enter Desired(in degrees): ");
scanf("%lf",&desired_angle);
desired_tick = degree_tick(desired_angle);
printf("Desired(in ticks) %d\n", desired_tick);
NiFpga_MergeStatus(&status,
NiFpga_WriteI32(session,NiFpga_Desired,desired_tick));
printf("Entering loop\n");
while(error != 0) {
NiFpga_MergeStatus(&status,
NiFpga_ReadI32(session,NiFpga_RealError,&error)); printf("Error:%d\n", error);
}
/********************************************************************/
/* Stopping */
printf("Press <Enter> to stop and quit...");
getchar();
/* stop the FPGA loops */
printf("Stopping the FPGA...\n");
/* close the session now that we're done */
printf("Closing the session...\n");
NiFpga_MergeStatus(&status, NiFpga_Close(session, 0));
}
/* must be called after all other calls */
printf("Finalizing...\n");
NiFpga_MergeStatus(&status, NiFpga_Finalize());
}
/* check if anything went wrong */
if (NiFpga_IsError(status))
{
printf("Error %d!\n", status);
printf("Press <Enter> to quit...\n");
getchar();
}
return status;
}
bool nifpga::Open(char* bitfile, char* signature, char* resource, uint32_t attribute) {
bool status = false;
if (driverInitialized && !sessionOpen) status = HandleStatus(NiFpga_Open(bitfile, signature, resource, attribute, &sessionHandle));
if (status) sessionOpen = true;
return status;
}
