<commit_before>#include "ros/ros.h"
#include "std_msgs/Float32.h"
#include "std_msgs/Float64MultiArray.h"
#include "Robosub/HighLevelControl.h"
#include "Robosub/Point.h"
#include "SubMotorController/MotorMessage.h"

#include <string>

using namespace std;

#define LEFT_DRIVE_BIT  0x01
#define RIGHT_DRIVE_BIT 0x02
#define FRONT_DEPTH_BIT 0x04
#define REAR_DEPTH_BIT  0x08
#define FRONT_TURN_BIT  0x10
#define REAR_TURN_BIT   0x20

const double FORWARD_SPEED_CONST = .0000006;
const double FORWARD_DRAG_CONST = .99;
const double STRAF_SPEED_CONST = .0000004;
const double STRAF_DRAG_CONST = .98;

enum Mode {
	MANUAL,
	AUTOMATIC
};

const double Epsilon = .4;

Mode ForwardMode = MANUAL;
Mode StrafMode = MANUAL;
Mode TurnMode = MANUAL;
Mode DepthMode = MANUAL;
Mode YawMode = MANUAL;
Mode PitchMode = MANUAL;

double ForwardSpeed = 0;
double StrafSpeed = 0;
double TurnSpeed = 0;
double DepthSpeed = 0;
double YawSpeed = 0;
double PitchSpeed = 0;

double ForwardOffset = 0;
double StrafOffset = 0;
double TurnOffset = 0;
double DepthOffset = 0;
double YawOffset = 0;
double PitchOffset = 0;

double CurrentDepth = 0;
double CurrentYaw = 0;
double CurrentTurn = 0;
double CurrentPitch = 0;
double ForwardVelocity = 0;
double StrafVelocity = 0;

bool DepthInMotion = false;
ros::Publisher* motorPub;

void CenterOnPointCallback(Robosub::Point::ConstPtr msg) {
	TurnMode = AUTOMATIC;
	TurnOffset = msg->x * 60 / 1640;
	DepthMode = AUTOMATIC;
	DepthOffset = -msg->y / 80.0;
}

void commandCallback(Robosub::HighLevelControl::ConstPtr msg) {
	if(msg->Direction == "Forward" && msg->MotionType == "Offset") {
		ForwardMode = AUTOMATIC;
		ForwardOffset = msg->Value;
	} else if (msg->Direction == "Forward" && msg->MotionType == "Manual") {
		ForwardMode = MANUAL;<commit_msg>small changes to High Level controller<commit_after>#include "ros/ros.h"
#include "std_msgs/Float32.h"
#include "std_msgs/Float64MultiArray.h"
#include "Robosub/HighLevelControl.h"
#include "Robosub/Point.h"
#include "SubMotorController/MotorMessage.h"

#include <string>

using namespace std;

#define LEFT_DRIVE_BIT  0x01
#define RIGHT_DRIVE_BIT 0x02
#define FRONT_DEPTH_BIT 0x04
#define REAR_DEPTH_BIT  0x08
#define FRONT_TURN_BIT  0x10
#define REAR_TURN_BIT   0x20

const double FORWARD_SPEED_CONST = .0000006;
const double FORWARD_DRAG_CONST = .99;
const double STRAF_SPEED_CONST = .0000004;
const double STRAF_DRAG_CONST = .98;

enum Mode {
	MANUAL,
	AUTOMATIC
};

const double Epsilon = .4;

Mode ForwardMode = MANUAL;
Mode StrafMode = MANUAL;
Mode TurnMode = MANUAL;
Mode DepthMode = MANUAL;
Mode YawMode = MANUAL;
Mode PitchMode = MANUAL;

double ForwardSpeed = 0;
double StrafSpeed = 0;
double TurnSpeed = 0;
double DepthSpeed = 0;
double YawSpeed = 0;
double PitchSpeed = 0;

double ForwardOffset = 0;
double StrafOffset = 0;
double TurnOffset = 0;
double DepthOffset = 0;
double YawOffset = 0;
double PitchOffset = 0;

double CurrentDepth = 0;
double CurrentYaw = 0;
double CurrentTurn = 0;
double CurrentPitch = 0;
double ForwardVelocity = 0;
double StrafVelocity = 0;

bool DepthInMotion = false;
ros::Publisher* motorPub;

void CenterOnPointCallback(Robosub::Point::ConstPtr msg) {
	TurnMode = AUTOMATIC;
	TurnOffset = msg->x * 60 / 940;
	DepthMode = AUTOMATIC;
	DepthOffset = (40 - msg->y) / 150.0;
}

void commandCallback(Robosub::HighLevelControl::ConstPtr msg) {
	if(msg->Direction == "Forward" && msg->MotionType == "Offset") {
		ForwardMode = AUTOMATIC;
		ForwardOffset = msg->Value;
	} else if (msg->Direction == "Forward" && msg->MotionType == "Manual") {
		ForwardMode = MANUAL;
{'fraction_non_alphanumeric': 0.10747271200671704, 'fraction_numerical': 0.038623005877413935, 'mean_word_length': 5.248251748251748, 'pattern_counts': {'":': 0, '<': 5, '<?xml version=': 0, '>': 19, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '5625516', 'n_tokens_mistral': 1442, 'n_tokens_neox': 1291, 'n_words': 343}