Fixed a problem where the OPControl was trying to pull values from an array that never had any values to begin with
causeing a segmentation fault and crashing the brain. Also changed how the function lift_range_finder worked. Instead of just makeing a variable eqaul to a number it also returns the value so we can use it outside of the function. We also added actual values to all the arrays.
This commit is contained in:
Carter committed
1 parent
c46a581eb1
commit
24ee947fd5
8 files changed
+340
-192
No files matched your search
@@ -5,22 +5,22 @@ using namespace ez;
|
||||
|
||||
Pneumatics::Pneumatics(){}
|
||||
|
||||
void Pneumatics::control_piston(Pistons activePiston, ControlPeriod currPeriod, Controller currController, Buttons buttonUsed) {
|
||||
void Pneumatics::control_piston(PistonsP activePiston, ControlPeriodP currPeriod, ControllerP currController, ButtonsP buttonUsed) {
|
||||
|
||||
if (currPeriod == ControlPeriod::driver) {
|
||||
if (currPeriod == ControlPeriodP::driver) {
|
||||
switch (activePiston)
|
||||
{
|
||||
case Pistons::claw_piston:
|
||||
case PistonsP::claw_piston:
|
||||
/*More code can go here if the claw piston is selected*/
|
||||
|
||||
switch (currController)
|
||||
{
|
||||
case Controller::master:
|
||||
case ControllerP::master:
|
||||
/*More code can go here if we want to use the master controller*/
|
||||
|
||||
switch (buttonUsed)
|
||||
{
|
||||
case Buttons::arrow_buttons:
|
||||
case ButtonsP::arrow_buttons:
|
||||
if (master.get_digital(DIGITAL_RIGHT)) {
|
||||
claw.set(true);
|
||||
}
|
||||
@@ -28,7 +28,7 @@ void Pneumatics::control_piston(Pistons activePiston, ControlPeriod currPeriod,
|
||||
claw.set(false);
|
||||
}
|
||||
break;
|
||||
case Buttons::Y_B_buttons:
|
||||
case ButtonsP::Y_B_buttons:
|
||||
if (master.get_digital(DIGITAL_Y)) {
|
||||
claw.set(true);
|
||||
}
|
||||
@@ -36,7 +36,7 @@ void Pneumatics::control_piston(Pistons activePiston, ControlPeriod currPeriod,
|
||||
claw.set(false);
|
||||
}
|
||||
break;
|
||||
case Buttons::left_shoulder_buttons:
|
||||
case ButtonsP::left_shoulder_buttons:
|
||||
if (master.get_digital(DIGITAL_L1)) {
|
||||
claw.set(true);
|
||||
}
|
||||
@@ -44,7 +44,7 @@ void Pneumatics::control_piston(Pistons activePiston, ControlPeriod currPeriod,
|
||||
claw.set(false);
|
||||
}
|
||||
break;
|
||||
case Buttons::right_shoulder_buttons:
|
||||
case ButtonsP::right_shoulder_buttons:
|
||||
if (master.get_digital(DIGITAL_R1)) {
|
||||
claw.set(true);
|
||||
}
|
||||
@@ -56,11 +56,11 @@ void Pneumatics::control_piston(Pistons activePiston, ControlPeriod currPeriod,
|
||||
break;
|
||||
}
|
||||
break;
|
||||
case Controller::slave:
|
||||
case ControllerP::slave:
|
||||
|
||||
switch (buttonUsed)
|
||||
{
|
||||
case Buttons::arrow_buttons:
|
||||
case ButtonsP::arrow_buttons:
|
||||
if (slave.get_digital(DIGITAL_RIGHT)) {
|
||||
claw.set(true);
|
||||
}
|
||||
@@ -68,7 +68,7 @@ void Pneumatics::control_piston(Pistons activePiston, ControlPeriod currPeriod,
|
||||
claw.set(false);
|
||||
}
|
||||
break;
|
||||
case Buttons::Y_B_buttons:
|
||||
case ButtonsP::Y_B_buttons:
|
||||
if (slave.get_digital(DIGITAL_Y)) {
|
||||
claw.set(true);
|
||||
}
|
||||
@@ -76,7 +76,7 @@ void Pneumatics::control_piston(Pistons activePiston, ControlPeriod currPeriod,
|
||||
claw.set(false);
|
||||
}
|
||||
break;
|
||||
case Buttons::left_shoulder_buttons:
|
||||
case ButtonsP::left_shoulder_buttons:
|
||||
if (slave.get_digital(DIGITAL_L1)) {
|
||||
claw.set(true);
|
||||
}
|
||||
@@ -84,7 +84,7 @@ void Pneumatics::control_piston(Pistons activePiston, ControlPeriod currPeriod,
|
||||
claw.set(false);
|
||||
}
|
||||
break;
|
||||
case Buttons::right_shoulder_buttons:
|
||||
case ButtonsP::right_shoulder_buttons:
|
||||
if (slave.get_digital(DIGITAL_R1)) {
|
||||
claw.set(true);
|
||||
}
|
||||
@@ -111,7 +111,7 @@ void Pneumatics::control_piston(Pistons activePiston, ControlPeriod currPeriod,
|
||||
else {
|
||||
switch (activePiston)
|
||||
{
|
||||
case Pistons::claw_piston:
|
||||
case PistonsP::claw_piston:
|
||||
/*code here*/
|
||||
break;
|
||||
default:
|
||||
|
||||
@@ -3,52 +3,69 @@
|
||||
|
||||
using namespace ez;
|
||||
|
||||
LiftMech liftObj;
|
||||
LiftMech liftRangeAccess; // Lift object to use lift variables and functions
|
||||
|
||||
/**
|
||||
* Default constructor
|
||||
*/
|
||||
ArmMech::ArmMech() {
|
||||
currArmState = 0;
|
||||
armTarget = 0;
|
||||
targetPos = 0;
|
||||
set_arm_kp(0.0173f);
|
||||
|
||||
selectedStates = {0, 0}; // Sets default array values
|
||||
}
|
||||
|
||||
void ArmMech::arm_driver_control(Controller currController, Buttons buttonUsed) {
|
||||
/**
|
||||
* control the arm during driver
|
||||
*
|
||||
* \param currController
|
||||
* Input the controller being used
|
||||
* \param buttonUsed
|
||||
* Select the button config you want use
|
||||
*/
|
||||
void ArmMech::arm_driver_control(ControllerR currController, ButtonsR buttonUsed) {
|
||||
|
||||
switch (currController)
|
||||
switch (currController) // Switch that check which controller was selected
|
||||
{
|
||||
case Controller::master:
|
||||
|
||||
case ControllerR::master:
|
||||
// Code you want to run when the master controller is selcted
|
||||
switch (buttonUsed)
|
||||
{
|
||||
case Buttons::arrow_buttons:
|
||||
if (master.get_digital_new_press(DIGITAL_RIGHT)){
|
||||
case ButtonsR::arrow_buttons:
|
||||
// If the arrow_buttons are selected run this code
|
||||
if (master.get_digital(DIGITAL_RIGHT)){
|
||||
arm.move(127);
|
||||
}
|
||||
else if (master.get_digital_new_press(DIGITAL_DOWN)) {
|
||||
else if (master.get_digital(DIGITAL_DOWN)) {
|
||||
arm.move(-127);
|
||||
}
|
||||
break;
|
||||
case Buttons::Y_B_buttons:
|
||||
if (master.get_digital_new_press(DIGITAL_Y)){
|
||||
case ButtonsR::Y_B_buttons:
|
||||
// If the Y_B_buttons are selected run this code
|
||||
if (master.get_digital(DIGITAL_Y)){
|
||||
arm.move(127);
|
||||
}
|
||||
else if (master.get_digital_new_press(DIGITAL_B)) {
|
||||
else if (master.get_digital(DIGITAL_B)) {
|
||||
arm.move(-127);
|
||||
}
|
||||
break;
|
||||
case Buttons::left_shoulder_buttons:
|
||||
if (master.get_digital_new_press(DIGITAL_L1)){
|
||||
case ButtonsR::left_shoulder_buttons:
|
||||
// If the left-shoulder_buttons are selected run this code
|
||||
if (master.get_digital(DIGITAL_L1)){
|
||||
arm.move(127);
|
||||
}
|
||||
else if (master.get_digital_new_press(DIGITAL_L2)) {
|
||||
else if (master.get_digital(DIGITAL_L2)) {
|
||||
arm.move(-127);
|
||||
}
|
||||
break;
|
||||
case Buttons::right_shoulder_buttons:
|
||||
if (master.get_digital_new_press(DIGITAL_R1)){
|
||||
case ButtonsR::right_shoulder_buttons:
|
||||
// If the right_shoulder_buttons are selected run this code
|
||||
if (master.get_digital(DIGITAL_R1)){
|
||||
arm.move(127);
|
||||
}
|
||||
else if (master.get_digital_new_press(DIGITAL_R2)) {
|
||||
else if (master.get_digital(DIGITAL_R2)) {
|
||||
arm.move(-127);
|
||||
}
|
||||
break;
|
||||
@@ -56,39 +73,43 @@ void ArmMech::arm_driver_control(Controller currController, Buttons buttonUsed)
|
||||
break;
|
||||
}
|
||||
break;
|
||||
case Controller::slave:
|
||||
|
||||
case ControllerR::slave:
|
||||
// Code you want to run when the slave controller is selected
|
||||
switch (buttonUsed)
|
||||
{
|
||||
case Buttons::arrow_buttons:
|
||||
if (slave.get_digital_new_press(DIGITAL_RIGHT)){
|
||||
case ButtonsR::arrow_buttons:
|
||||
// If the arrow_buttons are selected run this code
|
||||
if (slave.get_digital(DIGITAL_RIGHT)){
|
||||
cycle_states();
|
||||
}
|
||||
else if (slave.get_digital_new_press(DIGITAL_DOWN)) {
|
||||
else if (slave.get_digital(DIGITAL_DOWN)) {
|
||||
reset_state();
|
||||
}
|
||||
break;
|
||||
case Buttons::Y_B_buttons:
|
||||
if (slave.get_digital_new_press(DIGITAL_Y)){
|
||||
case ButtonsR::Y_B_buttons:
|
||||
// If the Y_B_buttons are selected run this code
|
||||
if (slave.get_digital(DIGITAL_Y)){
|
||||
cycle_states();
|
||||
}
|
||||
else if (slave.get_digital_new_press(DIGITAL_B)) {
|
||||
else if (slave.get_digital(DIGITAL_B)) {
|
||||
reset_state();
|
||||
}
|
||||
break;
|
||||
case Buttons::left_shoulder_buttons:
|
||||
if (slave.get_digital_new_press(DIGITAL_L1)){
|
||||
case ButtonsR::left_shoulder_buttons:
|
||||
// If the left_shoulder_buttons are selected run this code
|
||||
if (slave.get_digital(DIGITAL_L1)){
|
||||
cycle_states();
|
||||
}
|
||||
else if (slave.get_digital_new_press(DIGITAL_L2)) {
|
||||
else if (slave.get_digital(DIGITAL_L2)) {
|
||||
reset_state();
|
||||
}
|
||||
break;
|
||||
case Buttons::right_shoulder_buttons:
|
||||
if (slave.get_digital_new_press(DIGITAL_R1)){
|
||||
case ButtonsR::right_shoulder_buttons:
|
||||
// If the right_shoulder_buttons are selected run this code
|
||||
if (slave.get_digital(DIGITAL_R1)){
|
||||
cycle_states();
|
||||
}
|
||||
else if (slave.get_digital_new_press(DIGITAL_R2)) {
|
||||
else if (slave.get_digital(DIGITAL_R2)) {
|
||||
reset_state();
|
||||
}
|
||||
break;
|
||||
@@ -101,45 +122,51 @@ void ArmMech::arm_driver_control(Controller currController, Buttons buttonUsed)
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Control the arm during auton
|
||||
*
|
||||
* \param pickState
|
||||
* Variable to move the arm. Input: 1 to move or Input: 0 which won't move it
|
||||
*/
|
||||
void ArmMech::arm_auton_control(int pickState) {
|
||||
liftObj.lift_range_finder();
|
||||
liftRangeAccess.lift_range_finder(); // Returns a value 0 - 3 based on where the lift is currently
|
||||
|
||||
switch (targetPos)
|
||||
{
|
||||
case 0:
|
||||
selectedStates = levelOneArmStates;
|
||||
selectedStates = levelOneArmStates; // Makes the main array equal the level one array
|
||||
if (pickState == 1) {
|
||||
selectedStates[1];
|
||||
armTarget = selectedStates[1]; // Makes the target value the second in the array
|
||||
}
|
||||
else {
|
||||
selectedStates[0];
|
||||
armTarget = selectedStates[0]; // Makes the target value the first in the array
|
||||
}
|
||||
break;
|
||||
case 1:
|
||||
selectedStates = levelOneArmStates;
|
||||
selectedStates = levelOneArmStates; // Make the main array equal the level two array
|
||||
if (pickState == 1) {
|
||||
selectedStates[1];
|
||||
armTarget = selectedStates[1];
|
||||
}
|
||||
else {
|
||||
selectedStates[0];
|
||||
armTarget = selectedStates[0];
|
||||
}
|
||||
break;
|
||||
case 2:
|
||||
selectedStates = levelTwoArmStates;
|
||||
selectedStates = levelTwoArmStates; // Makes the main array equal the level three array
|
||||
if (pickState == 1) {
|
||||
selectedStates[1];
|
||||
armTarget = selectedStates[1];
|
||||
}
|
||||
else {
|
||||
selectedStates[0];
|
||||
armTarget = selectedStates[0];
|
||||
}
|
||||
break;
|
||||
case 3:
|
||||
selectedStates = levelThreeArmStates;
|
||||
selectedStates = levelThreeArmStates; // Makes the main array equal the level four array
|
||||
if (pickState == 1) {
|
||||
selectedStates[1];
|
||||
armTarget = selectedStates[1];
|
||||
}
|
||||
else {
|
||||
selectedStates[0];
|
||||
armTarget = selectedStates[0];
|
||||
}
|
||||
break;
|
||||
default:
|
||||
@@ -148,55 +175,95 @@ void ArmMech::arm_auton_control(int pickState) {
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* Function to cycle through each value in the array
|
||||
*/
|
||||
void ArmMech::cycle_states() {
|
||||
liftObj.lift_range_finder();
|
||||
liftRangeAccess.lift_range_finder(); // Returns a value 0 - 3 based on were the lift is currently
|
||||
|
||||
switch (targetPos)
|
||||
{
|
||||
case 0:
|
||||
selectedStates = levelOneArmStates;
|
||||
break;
|
||||
case 1:
|
||||
selectedStates = levelTwoArmStates;
|
||||
break;
|
||||
case 2:
|
||||
selectedStates = levelThreeArmStates;
|
||||
break;
|
||||
case 3:
|
||||
selectedStates = levelFourArmStates;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
// Checks the array to make sure it isn't empty to prevent a segmentation fault(trying to access unassigned memory)
|
||||
if (!selectedStates.empty()) {
|
||||
// Based on the value returned by the lift_range_finder function change the array to match the level of the lift
|
||||
switch (targetPos)
|
||||
{
|
||||
case 0:
|
||||
selectedStates = levelOneArmStates;
|
||||
break;
|
||||
case 1:
|
||||
selectedStates = levelTwoArmStates;
|
||||
break;
|
||||
case 2:
|
||||
selectedStates = levelThreeArmStates;
|
||||
break;
|
||||
case 3:
|
||||
selectedStates = levelFourArmStates;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
currArmState ++;
|
||||
currArmState ++;
|
||||
|
||||
if (currArmState == numArmStates) {
|
||||
if (currArmState >= numArmStates) {
|
||||
currArmState = 0;
|
||||
}
|
||||
armTarget = selectedStates[currArmState];
|
||||
armTarget = selectedStates[currArmState]; // Makes the target value equal the next spot in the array
|
||||
}
|
||||
|
||||
/**
|
||||
* Function reset the array indexer to zero
|
||||
*/
|
||||
void ArmMech::reset_state() {
|
||||
currArmState = 0;
|
||||
armTarget = selectedStates[currArmState];
|
||||
|
||||
// Checks the array to make sure it isn't empty to prevent a segmentation fault(trying to access unassigned memory)
|
||||
if (!selectedStates.empty()) {
|
||||
armTarget = selectedStates[currArmState]; // Makes the target value equal the first value in the array
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Function to set the velocity of the arm
|
||||
*/
|
||||
void ArmMech::arm_control() {
|
||||
armError = armTarget - armRotation.get_position();
|
||||
armVelocity = get_arm_kp() * armError;
|
||||
arm.move(armVelocity);
|
||||
armError = armTarget - armRotation.get_position(); // Sets the error value
|
||||
armVelocity = get_arm_kp() * armError; // Sets the velocity
|
||||
arm.move(armVelocity); // moves the arm
|
||||
}
|
||||
|
||||
/**
|
||||
* Setup the four different level arrays
|
||||
*
|
||||
* \param _levelOneArmStates
|
||||
* The array used if the lift is at level one
|
||||
* \param _levelTwoArmStates
|
||||
* The array used if the lift is at level two
|
||||
* \param _levelThreeArmStates
|
||||
* The array used if the lift is at level three
|
||||
* \param _levelFourArmStates
|
||||
* The array used if the lift is at level four
|
||||
*/
|
||||
void ArmMech::set_state_values(std::vector<int> _levelOneArmStates, std::vector<int> _levelTwoArmStates, std::vector<int> _levelThreeArmStates, std::vector<int> _levelFourArmStates) {
|
||||
levelOneArmStates = _levelOneArmStates;
|
||||
levelTwoArmStates = _levelTwoArmStates;
|
||||
levelThreeArmStates = _levelThreeArmStates;
|
||||
levelFourArmStates = _levelFourArmStates;
|
||||
}
|
||||
/**
|
||||
* Gives the ability to set the private arm kp value
|
||||
*
|
||||
* \param _armKP
|
||||
* The input value that is then used for the kp value
|
||||
*/
|
||||
void ArmMech::set_arm_kp(float _armKP) {
|
||||
armKP = _armKP;
|
||||
}
|
||||
|
||||
int ArmMech::get_arm_state() {return selectedStates[currArmState];}
|
||||
int ArmMech::get_arm_state() {
|
||||
if (selectedStates.empty()) {
|
||||
return 0;
|
||||
}
|
||||
return selectedStates[currArmState];
|
||||
}
|
||||
int ArmMech::get_num_arm_states() {return numArmStates;}
|
||||
float ArmMech::get_arm_kp() {return armKP;}
|
||||
@@ -5,7 +5,13 @@ using namespace ez;
|
||||
|
||||
ArmMech armObjRange;
|
||||
|
||||
LiftMech::LiftMech() {}
|
||||
LiftMech::LiftMech() {
|
||||
liftHeight = 0;
|
||||
liftError = 0;
|
||||
liftVelocity = 0;
|
||||
|
||||
liftStates = {0, 0, 0, 0};
|
||||
}
|
||||
LiftMech::LiftMech(std::initializer_list<int> _liftStates, int _numLiftStates) {
|
||||
liftStates = _liftStates;
|
||||
numLiftStates = _numLiftStates;
|
||||
@@ -14,14 +20,14 @@ LiftMech::LiftMech(std::initializer_list<int> _liftStates, int _numLiftStates) {
|
||||
set_lift_kp(0.0173f);
|
||||
}
|
||||
|
||||
void LiftMech::lift_driver_control(Controller currController, Buttons buttonUsed) {
|
||||
void LiftMech::lift_driver_control(ControllerR currController, ButtonsR buttonUsed) {
|
||||
switch (currController)
|
||||
{
|
||||
case Controller::master:
|
||||
case ControllerR::master:
|
||||
|
||||
switch (buttonUsed)
|
||||
{
|
||||
case Buttons::arrow_buttons:
|
||||
case ButtonsR::arrow_buttons:
|
||||
if (master.get_digital(DIGITAL_RIGHT)) {
|
||||
lift.move(127);
|
||||
}
|
||||
@@ -29,7 +35,7 @@ void LiftMech::lift_driver_control(Controller currController, Buttons buttonUsed
|
||||
lift.move(-127);
|
||||
}
|
||||
break;
|
||||
case Buttons::Y_B_buttons:
|
||||
case ButtonsR::Y_B_buttons:
|
||||
if (master.get_digital(DIGITAL_Y)) {
|
||||
lift.move(127);
|
||||
}
|
||||
@@ -37,7 +43,7 @@ void LiftMech::lift_driver_control(Controller currController, Buttons buttonUsed
|
||||
lift.move(-127);
|
||||
}
|
||||
break;
|
||||
case Buttons::left_shoulder_buttons:
|
||||
case ButtonsR::left_shoulder_buttons:
|
||||
if (master.get_digital(DIGITAL_L1)) {
|
||||
lift.move(127);
|
||||
}
|
||||
@@ -45,7 +51,7 @@ void LiftMech::lift_driver_control(Controller currController, Buttons buttonUsed
|
||||
lift.move(-127);
|
||||
}
|
||||
break;
|
||||
case Buttons::right_shoulder_buttons:
|
||||
case ButtonsR::right_shoulder_buttons:
|
||||
if (master.get_digital(DIGITAL_R1)) {
|
||||
lift.move(127);
|
||||
}
|
||||
@@ -57,11 +63,11 @@ void LiftMech::lift_driver_control(Controller currController, Buttons buttonUsed
|
||||
break;
|
||||
}
|
||||
break;
|
||||
case Controller::slave:
|
||||
case ControllerR::slave:
|
||||
|
||||
switch (buttonUsed)
|
||||
{
|
||||
case Buttons::arrow_buttons:
|
||||
case ButtonsR::arrow_buttons:
|
||||
if (slave.get_digital(DIGITAL_RIGHT)) {
|
||||
cycle_states();
|
||||
}
|
||||
@@ -69,7 +75,7 @@ void LiftMech::lift_driver_control(Controller currController, Buttons buttonUsed
|
||||
reset_state();
|
||||
}
|
||||
break;
|
||||
case Buttons::Y_B_buttons:
|
||||
case ButtonsR::Y_B_buttons:
|
||||
if (slave.get_digital(DIGITAL_Y)) {
|
||||
cycle_states();
|
||||
}
|
||||
@@ -77,7 +83,7 @@ void LiftMech::lift_driver_control(Controller currController, Buttons buttonUsed
|
||||
reset_state();
|
||||
}
|
||||
break;
|
||||
case Buttons::left_shoulder_buttons:
|
||||
case ButtonsR::left_shoulder_buttons:
|
||||
if (slave.get_digital(DIGITAL_L1)) {
|
||||
cycle_states();
|
||||
}
|
||||
@@ -85,7 +91,7 @@ void LiftMech::lift_driver_control(Controller currController, Buttons buttonUsed
|
||||
reset_state();
|
||||
}
|
||||
break;
|
||||
case Buttons::right_shoulder_buttons:
|
||||
case ButtonsR::right_shoulder_buttons:
|
||||
if (slave.get_digital(DIGITAL_R1)) {
|
||||
cycle_states();
|
||||
}
|
||||
@@ -126,15 +132,20 @@ void LiftMech::lift_auton_control(int pickState) {
|
||||
void LiftMech::cycle_states() {
|
||||
currLiftState ++;
|
||||
|
||||
if (currLiftState == numLiftStates) {
|
||||
currLiftState = 0;
|
||||
if (!liftStates.empty()) {
|
||||
if (currLiftState == numLiftStates) {
|
||||
currLiftState = 0;
|
||||
}
|
||||
liftTarget = liftStates[currLiftState];
|
||||
}
|
||||
liftTarget = liftStates[currLiftState];
|
||||
}
|
||||
|
||||
void LiftMech::reset_state() {
|
||||
currLiftState = 0;
|
||||
liftTarget = liftStates[currLiftState];
|
||||
|
||||
if (!liftStates.empty()) {
|
||||
liftTarget = liftStates[currLiftState];
|
||||
}
|
||||
}
|
||||
|
||||
void LiftMech::lift_control() {
|
||||
@@ -143,20 +154,20 @@ void LiftMech::lift_control() {
|
||||
lift.move(liftVelocity);
|
||||
}
|
||||
|
||||
void LiftMech::lift_range_finder() {
|
||||
int LiftMech::lift_range_finder() {
|
||||
liftHeight = liftRotation.get_position();
|
||||
|
||||
if (liftHeight >= firstRange[0] && liftHeight <= firstRange[1]) {
|
||||
armObjRange.targetPos = 0;
|
||||
if (firstRange.size() >= 2 && liftHeight >= firstRange[0] && liftHeight <= firstRange[1]) {
|
||||
return armObjRange.targetPos = 0;
|
||||
}
|
||||
else if (liftHeight >= secondRange[0] && liftHeight <= secondRange[1]) {
|
||||
armObjRange.targetPos = 1;
|
||||
else if (secondRange.size() >= 2 && liftHeight >= secondRange[0] && liftHeight <= secondRange[1]) {
|
||||
return armObjRange.targetPos = 1;
|
||||
}
|
||||
else if (liftHeight >= thirdRange[0] && liftHeight <= thirdRange[1]) {
|
||||
armObjRange.targetPos = 2;
|
||||
else if (thirdRange.size() >= 2 && liftHeight >= thirdRange[0] && liftHeight <= thirdRange[1]) {
|
||||
return armObjRange.targetPos = 2;
|
||||
}
|
||||
else if (liftHeight >= fourthRange[0] && liftHeight <= fourthRange[1]) {
|
||||
armObjRange.targetPos = 3;
|
||||
else if (fourthRange.size() >= 2 && liftHeight >= fourthRange[0] && liftHeight <= fourthRange[1]) {
|
||||
return armObjRange.targetPos = 3;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -171,6 +182,10 @@ void LiftMech::set_ranges(std::vector<int> _firstRange, std::vector<int> _second
|
||||
fourthRange = _fourthRange;
|
||||
}
|
||||
|
||||
int LiftMech::get_lift_state() {return liftStates[currLiftState];}
|
||||
int LiftMech::get_lift_state() {
|
||||
if (!liftStates.empty()) {
|
||||
return liftStates[currLiftState];
|
||||
}
|
||||
}
|
||||
int LiftMech::get_num_lift_states() {return numLiftStates;}
|
||||
float LiftMech::get_lift_kp() {return liftKP;}
|
||||
@@ -3,7 +3,12 @@
|
||||
|
||||
using namespace ez;
|
||||
|
||||
WristMech::WristMech(){}
|
||||
WristMech::WristMech() {
|
||||
wristError = 0;
|
||||
wristVelocity = 0;
|
||||
|
||||
wristStates = {0, 0};
|
||||
}
|
||||
WristMech::WristMech(std::initializer_list<int> _wristStates, int _numWristStates) {
|
||||
wristStates = _wristStates;
|
||||
numWristStates = _numWristStates;
|
||||
@@ -12,29 +17,29 @@ WristMech::WristMech(std::initializer_list<int> _wristStates, int _numWristState
|
||||
set_wrist_kp(0.0173f);
|
||||
}
|
||||
|
||||
void WristMech::wrist_driver_control(Controller currController, Buttons buttonUsed) {
|
||||
void WristMech::wrist_driver_control(ControllerR currController, ButtonsR buttonUsed) {
|
||||
switch (currController)
|
||||
{
|
||||
case Controller::master:
|
||||
case ControllerR::master:
|
||||
|
||||
switch (buttonUsed)
|
||||
{
|
||||
case Buttons::arrow_buttons:
|
||||
case ButtonsR::arrow_buttons:
|
||||
if (master.get_digital(DIGITAL_DOWN)) {
|
||||
reset_state();
|
||||
}
|
||||
break;
|
||||
case Buttons::Y_B_buttons:
|
||||
case ButtonsR::Y_B_buttons:
|
||||
if (master.get_digital(DIGITAL_Y)) {
|
||||
reset_state();
|
||||
}
|
||||
break;
|
||||
case Buttons::left_shoulder_buttons:
|
||||
case ButtonsR::left_shoulder_buttons:
|
||||
if (master.get_digital(DIGITAL_L1)) {
|
||||
reset_state();
|
||||
}
|
||||
break;
|
||||
case Buttons::right_shoulder_buttons:
|
||||
case ButtonsR::right_shoulder_buttons:
|
||||
if (master.get_digital(DIGITAL_R1)) {
|
||||
reset_state();
|
||||
}
|
||||
@@ -43,26 +48,26 @@ void WristMech::wrist_driver_control(Controller currController, Buttons buttonUs
|
||||
break;
|
||||
}
|
||||
break;
|
||||
case Controller::slave:
|
||||
case ControllerR::slave:
|
||||
|
||||
switch (buttonUsed)
|
||||
{
|
||||
case Buttons::arrow_buttons:
|
||||
case ButtonsR::arrow_buttons:
|
||||
if (slave.get_digital(DIGITAL_DOWN)) {
|
||||
reset_state();
|
||||
}
|
||||
break;
|
||||
case Buttons::Y_B_buttons:
|
||||
case ButtonsR::Y_B_buttons:
|
||||
if (slave.get_digital(DIGITAL_Y)) {
|
||||
reset_state();
|
||||
}
|
||||
break;
|
||||
case Buttons::left_shoulder_buttons:
|
||||
case ButtonsR::left_shoulder_buttons:
|
||||
if (slave.get_digital(DIGITAL_L1)) {
|
||||
reset_state();
|
||||
}
|
||||
break;
|
||||
case Buttons::right_shoulder_buttons:
|
||||
case ButtonsR::right_shoulder_buttons:
|
||||
if (slave.get_digital(DIGITAL_R1)) {
|
||||
reset_state();
|
||||
}
|
||||
@@ -87,16 +92,22 @@ void WristMech::rotate_wrist(int wristThreshold) {
|
||||
|
||||
void WristMech::reset_state() {
|
||||
currWristState = 0;
|
||||
wristTarget = wristStates[currWristState];
|
||||
if (!wristStates.empty()) {
|
||||
wristTarget = wristStates[currWristState];
|
||||
}
|
||||
}
|
||||
|
||||
void WristMech::wrist_control() {
|
||||
wristError = wristTarget - armRotation.get_position();
|
||||
wristError = wristTarget - wristRotation.get_position();
|
||||
wristVelocity = get_wrist_kp() * wristError;
|
||||
wrist.move(wristVelocity);
|
||||
}
|
||||
|
||||
int WristMech::get_wrist_state() {return wristStates[currWristState];}
|
||||
int WristMech::get_wrist_state() {
|
||||
if (!wristStates.empty()) {
|
||||
return wristStates[currWristState];
|
||||
}
|
||||
}
|
||||
int WristMech::get_num_wrist_states() {return numWristStates;}
|
||||
float WristMech::get_wrist_kp() {return wristKP;}
|
||||
|
||||
|
||||
Reference in new issue
Block a user