Skip to content

Commit

Permalink
Removing commented out lines
Browse files Browse the repository at this point in the history
  • Loading branch information
NathanDuPont committed Feb 28, 2021
1 parent 9818268 commit 7fc4012
Showing 1 changed file with 1 addition and 14 deletions.
15 changes: 1 addition & 14 deletions src/v5_hal/firmware/src/DataNodes/DriverControlNode.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -64,19 +64,13 @@ void DriverControlNode::m_spinEjectionRollerVoltage(int voltage) {
ejection_roller->moveVoltage(voltage);
}

void DriverControlNode::periodic() {
// printf("Inertial Sensor Yaw: %f\n", inertial_sensor->getYaw());
// double sensor_yaw = inertial_sensor->getYaw();
// robot_angle = Eigen::Rotation2Dd(-sensor_yaw);

void DriverControlNode::periodic() {
controller_target_velocity(0) = (controller_primary->get_analog(pros::E_CONTROLLER_ANALOG_LEFT_X) / 127.0) * max_velocity;
controller_target_velocity(1) = (controller_primary->get_analog(pros::E_CONTROLLER_ANALOG_LEFT_Y) / 127.0) * max_velocity;
rotation_velocity = -(controller_primary->get_analog(pros::E_CONTROLLER_ANALOG_RIGHT_X) / 127.0) * max_rotation_velocity;

field_target_velocity = robot_angle.inverse() * controller_target_velocity;

// printf("Field Target Velocity: %f\n", field_target_velocity);

swerveController.assignActualAngle(left_swerve_pot->getValue(), right_swerve_pot->getValue(), rear_swerve_pot->getValue());

MotorPowers left_motor_powers = swerveController.calculateLeftModule(field_target_velocity, rotation_velocity);
Expand All @@ -90,13 +84,6 @@ void DriverControlNode::periodic() {
rear_swerve_1->move(rear_motor_powers.left_motor_power);
rear_swerve_2->move(rear_motor_powers.right_motor_power);

// printf("Left Motor 1 Power: %d\n", left_motor_powers.left_motor_power);
// printf("Left Motor 2 Power: %d\n", left_motor_powers.right_motor_power);
// printf("Right Motor 1 Power: %d\n", right_motor_powers.left_motor_power);
// printf("Right Motor 2 Power: %d\n", right_motor_powers.right_motor_power);
// printf("Rear Motor 1 Power: %d\n", rear_motor_powers.left_motor_power);
// printf("Rear Motor 2 Power: %d\n", rear_motor_powers.right_motor_power);

int intake_voltage = 0;
int main_rollers_voltage = 0;
int ejection_roller_voltage = 0;
Expand Down

0 comments on commit 7fc4012

Please sign in to comment.