<commit_before>#include "ros/ros.h"
#include "std_msgs/Float64"
#include <algorithm>

// Constructor that stores desired gains in class variable
SpeedController::SpeedController(double kp, double ki, double kd){

  _Kp = kp;
  _Ki = ki;
  _Kd = kd;

}

// Gets current wheel velocity in rad/s
// ToDo : implement low pass filter on data?
private double SpeedController::get_wheel_vels(){
  
  // Get the current time
  enc_time = ros::Time::now().toSec();
  
  // Calculate unfiltered encoder velocity in pulses per sec
  omega = (enc_count - prev_enc_count)/(enc_time - prev_enc_time);

  // Convert from pulses per sec to rads per sec
  omega /= pulses_per_rev * 2 * PI;

  return omega;

}

// Write value to Pololu motor Driver
// Need to wait until ROS Serial architecture is figured out
private void SpeedController::assign(int val){

}

// Takes a command velocity in rad/s and uses PID to control
// motor to that speed
public double SpeedController::control(double w_cmd){


  // Get current wheel angular velocity
  double w_act = get_wheel_vels();
  // Calculate Current Error
  double cur_error = w_act - w_cmd;
  
  // Get time difference between now and last control time
  double dt = ros::Time.now().toSec() - last_control_time;
  
  // Derivative of Error
  double dedt = (cur_error - prev_error) / dt;

  // Calculate PID Output
  output += _Kp * cur_error + _Kd * dedt + _Ki * int_error;
  
  assign(output);

  // Update Error Values
  int_error += (cur_error + prev_error) / 2.0 * dt;
  int_error = min(int_error, int_error_max);
  last_control_time = ros::Time.now().toSec();

}
<commit_msg>Changes to Speed Controller<commit_after>#include "ros/ros.h"
#include "std_msgs/Float64"
#include <algorithm>

// Constructor that stores desired gains in class variable
SpeedController::SpeedController(double kp, double ki, double kd){

  _Kp = kp;
  _Ki = ki;
  _Kd = kd;

}

// Gets current wheel velocity in rad/s
// ToDo : implement low pass filter on data?
private double SpeedController::get_wheel_vels(){
  
  // Get the current time
  enc_time = ros::Time::now().toSec();
  
  // Calculate unfiltered encoder velocity in pulses per sec
  double omega = (enc_count - prev_enc_count) /
                 (enc_time - prev_enc_time);

  // Convert from pulses per sec to rads per sec
  omega /= pulses_per_rev * 2 * PI;

  return omega;

}

// Write value to Pololu motor Driver
// Need to wait until ROS Serial architecture is figured out
private void SpeedController::assign(int val){

}

// Takes a command velocity in rad/s and uses PID to control
// motor to that speed
public double SpeedController::control(double w_cmd){


  // Get current wheel angular velocity
  double w_act = get_wheel_vels();
  // Calculate Current Error
  double cur_error = w_act - w_cmd;
  
  // Get time difference between now and last control time
  double dt = ros::Time.now().toSec() - last_control_time;
  
  // Derivative of Error
  double dedt = (cur_error - prev_error) / dt;

  // Calculate PID Output
  output += _Kp * cur_error + _Kd * dedt + _Ki * int_error;
  
  // Write value to Motor Driver
  assign(output);

  // Update Error Values
  int_error += (cur_error + prev_error) / 2.0 * dt;
  int_error = min(int_error, int_error_max);
  last_control_time = ros::Time.now().toSec();

}

{'fraction_non_alphanumeric': 0.1024552894816611, 'fraction_numerical': 0.003031221582297666, 'mean_word_length': 3.411764705882353, 'pattern_counts': {'":': 0, '<': 5, '<?xml version=': 0, '>': 5, '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': '218544', 'n_tokens_mistral': 1104, 'n_tokens_neox': 1052, 'n_words': 358}