middle step for #49 and #54

Makefile

@@ -27,7 +27,7 @@ EXCLUDE_COLD_LIBRARIES:=
 IS_LIBRARY:=1
 # TODO: CHANGE THIS!
 LIBNAME:=EZ-Template
-VERSION:=2.1.2-RC1
+VERSION:=2.2.0-RC1
 # EXCLUDE_SRC_FROM_LIB= $(SRCDIR)/unpublishedfile.c
 # this line excludes opcontrol.c and similar files
 EXCLUDE_SRC_FROM_LIB+=$(foreach file, $(SRCDIR)/autons $(SRCDIR)/main,$(foreach cext,$(CEXTS),$(file).$(cext)) $(foreach cxxext,$(CXXEXTS),$(file).$(cxxext)))

include/EZ-Template/api.hpp

@@ -10,5 +10,7 @@ file, You can obtain one at http://mozilla.org/MPL/2.0/.
 #include "EZ-Template/auton.hpp"
 #include "EZ-Template/auton_selector.hpp"
 #include "EZ-Template/drive/drive.hpp"
+//#include "EZ-Template/piston.hpp"
+//#include "EZ-Template/piston_group.hpp"
 #include "EZ-Template/sdcard.hpp"
 #include "EZ-Template/util.hpp"

include/EZ-Template/drive/drive.hpp

