Skip to content

Commit

Permalink
Updating take drive nodes
Browse files Browse the repository at this point in the history
  • Loading branch information
@NathanDuPont
NathanDuPont committed Feb 12, 2022
1 parent 3dbcd57 commit ecfa055
Show file tree
Hide file tree
Showing 4 changed files with 59 additions and 56 deletions.
17 changes: 13 additions & 4 deletions src/v5_hal/firmware/include/nodes/TankDriveNode.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,12 +31,8 @@ class TankDriveNode : public IDriveNode {

virtual void setDriveVoltage(int x_voltage, int theta_voltage);

virtual void setDriveVoltage(int x_voltage, int y_voltage, int theta_voltage);

virtual void setDriveVelocity(float x_velocity, float theta_velocity);

virtual void setDriveVelocity(float x_velocity, float y_velocity, float theta_velocity);

virtual void teleopPeriodic();

virtual void autonPeriodic();
Expand All @@ -52,4 +48,17 @@ class TankDriveNode : public IDriveNode {

TankDriveKinematics m_kinematics;

Eigen::Vector2d controller_target_velocity;

void m_setLeftPosition(float distance, int max_velocity);

void m_setRightPosition(float distance, int max_velocity);

void m_setLeftVoltage(int voltage);

void m_setRightVoltage(int voltage);

void m_setLeftVelocity(float velocity);

void m_setRightVelocity(float velocity);
};
2 changes: 1 addition & 1 deletion src/v5_hal/firmware/lib
Submodule lib updated 6 files
+3 −3 include/kinematics/HolonomicDriveKinematics.h
+7 −0 include/kinematics/IDriveKinematics.h
+48 −0 include/kinematics/TankDriveKinematics.h
+1 −1 include/nodes/subsystems/IDriveNode.h
+1 −1 src/kinematics/HolonomicDriveKinematics.cpp
+35 −0 src/kinematics/TankDriveKinematics.cpp
5 changes: 2 additions & 3 deletions src/v5_hal/firmware/src/nodes/HolonomicDriveNode.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -98,8 +98,7 @@ void HolonomicDriveNode::setDriveVoltage(int x_voltage, int theta_voltage) {
}

void HolonomicDriveNode::setDriveVoltage(int x_voltage, int y_voltage, int theta_voltage) {
IDriveKinematics::FourMotorPercentages motor_percentages =
m_kinematics.inverseKinematics(x_voltage, y_voltage, theta_voltage, MAX_MOTOR_VOLTAGE);
IDriveKinematics::FourMotorPercentages motor_percentages = m_kinematics.inverseKinematics(x_voltage, y_voltage, theta_voltage, MAX_MOTOR_VOLTAGE);

m_setLeftFrontVoltage(motor_percentages.left_front_percent * MAX_MOTOR_VOLTAGE);
m_setLeftRearVoltage(motor_percentages.left_rear_percent * MAX_MOTOR_VOLTAGE);
Expand All @@ -109,7 +108,7 @@ void HolonomicDriveNode::setDriveVoltage(int x_voltage, int y_voltage, int theta

void HolonomicDriveNode::setDriveVoltage(int left_x, int left_y, int right_x, int right_y) {
IDriveKinematics::FourMotorPercentages motor_percentages =
m_kinematics.tankKinematics(0, left_y, 0, right_y, MAX_MOTOR_VOLTAGE);
m_kinematics.simpleKinematics(0, left_y, 0, right_y, MAX_MOTOR_VOLTAGE);

m_setLeftFrontVoltage(motor_percentages.left_front_percent * MAX_MOTOR_VOLTAGE);
m_setLeftRearVoltage(motor_percentages.left_rear_percent * MAX_MOTOR_VOLTAGE);
Expand Down
91 changes: 43 additions & 48 deletions src/v5_hal/firmware/src/nodes/TankDriveNode.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,73 +9,71 @@ TankDriveNode::TankDriveNode(NodeManager* node_manager, std::string handle_name,
m_handle_name = handle_name.insert(0, "robot/");
}

void TankDriveNode::resetEncoders() {
m_motors.left_1_motor->resetEncoder();
m_motors.left_2_motor->resetEncoder();
m_motors.left_3_motor->resetEncoder();
m_motors.left_4_motor->resetEncoder();
m_motors.right_1_motor->resetEncoder();
m_motors.right_2_motor->resetEncoder();
m_motors.right_3_motor->resetEncoder();
m_motors.right_4_motor->resetEncoder();
void TankDriveNode::m_setLeftPosition(float distance, int max_velocity) {
m_motors.left_1_motor->moveAbsolute(distance, max_velocity);
m_motors.left_2_motor->moveAbsolute(distance, max_velocity);
m_motors.left_3_motor->moveAbsolute(distance, max_velocity);
m_motors.left_4_motor->moveAbsolute(distance, max_velocity);
}

void TankDriveNode::m_setRightPosition(float distance, int max_velocity) {
m_motors.right_1_motor->moveAbsolute(distance, max_velocity);
m_motors.right_2_motor->moveAbsolute(distance, max_velocity);
m_motors.right_3_motor->moveAbsolute(distance, max_velocity);
m_motors.right_4_motor->moveAbsolute(distance, max_velocity);
}

void TankDriveNode::m_setLeftVoltage(int voltage) {
m_left_front_motor->moveVoltage(voltage);
m_left_rear_motor->moveVoltage(voltage);
m_motors.left_1_motor->moveVoltage(voltage);
m_motors.left_2_motor->moveVoltage(voltage);
m_motors.left_3_motor->moveVoltage(voltage);
m_motors.left_4_motor->moveVoltage(voltage);
}

void TankDriveNode::m_setRightVoltage(int voltage) {
m_right_front_motor->moveVoltage(voltage);
m_right_rear_motor->moveVoltage(voltage);
m_motors.right_1_motor->moveVoltage(voltage);
m_motors.right_2_motor->moveVoltage(voltage);
m_motors.right_3_motor->moveVoltage(voltage);
m_motors.right_4_motor->moveVoltage(voltage);
}

void TankDriveNode::m_setLeftVelocity(float velocity) {
m_left_front_motor->moveVelocity(velocity);
m_left_rear_motor->moveVelocity(velocity);
m_motors.left_1_motor->moveVelocity(velocity);
m_motors.left_2_motor->moveVelocity(velocity);
m_motors.left_3_motor->moveVelocity(velocity);
m_motors.left_4_motor->moveVelocity(velocity);
}

void TankDriveNode::m_setRightVelocity(float velocity) {
m_right_front_motor->moveVelocity(velocity);
m_right_rear_motor->moveVelocity(velocity);
}

void TankDriveNode::m_setLeftDistancePosition(double distance, int max_velocity) {
m_left_front_motor->moveAbsolute(distance, max_velocity);
m_left_rear_motor->moveAbsolute(distance, max_velocity);
}

void TankDriveNode::m_setRightDistancePosition(double distance, int max_velocity) {
m_right_front_motor->moveAbsolute(distance, max_velocity);
m_right_rear_motor->moveAbsolute(distance, max_velocity);
}

int TankDriveNode::getLeftDistancePosition() {
return m_left_front_motor->getPosition();
m_motors.right_1_motor->moveVelocity(velocity);
m_motors.right_2_motor->moveVelocity(velocity);
m_motors.right_3_motor->moveVelocity(velocity);
m_motors.right_4_motor->moveVelocity(velocity);
}

int TankDriveNode::getRightDistancePosition() {
return m_right_front_motor->getPosition();
void TankDriveNode::resetEncoders() {
m_motors.left_1_motor->resetEncoder();
m_motors.left_2_motor->resetEncoder();
m_motors.left_3_motor->resetEncoder();
m_motors.left_4_motor->resetEncoder();
m_motors.right_1_motor->resetEncoder();
m_motors.right_2_motor->resetEncoder();
m_motors.right_3_motor->resetEncoder();
m_motors.right_4_motor->resetEncoder();
}

void TankDriveNode::initialize() {
resetEncoders();
}

void TankDriveNode::setDriveVoltage(int left_voltage, int right_voltage) {
m_setLeftVoltage(left_voltage);
m_setRightVoltage(right_voltage);
}
void TankDriveNode::setDriveVoltage(int x_voltage, int theta_voltage) {
IDriveKinematics::FourMotorPercentages motor_percentages = m_kinematics.inverseKinematics(x_voltage, 0, theta_voltage, MAX_MOTOR_VOLTAGE);

void TankDriveNode::setDriveVelocity(float left_velocity, float right_velocity) { //incoming values should be in m/s so we convert to rpm here
m_setLeftVelocity(left_velocity / MAX_ROBOT_SPEED * 200);
m_setRightVelocity(right_velocity / MAX_ROBOT_SPEED * 200);

}

void TankDriveNode::setDriveDistancePID(double left_distance, double right_distance, int max_velocity) {
resetEncoders();
m_setLeftDistancePID(left_distance, max_velocity);
m_setRightDistancePID(right_distance, max_velocity);
void TankDriveNode::setDriveVelocity(float x_velocity, float theta_velocity) { //incoming values should be in m/s so we convert to rpm here
//
}

void TankDriveNode::teleopPeriodic() {
Expand All @@ -88,8 +86,5 @@ void TankDriveNode::autonPeriodic() {
}

TankDriveNode::~TankDriveNode() {
delete m_left_front_motor;
delete m_left_rear_motor;
delete m_right_front_motor;
delete m_right_rear_motor;

}

0 comments on commit ecfa055

Please sign in to comment.