RobotConfiguration.cpp

#include <iostream>
#include <fstream>
#include <string>
#include <cctype>
#include <cstring>

#include "WPILib.h"
#include "RobotConfiguration.h"
#include "DebugUtil.h"

std::map<std::string, float> * RobotConfiguration::m_map = NULL;
bool RobotConfiguration::m_updated = false; 

void RobotConfiguration::Init()
{
	// Initialize the configuration.
	// Defaults should be referenced here 
	// in case the robot's file is gone.
	
	printf("Initializing configuration.\n");
	m_map = new std::map<std::string, float>();
	
	SetConfigurationVariable("DRIVE_TRAIN_P", 1);
	SetConfigurationVariable("DRIVE_TRAIN_I", 0);
	SetConfigurationVariable("DRIVE_TRAIN_D", 0);
	//1,.02,0.0
	SetConfigurationVariable("ARM_P", 1);
	SetConfigurationVariable("ARM_I", .02);
	SetConfigurationVariable("ARM_D", 0);
	SetConfigurationVariable("ARM_PID_TOLERANCE", 10);
	SetConfigurationVariable("ARM_INPUT_RANGE_MIN", -165);
	SetConfigurationVariable("ARM_INPUT_RANGE_MAX", 165);

	//4,.02,0
	SetConfigurationVariable("SHUTTLE_P", 4);
	SetConfigurationVariable("SHUTTLE_I", .02);
	SetConfigurationVariable("SHUTTLE_D", 0);
	SetConfigurationVariable("SHUTTLE_PID_TOLERANCE", 10);
	SetConfigurationVariable("SHUTTLE_INPUT_RANGE_MIN", -18);
	SetConfigurationVariable("SHUTTLE_INPUT_RANGE_MAX", 10);
	
	SetConfigurationVariable("DRIVE_MAX_OUTPUT", 150);
	SetConfigurationVariable("NORMAL_AUTO_SPEED", .4);
	SetConfigurationVariable("NORMAL_INSIDE_SPEED", .2);
	SetConfigurationVariable("NORMAL_OUTSIDE_SPEED", .6);
	SetConfigurationVariable("NORMAL_EXTRA_SPEED", .8);
	
	SetConfigurationVariable("GRABBER_FASTER_SPEED", .6);
	SetConfigurationVariable("GRABBER_SLOWER_SPEED", .4);
	SetConfigurationVariable("GRABBER_MIDDLE_SPEED", .5);
	
	SetConfigurationVariable("SHUTTLE_MOVE_TIME", 1);
	SetConfigurationVariable("SHUTTLE_UP_SPEED", .55);
	
	// Arm setpoints
	
	SetConfigurationVariable("POSE_SHUTTLE_LOW", -18);
	SetConfigurationVariable("POSE_SHUTTLE_HIGH", 8);
	SetConfigurationVariable("POSE_SHUTTLE_MID", 4);
	
	SetConfigurationVariable("POSE_ARM_1", -141);
	SetConfigurationVariable("POSE_ARM_2", -45);
	SetConfigurationVariable("POSE_ARM_3", -108);
	SetConfigurationVariable("POSE_ARM_4", -97);
	SetConfigurationVariable("POSE_ARM_5", -90);
	SetConfigurationVariable("POSE_ARM_6", -80);
	SetConfigurationVariable("POSE_ARM_7", -44);
	SetConfigurationVariable("POSE_ARM_8", -39);
	SetConfigurationVariable("POSE_ARM_9", 0);
	SetConfigurationVariable("POSE_ARM_18", -156);
	SetConfigurationVariable("POSE_ARM_0", -34);
	SetConfigurationVariable("AUTO_SCORE_DISTANCE", 15);
	SetConfigurationVariable("DEPLOY_SPEED", .6);
	
	
	SetConfigurationVariable("AUTO_BACKUP_SPEED", -.55);
	SetConfigurationVariable("AUTO_BACKUP_DISTANCE", -15);
	SetConfigurationVariable("AUTO_FINAL_BACKUP_DISTANCE", -30);
	
	SetConfigurationVariable("AUTO_HANGING_POSE", 1); 
	SetConfigurationVariable("AUTO_EJECT_TIME", 5);
	SetConfigurationVariable("AUTO_ROTATE_TIME", 3);

	SetConfigurationVariable("ARM_OUTPUT_MIN", -.4);
	SetConfigurationVariable("ARM_OUTPUT_MAX", .4);

	SetConfigurationVariable("SHUTTLE_OUTPUT_MIN", -1);
	SetConfigurationVariable("SHUTTLE_OUTPUT_MAX", 1);
	
	SetConfigurationVariable("AUTO_DRIVE_DISTANCE", 174);
	SetConfigurationVariable("ENABLE_LINE_FOLLOWING", 1);
	
	SetConfigurationVariable("DRIVE_STRAIGHT_P", .1);
	

	printf("Opening file.\n");
	bool success = OpenAndParseFile("config.txt");
	if (success) {
		printf("File read!\n");
		DumpConfiguration();
	} else {
		
		printf("File failed.\n");
		printf("Creating file...");
		RobotConfiguration::SaveSettings();
		printf("done.\n");
		
	}
	printf("Configuration initialization complete.\n");
	m_updated = true;
}

void RobotConfiguration::Process()
{
	// Nothing right now.
	
}

