Skip to content

Commit

Permalink
Added limits to angles in position mode, codriver controls updated, a…
Browse files Browse the repository at this point in the history 
…dded gripper idle speed and increased the armavator overall speed
  • Loading branch information
@Superbro525Alt
Superbro525Alt committed Aug 12, 2023
1 parent 05d13fa commit 3f9849f
Show file tree
Hide file tree
Showing 3 changed files with 86 additions and 15 deletions.
2 changes: 1 addition & 1 deletion src/main/cpp/Gripper.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,7 @@ void Gripper::OnUpdate(units::second_t dt) {

switch (_state) {
case GripperState::kIdle:
voltage = 0_V;
voltage = 1_V;
break;

case GripperState::kIntaking:
Expand Down
93 changes: 82 additions & 11 deletions src/main/cpp/behaviour/ArmavatorBehaviour.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -106,14 +106,26 @@ void ArmavatorManualBehaviour::OnTick(units::second_t dt) {
av.angleSpeed = -wom::spow2(wom::deadzone(_codriver.GetLeftY(), 0.1)) * (180_deg / 1_s);
av.elevatorSpeed = -wom::spow2(wom::deadzone(_codriver.GetRightY(), 0.1)) * (2_m / 1_s);


units::meter_t height = _armavator->GetCurrentPosition().height;
units::degree_t angle = _armavator->GetCurrentPosition().angle;
//bool above_height = true;
//sets hard limits in place, necessary to make the system not break itself (you will burn the neos out if they try to go past what they can)
// if (av.elevatorSpeed > _max_height) {
// _manualSetpoint.height = _max_height;
// } else if (_manualSetpoint.height < _min_height) {
// _manualSetpoint.height = _min_height;
// }
// else {
// av.elevatorSpeed = -wom::spow2(wom::deadzone(_codriver.GetRightY(), 0.1)) * (2_m / 1_s);
// }

units::meter_t max_height = (1.7_m - 0.51_m) - _armavator->arm->GetConfig().armLength * units::math::sin(_manualSetpoint.angle);
units::degree_t max_angle_front = units::math::acos(_armavator->GetCurrentPosition().height/_armavator->arm->GetConfig().armLength) + 10_deg - 120_deg;
units::degree_t max_angle_back = 360_deg - units::math::acos(_armavator->GetCurrentPosition().height/_armavator->arm->GetConfig().armLength) - 5_deg - 50_deg;


units::meter_t height = _armavator->GetCurrentPosition().height;
units::degree_t angle = _armavator->GetCurrentPosition().angle;
//units::meter_t height = _armavator->GetCurrentPosition().height;
//units::degree_t angle = _armavator->GetCurrentPosition().angle;

frc::SmartDashboard::PutNumber("Max Angle Front: ", max_angle_front.value());
frc::SmartDashboard::PutNumber("Max Angle Back: ", max_angle_back.value());
Expand Down Expand Up @@ -203,36 +215,95 @@ void ArmavatorManualBehaviour::OnTick(units::second_t dt) {
units::meter_t height = _armavator->GetCurrentPosition().height;
units::degree_t angle = _armavator->GetCurrentPosition().angle;
_armavator->SetSpeedValues(0.5, 0.3);

//bool above_height = true;
//sets hard limits in place, necessary to make the system not break itself (you will burn the neos out if they try to go past what they can)
if (_manualSetpoint.height > _max_height) {
_manualSetpoint.height = _max_height;
} else if (_manualSetpoint.height < _min_height) {
_manualSetpoint.height = _min_height;
} else {
_manualSetpoint.height -= (wom::deadzone(_codriver.GetRightY(), 0.2) * 1_m * 0.05); //slows the system down, otherwise it's wayyy too fast
}
else {
_manualSetpoint.height -= (wom::deadzone(_codriver.GetRightY(), 0.2) * 1_m * 0.1); //slows the system down, otherwise it's wayyy too fast
}

if (_manualSetpoint.angle > _max_angle) {
_manualSetpoint.angle = _max_angle;
} else if (_manualSetpoint.angle < _min_angle) {
_manualSetpoint.angle = _min_angle;
} else {
_manualSetpoint.angle -= (wom::deadzone(_codriver.GetLeftY(), 0.2) * 1_deg * 1);
}
else {
_manualSetpoint.angle -= (wom::deadzone(_codriver.GetLeftY(), 0.2) * 1_deg * 1.5);
}

//
// _manualSetpoint.height -= (wom::deadzone(_codriver.GetRightY(), 0.2) * 1_m * 0.05); //slows the system down, otherwise it's wayyy too fast
// _manualSetpoint.angle -= (wom::deadzone(_codriver.GetLeftY(), 0.2) * 1_deg * 1);

//some funky math which makes the armavator stay within extension limits
units::meter_t max_height = (1.9_m - 0.51_m) - _armavator->arm->GetConfig().armLength * units::math::sin(_manualSetpoint.angle);
//some funky math which makes the armavator stay within extension limits
units::meter_t max_height = (1.6_m - 0.51_m) - _armavator->arm->GetConfig().armLength * units::math::sin(_manualSetpoint.angle);
ArmavatorPosition sp{
units::math::min(_manualSetpoint.height, max_height),
_manualSetpoint.angle
};


//units::meter_t max_height = (1.7_m - 0.51_m) - _armavator->arm->GetConfig().armLength * units::math::sin(_manualSetpoint.angle);
units::degree_t max_angle_front = units::math::acos(_armavator->GetCurrentPosition().height/_armavator->arm->GetConfig().armLength) + 10_deg - 120_deg;
units::degree_t max_angle_back = 360_deg - units::math::acos(_armavator->GetCurrentPosition().height/_armavator->arm->GetConfig().armLength) - 5_deg - 50_deg;


//units::meter_t height = _armavator->GetCurrentPosition().height;
//units::degree_t angle = _armavator->GetCurrentPosition().angle;

frc::SmartDashboard::PutNumber("Max Angle Front: ", max_angle_front.value());
frc::SmartDashboard::PutNumber("Max Angle Back: ", max_angle_back.value());
frc::SmartDashboard::PutNumber("Current Angle", angle.value());

frc::SmartDashboard::PutNumber("Height", height.value());
frc::SmartDashboard::PutNumber("Max Height", max_height.value());



if (height > 0.675_m) {
_armavator->SetPosition(sp);
}
else if (angle < max_angle_front) {
//_armavator->SetArmPosition(max_angle_front * (std::numbers::pi / 180));
if (sp.angle > max_angle_front) {
_armavator->SetPosition(sp);
}
else {
//sp.angle = max_angle_front / 60;
_manualSetpoint.angle = max_angle_front;
_armavator->SetPosition(sp);
}
}
else if (angle > max_angle_back) {
if (sp.angle < max_angle_back) {
_armavator->SetPosition(sp);
}
else {
_manualSetpoint.angle = max_angle_back;
_armavator->SetPosition(sp);
}

}

else {
//if (wom::deadzone(_codriver.GetLeftY(), 0.2)) {
//_manualSetpoint.angle -= (wom::deadzone(_codriver.GetLeftY(), 0.2) * 1_deg * 1);
//}
//if (wom::deadzone(_codriver.GetRightY(), 0.2)) {
//_manualSetpoint.height -= (wom::deadzone(_codriver.GetRightY(), 0.2) * 1_m * 0.05); //slows the system down, otherwise it's wayyy too fast
//}
_armavator->SetPosition(sp);

frc::SmartDashboard::PutNumber("Current Angle", _manualSetpoint.angle.value());
frc::SmartDashboard::PutNumber("Current Height", _manualSetpoint.height.value());


}

_armavator->SetPosition(sp);

//print the values, REMOVE THESE BEFORE WARP, too many print out clog the system and can make the robot loose connection to the field
std::cout << "set position height: " << _manualSetpoint.height.value() << std::endl;
Expand Down
6 changes: 3 additions & 3 deletions src/main/cpp/behaviour/GripperBehaviour.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,9 +24,9 @@ GripperBehaviour::GripperBehaviour(Gripper *gripper, frc::XboxController &codriv

void GripperBehaviour::OnTick(units::second_t dt) {
//setting the gripper mode
if (_codriver.GetRightTriggerAxis() > 0.2) {
gripper->SetOutaking(_codriver.GetRightTriggerAxis());
} else if (_codriver.GetLeftTriggerAxis() > 0.2) {
if (_codriver.GetLeftTriggerAxis() > 0.2) {
gripper->SetOutaking(_codriver.GetLeftTriggerAxis());
} else if (_codriver.GetRightTriggerAxis() > 0.2) {
gripper->SetIntaking();
} else {
gripper->SetIdle();
Expand Down

0 comments on commit 3f9849f

Please sign in to comment.