Big boi

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}

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,18 +38,15 @@ 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;
 
   static constexpr double POS_P = 8;
   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;
 };

FinalCode_In_Theory/include/Enums.h

@@ -1,6 +1,10 @@
 #pragma once
 
+#include <string>
 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};
+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);

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;

FinalCode_In_Theory/include/Lift.h

@@ -11,41 +11,56 @@ 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);
 
   void setTargetPos(int32_t target);
 
   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<double> lift_pid;
-
-  double liftmotor_base;
-
-  static constexpr double P = 100;
-  static constexpr double I = 10;
-  static constexpr double D = 0;
+  PID<double> lift_pid_left;
+  PID<double> 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);
 };

FinalCode_In_Theory/include/PID.h

@@ -104,6 +104,9 @@ class PID
     initialized = false;
     time.clear();
   }
+  T getTarget(){
+    return target;
+  }
 
 private:
   T p_term;

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;
+
 }

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);
 };

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;

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);
   }
 }
 

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"};
+  }
+}

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);
+  }
 }