Esempio n. 1
0
//  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);
}
Esempio n. 2
0
bool nifpga::Download() {
	bool status = false;
	if (sessionOpen) status = HandleStatus(NiFpga_Download(sessionHandle));
	if (status) sessionRunning = false;
	return status;
}