diff --git a/.DS_Store b/.DS_Store index a20116e..fd7fba5 100644 Binary files a/.DS_Store and b/.DS_Store differ diff --git a/FinalCode_In_Theory/.DS_Store b/FinalCode_In_Theory/.DS_Store index 8078eb2..b827d4e 100644 Binary files a/FinalCode_In_Theory/.DS_Store and b/FinalCode_In_Theory/.DS_Store differ diff --git a/FinalCode_In_Theory/FinalCode_In_Theory.v5code b/FinalCode_In_Theory/FinalCode_In_Theory.v5code deleted file mode 100644 index 0a8be30..0000000 --- a/FinalCode_In_Theory/FinalCode_In_Theory.v5code +++ /dev/null @@ -1 +0,0 @@ -{"title":"FinalCode_In_Theory","description":"Empty V5 C++ Project","icon":"USER921x.bmp","version":"19.12.1916","sdk":"","language":"cpp","competition":false,"files":[{"name":"include/robot-config.h","type":"File","specialType":"device_config"},{"name":"include/vex.h","type":"File","specialType":""},{"name":"include/DriveTrain.h","type":"File","specialType":""},{"name":"include/Lift.h","type":"File","specialType":""},{"name":"include/Intake.h","type":"File","specialType":""},{"name":"include/Tray.h","type":"File","specialType":""},{"name":"include/Ports.h","type":"File","specialType":""},{"name":"include/Enums.h","type":"File","specialType":""},{"name":"include/Joystick.h","type":"File","specialType":""},{"name":"include/joystick_config.h","type":"File","specialType":""},{"name":"include/AutoDrive.h","type":"File","specialType":""},{"name":"makefile","type":"File","specialType":""},{"name":"src/main.cpp","type":"File","specialType":""},{"name":"src/robot-config.cpp","type":"File","specialType":"device_config"},{"name":"src/Joystick.cpp","type":"File","specialType":""},{"name":"src/Lift.cpp","type":"File","specialType":""},{"name":"src/Tray.cpp","type":"File","specialType":""},{"name":"src/Intake.cpp","type":"File","specialType":""},{"name":"src/DriveTrain.cpp","type":"File","specialType":""},{"name":"src/AutoDrive.cpp","type":"File","specialType":""},{"name":"vex/mkenv.mk","type":"File","specialType":""},{"name":"vex/mkrules.mk","type":"File","specialType":""},{"name":"include","type":"Directory"},{"name":"src","type":"Directory"},{"name":"vex","type":"Directory"}],"device":{"slot":1,"uid":"276-4810","options":{}},"isExpertMode":true,"isExpertModeRC":true,"isVexFileImport":false,"robotconfig":[],"neverUpdate":null} \ No newline at end of file diff --git a/FinalCode_In_Theory/FinalCode_In_Theory_Small_Red.v5code b/FinalCode_In_Theory/FinalCode_In_Theory_Small_Red.v5code new file mode 100644 index 0000000..4b4e9ce --- /dev/null +++ b/FinalCode_In_Theory/FinalCode_In_Theory_Small_Red.v5code @@ -0,0 +1 @@ +{"title":"FinalCode_In_Theory_Small_Red","description":"Empty V5 C++ Project","icon":"USER921x.bmp","version":"19.12.1916","sdk":"20191206_14_00_00","language":"cpp","competition":false,"files":[{"name":"include/robot-config.h","type":"File","specialType":"device_config"},{"name":"include/vex.h","type":"File","specialType":""},{"name":"include/DriveTrain.h","type":"File","specialType":""},{"name":"include/Lift.h","type":"File","specialType":""},{"name":"include/Intake.h","type":"File","specialType":""},{"name":"include/Tray.h","type":"File","specialType":""},{"name":"include/Ports.h","type":"File","specialType":""},{"name":"include/Enums.h","type":"File","specialType":""},{"name":"include/Joystick.h","type":"File","specialType":""},{"name":"include/joystick_config.h","type":"File","specialType":""},{"name":"include/AutoDrive.h","type":"File","specialType":""},{"name":"makefile","type":"File","specialType":""},{"name":"src/main.cpp","type":"File","specialType":""},{"name":"src/robot-config.cpp","type":"File","specialType":"device_config"},{"name":"src/Joystick.cpp","type":"File","specialType":""},{"name":"src/Lift.cpp","type":"File","specialType":""},{"name":"src/Tray.cpp","type":"File","specialType":""},{"name":"src/Intake.cpp","type":"File","specialType":""},{"name":"src/DriveTrain.cpp","type":"File","specialType":""},{"name":"src/AutoDrive.cpp","type":"File","specialType":""},{"name":"src/Enums.cpp","type":"File","specialType":""},{"name":"vex/mkenv.mk","type":"File","specialType":""},{"name":"vex/mkrules.mk","type":"File","specialType":""},{"name":"include","type":"Directory"},{"name":"src","type":"Directory"},{"name":"vex","type":"Directory"}],"device":{"slot":2,"uid":"276-4810","options":{}},"isExpertMode":true,"isExpertModeRC":true,"isVexFileImport":false,"robotconfig":[],"neverUpdate":null} \ No newline at end of file diff --git a/FinalCode_In_Theory/include/DriveTrain.h b/FinalCode_In_Theory/include/DriveTrain.h index 231a792..77c6130 100644 --- a/FinalCode_In_Theory/include/DriveTrain.h +++ b/FinalCode_In_Theory/include/DriveTrain.h @@ -9,7 +9,10 @@ class DriveTrain { public: - + V5_DeviceT topleftMotor; + V5_DeviceT toprightMotor; + V5_DeviceT bottomleftMotor; + V5_DeviceT bottomrightMotor; static constexpr double TICKS_PER_IN = 63.88; DriveTrain(int topleftMotorport, @@ -24,6 +27,7 @@ class DriveTrain void update(DriveTrain_State state); void updateSystemState(System_State state); + void updateAuto(Auto_State state); int32_t getLeftEncoder(); @@ -34,10 +38,7 @@ class DriveTrain void setPower(int32_t left, int32_t right); private: - V5_DeviceT topleftMotor; - V5_DeviceT toprightMotor; - V5_DeviceT bottomleftMotor; - V5_DeviceT bottomrightMotor; + vex::controller *controller_ptr; AutoDrive auto_drive; @@ -45,7 +46,7 @@ class DriveTrain static constexpr double POS_I = 0; static constexpr double POS_D = 0; - static constexpr double VEL_P = 4; + static constexpr double VEL_P = 5; static constexpr double VEL_I = 0.4; static constexpr double VEL_D = 0; }; \ No newline at end of file diff --git a/FinalCode_In_Theory/include/Enums.h b/FinalCode_In_Theory/include/Enums.h index 8c9bb41..4bdefbe 100644 --- a/FinalCode_In_Theory/include/Enums.h +++ b/FinalCode_In_Theory/include/Enums.h @@ -1,6 +1,10 @@ #pragma once +#include enum DriveTrain_State{DRIVE, DRIVE_BACK_STATE, OFF}; -enum System_State{UNFOLD, UNFOLD_ARM_ZERO, TRAY_ZERO, BASE, ARM1, ARM2, ARM_ZERO, POSITION_CUBES, TRAY_VERTICAL}; +enum System_State{UNFOLD, UNFOLD_ARM_ZERO, TRAY_ZERO, BASE_ARM, BASE_TRAY, ARM1, ARM2, ARM_ZERO, POSITION_CUBES, TRAY_VERTICAL}; +enum Auto_State{FORWARD, BACK, CLEAR, TURN_THREE, TO_STACK, POSITION_CUBES_AUTO, POSITION_CUBES_MORE_AUTO, TRAY_VERTICAL_AUTO, OUTTAKE, STOP_AUTO}; enum Button_Enums{BUTTONA, BUTTONB, BUTTONY, BUTTONX, BUTTONL1, BUTTONL2, BUTTONR1, BUTTONR2, BUTTONUP, BUTTONRIGHT, BUTTONLEFT, BUTTONDOWN}; -enum Axis_Enums{AXIS1, AXIS2, AXIS3, AXIS4}; \ No newline at end of file +enum Axis_Enums{AXIS1, AXIS2, AXIS3, AXIS4}; +std::string drive_train_state_to_string(DriveTrain_State state); +std::string system_state_to_string(System_State state); \ No newline at end of file diff --git a/FinalCode_In_Theory/include/Intake.h b/FinalCode_In_Theory/include/Intake.h index 7d5ae2e..7a1a7df 100644 --- a/FinalCode_In_Theory/include/Intake.h +++ b/FinalCode_In_Theory/include/Intake.h @@ -9,6 +9,8 @@ class Intake { public: + bool toggle_on; + Intake(int intakeport, int intake1port, vex::controller *controller_p); @@ -18,8 +20,10 @@ class Intake void joystickIntake(int32_t power); void update(System_State state); + void updateAuto(Auto_State state); private: + bool toggle_pressed; V5_DeviceT intake; V5_DeviceT intake1; vex::controller *controller_ptr; diff --git a/FinalCode_In_Theory/include/Lift.h b/FinalCode_In_Theory/include/Lift.h index addd97a..48ef19b 100644 --- a/FinalCode_In_Theory/include/Lift.h +++ b/FinalCode_In_Theory/include/Lift.h @@ -11,22 +11,24 @@ class Lift public: Lift(int liftmotorport, int liftmotorport1, - vex::triport::port *limitswitchport, + vex::triport::port *leftlimitswitchport, + vex::triport::port *rightimitswitchport, vex::brain *brain_p); void movePID(); - void moveConst(int32_t speed ); + void moveConst(int32_t speed_left, int32_t speed_right); void movebyJoy(int32_t vel); void zeroEncoder(); - void update(System_State state); + void update(System_State state, bool go); bool getLimitSwitch(); - uint32_t getLiftRotation(); + uint32_t getLeftLiftRotation(); + uint32_t getRightLiftRotation(); void setPIDBounds(int32_t min_power, int32_t max_power); @@ -34,18 +36,31 @@ class Lift void stopPID(); + int32_t getLeftSetPosition(); + int32_t getRightSetPosition(); + bool getLeftLimitSwitch(); + bool getRightLimitSwitch(); + bool isZeroed(); + +public: + V5_DeviceT lift_motor_left; + V5_DeviceT lift_motor_right; + bool left_zeroed; + bool right_zeroed; + bool switch_val; private: - V5_DeviceT liftmotor; - V5_DeviceT liftmotor1; - vex::limit zero_switch; + vex::limit zero_switch_left; + vex::limit zero_switch_right; vex::brain *brain_ptr; - PID lift_pid; - - double liftmotor_base; - - static constexpr double P = 100; - static constexpr double I = 10; - static constexpr double D = 0; + PID lift_pid_left; + PID lift_pid_right; +public: + double lift_left_base; + double lift_right_base; +private: + static constexpr double P = 150; + static constexpr double I = 15; + static constexpr double D = 3; - void setMotors(int32_t input); + void setMotors(int32_t input, int32_t input1); }; \ No newline at end of file diff --git a/FinalCode_In_Theory/include/PID.h b/FinalCode_In_Theory/include/PID.h index 9f0c815..02557a0 100644 --- a/FinalCode_In_Theory/include/PID.h +++ b/FinalCode_In_Theory/include/PID.h @@ -104,6 +104,9 @@ class PID initialized = false; time.clear(); } + T getTarget(){ + return target; + } private: T p_term; diff --git a/FinalCode_In_Theory/include/Ports.h b/FinalCode_In_Theory/include/Ports.h index 481278e..42d48ef 100644 --- a/FinalCode_In_Theory/include/Ports.h +++ b/FinalCode_In_Theory/include/Ports.h @@ -25,9 +25,12 @@ namespace Ports static constexpr double POSITION_CUBE_TIMEOUT = 2.0; static constexpr int32_t TRAY_BASE_POSITION = 0; - static constexpr int32_t TRAY_VERTICAL_POSITION = 2100; + static constexpr int32_t TRAY_VERTICAL_POSITION = 2250; static constexpr int32_t LIFT_BASE_POSITION = 0; static constexpr int32_t LIFT_BASE_POSITION1 = 400; static constexpr int32_t LIFT_BASE_POSITION2 = 800; + static constexpr int32_t TRAY_CONSTANT = 10; + static constexpr int32_t ARM_CONSTANT = 10; + } \ No newline at end of file diff --git a/FinalCode_In_Theory/include/Tray.h b/FinalCode_In_Theory/include/Tray.h index 1b08aff..c3f79ee 100644 --- a/FinalCode_In_Theory/include/Tray.h +++ b/FinalCode_In_Theory/include/Tray.h @@ -23,7 +23,8 @@ class Tray void zeroEncoder(); - void update(System_State state); + void update(System_State state, bool go); + void updateAuto(Auto_State state); bool getLimitSwitch(); @@ -37,6 +38,7 @@ class Tray void stopPID(); + int32_t getSetPosition(); private: V5_DeviceT traymotor; V5_DeviceT traymotor1; @@ -50,6 +52,7 @@ class Tray static constexpr double P = 100; static constexpr double I = 10; static constexpr double D = 0; + void setMotors(int32_t input); }; \ No newline at end of file diff --git a/FinalCode_In_Theory/include/joystick_config.h b/FinalCode_In_Theory/include/joystick_config.h index 8bf112e..9f50455 100644 --- a/FinalCode_In_Theory/include/joystick_config.h +++ b/FinalCode_In_Theory/include/joystick_config.h @@ -4,13 +4,18 @@ namespace joystick_config{ - static constexpr Button_Enums BASE_BUTTON = Button_Enums::BUTTONA; - static constexpr Button_Enums ARM1_BUTTON = Button_Enums::BUTTONB; + static constexpr Button_Enums BASE_BUTTON = Button_Enums::BUTTONB; + static constexpr Button_Enums TOGGLE_BUTTON = Button_Enums::BUTTONDOWN; + static constexpr Button_Enums ARM1_BUTTON = Button_Enums::BUTTONA; static constexpr Button_Enums ARM2_BUTTON = Button_Enums::BUTTONX; static constexpr Button_Enums VERTICAL_BUTTON = Button_Enums::BUTTONY; static constexpr Button_Enums INTAKE_BUTTON = Button_Enums::BUTTONL1; static constexpr Button_Enums OUTTAKE_BUTTON = Button_Enums::BUTTONL2; static constexpr Button_Enums UNFOLD_BUTTON = Button_Enums::BUTTONUP; + static constexpr Button_Enums TRAY_FLIP_BUTTON = Button_Enums::BUTTONRIGHT; + static constexpr Button_Enums ARM_FLIP_BUTTON = Button_Enums::BUTTONLEFT; + static constexpr Button_Enums UP_BUTTON = Button_Enums::BUTTONR1; + static constexpr Button_Enums DOWN_BUTTON = Button_Enums::BUTTONR2; static constexpr Axis_Enums FORWARD_AXIS = Axis_Enums::AXIS3; static constexpr Axis_Enums TURN_AXIS = Axis_Enums::AXIS1; diff --git a/FinalCode_In_Theory/src/DriveTrain.cpp b/FinalCode_In_Theory/src/DriveTrain.cpp index de841ba..b4d12ef 100644 --- a/FinalCode_In_Theory/src/DriveTrain.cpp +++ b/FinalCode_In_Theory/src/DriveTrain.cpp @@ -90,13 +90,39 @@ void DriveTrain::update(DriveTrain_State state) { if(state == DRIVE) { - arcadeDrive(JoystickAxis(*controller_ptr, joystick_config::FORWARD_AXIS), + arcadeDrive((JoystickAxis(*controller_ptr, joystick_config::FORWARD_AXIS)), JoystickAxis(*controller_ptr, joystick_config::TURN_AXIS)); } if(state == DRIVE_BACK_STATE){ if(JoystickButtonPressed(*controller_ptr, joystick_config::OUTTAKE_BUTTON)){ - setPower(-4000, -4000); + arcadeDrive((JoystickAxis(*controller_ptr, joystick_config::FORWARD_AXIS)) - 73, + JoystickAxis(*controller_ptr, joystick_config::TURN_AXIS)); } + else{ + arcadeDrive((JoystickAxis(*controller_ptr, joystick_config::FORWARD_AXIS)), + JoystickAxis(*controller_ptr, joystick_config::TURN_AXIS)); + } + } +} + +void DriveTrain::updateAuto(Auto_State state) +{ + if(state == FORWARD || + state == BACK || + state == CLEAR || + state == TURN_THREE || + state == TO_STACK) + { + auto_drive.update(getLeftEncoder(), getRightEncoder()); + setPower(auto_drive.leftPower(), auto_drive.rightPower()); + } + else if(state == OUTTAKE) + { + setPower(-4000, -4000); + } + else + { + setPower(0, 0); } } diff --git a/FinalCode_In_Theory/src/Enums.cpp b/FinalCode_In_Theory/src/Enums.cpp new file mode 100644 index 0000000..c177433 --- /dev/null +++ b/FinalCode_In_Theory/src/Enums.cpp @@ -0,0 +1,41 @@ +#include "Enums.h" + +std::string drive_train_state_to_string(DriveTrain_State state){ + switch(state){ + case DRIVE: + return "DRIVE"; + case DRIVE_BACK_STATE: + return "DRIVE_BACK_STATE"; + case OFF: + return "OFF"; + default: + return "Unkown drive state"; + } +} + +std::string system_state_to_string(System_State state){ + switch(state){ + case UNFOLD: + return std::string{"UNFOLD"}; + case UNFOLD_ARM_ZERO: + return std::string{"UNFOLD_ARM_ZERO"}; + case TRAY_ZERO: + return std::string{"TRAY_ZERO"}; + case BASE_ARM: + return std::string{"BASE ARM"}; + case BASE_TRAY: + return std::string{"BASE TRAY"}; + case ARM1: + return std::string{"ARM1"}; + case ARM2: + return std::string{"ARM2"}; + case ARM_ZERO: + return std::string{"ARM_ZERO"}; + case POSITION_CUBES: + return std::string{"POSITION_CUBES"}; + case TRAY_VERTICAL: + return std::string{"TRAY_VERTICAL"}; + default: + return std::string{"Unknown system state"}; + } +} \ No newline at end of file diff --git a/FinalCode_In_Theory/src/Intake.cpp b/FinalCode_In_Theory/src/Intake.cpp index 79194f4..fdac300 100644 --- a/FinalCode_In_Theory/src/Intake.cpp +++ b/FinalCode_In_Theory/src/Intake.cpp @@ -3,7 +3,7 @@ Intake::Intake(int intakeport, int intake1port, vex::controller *controller_p) : - intake(vexDeviceGetByIndex(intakeport - 1)), + toggle_on(true), intake(vexDeviceGetByIndex(intakeport - 1)), intake1(vexDeviceGetByIndex(intake1port - 1)), controller_ptr(controller_p) { @@ -25,10 +25,15 @@ void Intake::joystickIntake(int32_t power) { moveConst(-power); } - else + else if(toggle_on) { + moveConst(6000); + } + else{ moveConst(0); } + + } void Intake::update(System_State state) @@ -37,7 +42,8 @@ void Intake::update(System_State state) { moveConst(-12000); } - else if (state == BASE || + else if (state == BASE_ARM || + state == BASE_TRAY || state == ARM1 || state == ARM2) { @@ -46,9 +52,40 @@ void Intake::update(System_State state) else if (state == POSITION_CUBES) { moveConst(-6000); + } + else if (state == BASE_ARM || state == BASE_TRAY) { + } else if(state == TRAY_VERTICAL) { joystickIntake(6000); } + + if(!JoystickButtonPressed(*controller_ptr, joystick_config::TOGGLE_BUTTON)){ + toggle_pressed = false; + } + if(!toggle_pressed && JoystickButtonPressed(*controller_ptr, joystick_config::TOGGLE_BUTTON)){ + toggle_pressed = true; + toggle_on = !toggle_on; + } +} + +void Intake::updateAuto(Auto_State state) +{ + if(state == FORWARD) + { + moveConst(12000); + } + else if(state == POSITION_CUBES_AUTO || state == POSITION_CUBES_MORE_AUTO) + { + moveConst(-9000); + } + else if(state == OUTTAKE) + { + moveConst(-6000); + } + else + { + moveConst(0); + } } \ No newline at end of file diff --git a/FinalCode_In_Theory/src/Lift.cpp b/FinalCode_In_Theory/src/Lift.cpp index d2811da..b1463a8 100644 --- a/FinalCode_In_Theory/src/Lift.cpp +++ b/FinalCode_In_Theory/src/Lift.cpp @@ -2,93 +2,172 @@ Lift::Lift(int liftmotorport, int liftmotorport1, - vex::triport::port *limitswitchport, + vex::triport::port *leftlimitswitchport, + vex::triport::port *rightimitswitchport, vex::brain *brain_p) : - liftmotor(vexDeviceGetByIndex(liftmotorport - 1)), - liftmotor1(vexDeviceGetByIndex(liftmotorport1 - 1)), - zero_switch(*limitswitchport), + lift_motor_left(vexDeviceGetByIndex(liftmotorport - 1)), + lift_motor_right(vexDeviceGetByIndex(liftmotorport1 - 1)), + zero_switch_left(*leftlimitswitchport), + zero_switch_right(*rightimitswitchport), brain_ptr(brain_p), - lift_pid(0, P, I, D, -12000, 12000, -12000, 12000), - liftmotor_base(0.0) + lift_pid_left(0, P, I, D, -12000, 12000, -12000, 12000 ), + lift_pid_right(0, P, I, D, -12000, 12000, -12000, 12000), + lift_left_base(0.0), + lift_right_base(0.0), + left_zeroed(false), + right_zeroed(false) { } void Lift::movePID() { - setMotors((int32_t) lift_pid.compute(getLiftRotation())); + lift_pid_right.setTarget(getLeftLiftRotation()); + int32_t left_val = lift_pid_left.compute(getLeftLiftRotation()); + int32_t right_val = lift_pid_right.compute(getRightLiftRotation()); + setMotors(left_val, right_val); } -void Lift::moveConst(int32_t speed ) +void Lift::moveConst(int32_t speed_left, int32_t speed_right) { - setMotors(speed); + setMotors(speed_left, speed_right); } void Lift::zeroEncoder() { - liftmotor_base = vexDeviceMotorPositionGet(liftmotor); + lift_left_base = vexDeviceMotorPositionRawGet(lift_motor_left, nullptr); + lift_right_base = vexDeviceMotorPositionRawGet(lift_motor_right, nullptr); } void Lift::movebyJoy(int32_t vel) { - setMotors(vel); + setMotors(vel, vel); brain_ptr->Screen.clearLine(0,vex::color::black); brain_ptr->Screen.clearLine(1,vex::color::black); brain_ptr->Screen.setCursor(1,0); - brain_ptr->Screen.print("Arm rotation: %f degrees", getLiftRotation()); + // brain_ptr->Screen.print("Arm rotation: %f degrees", getLiftRotation()); brain_ptr->Screen.setCursor(2,0); // Brain.Screen.print("Claw rotation: %f degrees",motor_claw.rotation(vex::rotationUnits::deg)); } -void Lift::update(System_State state) +void Lift::update(System_State state, bool go) { if(state == UNFOLD_ARM_ZERO || state == ARM_ZERO) { - moveConst(-6000); + if(!switch_val){ + switch_val = true; + left_zeroed = false; + right_zeroed = false; + } + int32_t left_val = 0; + int32_t right_val = 0; + if(!left_zeroed){ + if(getLeftLimitSwitch()){ + left_zeroed = true; + lift_left_base = vexDeviceMotorPositionRawGet(lift_motor_left, nullptr); + } + else{ + left_val = -6000; + } + } + if(!right_zeroed){ + if(getRightLimitSwitch()){ + right_zeroed = true; + lift_right_base = vexDeviceMotorPositionRawGet(lift_motor_right, nullptr); + } + else{ + right_val = -6000; + } + } + + if(left_zeroed){ + left_val = 0; + } + if(right_zeroed){ + right_val = 0; + } + + moveConst(left_val, right_val); + } + else if(state == BASE_ARM){ + if(go){ + movePID(); + } + switch_val = false; } else if (state == TRAY_ZERO || - state == BASE || + state == BASE_TRAY || state == POSITION_CUBES || state == TRAY_VERTICAL || state == ARM1 || - state == ARM2) - { + state == ARM2){ movePID(); + switch_val = false; } else if(state == UNFOLD) { - moveConst(0); + moveConst(0, 0); + switch_val = false; + } + else{ + switch_val = false; } } -uint32_t Lift::getLiftRotation() +uint32_t Lift::getLeftLiftRotation() +{ + uint32_t time; + return vexDeviceMotorPositionRawGet(lift_motor_left, &time) - lift_left_base; +} +uint32_t Lift::getRightLiftRotation() { uint32_t time; - return vexDeviceMotorPositionRawGet(liftmotor, &time) - liftmotor_base; + return -(vexDeviceMotorPositionRawGet(lift_motor_right, &time) - lift_right_base); } -bool Lift::getLimitSwitch() + +bool Lift::getLeftLimitSwitch() +{ + return zero_switch_left.value(); +} +bool Lift::getRightLimitSwitch() { - return zero_switch.value(); + return zero_switch_right.value(); +} +bool Lift::getLimitSwitch(){ + return (getRightLimitSwitch()) && (getLeftLimitSwitch()); } void Lift::setPIDBounds(int32_t min_power, int32_t max_power) { - lift_pid.setBounds(min_power, max_power); - lift_pid.setIBounds(min_power, max_power); + lift_pid_left.setBounds(min_power, max_power); + lift_pid_right.setIBounds(min_power, max_power); } -void Lift::setMotors(int32_t input) +void Lift::setMotors(int32_t input_left, int32_t input_right) { - vexDeviceMotorVoltageSet(liftmotor, input); - vexDeviceMotorVoltageSet(liftmotor1, -input); + vexDeviceMotorVoltageSet(lift_motor_left, input_left); + vexDeviceMotorVoltageSet(lift_motor_right, -input_right); } void Lift::setTargetPos(int32_t target) { - lift_pid.setTarget(target); + lift_pid_left.setTarget(target); } void Lift::stopPID() { - lift_pid.stop(); + lift_pid_left.stop(); + lift_pid_right.stop(); +} + int32_t Lift::getLeftSetPosition() + { + return lift_pid_left.getTarget(); + } + int32_t Lift::getRightSetPosition() + { + return lift_pid_right.getTarget(); + } + +bool Lift::isZeroed(){ + return left_zeroed && right_zeroed; } \ No newline at end of file diff --git a/FinalCode_In_Theory/src/Tray.cpp b/FinalCode_In_Theory/src/Tray.cpp index 5fecc3e..192fdd1 100644 --- a/FinalCode_In_Theory/src/Tray.cpp +++ b/FinalCode_In_Theory/src/Tray.cpp @@ -41,13 +41,18 @@ void Tray::zeroEncoder() traymotor_base = vexDeviceMotorPositionGet(traymotor); } -void Tray::update(System_State state) +void Tray::update(System_State state, bool go) { if(state == TRAY_ZERO) { moveConst(-6000); } - else if (state == BASE || + if(state == BASE_TRAY){ + if(go){ + movePID(); + } + } + else if (state == BASE_ARM || state == ARM1 || state == ARM2 || state == POSITION_CUBES || @@ -98,4 +103,21 @@ void Tray::setTargetPos(int32_t target) void Tray::stopPID() { tray_pid.stop(); +} + +int32_t Tray::getSetPosition(){ + return tray_pid.getTarget(); +} + +void Tray::updateAuto(Auto_State state) +{ + if(state == TRAY_VERTICAL_AUTO || + state == OUTTAKE) + { + movePID(); + } + else + { + moveConst(0); + } } \ No newline at end of file diff --git a/FinalCode_In_Theory/src/main.cpp b/FinalCode_In_Theory/src/main.cpp index 38659ca..503d961 100644 --- a/FinalCode_In_Theory/src/main.cpp +++ b/FinalCode_In_Theory/src/main.cpp @@ -19,11 +19,19 @@ #include "Lift.h" #include "Ports.h" + + void setDriveState(DriveTrain_State new_state); void setSystemState(System_State new_state); +void setAutoState(Auto_State new_state); bool init_drive_state; bool init_system_state; +bool init_auto_state; +bool startuptoggle = true; +int32_t trayjoystickposition; +int32_t liftjoystickposition; +Auto_State currentAutoState; DriveTrain_State currentDriveTrainState; System_State currentSystemState; vex::competition comp; @@ -44,6 +52,7 @@ Intake intake(Ports::INTAKE_PORT_0, Lift lift(Ports::LIFT_PORT_0, Ports::LIFT_PORT_1, &(tri.C), + &(tri.D), &Brain); Tray tray(Ports::TRAY_PORT_0, @@ -55,64 +64,194 @@ Tray tray(Ports::TRAY_PORT_0, vex::timer timer_; const AutoDriveConfig stop(1, 1, 0); - const AutoDriveConfig s1(48*DriveTrain::TICKS_PER_IN, 48*DriveTrain::TICKS_PER_IN, 30*DriveTrain::TICKS_PER_IN); + const AutoDriveConfig s1(42*DriveTrain::TICKS_PER_IN, 42*DriveTrain::TICKS_PER_IN, 30*DriveTrain::TICKS_PER_IN); const AutoDriveConfig s2(6*DriveTrain::TICKS_PER_IN, 6*DriveTrain::TICKS_PER_IN, 20*DriveTrain::TICKS_PER_IN); const AutoDriveConfig s3(11.5*DriveTrain::TICKS_PER_IN, -11.5*DriveTrain::TICKS_PER_IN, 30*DriveTrain::TICKS_PER_IN); const AutoDriveConfig s4(24*DriveTrain::TICKS_PER_IN, 24*DriveTrain::TICKS_PER_IN, 30*DriveTrain::TICKS_PER_IN); const AutoDriveConfig s5(-24*DriveTrain::TICKS_PER_IN, -24*DriveTrain::TICKS_PER_IN, 30*DriveTrain::TICKS_PER_IN); const AutoDriveConfig s6(-11.5*DriveTrain::TICKS_PER_IN, 11.5*DriveTrain::TICKS_PER_IN, 30*DriveTrain::TICKS_PER_IN); + const AutoDriveConfig s7(-42*DriveTrain::TICKS_PER_IN, -42*DriveTrain::TICKS_PER_IN, 30*DriveTrain::TICKS_PER_IN); + const AutoDriveConfig s8(4*DriveTrain::TICKS_PER_IN, 4*DriveTrain::TICKS_PER_IN, 20*DriveTrain::TICKS_PER_IN); + const AutoDriveConfig s9(-11.5*DriveTrain::TICKS_PER_IN, 11.5*DriveTrain::TICKS_PER_IN, 30*DriveTrain::TICKS_PER_IN); + const AutoDriveConfig s10(36*DriveTrain::TICKS_PER_IN, 36*DriveTrain::TICKS_PER_IN, 30*DriveTrain::TICKS_PER_IN); void autonomous(){ - - drive.getAutoDrive().initDrive(&s1, &stop, 2*DriveTrain::TICKS_PER_IN, 1*DriveTrain::TICKS_PER_IN); - while(!drive.getAutoDrive().complete()) - { - drive.getAutoDrive().update(drive.getLeftEncoder(), drive.getRightEncoder()); - drive.setPower(drive.getAutoDrive().leftPower(), drive.getAutoDrive().rightPower()); - this_thread::sleep_for(10); - } - - drive.getAutoDrive().initDrive(&s2, &stop, 2*DriveTrain::TICKS_PER_IN, 1*DriveTrain::TICKS_PER_IN); - while(!drive.getAutoDrive().complete()) - { - drive.getAutoDrive().update(drive.getLeftEncoder(), drive.getRightEncoder()); - drive.setPower(drive.getAutoDrive().leftPower(), drive.getAutoDrive().rightPower()); - this_thread::sleep_for(10); - } - - drive.getAutoDrive().initDrive(&s3, &stop, 1*DriveTrain::TICKS_PER_IN, 1*DriveTrain::TICKS_PER_IN); - while(!drive.getAutoDrive().complete()) - { - drive.getAutoDrive().update(drive.getLeftEncoder(), drive.getRightEncoder()); - drive.setPower(drive.getAutoDrive().leftPower(), drive.getAutoDrive().rightPower()); - this_thread::sleep_for(10); - } - - drive.getAutoDrive().initDrive(&s4, &stop, 2*DriveTrain::TICKS_PER_IN, 1*DriveTrain::TICKS_PER_IN); - while(!drive.getAutoDrive().complete()) - { - drive.getAutoDrive().update(drive.getLeftEncoder(), drive.getRightEncoder()); - drive.setPower(drive.getAutoDrive().leftPower(), drive.getAutoDrive().rightPower()); - this_thread::sleep_for(10); - } - - drive.getAutoDrive().initDrive(&s5, &stop, 2*DriveTrain::TICKS_PER_IN, 1*DriveTrain::TICKS_PER_IN); - while(!drive.getAutoDrive().complete()) - { - drive.getAutoDrive().update(drive.getLeftEncoder(), drive.getRightEncoder()); - drive.setPower(drive.getAutoDrive().leftPower(), drive.getAutoDrive().rightPower()); - this_thread::sleep_for(10); - } - - drive.getAutoDrive().initDrive(&s6, &stop, 1*DriveTrain::TICKS_PER_IN, 1*DriveTrain::TICKS_PER_IN); - while(!drive.getAutoDrive().complete()) - { - drive.getAutoDrive().update(drive.getLeftEncoder(), drive.getRightEncoder()); - drive.setPower(drive.getAutoDrive().leftPower(), drive.getAutoDrive().rightPower()); - this_thread::sleep_for(10); - } - + drive.setPower(6000, 6000); + this_thread::sleep_for(2000); + drive.setPower(-6000, -6000); + this_thread::sleep_for(2000); drive.setPower(0, 0); + // setAutoState(FORWARD); + + // while(true) + // { + + // switch(currentAutoState) + // { + // case(FORWARD): + // if(init_auto_state) + // { + // drive.getAutoDrive().initDrive(&s1, + // &stop, + // 2*DriveTrain::TICKS_PER_IN, + // 1*DriveTrain::TICKS_PER_IN); + // init_auto_state = false; + // } + + // if(drive.getAutoDrive().complete()) + // { + // setAutoState(BACK); + // } + + // break; + + // case(BACK): + // if(init_auto_state) + // { + // drive.getAutoDrive().initDrive(&s7, + // &stop, + // 2*DriveTrain::TICKS_PER_IN, + // 1*DriveTrain::TICKS_PER_IN); + // init_auto_state = false; + // } + + // if(drive.getAutoDrive().complete()) + // { + // setAutoState(CLEAR); + // } + + // break; + + // case(CLEAR): + // if(init_auto_state) + // { + // drive.getAutoDrive().initDrive(&s8, + // &stop, + // 1*DriveTrain::TICKS_PER_IN, + // 1*DriveTrain::TICKS_PER_IN); + // init_auto_state = false; + // } + + // if(drive.getAutoDrive().complete()) + // { + // setAutoState(TURN_THREE); + // } + + // break; + + // case(TURN_THREE): + // if(init_auto_state) + // { + // drive.getAutoDrive().initDrive(&s9, + // &stop, + // 1*DriveTrain::TICKS_PER_IN, + // 1*DriveTrain::TICKS_PER_IN); + // init_auto_state = false; + // } + + // if(drive.getAutoDrive().complete()) + // { + // setAutoState(TO_STACK); + // } + + // break; + + // case(TO_STACK): + // if(init_auto_state) + // { + // drive.getAutoDrive().initDrive(&s10, + // &stop, + // 2*DriveTrain::TICKS_PER_IN, + // 1*DriveTrain::TICKS_PER_IN); + // init_auto_state = false; + // } + + // if(drive.getAutoDrive().complete()) + // { + // setAutoState(POSITION_CUBES_AUTO); + // } + + // break; + + // case(POSITION_CUBES_AUTO): + // if(init_auto_state) + // { + + // init_auto_state = false; + // } + + // if(tray.getCubeSwitch()) + // { + // setAutoState(POSITION_CUBES_MORE_AUTO); + // } + + // break; + + // case(POSITION_CUBES_MORE_AUTO): + // if(init_auto_state) + // { + // timer_.clear(); + // init_auto_state = false; + // } + + // if(timer_.time() > 50) + // { + // setAutoState(TRAY_VERTICAL_AUTO); + // } + + // break; + + // case(TRAY_VERTICAL_AUTO): + // if(init_auto_state) + // { + // tray.zeroEncoder(); + // tray.setPIDBounds(-6000, 6000); + // tray.setTargetPos(Ports::TRAY_VERTICAL_POSITION); + // timer_.clear(); + // init_auto_state = false; + // } + + // if(timer_.time() > 5000) + // { + // setAutoState(OUTTAKE); + // } + + // break; + + // case(OUTTAKE): + // if(init_auto_state) + // { + // tray.setPIDBounds(-6000, 6000); + // tray.setTargetPos(Ports::TRAY_VERTICAL_POSITION); + // timer_.clear(); + // init_auto_state = false; + // } + + // if(timer_.time() > 5000) + // { + // setAutoState(STOP_AUTO); + // } + + // break; + + // case(STOP_AUTO): + // if(init_auto_state) + // { + // tray.setPIDBounds(-6000, 6000); + // tray.setTargetPos(Ports::TRAY_BASE_POSITION); + // timer_.clear(); + // init_auto_state = false; + // } + + // break; + // } + + // drive.updateAuto(currentAutoState); + // intake.updateAuto(currentAutoState); + // tray.updateAuto(currentAutoState); + + // this_thread::sleep_for(10); + // } } void teleop(){ @@ -120,6 +259,8 @@ void teleop(){ setSystemState(UNFOLD); timer_.clear(); + bool lift_go = true; + bool tray_go = true; while(true) { @@ -128,8 +269,8 @@ void teleop(){ drive.update(currentDriveTrainState); //drive.updateSystemState(currentSystemState); intake.update(currentSystemState); - lift.update(currentSystemState); - tray.update(currentSystemState); + lift.update(currentSystemState, lift_go); + tray.update(currentSystemState, tray_go); switch(currentDriveTrainState) { @@ -170,11 +311,15 @@ void teleop(){ init_system_state = false; } - if(lift.getLimitSwitch()) + if(lift.isZeroed()) { + setSystemState(TRAY_ZERO); + } + if(JoystickButtonPressed(cont, joystick_config::INTAKE_BUTTON)){ lift.zeroEncoder(); setSystemState(TRAY_ZERO); } + break; case ARM_ZERO: @@ -186,11 +331,15 @@ void teleop(){ init_system_state = false; } - if(lift.getLimitSwitch()) + if(lift.isZeroed()) { + setSystemState(TRAY_ZERO); + } + if(JoystickButtonPressed(cont, joystick_config::INTAKE_BUTTON)){ lift.zeroEncoder(); - setSystemState(BASE); + setSystemState(TRAY_ZERO); } + break; case TRAY_ZERO: @@ -205,13 +354,19 @@ void teleop(){ if(tray.getLimitSwitch()) { tray.zeroEncoder(); - setSystemState(BASE); + setSystemState(BASE_TRAY); + } + if(JoystickButtonPressed(cont, joystick_config::INTAKE_BUTTON)) + { + tray.zeroEncoder(); + setSystemState(BASE_TRAY); } break; - case BASE: + case BASE_TRAY: if(init_system_state) { + trayjoystickposition=0; lift.setPIDBounds(-8000, 8000); lift.setTargetPos(Ports::LIFT_BASE_POSITION); tray.setPIDBounds(-6000, 6000); @@ -222,21 +377,118 @@ void teleop(){ if (JoystickButtonPressed(cont, joystick_config::ARM1_BUTTON)) { setSystemState(ARM1); + trayjoystickposition=0; } else if (JoystickButtonPressed(cont, joystick_config::ARM2_BUTTON)) { setSystemState(ARM2); + trayjoystickposition=0; } else if (JoystickButtonPressed(cont, joystick_config::VERTICAL_BUTTON)) { setSystemState(POSITION_CUBES); + trayjoystickposition=0; } else if (JoystickButtonPressed(cont, joystick_config::UNFOLD_BUTTON)) { setSystemState(UNFOLD); + trayjoystickposition=0; } + else if (JoystickButtonPressed(cont, joystick_config::UP_BUTTON)){ + tray_go = false; + tray.moveConst(12000); + } + else if (JoystickButtonPressed(cont, joystick_config::DOWN_BUTTON)){ + tray_go = false; + tray.moveConst(-12000); + } + else{ + if(!tray_go){ + tray_go = true; + tray.setTargetPos(tray.getTrayRotation()); + } + } + + + if (JoystickButtonPressed(cont, joystick_config::BASE_BUTTON)) + { + trayjoystickposition=0; + setSystemState(TRAY_ZERO); + } + else if (JoystickButtonPressed(cont, joystick_config::ARM_FLIP_BUTTON)) + { + setSystemState(BASE_ARM); + liftjoystickposition=0; + } + + break; + case BASE_ARM: + if(init_system_state) + { + liftjoystickposition=0; + lift.setPIDBounds(-8000, 8000); + lift.setTargetPos(Ports::LIFT_BASE_POSITION); + tray.setPIDBounds(-6000, 6000); + tray.setTargetPos(Ports::TRAY_BASE_POSITION); + init_system_state = false; + } + + if (JoystickButtonPressed(cont, joystick_config::ARM1_BUTTON)) + { + setSystemState(ARM1); + liftjoystickposition=0; + } + else if (JoystickButtonPressed(cont, joystick_config::ARM2_BUTTON)) + { + setSystemState(ARM2); + liftjoystickposition=0; + } + else if (JoystickButtonPressed(cont, joystick_config::VERTICAL_BUTTON)) + { + setSystemState(POSITION_CUBES); + liftjoystickposition=0; + } + else if (JoystickButtonPressed(cont, joystick_config::UNFOLD_BUTTON)) + { + setSystemState(UNFOLD); + liftjoystickposition=0; + } + else if (JoystickButtonPressed(cont, joystick_config::TRAY_FLIP_BUTTON)) + { + setSystemState(BASE_TRAY); + liftjoystickposition=0; + } + + else if (JoystickButtonPressed(cont, joystick_config::UP_BUTTON)){ + lift_go = false; + int32_t speed = 12000; + lift.moveConst(speed, speed); + } + else if (JoystickButtonPressed(cont, joystick_config::DOWN_BUTTON)){ + lift_go = false; + int32_t speed = -12000; + lift.moveConst(speed, speed); + } + else{ + if(!lift_go){ + lift_go = true; + lift.setTargetPos(lift.getLeftLiftRotation()); + } + } + + + if (JoystickButtonPressed(cont, joystick_config::BASE_BUTTON)) + { + setSystemState(ARM_ZERO); + liftjoystickposition=0; + trayjoystickposition=0; + } + + + break; + case ARM1: if(init_system_state) @@ -287,7 +539,7 @@ void teleop(){ lift.setPIDBounds(-8000, 8000); lift.setTargetPos(Ports::LIFT_BASE_POSITION); tray.setPIDBounds(-6000, 6000); - tray.setTargetPos(Ports::TRAY_BASE_POSITION); + // tray.setTargetPos(Ports::TRAY_BASE_POSITION); timer_.clear(); init_system_state = false; } @@ -327,11 +579,27 @@ void teleop(){ break; } - Brain.Screen.printAt(10, 20, true, "Tray switch: %d", tray.getLimitSwitch()); - Brain.Screen.printAt(10, 40, true, "Cube switch: %d", tray.getCubeSwitch()); - Brain.Screen.printAt(10, 60, true, "Lift switch: %d", lift.getLimitSwitch()); - Brain.Screen.printAt(10, 80, true, "Tray pos: %d", tray.getTrayRotation()); - Brain.Screen.printAt(10, 100, true, "Lift pos: %d", lift.getLiftRotation()); + Brain.Screen.printAt(10, 20, true, "Tray setPosition: %d", tray.getSetPosition()); + Brain.Screen.printAt(10, 40, true, "Lift Set Position: %d, %d", lift.getLeftSetPosition(), lift.getRightSetPosition()); + Brain.Screen.printAt(10, 60, true, "Left Motor: %f, %f", vexDeviceMotorTemperatureGet(drive.topleftMotor), vexDeviceMotorTemperatureGet(drive.bottomleftMotor)); + Brain.Screen.printAt(10, 80, true, "Right Motor: %f, %f", vexDeviceMotorTemperatureGet(drive.toprightMotor), vexDeviceMotorTemperatureGet(drive.bottomrightMotor)); + Brain.Screen.printAt(10, 100, true, "Lift pos: %d, %d", lift.getLeftLiftRotation(), lift.getRightLiftRotation()); + Brain.Screen.printAt(10, 120, true, "SystemState: %s", system_state_to_string(currentSystemState).c_str()); + Brain.Screen.printAt(10, 140, true, "DriveState: %s", drive_train_state_to_string(currentDriveTrainState).c_str()); + Brain.Screen.printAt(10, 160, true, "Limit %d", lift.getLimitSwitch()); + Brain.Screen.printAt(10, 180, true, "Limit Left %d, %d", lift.getLeftLimitSwitch(), lift.getRightLimitSwitch()); + Brain.Screen.printAt(10, 200, true, "Base val: %.2f, %.2f", lift.lift_left_base, lift.lift_right_base); + Brain.Screen.printAt(10, 220, true, "Actual Lift: %d, %d", vexDeviceMotorPositionRawGet(lift.lift_motor_left, nullptr), vexDeviceMotorPositionRawGet(lift.lift_motor_right, nullptr)); + + /* cont.Screen.clearScreen(); + cont.Screen.setCursor(0, 0); + cont.Screen.print("L %.0f %.0f", vexDeviceMotorTemperatureGet(drive.topleftMotor), vexDeviceMotorTemperatureGet(drive.bottomleftMotor)); + cont.Screen.newLine(); + cont.Screen.print("R %.0f %.0f", vexDeviceMotorTemperatureGet(drive.toprightMotor), vexDeviceMotorTemperatureGet(drive.bottomrightMotor)); + cont.Screen.newLine(); + cont.Screen.print("T %d L %d", tray.getSetPosition(), lift.getSetPosition());*/ + + // Brain.Screen.printAt(10, 120, true) Brain.Screen.render(); @@ -361,4 +629,8 @@ void setSystemState(System_State new_state) init_system_state = true; } - +void setAutoState(Auto_State new_state) +{ + currentAutoState = new_state; + init_auto_state = true; +}