Branch-5

53C_New Code/include/globals.hpp

@@ -1,67 +1,76 @@
 #include "lemlib/chassis/chassis.hpp"
 #include "lemlib/api.hpp"
+#include "pros/adi.h"
+#include "pros/adi.hpp"
+#include "pros/rotation.hpp"
+#include <new>
 
 //controller
 inline pros::Controller controller(pros::E_CONTROLLER_MASTER);
 
 inline pros::Controller master(pros::E_CONTROLLER_MASTER);
-inline pros::Motor bottomMotor(2, pros::MotorGearset::green); 
-inline pros::Motor topMotor(15, pros::MotorGearset::green);
-inline pros::Motor hopperMotor(4, pros::MotorGearset::green);
+inline pros::Motor bottomMotor(1, pros::MotorGearset::green); 
+inline pros::Motor topMotor(2, pros::MotorGearset::green);
 
-/*** Drivetain ***/
-inline pros::Imu imu(1);
+/*** Drivetrain ***/
+inline pros::Imu imu(3);
 
-inline pros::MotorGroup leftMotors({-11, -12, 13},
+inline pros::MotorGroup leftMotors({-11, -12, -13},
                                   pros::MotorGearset::blue);
 // Creates a motor group for the left side of the drive train
 
-inline pros::MotorGroup rightMotors({18, 19, -20},
+inline pros::MotorGroup rightMotors({18, 19, 20},
                                     pros::MotorGearset::blue);
 // Creates a motor group for the right side of the drive train
 
 inline lemlib::Drivetrain drivetrain(&leftMotors, // left motor group
                               &rightMotors, // right motor group
                               11.5, // track width
-                              lemlib::Omniwheel::NEW_325, // wheel size
+                              lemlib::Omniwheel::NEW_275, // wheel size
                               450, // drivetrain rpm
-                              2 // horizontal drift
+                              8 // horizontal drift
 );
 
 
 /* * * Sensors * * */
 
-inline lemlib::OdomSensors sensors(nullptr, // vertical tracking wheel 1
+inline pros::Rotation verticalRotation(-17); // port, reversed
+inline lemlib::TrackingWheel vertical_tracking_wheel(&verticalRotation, lemlib::Omniwheel::NEW_275, 0.5);
+
+inline lemlib::OdomSensors sensors(&vertical_tracking_wheel, // vertical tracking wheel 1
                             nullptr, // vertical tracking wheel 2
                             nullptr, // horizontal tracking wheel 1
                             nullptr, // horizontal tracking wheel 2
                             &imu // inertial sensor
 );
 
+
+
 /* * * Motion Controllers * * */
-inline lemlib::ControllerSettings lateral_controller(2, // proportional gain (kP)
+inline lemlib::ControllerSettings lateral_controller(
+                                              10, // proportional gain (kP)
                                               0, // integral gain (kI)
-                                              10, // derivative gain (kD)
+                                              35, // derivative gain (kD)
                                               3, // anti windup
                                               1, // small error range, in inches
                                               100, // small error range timeout, in milliseconds
                                               3, // large error range, in inches
                                               500, // large error range timeout, in milliseconds
-                                              0 // maximum acceleration (slew)
+                                              20 // maximum acceleration (slew)
 );
 
-inline lemlib::ControllerSettings angular_controller(3, // proportional gain (kP)
-                                              0, // integral gain (kI)
-                                              18, // derivative gain (kD)
-                                              0, // anti windup
-                                              0, // small error range, in inches
-                                              0, // small error range timeout, in milliseconds
-                                              0, // large error range, in inches
-                                              0, // large error range timeout, in milliseconds
+inline lemlib::ControllerSettings angular_controller(
+                                              4, // proportional gain (kP) 
+                                              -0.1, // integral gain (kI) 
+                                              38, // derivative gain (kD)
+                                              4.0867, // anti windup 
+                                              1, // small error range, in inches
+                                              100, // small error range timeout, in milliseconds
+                                              3, // large error range, in inches
+                                              500, // large error range timeout, in milliseconds
                                               0 // maximum acceleration (slew)
 );
 
-
 /* * * Drive Curve * * */
 // input curve for throttle input during driver control
 
@@ -86,7 +95,7 @@ inline lemlib::Chassis chassis(drivetrain, // drivetrain settings
                         //&steer_curve
 ); 
 
-
-
-
-// inline pros::Optical myOptical(3);
+// Set these to the correct ports for the pistons 
+inline pros::adi::Pneumatics pistonE('E', false); /* ~ Middle Outtake Piston ~ */
+inline pros::adi::Pneumatics pistonD('D', false); /* ~ Scapper Piston ~ */
+inline pros::adi::Pneumatics pistonA('A', false); /* ~ Descoring Mech Piston ~ */

53C_New Code/include/variable.h

@@ -9,13 +9,12 @@
 extern pros::Controller controller;
 extern pros::Motor bottomMotor;
 extern pros::Motor topMotor;
-extern pros::Motor hopperMotor;
 extern bool tank;
 extern int leftX;
 extern int leftY;
 extern int rightX;
 extern int rightY;
 extern lemlib::Chassis chassis;
-
-
-
+extern pros::adi::DigitalOut PistonE;
+extern pros::adi::DigitalOut PistonD;
+extern pros::adi::DigitalOut PistonA;