Unify CAN Motor implementation (#317)

* RM unused CANMotor_AVR.cpp

* Merge CANMotor_PSOC into CANMotor

* Fixed function move

* Removed double function

* Removed unused CANMotor includes

src/CAN/CANMotor.cpp

@@ -19,6 +19,39 @@ using robot::types::LimitSwitchData;
 
 namespace can::motor {
 
+void initEncoder(deviceserial_t serial, bool invertEncoder, bool zeroEncoder,
+				 int32_t pulsesPerJointRev,
+				 std::optional<std::chrono::milliseconds> telemetryPeriod) {
+	auto motorGroupCode = static_cast<uint8_t>(devicegroup_t::motor);
+	CANPacket p;
+	AssembleEncoderInitializePacket(&p, motorGroupCode, serial, sensor_t::encoder,
+									invertEncoder, zeroEncoder);
+	sendCANPacket(p);
+	std::this_thread::sleep_for(1000us);
+	AssembleEncoderPPJRSetPacket(&p, motorGroupCode, serial, pulsesPerJointRev);
+	sendCANPacket(p);
+	std::this_thread::sleep_for(1000us);
+	if (telemetryPeriod) {
+		scheduleTelemetryPull(std::make_pair(devicegroup_t::motor, serial), telemtype_t::angle,
+							  telemetryPeriod.value());
+	}
+}
+
+void initPotentiometer(deviceserial_t serial, int32_t posLo, int32_t posHi, uint16_t adcLo,
+					   uint16_t adcHi,
+					   std::optional<std::chrono::milliseconds> telemetryPeriod) {
+	CANPacket packet;
+	auto group = static_cast<uint8_t>(devicegroup_t::motor);
+	AssemblePotHiSetPacket(&packet, group, serial, adcHi, posHi);
+	sendCANPacket(packet);
+	AssemblePotLoSetPacket(&packet, group, serial, adcLo, posLo);
+	sendCANPacket(packet);
+	if (telemetryPeriod) {
+		scheduleTelemetryPull(std::make_pair(devicegroup_t::motor, serial), telemtype_t::angle,
+							  telemetryPeriod.value());
+	}
+}
+
 void emergencyStopMotors() {
 	CANPacket p;
 	AssembleGroupBroadcastingEmergencyStopPacket(&p, static_cast<uint8_t>(0x0),

src/CMakeLists.txt

@@ -167,7 +167,6 @@ target_link_libraries(utils ${OpenCV_LIBS})
 # No clue why but CAN I/O goes to shit. Don't do it.
 add_library(can_interface STATIC)
 target_sources(can_interface PRIVATE
-  CAN/CANMotor_PSOC.cpp
   CAN/CANMotor.cpp
   CAN/CANUtils.cpp
 )