Merge branch 'CAN-updates' of github.com:huskyroboticsteam/Resurgence into CAN-updates

Author: imisaacwu

src/CAN/CANBoard.cpp

@@ -162,19 +162,23 @@ void CANBoard::setMotorVel(int8_t velocity) {
     sendCANPacket(p);
 }
 
+void CANBoard::setStepperRevs(float revs) {
+    if (!this->device.motorDomain) {
+        LOG_F(WARNING, "setMotorPower called for board not in motor domain!");
+        return;
+    }
+
+    CANPacket_t p = CANMotorPacket_Stepper_DriveRevolutions(
+        Constants::JETSON_DEVICE, this->device, revs
+    );
+    sendCANPacket(p);
+}
+
 void CANBoard::read(uint16_t endpoint) {
     CANPacket_t p = CANMotorPacket_BLDC_DirectRead(
         Constants::JETSON_DEVICE, this->device, endpoint
     );
     sendCANPacket(p);
 }
 
-/*
-void setStepper(float revs) {
-    CANMotorPacket_Stepper_DriveRevolutions()
-    sendCanPacket
-}
-
-*/
-
 } // namespace can

src/CAN/CANBoard.h

@@ -16,9 +16,7 @@ class CANBoard {
     void setMotorPower(double power);
     void setMotorState(can::motor::axis_state_t state);
     void setMotorVel(int8_t velocity);
-
-    // Motor/Peripheral
-    void setStepper(float revs);
+    void setStepperRevs(float revs);
 
     // Universal
     void read(uint16_t endpoint);

src/control_interface.cpp

@@ -189,8 +189,7 @@ void setJointMotorPower(robot::types::jointid_t joint, double power) {
 		setMotorPower(boardid_t::wristDiffLeft, gearPwr.left);
 		setMotorPower(boardid_t::wristDiffRight, gearPwr.right);
 	} else if (joint == jointid_t::hand) {
-		// we need a stepper power packet in real_world_interface (by extension in world_interface and simuator_interface)
-		// that we can call here with boardid_t::hand and float revs
+		setStepperRevs(boardid_t::hand, static_cast<float>(power));
 	} else {
 		LOG_F(WARNING, "setJointPower called for currently unsupported joint %s",
 			  util::to_string(joint).c_str());

src/control_interface.h

@@ -50,5 +50,4 @@ void setJointPos(types::jointid_t joint, int32_t targetPos);
  */
 types::DataPoint<int32_t> getJointPos(types::jointid_t joint);
 
-void setScienceServoPos(uint8_t servoNum, int32_t angle);
 } // namespace robot

src/world_interface/real_world_interface.cpp

@@ -302,6 +302,13 @@ void handleMotorEncoderEstimate(robot::types::boardid_t board, int32_t positionM
 	}
 }
 
+void setStepperRevs(robot::types::boardid_t board, float revs) {
+	std::shared_ptr<can::CANBoard> board_ptr = getBoard_(board);
+	if (board_ptr) {
+		board_ptr->setStepperRevs(revs);
+	}
+}
+
 /*
 
 void setStepper(boardid_t board, float revs) {

src/world_interface/simulator_interface.cpp

@@ -476,9 +476,9 @@ void setIndicator(indication_t signal) {
 	}
 }
 
-void handleMotorEncoderEstimate(robot::types::boardid_t board, int32_t positionMdeg) {
+void handleMotorEncoderEstimate(robot::types::boardid_t board, int32_t positionMdeg) {}
 
-}
+void setStepperRevs(robot::types::boardid_t board, float revs) {}
 
 } // namespace robot
 

src/world_interface/world_interface.h

@@ -229,6 +229,10 @@ types::DataPoint<int32_t> getMotorPos(robot::types::boardid_t motor);
 
 void handleMotorEncoderEstimate(robot::types::boardid_t board, int32_t positionMdeg);
 
+void setStepperRevs(robot::types::boardid_t board, float revs);
+
+
+
 using callbackid_t = unsigned long long;
 
 callbackid_t addLimitSwitchCallback(