<commit_before>/******************************************************************************
* Controller_Functions.cpp
*
* Contains the marchPID() and depth_controller functions
******************************************************************************/
#include "Mapper.h"

/******************************************************************************
* float marchPID()
*
* Takes in readings from the IMU and returns a value between -1 and 1 (-100% -
* +100%) that the port and starboard thrusters should run at
******************************************************************************/

float marchPID(pid_data_t PID, float input)
{
	float err = input - PID.setpoint;

	if(PID.period > 0 && err > PID.period/2)
	{
		err -= period;
	}

	// Calculating errors
	PID.perr = err;
	PID.derr = (err - PID.old)/(PID.dt);
	PID.ierr += PID.dt * (err);

	//Check for integrator saturation
	if(PID.ierr > PID.isat)
	{
		PID.ierr = PID.isat;
	}

	if(PID.ierr < -PID.isat)
	{
		PID.ierr = - PID.isat;
	}
	//Calculate motor output
	PID.output =   PID.kp * PID.perr
						+ PID.ki * PID.ierr
						+ PID.kd * PID.derr;
	//Check for output saturation
	if(PID.output > PID.sat)
	{
		PID.output = PID.sat;
	}


	if(PID.output < -PID.sat)
	{
		PID.output = -PID.sat;
	}
	// set current input to be the old input //
	PID.old = err;

	return PID.output;
}


/******************************************************************************
 * int initialize_motors(int channels[3], float freq)
 *
 * Initializes the PWM board and the ESCs that run the motors
******************************************************************************/
int initialize_motors(int channels[3], float freq)
{
	// setup PCA9685 PWM board //
	int fd = pca9685Setup(PIN_BASE, PCA9685_ADDR, HERTZ);
	if (fd < 0)
	{
		printf("Error in setup\n");
		return fd;
	}

	// reset all PCA9685 PWM board output //
	pca9685PWMReset(fd);

	// set motor outputs to 0 to initialize ESCs //
	for( int i = 0; i < 3; i++ )
	{
		set_motor(i, 0);
	}
	return fd;
}

/******************************************************************************
 * float set_motor(int motornum, float percent)
 *
 * Takes in a value from -1 to 1 (-100 to +100%) and sets the motor
 * outputs accordingly
******************************************************************************/
float set_motor(int motornum, float percent)
{
  // Define characteristics of PWM pulse, microseconds
	float amplitude  = PWM_HIGH_LIMIT - PWM_ZERO_VALUE;

	// Saturation limits
  if( percent >  1.0) percent =  1.0;
  if( percent < -1.0) percent = -1.0;

  // Deadzone check
  if( (percent < MOTOR_DEADZONE) && (percent > -MOTOR_DEADZONE) ) percent = 0.0;

	// Calculate corresponding pwm output value
	int motoroutput = percent * amplitude + PWM_ZERO_VALUE;

	// Spin those motors
  pwmWrite(motornum + PIN_BASE, motoroutput);

  #ifdef DEBUG
  // Print what we told the motor to spin at.
  printf("Set motor %d to %d \n", motornum, motoroutput);
  #endif
<commit_msg>entabbed<commit_after>/******************************************************************************
* Controller_Functions.cpp
*
* Contains the marchPID() and depth_controller functions
******************************************************************************/
#include "Mapper.h"

/******************************************************************************
* float marchPID()
*
* Takes in readings from the IMU and returns a value between -1 and 1 (-100% -
* +100%) that the port and starboard thrusters should run at
******************************************************************************/

float marchPID(pid_data_t PID, float input)
{
	float err = input - PID.setpoint;

	if(PID.period > 0 && err > PID.period/2)
	{
		err -= period;
	}

	// Calculating errors
	PID.perr = err;
	PID.derr = (err - PID.old)/(PID.dt);
	PID.ierr += PID.dt * (err);

	//Check for integrator saturation
	if(PID.ierr > PID.isat)
	{
		PID.ierr = PID.isat;
	}

	if(PID.ierr < -PID.isat)
	{
		PID.ierr = - PID.isat;
	}
	//Calculate motor output
	PID.output =	 PID.kp * PID.perr
						+ PID.ki * PID.ierr
						+ PID.kd * PID.derr;
	//Check for output saturation
	if(PID.output > PID.sat)
	{
		PID.output = PID.sat;
	}


	if(PID.output < -PID.sat)
	{
		PID.output = -PID.sat;
	}
	// set current input to be the old input //
	PID.old = err;

	return PID.output;
}


/******************************************************************************
 * int initialize_motors(int channels[3], float freq)
 *
 * Initializes the PWM board and the ESCs that run the motors
******************************************************************************/
int initialize_motors(int channels[3], float freq)
{
	// setup PCA9685 PWM board //
	int fd = pca9685Setup(PIN_BASE, PCA9685_ADDR, HERTZ);
	if (fd < 0)
	{
		printf("Error in setup\n");
		return fd;
	}

	// reset all PCA9685 PWM board output //
	pca9685PWMReset(fd);

	// set motor outputs to 0 to initialize ESCs //
	for( int i = 0; i < 3; i++ )
	{
		set_motor(i, 0);
	}
	return fd;
}

/******************************************************************************
 * float set_motor(int motornum, float percent)
 *
 * Takes in a value from -1 to 1 (-100 to +100%) and sets the motor
 * outputs accordingly
******************************************************************************/
float set_motor(int motornum, float percent)
{
	// Define characteristics of PWM pulse, microseconds
	float amplitude	 = PWM_HIGH_LIMIT - PWM_ZERO_VALUE;

	// Saturation limits
	if( percent >	 1.0) percent =	 1.0;
	if( percent < -1.0) percent = -1.0;

	// Deadzone check
	if( (percent < MOTOR_DEADZONE) && (percent > -MOTOR_DEADZONE) ) percent = 0.0;

	// Calculate corresponding pwm output value
	int motoroutput = percent * amplitude + PWM_ZERO_VALUE;

	// Spin those motors
	pwmWrite(motornum + PIN_BASE, motoroutput);

	#ifdef DEBUG
	// Print what we told the motor to spin at.
	printf("Set motor %d to %d \n", motornum, motoroutput);
	#endif

{'fraction_non_alphanumeric': 0.3123113049312311, 'fraction_numerical': 0.01845018450184502, 'mean_word_length': 4.027824620573356, 'pattern_counts': {'":': 0, '<': 15, '<?xml version=': 0, '>': 15, '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': '794232', 'n_tokens_mistral': 2093, 'n_tokens_neox': 1841, 'n_words': 532}