void RobotConfiguration::SaveSettings()
{
	std::ofstream config("config.txt");
	
	if (!config.is_open()) {
		printf("Config error: cannot save");
		return;
	}
	
	// Write out initial header.
	config << "# config.txt -- Configuration file for cRIO\n";
	config << "# Team 1741\n";
	config << std::endl;
	
	// Write out settings.
	
	std::map<std::string, float>::const_iterator itr;
	
	for (itr = m_map->begin(); itr != m_map->end(); ++itr) {
		std::string key = itr->first;
		float value = itr->second;
		
		// These are stored in some order to minimize the access
		// time of retrieval, so that might not make sense in the output.
		
		config << key << " " << value << std::endl;
	}
	
	config << std::endl;
	
	config.close();
	return;
	
}

void RobotConfiguration::DumpConfiguration()
{
	std::map<std::string, float>::const_iterator itr;
		
	printf("Configuration Variables:\n");
	for (itr = m_map->begin(); itr != m_map->end(); ++itr) {
		std::string key = itr->first;
		float value = itr->second;
			
		// These are stored in some order to minimize the access
		// time of retrieval, so that might not make sense in the output.
			
		printf("\t%s = %2.2f\n",key.c_str(), value);
	}
		
	printf("Dump complete.\n");
}

void RobotConfiguration::SetDefaults()
{
	RobotConfiguration::SetDriveTrainPID(1.0, 0.2, 0.0);
	
}

void RobotConfiguration::SetDriveTrainPID(float p, float i, float d)
{
	SetConfigurationVariable("DRIVE_TRAIN_P", p);
	SetConfigurationVariable("DRIVE_TRAIN_I", i);
	SetConfigurationVariable("DRIVE_TRAIN_D", d);
}
float RobotConfiguration::GetDriveTrainD()
{
	return GetConfigurationVariable("DRIVE_TRAIN_D");
}

float RobotConfiguration::GetDriveTrainI()
{
	return GetConfigurationVariable("DRIVE_TRAIN_I");
}

float RobotConfiguration::GetDriveTrainP()
{
	return GetConfigurationVariable("DRIVE_TRAIN_P");
}

bool RobotConfiguration::StringsEqual(char *a, char *b)
{
	while (*a != '\0' && *b != '\0') {
		if (toupper(*a) != toupper(*b))
		{
			return false;
		}
		
		a++; b++;
		
	}
	if (*a != '\0' || *b != '\0') {
		return false;
	}
	return true;
}
bool RobotConfiguration::OpenAndParseFile(char * filename)
{
	std::ifstream config(filename);
	if (!config.is_open()) {
		printf("Config error: cannot open file '%s'\n",
			filename);
		return false;
	} else {
		int line_number = 0;
		while (!config.eof()) {
			++line_number; 
			std::string entire_line = "";
			bool is_comment = false;
			bool is_blank = true;
			getline(config, entire_line);
			//printf("Read line: '%s'\n", entire_line.c_str());
			// Check to see if the line is a comment.
			for (unsigned int i = 0; i < entire_line.length(); ++i) {
				if (isspace(entire_line[i])) {
					continue; // Ignore whitespace
				} else if (entire_line[i] == '#') {
					// If we have a comment character
					is_comment = true;
					is_blank = false;
					break;
				} else {
					// Line is NOT a comment.
					is_comment = false;
					is_blank = false;
					break;
				}
			} // End for loop
			if (is_blank) {
				//printf("Line is blank.\n");
				continue;
			}
			
			if (is_comment) { 
				//printf("Line is comment.\n");
				continue; // Move to next line. 
				// Continue means the rest of the loop 
				// is skipped, we go back up to the
				// top where we read in another line.
			}
			
			// Ok, at this point we know that the line
			// contains a field name and value. Let's find 
			// out which one.
			// I could use string.find, but I'm going
			// to use a custom loop so I can use
			// tabs and other stuff in the config file.
			
			std::string field = "";
			std::string value = "";
			int split_point = -1;
			
			for (unsigned int i = 0; i < entire_line.length(); ++i)
			{
				if (isspace(entire_line[i])) {
					split_point = i;
					break;
				}
			}
			
			if (split_point == -1) {
				printf(	"Config Error: invalid syntax at line %d in '%s': '%s'\n",
						line_number, filename,
						entire_line.c_str());
				// Skip this for now.
				// Later we might want to stop parsing here.
				continue;
			} else {
				field = entire_line.substr(0, split_point);
				value = entire_line.substr(split_point+1, 
						entire_line.length() - split_point);
				
				printf("Config: '%s' = '%s'\n", field.c_str(), value.c_str());
			}
			std::remove(field.begin(), field.end(), ' ');
			std::remove(value.begin(), value.end(), ' ');
			// Ok, let's actually do the work of getting the values.
			
			if (!HasConfigurationVariable(field)) {
				printf("Config: Unknown field '%s'\n", field.c_str());
			} else {
				SetConfigurationVariable(field, atof(value.c_str()));
			}
		}
		config.close();
		
		return true;
	}
	
}

float RobotConfiguration::GetConfigurationVariable( std::string key )
{
	float value = 0;
	if (!HasConfigurationVariable(key)) {
		// element was not found. Complain.
		DPRINTF("Hey! Configuration variable '%s' was not found. Returning 0.", key.c_str() );
		value = 0;
	} else {
		value = (*m_map)[key]; 
	}
	
	return value;
}

void RobotConfiguration::SetConfigurationVariable(std::string key, float value)
{
	// According to std library, if key doesn't exist, it's created. 
	(*m_map)[key] = value;
}

bool RobotConfiguration::HasConfigurationVariable(std::string key)
{
	std::map<std::string, float>::const_iterator itr = m_map->find( key );
	return (itr != m_map->end() );
}