std::ostream &operator<<( std::ostream &dest, const SudokuBoard &source )
{
int** board = source.getBoard();
int board_size = source.getN();
Odometer odo;
dest << to_string(source.getN()) + " " + to_string(source.getQ()) + " " + to_string(source.getP()) + "\n";
for (int row = 0; row < board_size; row++)
{
for (int col = 0; col < board_size; col++ )
{
int board_val = board[row][col];
string to_add = to_string(board_val);
if (board_val > 9){
to_add = odo.intToOdometer(board_val);
}
if(col != board_size-1){
dest << to_add + "|";
}
else{
dest << to_add + "\n";
}
}
}
return dest;
}
void setup() {
Serial.begin(BAUDRATE);
ENC_LEFT.attach(ENC_LEFT_PIN);
ENC_RIGHT.attach(ENC_RIGHT_PIN);
ENC_LEFT.begin();
ENC_RIGHT.begin();
PROX.attach(PROX_TRIGGER_PIN, PROX_ECHO_PIN);
MOTORS.init();
GYRO.attach();
GYRO.begin();
}
Odometer Odometer::operator++(int) {
/*Pre:
Purpose: increment the mileage for the Odometer
Post: return a temp Odometer
*/
Odometer temp;
temp.setMileage(mileage);
if (mileage >= MAX)
mileage = 0;
else
mileage++;
return *this;
}
/**
* This function gets called when the microcontroller publishes new
* state information.
*/
void onStateUpdate(const sb_msgs::RobotStateConstPtr& new_state)
{
odometer.onNewReading(new_state->odometer);
double time = ros::Time::now().toSec();///replace with t.to_nsec for nono seconds
double distance = odometer.getDistance();
double throttle = controller.onNewReading(distance, time);
if (throttle < 0.0) {
throttle = sb_util::clamp(throttle, -MAX_THROTTLE, -MIN_THROTTLE);
}
else {
throttle = sb_util::clamp(throttle, MIN_THROTTLE, MAX_THROTTLE);
}
throttle_cmd.value = throttle_ctl.getRawValue(throttle);
cmd_pub.publish(throttle_cmd);
ROS_INFO("%5.2f, %.2f, %.2f, %i",
time, distance, throttle, throttle_cmd.value);
}
int main(int argc, char** argv)
{
/* Initialize node */
ROS_INFO("Starting %s", NODE_NAME.c_str());
ros::init(argc, argv, NODE_NAME);
ros::NodeHandle node;
/* Load configuration file */
std::string cfgfile;
if (! sb_config::findConfigFile(argc, argv, cfgfile) ) {
ROS_FATAL("Can't find configuration file.");
ros::shutdown();
}
ROS_INFO("Loading configuration in %s", cfgfile.c_str());
throttle_ctl.loadConfig(cfgfile);
steering_ctl.loadConfig(cfgfile);
odometer.loadConfig(cfgfile);
controller.loadConfig(cfgfile);
/* Initialize the ServoCommand messages. */
throttle_cmd.id = throttle_ctl.getId();
throttle_cmd.value = 0;
steering_cmd.id = steering_ctl.getId();
steering_cmd.value = 0;
/* Create publisher and subscribers */
cmd_pub = node.advertise<sb_msgs::ServoCommand>(
CMD_TOPIC, MSG_QUEUE_SIZE
);
ros::Subscriber spd_sub = node.subscribe(
SPEED_TOPIC, MSG_QUEUE_SIZE, onVelocityUpdate
);
ros::Subscriber state_sub = node.subscribe(
STATE_TOPIC, MSG_QUEUE_SIZE, onStateUpdate
);
/* Run the main loop */
ROS_INFO("time, distance, throttle, servo");
while (ros::ok()) {
ros::spin();
}
ROS_INFO("Shutting down %s", NODE_NAME.c_str());
}
int main(int argc, const char* argv[])
{
auto time_start = std::chrono::system_clock::now().time_since_epoch();
if (argc < 4){
std::cout << "error" << std::endl;
return 1; //Should throw an exception here (?)
}
std::string token = "";
for (int j = 2; j < argc; j++){
token += argv[j];
}
bool FC = false;
bool MRV = false;
bool LCV = false;
bool DH = false;
if (token.find("GEN") != std::string::npos){
std::string output_name = argv[1];
int N = atoi(argv[3]);
int p = atoi(argv[4]);
int q = atoi(argv[5]);
int numAssignments = atoi(argv[6]);
SudokuBoardGenerator sudokuGenerator(N, p, q, numAssignments);
SudokuBoardReader sudokuReader = SudokuBoardReader();
std::vector<std::vector<int>> board = sudokuGenerator.returnBoard();
Odometer o;
int end = N - 1;
std::ofstream output_file(output_name);
if (output_file.is_open()){
output_file << N << " " << p << " " << q << " " << numAssignments << "\r\n";
for (int row = 0; row < N; ++row){
for (int col = 0; col < N; ++col){
output_file << o.intToOdometer(board[row][col]) << " ";
}
output_file << "\r\n";
}
}
}
else{
SudokuBoardReader sudokuReader = SudokuBoardReader();
SudokuFile input_sf;
std::string input_file = argv[1];
std::string output_name = argv[2];
long timeout = atoi(argv[3]);
std::cout << token << "; " << input_file << ", " << output_name << ", " << timeout << std::endl;
if (token.find("FC") != std::string::npos){
FC = true;
}
if (token.find("MRV") != std::string::npos){
MRV = true;
}
if (token.find("LCV") != std::string::npos){
LCV = true;
}
if (token.find("DH") != std::string::npos){
DH = true;
}
input_sf = sudokuReader.readFile(input_file);
std::ofstream output_file(output_name);
if (output_file.is_open()){
output_file << "TOTAL_START=";
//output_file << total_start time here
output_file << std::chrono::duration_cast<std::chrono::seconds>(time_start).count();
output_file << std::endl;
output_file << "\r\nPREPROCESSING_START=";
output_file << " 4"; //skip for now
output_file << "\r\nPREPROCESSING_DONE=";
output_file << "4"; //skip for now
output_file << "\r\nSEARCH_START=";
//start timer here
std::cout << timeout << FC << DH << LCV << MRV << std::endl;
SudokuBoardSolver SBS(input_sf, timeout, FC, DH, LCV, MRV);
auto search_start = SBS.getStartTime();
output_file << std::chrono::duration_cast<std::chrono::seconds>(search_start).count();
//std::cout << input_sf.toString() << std::endl;
output_file << "\r\nSEARCH_DONE=";
//get timer here math
auto search_done = std::chrono::system_clock::now().time_since_epoch();
output_file << std::chrono::duration_cast<std::chrono::seconds>(search_done).count() << "\n";
output_file << "\r\nSOLUTION_TIME=";
//do search_done - search start for now
output_file << std::chrono::duration_cast<std::chrono::seconds>(search_done - search_start).count() << "\n";
output_file << "\r\nSTATUS=";
//if success|timeout|error
if (SBS.returnResultType() == 1){
output_file << "success";
}
else if (SBS.returnResultType() == 3){
output_file << "timeout";
}
else if (SBS.returnResultType() == 4){
output_file << "error";
}
output_file << "\r\nSOLUTION=(";
//print solution
std::vector<std::vector<int>> board = SBS.returnBoard();
Odometer o;
int N = SBS.getN();
int end = N-1;
for (int row = 0; row < N; ++row){
for (int col = 0; col < N; ++col){
output_file << o.intToOdometer(board[row][col]);
if ((row == end) && (col == end)){
output_file << ")";
}
else{
output_file << ",";
}
}
}
output_file << "\r\nCOUNT_NODES=";
//get node
output_file << SBS.getNumNode();
output_file << "\r\nCOUNT_DEADENDS=";
//get num of backtracks
output_file << SBS.getBacktrack();
}
}
return 0;
}
int main(int argc, char** argv) {
Odometer od;
od.nextReading("1234");
return 0;
}
