diff --git a/library/stm32f072xb/encoder/vex-encoder/vex-encoder.cpp b/library/stm32f072xb/encoder/vex-encoder/vex-encoder.cpp
index 8a38737d7360b2b64cd1d37fe8aec329c052bb8f..17f931d16e440d796515e0ade25caf6e46be079c 100644
--- a/library/stm32f072xb/encoder/vex-encoder/vex-encoder.cpp
+++ b/library/stm32f072xb/encoder/vex-encoder/vex-encoder.cpp
@@ -1,15 +1,16 @@
#include "vex-encoder.hpp"
-VexEncoder::VexEncoder(VexEncoderInit& encoderInit)
+// VexEncoder* VexEncoder::lastEncoder = nullptr;
+
+VexEncoder::VexEncoder(VexEncoderInit& encoderInit, VexEncoder* lastEncoder)
: _i2c(encoderInit.i2c), _address(0), _position(0), _maxPosition(encoderInit.maxPosition) {
uint8_t newAddress = VEX_STARTING_ADDRESS;
- if (lastEncoder) {
+
+ if (lastEncoder != nullptr) {
lastEncoder->unterminate();
newAddress = lastEncoder->_address + 0x2;
}
- lastEncoder = this;
-
setAddress(newAddress);
zero();
}
diff --git a/library/stm32f072xb/encoder/vex-encoder/vex-encoder.hpp b/library/stm32f072xb/encoder/vex-encoder/vex-encoder.hpp
index d33a5753d54ef6cfbf3c06d522b27b7478fed83d..f9602a10ac3c4f348eb8a579f2416291723183dc 100644
--- a/library/stm32f072xb/encoder/vex-encoder/vex-encoder.hpp
+++ b/library/stm32f072xb/encoder/vex-encoder/vex-encoder.hpp
@@ -28,9 +28,8 @@ constexpr float MOTOR_393_TORQUE_ROTATION = static_cast<float>(1 / 39.2);
class VexEncoder {
public:
- static inline VexEncoder* lastEncoder = nullptr;
-
- VexEncoder(VexEncoderInit& encoderinit);
+ // static VexEncoder* lastEncoder;
+ VexEncoder(VexEncoderInit& encoderinit, VexEncoder* encoder);
~VexEncoder() = default;
float getPosition();
float getMaxPosition();
diff --git a/library/stm32f072xb/motor/servo/servo.hpp b/library/stm32f072xb/motor/servo/servo.hpp
index 8f30b653de78ca050bf57971ef8c3c39e5d5ab3f..fd475170dd40bca88b21722c828c2b1d7d3152e9 100644
--- a/library/stm32f072xb/motor/servo/servo.hpp
+++ b/library/stm32f072xb/motor/servo/servo.hpp
@@ -10,25 +10,33 @@
using namespace stm32f072xb;
// usage sample
+
/*!
- \code {.cpp}
- ServoInit servoInit = {TIM2, OutputCompareChannel::OCC2, GpioPin::P1, AlternateFunction::AF2};
- // For the HS-422
- Servo<Port::A> servo(servoInit, 90, -90, 90, 450, 2300);
- servo.setAngle(0);
- \endcode
-*/
+ * @brief Construct a new Servo object
+ * @code {.cpp}
+ * servo(servoInit, 90, -90, 90, 450, 2300); // For the HS-422
+ * @endcode
+ *
+ * @param servoInit Servo initialization structure
+ * @param amplitude Amplitude of the servo motor
+ * @param minAngle Minimum angle the servo can go to (software barrier)
+ * @param maxAngle Maximum angle the servo can go to (software barrier)
+ * @param pulseWidthMIN Pulse Width minimum value (differs based on the servo model, default = 900)
+ * @param pulseWidthMAX Pulse Width maximum value (differs based on the servo model, default = 2100)
+ */
/**
* We found the maximum and minimum frequency range in microseconds for the following servos.
* This is the template for displaying the informations: Servo Name : minimum pulse width-maximum pulse width
* HS-422 : 450-2300
* HS-485HB : 900-2100
* MG-996R : 544-2400
+ * HS-785HB :1050-1950
+ * MG90S : 500-3000
**/
template <Port pwmPort>
class Servo {
private:
- static const uint16_t PWM_RESOLUTION = 20000;
+ static const uint16_t PWM_RESOLUTION = 30000;
static const uint16_t PWM_FREQUENCY = 50;
uint16_t _zeroDegreeDutyCycle;
int16_t _currentAngle;
@@ -40,6 +48,17 @@ class Servo {
AlternateGpio<pwmPort> _pwmOutputPin;
public:
+ /**
+ * @brief Construct a new Servo object
+ *
+ * @param servoInit Servo initialization structure
+ * @param amplitude Amplitude of the servo motor
+ * @param minAngle Minimum angle the servo can go to (software barrier)
+ * @param maxAngle Maximum angle the servo can go to (software barrier)
+ * @param startAngle Start angle of the servo
+ * @param pulseWidthMIN Pulse Width minimum value (differs based on the servo model, default = 900)
+ * @param pulseWidthMAX Pulse Width maximum value (differs based on the servo model, default = 2100)
+ */
Servo(ServoInit& servoInit,
uint16_t amplitude,
int16_t minAngle,
@@ -47,13 +66,13 @@ class Servo {
uint16_t pulseWidthMIN = 900,
uint16_t pulseWidthMAX = 2100)
: _zeroDegreeDutyCycle((pulseWidthMIN + pulseWidthMAX) / 2),
- _currentAngle(minAngle),
+ _currentAngle((maxAngle + minAngle) / 2),
_minAngle(minAngle),
_maxAngle(maxAngle),
_scaler((float)(_zeroDegreeDutyCycle - pulseWidthMIN) / (float)amplitude),
_timer(servoInit.timer, servoInit.OCC, PWM_RESOLUTION, PWM_FREQUENCY, _zeroDegreeDutyCycle),
_pwmOutputPin({servoInit.pwmPin, Pupdr::NONE, servoInit.alternateFunction}) {
- setAngle(_currentAngle);
+ setAngle((maxAngle + minAngle) / 2);
}
void setAngle(int16_t angle) {
diff --git a/library/stm32f072xb/motor/vex-motor/vex-motor.hpp b/library/stm32f072xb/motor/vex-motor/vex-motor.hpp
index 9a9627be44fa88f51d76b151b9cbd4523ea03ebc..9c455e8c6bd8df8320c8416362e07556b72780f9 100644
--- a/library/stm32f072xb/motor/vex-motor/vex-motor.hpp
+++ b/library/stm32f072xb/motor/vex-motor/vex-motor.hpp
@@ -13,10 +13,10 @@ using namespace stm32f072xb;
* @tparam directionPort
*/
template <Port pwmPort, Port directionPort>
-class VexMotor : public BrushedMotor<pwmPort, directionPort>, public VexEncoder {
+class VexMotor : public BrushedMotor<pwmPort, directionPort> {
public:
- VexMotor(BrushedMotorInit& motorInit, VexEncoderInit& encoderInit)
- : BrushedMotor<pwmPort, directionPort>(motorInit, 255), VexEncoder(encoderInit) {}
+ VexMotor(BrushedMotorInit& motorInit, VexEncoder* vexEncoder)
+ : BrushedMotor<pwmPort, directionPort>(motorInit, INT16_MAX), _vexEncoder(vexEncoder) {}
void setSpeed(int16_t speed) {
if (isAllowedToMove(speed)) {
@@ -26,14 +26,17 @@ class VexMotor : public BrushedMotor<pwmPort, directionPort>, public VexEncoder
}
}
+ VexEncoder* getEncoder() { return _vexEncoder; }
+
private:
+ VexEncoder* _vexEncoder;
bool isAllowedToMove(int16_t speed) {
const float minPosition = 0.05f;
bool isAllow = true;
- float absolutePosition = abs(this->getPosition());
+ float absolutePosition = abs(this->_vexEncoder->getPosition());
// Prevent going foward if we are already at the maximum forward
- if (speed >= 0 && absolutePosition >= this->getMaxPosition()) {
+ if (speed >= 0 && absolutePosition >= this->_vexEncoder->getMaxPosition()) {
isAllow = false;
} else if (speed <= 0 && absolutePosition < minPosition) {
isAllow = false;