add ability to reverse encoders

PYRORobotics · Feb 2, 2023 · 23a035d · 23a035d
1 parent 3c2098f
commit 23a035d
Show file tree

Hide file tree

Showing 3 changed files with 6 additions and 7 deletions.
diff --git a/include/AMT21.h b/include/AMT21.h
@@ -18,7 +18,7 @@
 
 class AMT21 {
 public:
-    AMT21(int port, uint8_t address);
+    AMT21(int port, uint8_t address, bool reversed = false);
 
     uint16_t get_position();
     uint16_t get_position_safe();
@@ -35,6 +35,7 @@ class AMT21 {
     pros::Serial serial_port;
     long offset = 0;
     long last_value = 0;
+    bool reversed;
 
 };
 

diff --git a/src/AMT21.cpp b/src/AMT21.cpp
@@ -6,7 +6,7 @@
 
 #define DELAY_TIME 500
 
-AMT21::AMT21(int port, uint8_t address) : serial_port(port), address(address){
+AMT21::AMT21(int port, uint8_t address, bool reversed) : serial_port(port), address(address), reversed(reversed){
     //default baud for our model is 115200
     serial_port.set_baudrate(115200);
 
@@ -93,7 +93,7 @@ long AMT21::get_value(){
     }
     last_value = (turns * 16384) + position + offset;
 
-    return last_value;
+    return last_value * (reversed ? -1 : 1);
 }
 
 void AMT21::reset(){

diff --git a/src/main.cpp b/src/main.cpp
@@ -176,8 +176,6 @@ void opcontrol() {
 	int progress = 0;
 	RobotControl robot1;
 	//AMT21 amt21_left(19, 0x58);
-    AMT21 amt21_right(19, 0x5C);
-    AMT21 amt21_middle(19, 0x54);
 	imu1.reset();
 	while(imu1.is_calibrating()==true) {
 		pros::delay(20);
@@ -190,8 +188,8 @@ void opcontrol() {
 	pros::Motor intake ();
 	while (true) {
 		//pros::lcd::print(0, "AMT21_left value: %d", amt21_left.get_value());
-        pros::lcd::print(2, "AMT21_right value: %d", amt21_right.get_value());
-        pros::lcd::print(4, "AMT21_middle value: %d", amt21_middle.get_value());
+        pros::lcd::print(2, "AMT21_right value: %d", up.get_value());
+        pros::lcd::print(4, "AMT21_middle value: %d", sideways.get_value());
         //pros::lcd::print(6, "Response time: %ld micros", pros::micros() - start);
         //pros::lcd::print(7, "heading: %f", navx.getHeading());
 		odom1.updateOdom();