@@ -699,6 +699,8 @@ class Drive {
    */
   double slew_calculate(slew_ &input, double current);
 
+  bool using_inches = false;
+
  private:  // !Auton
   bool drive_toggle = true;
   bool print_toggle = true;

include/EZ-Template/piston.hpp

@@ -0,0 +1,24 @@
+/*
+This Source Code Form is subject to the terms of the Mozilla Public
+License, v. 2.0. If a copy of the MPL was not distributed with this
+file, You can obtain one at http://mozilla.org/MPL/2.0/.
+*/
+
+#pragma once
+
+#include "api.h"
+
+class Piston {
+ public:
+  pros::ADIDigitalOut piston;
+  Piston(int input_port, bool default_state = false);
+  void set(bool input);
+  bool get();
+  void button_toggle(int toggle);
+  void button(int active, int deactive);
+
+ private:
+  bool reversed = false;
+  bool current = false;
+  int last_press = 0;
+};

include/EZ-Template/piston_group.hpp

@@ -0,0 +1,19 @@
+/*
+This Source Code Form is subject to the terms of the Mozilla Public
+License, v. 2.0. If a copy of the MPL was not distributed with this
+file, You can obtain one at http://mozilla.org/MPL/2.0/.
+*/
+
+#pragma once
+
+#include "EZ-Template/piston.hpp"
+
+class PistonGroup {
+ public:
+  std::vector<Piston> pistons;
+  PistonGroup(std::vector<int> input_ports, bool default_state = false);
+  void set(bool input);
+  bool get();
+  void button_toggle(int toggle);
+  void button(int active, int deactive);
+};

include/autons.hpp

@@ -13,5 +13,6 @@ void combining_movements();
 void interfered_example();
 
 void default_constants();
+void using_inches_constants();
 void one_mogo_constants();
 void two_mogo_constants();

src/EZ-Template/drive/pid_tasks.cpp

@@ -31,8 +31,13 @@ void Drive::ez_auto_task() {
 // Drive PID task
 void Drive::drive_pid_task() {
   // Compute PID
-  leftPID.compute(left_sensor());
-  rightPID.compute(right_sensor());
+  if (!using_inches) {
+    leftPID.compute(left_sensor());
+    rightPID.compute(right_sensor());
+  } else {
+    leftPID.compute(left_sensor() / TICK_PER_INCH);
+    rightPID.compute(right_sensor() / TICK_PER_INCH);
+  }
   headingPID.compute(get_gyro());
 
   // Compute slew

src/EZ-Template/drive/set_pid.cpp

@@ -56,8 +56,15 @@ void Drive::set_drive_pid(double target, int speed, bool slew_on, bool toggle_he
   double l_target_encoder, r_target_encoder;
 
   // Figure actual target value
-  l_target_encoder = l_start + (target * TICK_PER_INCH);
-  r_target_encoder = r_start + (target * TICK_PER_INCH);
+  if (!using_inches) {
+    l_target_encoder = l_start + (target * TICK_PER_INCH);
+    r_target_encoder = r_start + (target * TICK_PER_INCH);
+  } else {
+    l_target_encoder = (l_start / TICK_PER_INCH) + target;
+    r_target_encoder = (r_start / TICK_PER_INCH) + target;
+  }
+
+  printf("\nltar %f  rtar %f", l_target_encoder, r_target_encoder);
 
   // Figure out if going forward or backward
   if (l_target_encoder < l_start && r_target_encoder < r_start) {

src/EZ-Template/piston.cpp

@@ -0,0 +1,38 @@
+/*
+This Source Code Form is subject to the terms of the Mozilla Public
+License, v. 2.0. If a copy of the MPL was not distributed with this
+file, You can obtain one at http://mozilla.org/MPL/2.0/.
+*/
+
+#include "EZ-Template/piston.hpp"
+
+// Constructor for one piston
+Piston::Piston(int input_port, bool default_state)
+    : piston(input_port, default_state) {
+  reversed = default_state;
+}
+
+// Set piston
+void Piston::set(bool input) {
+  piston.set_value(reversed ? !input : input);
+  current = input;
+}
+
+// Get the current state
+bool Piston::get() { return current; }
+
+// Toggle for user control
+void Piston::button_toggle(int toggle) {
+  if (toggle && !last_press) {
+    set(!get());
+  }
+  last_press = toggle;
+}
+
+// Two button control for piston
+void Piston::button(int active, int deactive) {
+  if (active && !get())
+    set(true);
+  else if (deactive && get())
+    set(false);
+}

src/EZ-Template/piston_group.cpp

@@ -0,0 +1,38 @@
+/*
+This Source Code Form is subject to the terms of the Mozilla Public
+License, v. 2.0. If a copy of the MPL was not distributed with this
+file, You can obtain one at http://mozilla.org/MPL/2.0/.
+*/
+
+#include "EZ-Template/piston_group.hpp"
+
+// Constructor for multiple pistons
+PistonGroup::PistonGroup(std::vector<int> input_ports, bool default_state) {
+  for (auto i : input_ports) {
+    Piston temp(i, default_state);
+    pistons.push_back(temp);
+  }
+}
+
+void PistonGroup::set(bool input) {
+  for (auto i : pistons) {
+    i.set(input);
+  }
+}
+
+// Get the current state
+bool PistonGroup::get() { return pistons[0].get(); }
+
+// Toggle for user control
+void PistonGroup::button_toggle(int toggle) {
+  for (auto i : pistons) {
+    i.button_toggle(toggle);
+  }
+}
+
+// Two button control for piston
+void PistonGroup::button(int active, int deactive) {
+  for (auto i : pistons) {
+    i.button(active, deactive);
+  }
+}

src/autons.cpp

@@ -24,6 +24,10 @@ const int SWING_SPEED = 90;
 // If the objects are light or the cog doesn't change much, then there isn't a concern here.
 
 void default_constants() {
+  chassis.set_exit_condition(chassis.turn_exit, 100, 3, 500, 7, 500, 500);
+  chassis.set_exit_condition(chassis.swing_exit, 100, 3, 500, 7, 500, 500);
+  chassis.set_exit_condition(chassis.drive_exit, 80, 50, 300, 150, 500, 500);
+
   chassis.set_slew_min_power(80, 80);
   chassis.set_slew_distance(7, 7);
   chassis.set_pid_constants(&chassis.headingPID, 11, 0, 20, 0);
@@ -33,6 +37,20 @@ void default_constants() {
   chassis.set_pid_constants(&chassis.swingPID, 7, 0, 45, 0);
 }
 
+void using_inches_constants() {
+  chassis.set_exit_condition(chassis.turn_exit, 100, 3, 500, 7, 500, 500);
+  chassis.set_exit_condition(chassis.swing_exit, 100, 3, 500, 7, 500, 500);
+  chassis.set_exit_condition(chassis.drive_exit, 80, 1, 300, 3, 500, 500);
+
+  chassis.set_slew_min_power(80, 80);
+  chassis.set_slew_distance(7, 7);
+  chassis.set_pid_constants(&chassis.headingPID, 11, 0, 20, 0);
+  chassis.set_pid_constants(&chassis.forward_drivePID, 22.5, 0, 250, 0);
+  chassis.set_pid_constants(&chassis.backward_drivePID, 22.5, 0, 250, 0);
+  chassis.set_pid_constants(&chassis.turnPID, 5, 0.003, 35, 15);
+  chassis.set_pid_constants(&chassis.swingPID, 7, 0, 45, 0);
+}
+
 void one_mogo_constants() {
   chassis.set_slew_min_power(80, 80);
   chassis.set_slew_distance(7, 7);
@@ -57,7 +75,7 @@ void two_mogo_constants() {
 void modified_exit_condition() {
   chassis.set_exit_condition(chassis.turn_exit, 100, 3, 500, 7, 500, 500);
   chassis.set_exit_condition(chassis.swing_exit, 100, 3, 500, 7, 500, 500);
-  chassis.set_exit_condition(chassis.drive_exit, 80, 50, 300, 150, 500, 500);
+  chassis.set_exit_condition(chassis.drive_exit, 80, 1, 300, 3, 500, 500);
 }
 
 

src/main.cpp

@@ -5,39 +5,36 @@
 Drive chassis (
   // Left Chassis Ports (negative port will reverse it!)
   //   the first port is the sensored port (when trackers are not used!)
-  {-15, -16}
+  {-7, -8, 9}
 
   // Right Chassis Ports (negative port will reverse it!)
   //   the first port is the sensored port (when trackers are not used!)
-  ,{6, 5}
+  ,{4, 3, -2}
 
   // IMU Port
-  ,20
+  ,11
 
   // Wheel Diameter (Remember, 4" wheels are actually 4.125!)
   //    (or tracking wheel diameter)
-  ,2.5
+  ,4.125
 
   // Cartridge RPM
   //   (or tick per rotation if using tracking wheels)
-  ,1200
+  ,600
+
 
   // External Gear Ratio (MUST BE DECIMAL)
   //    (or gear ratio of tracking wheel)
   // eg. if your drive is 84:36 where the 36t is powered, your RATIO would be 2.333.
   // eg. if your drive is 36:60 where the 60t is powered, your RATIO would be 0.6.
-  ,2
-
+  ,1.875
 
   // Uncomment if using tracking wheels
   /*
   // Left Tracking Wheel Ports (negative port will reverse it!)
-  // ,{1, 2} // 3 wire encoder
-  // ,8 // Rotation sensor
-
+  ,{1, 2}
   // Right Tracking Wheel Ports (negative port will reverse it!)
-  // ,{-3, -4} // 3 wire encoder
-  // ,-9 // Rotation sensor
+  ,{3, 4}
   */
 
   // Uncomment if tracking wheels are plugged into a 3 wire expander