carter/VEX-OverRide
1
0 Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Fixed a problem where the OPControl was trying to pull values from an array that never had any values to begin with

Browse Source 
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 2026-09-09 13:29:07 -04:00
1 parent c46a581eb1
commit 24ee947fd5
8 files changed
+338 -190
Download Patch File Download Diff File Expand all files Collapse all files

No files matched your search

include/EZ-Template/pneumatics.hpp
+19 -8
View File
@@ -6,24 +6,35 @@
#include "EZ-Template/piston.hpp" #include "EZ-Template/piston.hpp"
namespace ez { namespace ez {
/**
enum class Pistons { * Enumeration to hold the current part of a match were in.
claw_piston */
}; enum class ControlPeriodP {
enum class ControlPeriod {
driver, driver,
auton auton
}; };
enum class Controller { /**
* Enumeration to hold the different controllers avalable.
*/
enum class ControllerP {
master, master,
slave slave
}; };
enum class Buttons { /**
* Enumeration to hold the different buttons to use.
*/
enum class ButtonsP {
arrow_buttons, arrow_buttons,
Y_B_buttons, Y_B_buttons,
left_shoulder_buttons, left_shoulder_buttons,
right_shoulder_buttons right_shoulder_buttons
}; };
/**
* Enumeration to hold all the pistons avalable.
*/
enum class PistonsP {
claw_piston
};
class Pneumatics{ class Pneumatics{
@@ -34,7 +45,7 @@ namespace ez {
Pneumatics(); Pneumatics();
void control_piston(enum Pistons, enum ControlPeriod, enum Controller, enum Buttons); void control_piston(enum PistonsP, enum ControlPeriodP, enum ControllerP, enum ButtonsP);
bool get_piston_state(); bool get_piston_state();
void set_piston_state(bool value); void set_piston_state(bool value);
include/EZ-Template/rotation_mechanisms/rotation_calc.hpp
+48 -23
View File
@@ -2,28 +2,43 @@
#include <vector> #include <vector>
#include "api.h" #include "api.h"
//#include "main.h"
#include "pros/adi.hpp" #include "pros/adi.hpp"
#include "pros/rotation.hpp" #include "pros/rotation.hpp"
namespace ez{ namespace ez{
/**
enum class ControlPeriod { * Enumeration to hold the current part of a match were in.
*/
enum class ControlPeriodR {
driver, driver,
auton auton
}; };
enum class Controller { /**
* Enumeration to hold the different controllers avalable.
*/
enum class ControllerR {
master, master,
slave slave
}; };
enum class Buttons { /**
* Enumeration to hold the different buttons to use.
*/
enum class ButtonsR {
arrow_buttons, arrow_buttons,
Y_B_buttons, Y_B_buttons,
left_shoulder_buttons, left_shoulder_buttons,
right_shoulder_buttons right_shoulder_buttons
}; };
/**
* Arm mechanism class.
*
* Sets up all constructors, variables, and functions.
*/
class ArmMech{ class ArmMech{
private: private:
// Arrays to hold different values for the arm.
std::vector<int> levelOneArmStates; std::vector<int> levelOneArmStates;
std::vector<int> levelTwoArmStates; std::vector<int> levelTwoArmStates;
std::vector<int> levelThreeArmStates; std::vector<int> levelThreeArmStates;
@@ -32,20 +47,20 @@ namespace ez{
float armKP; float armKP;
public: public:
std::vector<int> selectedStates; std::vector<int> selectedStates; // Array for the arm to actaully use.
int targetPos; int targetPos; // Value to be changed based on the height of the lift
int currArmState; int currArmState;
int armTarget; int armTarget;
float armError; float armError;
float armVelocity; float armVelocity;
ArmMech(); ArmMech(); // Defualt constructor
void arm_driver_control(enum Controller, enum Buttons); void arm_driver_control(enum ControllerR, enum ButtonsR);
void arm_auton_control(int pickState); void arm_auton_control(int pickState);
void cycle_states(); void cycle_states();
void reset_state(); void reset_state();
void arm_control(); void arm_control(); // Computes the velocity for the arm to move at.
void set_arm_kp(float _armKP); void set_arm_kp(float _armKP);
void set_state_values(std::vector<int> _levelOneArmStates, std::vector<int> _levelTwoArmStates, std::vector<int> _levelThreeArmStates, std::vector<int> _levelFourArmStates); void set_state_values(std::vector<int> _levelOneArmStates, std::vector<int> _levelTwoArmStates, std::vector<int> _levelThreeArmStates, std::vector<int> _levelFourArmStates);
@@ -55,32 +70,37 @@ namespace ez{
float get_arm_kp(); float get_arm_kp();
}; };
/**
* List mechanism class.
*
* Sets up all constructors, variables, and functions.
*/
class LiftMech { class LiftMech {
private: private:
std::vector<int> liftStates; std::vector<int> liftStates; // Array the lift uses to move.
int numLiftStates;
float liftKP;
std::vector<int> firstRange; std::vector<int> firstRange;
std::vector<int> secondRange; std::vector<int> secondRange;
std::vector<int> thirdRange; std::vector<int> thirdRange;
std::vector<int> fourthRange; std::vector<int> fourthRange;
int numLiftStates;
float liftKP;
public: public:
int liftHeight; // Current height of the lift in centdegrees
int currLiftState; int currLiftState;
int liftTarget; int liftTarget;
int liftHeight;
float liftError; float liftError;
float liftVelocity; float liftVelocity;
LiftMech(); LiftMech(); // Default constructor
LiftMech(std::initializer_list<int> _liftStates, int _numLiftStates); LiftMech(std::initializer_list<int> _liftStates, int _numLiftStates); // Constructor to initialze array and amount of values in array.
void lift_driver_control(enum Controller, enum Buttons); void lift_driver_control(enum ControllerR, enum ButtonsR);
void lift_auton_control(int pickState); void lift_auton_control(int pickState);
void cycle_states(); void cycle_states();
void reset_state(); void reset_state();
void lift_control(); void lift_control(); // Computes the velocity for the lift to move at.
void lift_range_finder(); int lift_range_finder(); // Finds the ranges for the arm to use
void set_lift_kp(float _liftKP); void set_lift_kp(float _liftKP);
void set_ranges(std::vector<int> _firstRange, std::vector<int> _secondRange, std::vector<int> _thirdRange, std::vector<int> _fourthRange); void set_ranges(std::vector<int> _firstRange, std::vector<int> _secondRange, std::vector<int> _thirdRange, std::vector<int> _fourthRange);
@@ -90,9 +110,14 @@ namespace ez{
float get_lift_kp(); float get_lift_kp();
}; };
/**
* Wrist mechanism class.
*
* Sets up all constructors, variables, and functions.
*/
class WristMech { class WristMech {
private: private:
std::vector<int> wristStates; std::vector<int> wristStates; // Array the wrist uses to move
int numWristStates; int numWristStates;
float wristKP; float wristKP;
@@ -102,12 +127,12 @@ namespace ez{
float wristError; float wristError;
float wristVelocity; float wristVelocity;
WristMech(); WristMech(); // Default constuctor
WristMech(std::initializer_list<int> _wristStates, int _numWristStates); WristMech(std::initializer_list<int> _wristStates, int _numWristStates); // Constructor to initialze array and amount of values in array.
void wrist_driver_control(enum Controller, enum Buttons); void wrist_driver_control(enum ControllerR, enum ButtonsR);
void rotate_wrist(int wristThreshold); void rotate_wrist(int wristThreshold); // Rotates the wrist based where the arm is.
void reset_state(); void reset_state();
void wrist_control(); void wrist_control();
include/setup/initialize.hpp
+20 -10
View File
@@ -8,29 +8,39 @@
#define CONFIG_H #define CONFIG_H
/** /**
* * Sets up the master and slave controllers.
*/ */
inline pros::Controller master(pros::E_CONTROLLER_MASTER); inline pros::Controller master(pros::E_CONTROLLER_MASTER);
inline pros::Controller slave(pros::E_CONTROLLER_PARTNER); inline pros::Controller slave(pros::E_CONTROLLER_PARTNER);
/** /**
* Declare mechinism motors * Declares mechinism motors and motor groups.
*/ */
inline pros::MotorGroup lift({10, 11}); inline pros::MotorGroup lift({15, -19});
inline pros::MotorGroup wrist({13, 14}); inline pros::MotorGroup wrist({1, 9});
inline pros::Motor arm({12}); inline pros::Motor arm({6});
/** /**
* Declare pistons here * Declares piston mechanisms.
*/ */
inline ez::Piston claw({'G',true}); inline ez::Piston claw({'G',true});
/** /**
* Declare sensors here * Declare limit switches and other ADI sensors
*/ */
inline pros::Rotation liftRotation({15}); inline pros::ADIButton liftReset({'A'});
inline pros::Rotation armRotation({16}); inline pros::ADIButton armReset({'B'});
inline pros::Rotation wristRotation({17});
/**
* Declares all sensors.
*
* [1] - Rotation sensors
*
* [2] - Color sensors
*/
inline pros::Rotation liftRotation({17});
inline pros::Rotation armRotation({8});
inline pros::Rotation wristRotation({21});
inline pros::Optical colorSensorOne({18}); inline pros::Optical colorSensorOne({18});
inline pros::Optical colorSensorTwo({19}); inline pros::Optical colorSensorTwo({19});
src/EZ-Template/pneumatics.cpp
+14 -14
View File
@@ -5,22 +5,22 @@ using namespace ez;
Pneumatics::Pneumatics(){} 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) switch (activePiston)
{ {
case Pistons::claw_piston: case PistonsP::claw_piston:
/*More code can go here if the claw piston is selected*/ /*More code can go here if the claw piston is selected*/
switch (currController) switch (currController)
{ {
case Controller::master: case ControllerP::master:
/*More code can go here if we want to use the master controller*/ /*More code can go here if we want to use the master controller*/
switch (buttonUsed) switch (buttonUsed)
{ {
case Buttons::arrow_buttons: case ButtonsP::arrow_buttons:
if (master.get_digital(DIGITAL_RIGHT)) { if (master.get_digital(DIGITAL_RIGHT)) {
claw.set(true); claw.set(true);
} }
@@ -28,7 +28,7 @@ void Pneumatics::control_piston(Pistons activePiston, ControlPeriod currPeriod,
claw.set(false); claw.set(false);
} }
break; break;
case Buttons::Y_B_buttons: case ButtonsP::Y_B_buttons:
if (master.get_digital(DIGITAL_Y)) { if (master.get_digital(DIGITAL_Y)) {
claw.set(true); claw.set(true);
} }
@@ -36,7 +36,7 @@ void Pneumatics::control_piston(Pistons activePiston, ControlPeriod currPeriod,
claw.set(false); claw.set(false);
} }
break; break;
case Buttons::left_shoulder_buttons: case ButtonsP::left_shoulder_buttons:
if (master.get_digital(DIGITAL_L1)) { if (master.get_digital(DIGITAL_L1)) {
claw.set(true); claw.set(true);
} }
@@ -44,7 +44,7 @@ void Pneumatics::control_piston(Pistons activePiston, ControlPeriod currPeriod,
claw.set(false); claw.set(false);
} }
break; break;
case Buttons::right_shoulder_buttons: case ButtonsP::right_shoulder_buttons:
if (master.get_digital(DIGITAL_R1)) { if (master.get_digital(DIGITAL_R1)) {
claw.set(true); claw.set(true);
} }
@@ -56,11 +56,11 @@ void Pneumatics::control_piston(Pistons activePiston, ControlPeriod currPeriod,
break; break;
} }
break; break;
case Controller::slave: case ControllerP::slave:
switch (buttonUsed) switch (buttonUsed)
{ {
case Buttons::arrow_buttons: case ButtonsP::arrow_buttons:
if (slave.get_digital(DIGITAL_RIGHT)) { if (slave.get_digital(DIGITAL_RIGHT)) {
claw.set(true); claw.set(true);
} }
@@ -68,7 +68,7 @@ void Pneumatics::control_piston(Pistons activePiston, ControlPeriod currPeriod,
claw.set(false); claw.set(false);
} }
break; break;
case Buttons::Y_B_buttons: case ButtonsP::Y_B_buttons:
if (slave.get_digital(DIGITAL_Y)) { if (slave.get_digital(DIGITAL_Y)) {
claw.set(true); claw.set(true);
} }
@@ -76,7 +76,7 @@ void Pneumatics::control_piston(Pistons activePiston, ControlPeriod currPeriod,
claw.set(false); claw.set(false);
} }
break; break;
case Buttons::left_shoulder_buttons: case ButtonsP::left_shoulder_buttons:
if (slave.get_digital(DIGITAL_L1)) { if (slave.get_digital(DIGITAL_L1)) {
claw.set(true); claw.set(true);
} }
@@ -84,7 +84,7 @@ void Pneumatics::control_piston(Pistons activePiston, ControlPeriod currPeriod,
claw.set(false); claw.set(false);
} }
break; break;
case Buttons::right_shoulder_buttons: case ButtonsP::right_shoulder_buttons:
if (slave.get_digital(DIGITAL_R1)) { if (slave.get_digital(DIGITAL_R1)) {
claw.set(true); claw.set(true);
} }
@@ -111,7 +111,7 @@ void Pneumatics::control_piston(Pistons activePiston, ControlPeriod currPeriod,
else { else {
switch (activePiston) switch (activePiston)
{ {
case Pistons::claw_piston: case PistonsP::claw_piston:
/*code here*/ /*code here*/
break; break;
default: default:
src/EZ-Template/rotation_mechanisms/arm_rotation.cpp
+135 -68
View File
@@ -3,52 +3,69 @@
using namespace ez; using namespace ez;
LiftMech liftObj; LiftMech liftRangeAccess; // Lift object to use lift variables and functions
/**
* Default constructor
*/
ArmMech::ArmMech() { ArmMech::ArmMech() {
currArmState = 0; currArmState = 0;
armTarget = 0; armTarget = 0;
targetPos = 0; targetPos = 0;
set_arm_kp(0.0173f); 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) switch (buttonUsed)
{ {
case Buttons::arrow_buttons: case ButtonsR::arrow_buttons:
if (master.get_digital_new_press(DIGITAL_RIGHT)){ // If the arrow_buttons are selected run this code
if (master.get_digital(DIGITAL_RIGHT)){
arm.move(127); arm.move(127);
} }
else if (master.get_digital_new_press(DIGITAL_DOWN)) { else if (master.get_digital(DIGITAL_DOWN)) {
arm.move(-127); arm.move(-127);
} }
break; break;
case Buttons::Y_B_buttons: case ButtonsR::Y_B_buttons:
if (master.get_digital_new_press(DIGITAL_Y)){ // If the Y_B_buttons are selected run this code
if (master.get_digital(DIGITAL_Y)){
arm.move(127); arm.move(127);
} }
else if (master.get_digital_new_press(DIGITAL_B)) { else if (master.get_digital(DIGITAL_B)) {
arm.move(-127); arm.move(-127);
} }
break; break;
case Buttons::left_shoulder_buttons: case ButtonsR::left_shoulder_buttons:
if (master.get_digital_new_press(DIGITAL_L1)){ // If the left-shoulder_buttons are selected run this code
if (master.get_digital(DIGITAL_L1)){
arm.move(127); arm.move(127);
} }
else if (master.get_digital_new_press(DIGITAL_L2)) { else if (master.get_digital(DIGITAL_L2)) {
arm.move(-127); arm.move(-127);
} }
break; break;
case Buttons::right_shoulder_buttons: case ButtonsR::right_shoulder_buttons:
if (master.get_digital_new_press(DIGITAL_R1)){ // If the right_shoulder_buttons are selected run this code
if (master.get_digital(DIGITAL_R1)){
arm.move(127); arm.move(127);
} }
else if (master.get_digital_new_press(DIGITAL_R2)) { else if (master.get_digital(DIGITAL_R2)) {
arm.move(-127); arm.move(-127);
} }
break; break;
@@ -56,39 +73,43 @@ void ArmMech::arm_driver_control(Controller currController, Buttons buttonUsed)
break; break;
} }
break; break;
case Controller::slave: case ControllerR::slave:
// Code you want to run when the slave controller is selected
switch (buttonUsed) switch (buttonUsed)
{ {
case Buttons::arrow_buttons: case ButtonsR::arrow_buttons:
if (slave.get_digital_new_press(DIGITAL_RIGHT)){ // If the arrow_buttons are selected run this code
if (slave.get_digital(DIGITAL_RIGHT)){
cycle_states(); cycle_states();
} }
else if (slave.get_digital_new_press(DIGITAL_DOWN)) { else if (slave.get_digital(DIGITAL_DOWN)) {
reset_state(); reset_state();
} }
break; break;
case Buttons::Y_B_buttons: case ButtonsR::Y_B_buttons:
if (slave.get_digital_new_press(DIGITAL_Y)){ // If the Y_B_buttons are selected run this code
if (slave.get_digital(DIGITAL_Y)){
cycle_states(); cycle_states();
} }
else if (slave.get_digital_new_press(DIGITAL_B)) { else if (slave.get_digital(DIGITAL_B)) {
reset_state(); reset_state();
} }
break; break;
case Buttons::left_shoulder_buttons: case ButtonsR::left_shoulder_buttons:
if (slave.get_digital_new_press(DIGITAL_L1)){ // If the left_shoulder_buttons are selected run this code
if (slave.get_digital(DIGITAL_L1)){
cycle_states(); cycle_states();
} }
else if (slave.get_digital_new_press(DIGITAL_L2)) { else if (slave.get_digital(DIGITAL_L2)) {
reset_state(); reset_state();
} }
break; break;
case Buttons::right_shoulder_buttons: case ButtonsR::right_shoulder_buttons:
if (slave.get_digital_new_press(DIGITAL_R1)){ // If the right_shoulder_buttons are selected run this code
if (slave.get_digital(DIGITAL_R1)){
cycle_states(); cycle_states();
} }
else if (slave.get_digital_new_press(DIGITAL_R2)) { else if (slave.get_digital(DIGITAL_R2)) {
reset_state(); reset_state();
} }
break; 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) { 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) switch (targetPos)
{ {
case 0: case 0:
selectedStates = levelOneArmStates; selectedStates = levelOneArmStates; // Makes the main array equal the level one array
if (pickState == 1) { if (pickState == 1) {
selectedStates[1]; armTarget = selectedStates[1]; // Makes the target value the second in the array
} }
else { else {
selectedStates[0]; armTarget = selectedStates[0]; // Makes the target value the first in the array
} }
break; break;
case 1: case 1:
selectedStates = levelOneArmStates; selectedStates = levelOneArmStates; // Make the main array equal the level two array
if (pickState == 1) { if (pickState == 1) {
selectedStates[1]; armTarget = selectedStates[1];
} }
else { else {
selectedStates[0]; armTarget = selectedStates[0];
} }
break; break;
case 2: case 2:
selectedStates = levelTwoArmStates; selectedStates = levelTwoArmStates; // Makes the main array equal the level three array
if (pickState == 1) { if (pickState == 1) {
selectedStates[1]; armTarget = selectedStates[1];
} }
else { else {
selectedStates[0]; armTarget = selectedStates[0];
} }
break; break;
case 3: case 3:
selectedStates = levelThreeArmStates; selectedStates = levelThreeArmStates; // Makes the main array equal the level four array
if (pickState == 1) { if (pickState == 1) {
selectedStates[1]; armTarget = selectedStates[1];
} }
else { else {
selectedStates[0]; armTarget = selectedStates[0];
} }
break; break;
default: 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() { 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) // Checks the array to make sure it isn't empty to prevent a segmentation fault(trying to access unassigned memory)
{ if (!selectedStates.empty()) {
case 0: // Based on the value returned by the lift_range_finder function change the array to match the level of the lift
selectedStates = levelOneArmStates; switch (targetPos)
break; {
case 1: case 0:
selectedStates = levelTwoArmStates; selectedStates = levelOneArmStates;
break; break;
case 2: case 1:
selectedStates = levelThreeArmStates; selectedStates = levelTwoArmStates;
break; break;
case 3: case 2:
selectedStates = levelFourArmStates; selectedStates = levelThreeArmStates;
break; break;
default: case 3:
break; selectedStates = levelFourArmStates;
break;
default:
break;
}
} }
currArmState ++; currArmState ++;
if (currArmState == numArmStates) { if (currArmState >= numArmStates) {
currArmState = 0; 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() { void ArmMech::reset_state() {
currArmState = 0; 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() { void ArmMech::arm_control() {
armError = armTarget - armRotation.get_position(); armError = armTarget - armRotation.get_position(); // Sets the error value
armVelocity = get_arm_kp() * armError; armVelocity = get_arm_kp() * armError; // Sets the velocity
arm.move(armVelocity); 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) { void ArmMech::set_state_values(std::vector<int> _levelOneArmStates, std::vector<int> _levelTwoArmStates, std::vector<int> _levelThreeArmStates, std::vector<int> _levelFourArmStates) {
levelOneArmStates = _levelOneArmStates; levelOneArmStates = _levelOneArmStates;
levelTwoArmStates = _levelTwoArmStates; levelTwoArmStates = _levelTwoArmStates;
levelThreeArmStates = _levelThreeArmStates; levelThreeArmStates = _levelThreeArmStates;
levelFourArmStates = _levelFourArmStates; 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) { void ArmMech::set_arm_kp(float _armKP) {
armKP = _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;} int ArmMech::get_num_arm_states() {return numArmStates;}
float ArmMech::get_arm_kp() {return armKP;} float ArmMech::get_arm_kp() {return armKP;}
src/EZ-Template/rotation_mechanisms/lift_rotation.cpp
+41 -26
View File
@@ -5,7 +5,13 @@ using namespace ez;
ArmMech armObjRange; 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) { LiftMech::LiftMech(std::initializer_list<int> _liftStates, int _numLiftStates) {
liftStates = _liftStates; liftStates = _liftStates;
numLiftStates = _numLiftStates; numLiftStates = _numLiftStates;
@@ -14,14 +20,14 @@ LiftMech::LiftMech(std::initializer_list<int> _liftStates, int _numLiftStates) {
set_lift_kp(0.0173f); 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) switch (currController)
{ {
case Controller::master: case ControllerR::master:
switch (buttonUsed) switch (buttonUsed)
{ {
case Buttons::arrow_buttons: case ButtonsR::arrow_buttons:
if (master.get_digital(DIGITAL_RIGHT)) { if (master.get_digital(DIGITAL_RIGHT)) {
lift.move(127); lift.move(127);
} }
@@ -29,7 +35,7 @@ void LiftMech::lift_driver_control(Controller currController, Buttons buttonUsed
lift.move(-127); lift.move(-127);
} }
break; break;
case Buttons::Y_B_buttons: case ButtonsR::Y_B_buttons:
if (master.get_digital(DIGITAL_Y)) { if (master.get_digital(DIGITAL_Y)) {
lift.move(127); lift.move(127);
} }
@@ -37,7 +43,7 @@ void LiftMech::lift_driver_control(Controller currController, Buttons buttonUsed
lift.move(-127); lift.move(-127);
} }
break; break;
case Buttons::left_shoulder_buttons: case ButtonsR::left_shoulder_buttons:
if (master.get_digital(DIGITAL_L1)) { if (master.get_digital(DIGITAL_L1)) {
lift.move(127); lift.move(127);
} }
@@ -45,7 +51,7 @@ void LiftMech::lift_driver_control(Controller currController, Buttons buttonUsed
lift.move(-127); lift.move(-127);
} }
break; break;
case Buttons::right_shoulder_buttons: case ButtonsR::right_shoulder_buttons:
if (master.get_digital(DIGITAL_R1)) { if (master.get_digital(DIGITAL_R1)) {
lift.move(127); lift.move(127);
} }
@@ -57,11 +63,11 @@ void LiftMech::lift_driver_control(Controller currController, Buttons buttonUsed
break; break;
} }
break; break;
case Controller::slave: case ControllerR::slave:
switch (buttonUsed) switch (buttonUsed)
{ {
case Buttons::arrow_buttons: case ButtonsR::arrow_buttons:
if (slave.get_digital(DIGITAL_RIGHT)) { if (slave.get_digital(DIGITAL_RIGHT)) {
cycle_states(); cycle_states();
} }
@@ -69,7 +75,7 @@ void LiftMech::lift_driver_control(Controller currController, Buttons buttonUsed
reset_state(); reset_state();
} }
break; break;
case Buttons::Y_B_buttons: case ButtonsR::Y_B_buttons:
if (slave.get_digital(DIGITAL_Y)) { if (slave.get_digital(DIGITAL_Y)) {
cycle_states(); cycle_states();
} }
@@ -77,7 +83,7 @@ void LiftMech::lift_driver_control(Controller currController, Buttons buttonUsed
reset_state(); reset_state();
} }
break; break;
case Buttons::left_shoulder_buttons: case ButtonsR::left_shoulder_buttons:
if (slave.get_digital(DIGITAL_L1)) { if (slave.get_digital(DIGITAL_L1)) {
cycle_states(); cycle_states();
} }
@@ -85,7 +91,7 @@ void LiftMech::lift_driver_control(Controller currController, Buttons buttonUsed
reset_state(); reset_state();
} }
break; break;
case Buttons::right_shoulder_buttons: case ButtonsR::right_shoulder_buttons:
if (slave.get_digital(DIGITAL_R1)) { if (slave.get_digital(DIGITAL_R1)) {
cycle_states(); cycle_states();
} }
@@ -126,15 +132,20 @@ void LiftMech::lift_auton_control(int pickState) {
void LiftMech::cycle_states() { void LiftMech::cycle_states() {
currLiftState ++; currLiftState ++;
if (currLiftState == numLiftStates) { if (!liftStates.empty()) {
currLiftState = 0; if (currLiftState == numLiftStates) {
currLiftState = 0;
}
liftTarget = liftStates[currLiftState];
} }
liftTarget = liftStates[currLiftState];
} }
void LiftMech::reset_state() { void LiftMech::reset_state() {
currLiftState = 0; currLiftState = 0;
liftTarget = liftStates[currLiftState];
if (!liftStates.empty()) {
liftTarget = liftStates[currLiftState];
}
} }
void LiftMech::lift_control() { void LiftMech::lift_control() {
@@ -143,20 +154,20 @@ void LiftMech::lift_control() {
lift.move(liftVelocity); lift.move(liftVelocity);
} }
void LiftMech::lift_range_finder() { int LiftMech::lift_range_finder() {
liftHeight = liftRotation.get_position(); liftHeight = liftRotation.get_position();
if (liftHeight >= firstRange[0] && liftHeight <= firstRange[1]) { if (firstRange.size() >= 2 && liftHeight >= firstRange[0] && liftHeight <= firstRange[1]) {
armObjRange.targetPos = 0; return armObjRange.targetPos = 0;
} }
else if (liftHeight >= secondRange[0] && liftHeight <= secondRange[1]) { else if (secondRange.size() >= 2 && liftHeight >= secondRange[0] && liftHeight <= secondRange[1]) {
armObjRange.targetPos = 1; return armObjRange.targetPos = 1;
} }
else if (liftHeight >= thirdRange[0] && liftHeight <= thirdRange[1]) { else if (thirdRange.size() >= 2 && liftHeight >= thirdRange[0] && liftHeight <= thirdRange[1]) {
armObjRange.targetPos = 2; return armObjRange.targetPos = 2;
} }
else if (liftHeight >= fourthRange[0] && liftHeight <= fourthRange[1]) { else if (fourthRange.size() >= 2 && liftHeight >= fourthRange[0] && liftHeight <= fourthRange[1]) {
armObjRange.targetPos = 3; return armObjRange.targetPos = 3;
} }
} }
@@ -171,6 +182,10 @@ void LiftMech::set_ranges(std::vector<int> _firstRange, std::vector<int> _second
fourthRange = _fourthRange; 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;} int LiftMech::get_num_lift_states() {return numLiftStates;}
float LiftMech::get_lift_kp() {return liftKP;} float LiftMech::get_lift_kp() {return liftKP;}
src/EZ-Template/rotation_mechanisms/wrist_rotation.cpp
+26 -15
View File
@@ -3,7 +3,12 @@
using namespace ez; using namespace ez;
WristMech::WristMech(){} WristMech::WristMech() {
wristError = 0;
wristVelocity = 0;
wristStates = {0, 0};
}
WristMech::WristMech(std::initializer_list<int> _wristStates, int _numWristStates) { WristMech::WristMech(std::initializer_list<int> _wristStates, int _numWristStates) {
wristStates = _wristStates; wristStates = _wristStates;
numWristStates = _numWristStates; numWristStates = _numWristStates;
@@ -12,29 +17,29 @@ WristMech::WristMech(std::initializer_list<int> _wristStates, int _numWristState
set_wrist_kp(0.0173f); 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) switch (currController)
{ {
case Controller::master: case ControllerR::master:
switch (buttonUsed) switch (buttonUsed)
{ {
case Buttons::arrow_buttons: case ButtonsR::arrow_buttons:
if (master.get_digital(DIGITAL_DOWN)) { if (master.get_digital(DIGITAL_DOWN)) {
reset_state(); reset_state();
} }
break; break;
case Buttons::Y_B_buttons: case ButtonsR::Y_B_buttons:
if (master.get_digital(DIGITAL_Y)) { if (master.get_digital(DIGITAL_Y)) {
reset_state(); reset_state();
} }
break; break;
case Buttons::left_shoulder_buttons: case ButtonsR::left_shoulder_buttons:
if (master.get_digital(DIGITAL_L1)) { if (master.get_digital(DIGITAL_L1)) {
reset_state(); reset_state();
} }
break; break;
case Buttons::right_shoulder_buttons: case ButtonsR::right_shoulder_buttons:
if (master.get_digital(DIGITAL_R1)) { if (master.get_digital(DIGITAL_R1)) {
reset_state(); reset_state();
} }
@@ -43,26 +48,26 @@ void WristMech::wrist_driver_control(Controller currController, Buttons buttonUs
break; break;
} }
break; break;
case Controller::slave: case ControllerR::slave:
switch (buttonUsed) switch (buttonUsed)
{ {
case Buttons::arrow_buttons: case ButtonsR::arrow_buttons:
if (slave.get_digital(DIGITAL_DOWN)) { if (slave.get_digital(DIGITAL_DOWN)) {
reset_state(); reset_state();
} }
break; break;
case Buttons::Y_B_buttons: case ButtonsR::Y_B_buttons:
if (slave.get_digital(DIGITAL_Y)) { if (slave.get_digital(DIGITAL_Y)) {
reset_state(); reset_state();
} }
break; break;
case Buttons::left_shoulder_buttons: case ButtonsR::left_shoulder_buttons:
if (slave.get_digital(DIGITAL_L1)) { if (slave.get_digital(DIGITAL_L1)) {
reset_state(); reset_state();
} }
break; break;
case Buttons::right_shoulder_buttons: case ButtonsR::right_shoulder_buttons:
if (slave.get_digital(DIGITAL_R1)) { if (slave.get_digital(DIGITAL_R1)) {
reset_state(); reset_state();
} }
@@ -87,16 +92,22 @@ void WristMech::rotate_wrist(int wristThreshold) {
void WristMech::reset_state() { void WristMech::reset_state() {
currWristState = 0; currWristState = 0;
wristTarget = wristStates[currWristState]; if (!wristStates.empty()) {
wristTarget = wristStates[currWristState];
}
} }
void WristMech::wrist_control() { void WristMech::wrist_control() {
wristError = wristTarget - armRotation.get_position(); wristError = wristTarget - wristRotation.get_position();
wristVelocity = get_wrist_kp() * wristError; wristVelocity = get_wrist_kp() * wristError;
wrist.move(wristVelocity); 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;} int WristMech::get_num_wrist_states() {return numWristStates;}
float WristMech::get_wrist_kp() {return wristKP;} float WristMech::get_wrist_kp() {return wristKP;}
src/main.cpp
+35 -26
View File
@@ -1,10 +1,5 @@
#include "main.h" #include "main.h"
#include <iostream>
// Global Variables
Pneumatics pistonControl;
ArmMech armObj;
LiftMech liftObj;
WristMech wristObj;
// Chassis constructor // Chassis constructor
ez::Drive chassis( ez::Drive chassis(
@@ -24,22 +19,19 @@ ez::Drive chassis(
ez::tracking_wheel horiz_tracker(8, 2.75, 4.0); // This tracking wheel is perpendicular to the drive wheels ez::tracking_wheel horiz_tracker(8, 2.75, 4.0); // This tracking wheel is perpendicular to the drive wheels
// ez::tracking_wheel vert_tracker(9, 2.75, 4.0); // This tracking wheel is parallel to the drive wheels // ez::tracking_wheel vert_tracker(9, 2.75, 4.0); // This tracking wheel is parallel to the drive wheels
// Arm constructor // Default Objects
ArmMech armSetup(); Pneumatics pistonControl;
ArmMech armObj;
// Lift constructor // Lift constructor
LiftMech liftSetup( LiftMech liftObj(
{0, 100, 200, 300}, {0, 63194, 94246, 139878}, 4);
4);
// Wrist constructor // Wrist constructor
WristMech wristSetup( WristMech wristObj(
{0, 100}, {0, -14529}, 2);
2);
/** /**
* Runs initialization code. This occurs as soon as the program is started. * Runs initialization code. This occurs as soon as the program is started.
@@ -53,7 +45,18 @@ void initialize() {
pros::delay(500); // Stop the user from doing anything while legacy ports configure pros::delay(500); // Stop the user from doing anything while legacy ports configure
armObj.set_state_values({1, 2}, {1, 2}, {1, 2}, {1, 2}); armObj.set_state_values(
{0, -5642},
{0, -22702},
{0, -19740},
{0, -17121}
);
liftObj.set_ranges(
{0, 63194},
{63194, 94246},
{94246, 139878},
{139878, 150000}
);
armRotation.reset_position(); armRotation.reset_position();
liftRotation.reset_position(); liftRotation.reset_position();
@@ -284,22 +287,28 @@ void opcontrol() {
ez_template_extras(); ez_template_extras();
// Call custom drive functions here // Call custom drive functions here
pistonControl.control_piston(Pistons::claw_piston, ControlPeriod::driver, pistonControl.control_piston(PistonsP::claw_piston, ControlPeriodP::driver,
Controller::slave, Buttons::arrow_buttons); ControllerP::slave, ButtonsP::arrow_buttons);
armObj.arm_driver_control(Controller::master, Buttons::left_shoulder_buttons); //armObj.arm_driver_control(ControllerR::master, ButtonsR::left_shoulder_buttons);
liftObj.lift_driver_control(Controller::master, Buttons::right_shoulder_buttons); //liftObj.lift_driver_control(ControllerR::master, ButtonsR::right_shoulder_buttons);
armObj.arm_driver_control(ControllerR::slave, ButtonsR::left_shoulder_buttons);
liftObj.lift_driver_control(ControllerR::slave, ButtonsR::right_shoulder_buttons);
wristObj.wrist_driver_control(ControllerR::slave, ButtonsR::Y_B_buttons);
//wristObj.rotate_wrist(90);
armObj.arm_driver_control(Controller::slave, Buttons::left_shoulder_buttons);
liftObj.lift_driver_control(Controller::slave, Buttons::right_shoulder_buttons);
wristObj.wrist_driver_control(Controller::slave, Buttons::arrow_buttons);
wristObj.rotate_wrist(90);
// chassis.opcontrol_tank(); // Tank control
chassis.opcontrol_arcade_standard(ez::SPLIT); // Standard split arcade chassis.opcontrol_arcade_standard(ez::SPLIT); // Standard split arcade
// chassis.opcontrol_tank(); // Tank control
// chassis.opcontrol_arcade_standard(ez::SINGLE); // Standard single arcade // chassis.opcontrol_arcade_standard(ez::SINGLE); // Standard single arcade
// chassis.opcontrol_arcade_flipped(ez::SPLIT); // Flipped split arcade // chassis.opcontrol_arcade_flipped(ez::SPLIT); // Flipped split arcade
// chassis.opcontrol_arcade_flipped(ez::SINGLE); // Flipped single arcade // chassis.opcontrol_arcade_flipped(ez::SINGLE); // Flipped single arcade
// Terminal debugging
std::cout << "Arm rotation sensor value: " << "\033[31m" << armRotation.get_position() << " cdg" << "mm\033[0m" << std::endl;
std::cout << "Lift rotation sensor value: " << "\033[32m" << liftRotation.get_position() << " cdg" << "mm\033[0m" << std::endl;
std::cout << "Wrist rotation sensor value: " << "\033[33m" << wristRotation.get_position() << " cdg" << "mm\033[0m" << std::endl;
pros::delay(ez::util::DELAY_TIME); // This is used for timer calculations! Keep this ez::util::DELAY_TIME pros::delay(ez::util::DELAY_TIME); // This is used for timer calculations! Keep this ez::util::DELAY_TIME
} }
} }