void FPGA_setRPIDDerGain(int16_t gain) {
NiFpga_MergeStatus(&FPGA_Status,NiFpga_WriteI16(FPGA_Session,NiFpga_mainFPGA_ControlI16_RPIDDerGain,gain));
if (NiFpga_IsError(FPGA_Status))
{
LOG.ERR("Set right PID derivative gain failed");
}
}
void FPGA_SetPIDdt(uint16_t dt) {
NiFpga_MergeStatus(&FPGA_Status,NiFpga_WriteU16(FPGA_Session,NiFpga_mainFPGA_ControlU16_PIDdt, dt));
if (NiFpga_IsError(FPGA_Status))
{
LOG.ERR("Set PID dt failed");
}
}
// 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 FPGA_setLPIDIntGain(int16_t gain) {
NiFpga_MergeStatus(&FPGA_Status,NiFpga_WriteI16(FPGA_Session,NiFpga_mainFPGA_ControlI16_LPIDIntGain,gain));
if (NiFpga_IsError(FPGA_Status))
{
LOG.ERR("Set left PID integral gain failed");
}
}
void FPGA_setMaxPIDSpeed(uint16_t max) {
NiFpga_MergeStatus(&FPGA_Status,NiFpga_WriteU16(FPGA_Session,NiFpga_mainFPGA_ControlU16_maxPIDSpeedTicksPerDt, max));
if (NiFpga_IsError(FPGA_Status))
{
LOG.ERR("Set max PID speed failed");
}
}
uint16_t FPGA_GetRCCH3() {
uint16_t retval;
NiFpga_MergeStatus(&FPGA_Status,NiFpga_ReadU16(FPGA_Session,NiFpga_mainFPGA_IndicatorU16_C3Mode,&retval));
if (NiFpga_IsError(FPGA_Status))
{
LOG.ERR("Get RC CH3 Read Failed.");
}
return retval;
}
int16_t FPGA_GetCRIOVLineRead() {
int16_t retval;
NiFpga_MergeStatus(&FPGA_Status,NiFpga_ReadI16(FPGA_Session,NiFpga_mainFPGA_IndicatorI16_cRIOVMonitormV,&retval));
if (NiFpga_IsError(FPGA_Status))
{
LOG.ERR("Get cRIOV Line Read Failed.");
}
return retval;
}
// Returns the tick value of the right motor
int32_t FPGA_GetRightMotorTicks()
{
int32_t ticks;
NiFpga_MergeStatus(&FPGA_Status,NiFpga_ReadI32(FPGA_Session,NiFpga_mainFPGA_IndicatorI32_RPosMotor,&ticks));
if (NiFpga_IsError(FPGA_Status))
{
LOG.ERR("Get Right Motor Ticks Failed.");
}
return ticks;
}
int32_t FPGA_GetGPSArrayData(uint8_t * data, uint32_t length)
{
NiFpga_MergeStatus(&FPGA_Status,NiFpga_ReadArrayU8(FPGA_Session, NiFpga_mainFPGA_IndicatorArrayU8_GPSPOSBytes, data, length));
if (NiFpga_IsError(FPGA_Status))
{
LOG.ERR("GetArrayFailed");
}
return 0;
}
void FPGA_SetMotorStatus(int value)
{
if(value)
{
NiFpga_MergeStatus(&FPGA_Status,NiFpga_WriteBool(FPGA_Session,NiFpga_mainFPGA_ControlBool_enableMotors,NiFpga_True));
if (NiFpga_IsError(FPGA_Status))
{
LOG.ERR("Set Motor Status Failed.");
}
}
else
{
NiFpga_MergeStatus(&FPGA_Status,NiFpga_WriteBool(FPGA_Session,NiFpga_mainFPGA_ControlBool_enableMotors,NiFpga_False));
if (NiFpga_IsError(FPGA_Status))
{
LOG.ERR("Set Motor Status Failed.");
}
}
}
int16_t FPGA_GetVersion()
{
int16_t version;
NiFpga_MergeStatus(&FPGA_Status,NiFpga_ReadI16(FPGA_Session,NiFpga_mainFPGA_IndicatorI16_FPGAVersion,&version));
if (NiFpga_IsError(FPGA_Status))
{
LOG.ERR("Get FPGA Version Failed.");
}
return version;
}
// Returns the tick value on the right wheel
int32_t FPGA_GetRightWheelTicks()
{
int32_t ticks;
NiFpga_MergeStatus(&FPGA_Status,NiFpga_ReadI32(FPGA_Session,NiFpga_mainFPGA_IndicatorI32_RPosWheel,&ticks));
if (NiFpga_IsError(FPGA_Status))
{
LOG.ERR("Get Right Wheel Ticks Failed.");
}
LOG.DATA("RwT = %d",ticks);
return ticks;
}
int16_t FPGA_GetYawRef()
{
int16_t value;
NiFpga_MergeStatus(&FPGA_Status,NiFpga_ReadI16(FPGA_Session,NiFpga_mainFPGA_IndicatorI16_YawRefmV,&value));
if (NiFpga_IsError(FPGA_Status))
{
LOG.ERR("Get Yaw Ref Failed.");
}
LOG.DATA("YawRef = %d",value);
return value;
}
// Returns the velocity of the left motor
float FPGA_GetLeftMotorVelocity()
{
int16_t ticks;
NiFpga_MergeStatus(&FPGA_Status,NiFpga_ReadI16(FPGA_Session,NiFpga_mainFPGA_IndicatorI16_LeftMotorVTicksPerDt,&ticks));
if (NiFpga_IsError(FPGA_Status))
{
LOG.ERR("Get Left Motor Velocity Failed.");
}
float Speed = ticks /leftWheelConversionConstant;
LOG.DATA("LMV = %f",Speed);
return Speed;
}
float FPGA_GetCompassHeading(void)
{
uint32_t current;
NiFpga_MergeStatus(&FPGA_Status,NiFpga_ReadU32(FPGA_Session,NiFpga_mainFPGA_IndicatorU32_HeadingFloat,¤t));
if(NiFpga_IsError(FPGA_Status))
{
LOG.ERR("Get Compass Heading Failed");
}
float out;
out = *((float *)¤t);
LOG.DATA("Compass %f",out);
return out;
}
uint32_t FPGA_IsCompassNew(void)
{
uint32_t current;
NiFpga_MergeStatus(&FPGA_Status,NiFpga_ReadU32(FPGA_Session,NiFpga_mainFPGA_IndicatorU32_CompassResponseCount,¤t));
if(NiFpga_IsError(FPGA_Status))
{
LOG.ERR("Get Compass Counter Failed");
}
if(current != FPGA_IsCompassNewPrevious)
{
current = FPGA_IsCompassNewPrevious;
return 1;
}
return 0;
}
uint32_t FPGA_IsGPSNew(void)
{
uint32_t current;
NiFpga_MergeStatus(&FPGA_Status,NiFpga_ReadU32(FPGA_Session,NiFpga_mainFPGA_IndicatorU32_GPSPOSCount,¤t));
if(NiFpga_IsError(FPGA_Status))
{
LOG.ERR("Get GPS Counter Failed");
}
if(current != FPGA_IsGPSNewPrevious)
{
FPGA_IsGPSNewPrevious = current;
return 1;
}
return 0;
}
// Sets the left wheel velocity
void FPGA_SetLeftWheelVelocity(float MetersPerSecond)
{
/* Check if NAN */
if (MetersPerSecond != MetersPerSecond)
{
MetersPerSecond = 0.0;
LOG.ERR("Left Wheel command was NaN. Sending 0 instead");
}
int16_t ticks = MetersPerSecond * leftWheelConversionConstant;
LOG.DATA("LwT Commanded: %d for %f", ticks, MetersPerSecond);
NiFpga_MergeStatus(&FPGA_Status,NiFpga_WriteI16(FPGA_Session,NiFpga_mainFPGA_ControlI16_PIDLCmdTicksPerDt,ticks));
if (NiFpga_IsError(FPGA_Status))
{
LOG.ERR("Setting Left Wheel Velocity Failed.");
}
}
uint8_t FPGA_GetRCESTOP() {
NiFpga_Bool rcestop;
NiFpga_MergeStatus(&FPGA_Status,NiFpga_ReadBool(FPGA_Session,NiFpga_mainFPGA_IndicatorBool_RCeSTOP,&rcestop));
if (NiFpga_IsError(FPGA_Status))
{
LOG.ERR("Get RCEStop Failed.");
}
if(rcestop == NiFpga_False)
{
LOG.DATA("RCEstop = False");
return 0;
}
else
{
LOG.DATA("RCEStop = True");
return 1;
}
}
uint8_t FPGA_GetESTOPTriggered()
{
NiFpga_Bool ReStopTriggered;
NiFpga_MergeStatus(&FPGA_Status,NiFpga_ReadBool(FPGA_Session,NiFpga_mainFPGA_IndicatorBool_eSTOPTriggered,&ReStopTriggered));
if (NiFpga_IsError(FPGA_Status))
{
LOG.ERR("Get EStop Status Failed.");
}
if(ReStopTriggered == NiFpga_False)
{
LOG.DATA("EStopTriggered = False");
return 0;
}
else
{
LOG.DATA("EStopTriggered = True");
return 1;
}
return 0;
}
uint8_t FPGA_GetRCOn()
{
NiFpga_Bool RC;
NiFpga_MergeStatus(&FPGA_Status,NiFpga_ReadBool(FPGA_Session,NiFpga_mainFPGA_IndicatorBool_RCOn,&RC));
if (NiFpga_IsError(FPGA_Status))
{
LOG.ERR("Get RCon Status Failed.");
}
if(RC == NiFpga_False)
{
LOG.DATA("RCOn = False");
return 0;
}
else
{
LOG.DATA("RCOn = True");
return 1;
}
return 0;
}
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;
}
// 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);
}
