From dce657b93efb55e314e7fa04fa64bd5cee516056 Mon Sep 17 00:00:00 2001
From: Alberto Nidasio <alberto.nidasio@skywarder.eu>
Date: Thu, 5 May 2022 23:15:47 +0200
Subject: [PATCH] [Servo] Implemented ServoData
---
src/shared/actuators/Servo/Servo.cpp | 27 ++++++++--------
src/shared/actuators/Servo/Servo.h | 28 ++++------------
src/shared/actuators/Servo/ServoData.h | 45 ++++++++++++++++++++++++++
src/shared/drivers/timer/BasicTimer.h | 36 +++++++++++++++++++++
src/shared/drivers/timer/PWM.cpp | 2 ++
src/shared/drivers/timer/PWM.h | 5 +++
src/shared/drivers/timer/TimerUtils.h | 2 +-
7 files changed, 109 insertions(+), 36 deletions(-)
create mode 100644 src/shared/actuators/Servo/ServoData.h
diff --git a/src/shared/actuators/Servo/Servo.cpp b/src/shared/actuators/Servo/Servo.cpp
index b4d9b8e64..24e09cd8e 100644
--- a/src/shared/actuators/Servo/Servo.cpp
+++ b/src/shared/actuators/Servo/Servo.cpp
@@ -22,6 +22,8 @@
#include "Servo.h"
+#include <drivers/timer/TimestampTimer.h>
+
#include "miosix.h"
namespace Boardcore
@@ -29,34 +31,24 @@ namespace Boardcore
#ifndef COMPILE_FOR_HOST
-Servo::Servo(TIM_TypeDef* const timer, TimerUtils::Channel pwmChannel,
- unsigned int minPulse, unsigned int maxPulse,
- unsigned int frequency, unsigned int resetPulse)
- : pwm(timer, frequency), pwmChannel(pwmChannel), minPulse(minPulse),
- maxPulse(maxPulse), resetPulse(resetPulse), frequency(frequency)
-{
- setPosition(resetPulse);
-}
-
Servo::Servo(TIM_TypeDef* const timer, TimerUtils::Channel pwmChannel,
unsigned int minPulse, unsigned int maxPulse,
unsigned int frequency)
- : Servo(timer, pwmChannel, minPulse, maxPulse, frequency, minPulse)
+ : pwm(timer, frequency), pwmChannel(pwmChannel), minPulse(minPulse),
+ maxPulse(maxPulse), frequency(frequency)
{
+ setPosition(0);
}
void Servo::enable() { pwm.enableChannel(pwmChannel); }
void Servo::disable() { pwm.disableChannel(pwmChannel); }
-void Servo::reset() { setPosition(resetPulse); }
-
#else
Servo::Servo(unsigned int minPulse, unsigned int maxPulse,
unsigned int frequency)
- : minPulse(minPulse), maxPulse(maxPulse), frequency(frequency),
- resetPulse(minPulse)
+ : minPulse(minPulse), maxPulse(maxPulse), frequency(frequency)
{
setPosition(0);
}
@@ -107,4 +99,11 @@ float Servo::getPosition180Deg() { return getPosition() * 1800; }
float Servo::getPosition360Deg() { return getPosition() * 3600; }
+ServoData Servo::getState()
+{
+ return {TimestampTimer::getInstance().getTimestamp(),
+ pwm.getTimer().getTimerNumber(), static_cast<uint8_t>(pwmChannel),
+ getPosition()};
+}
+
} // namespace Boardcore
diff --git a/src/shared/actuators/Servo/Servo.h b/src/shared/actuators/Servo/Servo.h
index 43c1ca54a..2e0a511eb 100644
--- a/src/shared/actuators/Servo/Servo.h
+++ b/src/shared/actuators/Servo/Servo.h
@@ -24,6 +24,8 @@
#include <drivers/timer/PWM.h>
#endif
+#include "ServoData.h"
+
#pragma once
namespace Boardcore
@@ -76,21 +78,6 @@ public:
explicit Servo(TIM_TypeDef* const timer, TimerUtils::Channel pwmChannel,
unsigned int minPulse = 1000, unsigned int maxPulse = 2000,
unsigned int frequency = 50);
- /**
- * @brief Prepare the timer and sets the PWM output to the minimum.
- *
- * @see Servo::Servo
- *
- * @param timer Timer peripheral used for the PWM signal.
- * @param pwmChannel Timer's channel used for the PWM signal.
- * @param frequency Frequency of the PWM driving the H-bridge.
- * @param minPulse Minimum signal pulse in microseconds.
- * @param maxPulse Maximum signal pulse in microseconds.
- * @param resetPulse Reset signal pulse in microseconds.
- */
- explicit Servo(TIM_TypeDef* const timer, TimerUtils::Channel pwmChannel,
- unsigned int minPulse, unsigned int maxPulse,
- unsigned int frequency, unsigned int resetPulse);
#else
explicit Servo(unsigned int minPulse = 1000, unsigned int maxPulse = 2000,
unsigned int frequency = 50);
@@ -106,11 +93,6 @@ public:
*/
void disable();
- /**
- * @brief Moves the servo to the reset position.
- */
- void reset();
-
/**
* @brief Set the position of the servomotor.
*
@@ -142,6 +124,11 @@ public:
float getPosition360Deg();
+ /**
+ * @brief Returns the current position and the current timestamp.
+ */
+ ServoData getState();
+
private:
// This class is not copyable!
Servo& operator=(const Servo&) = delete;
@@ -156,7 +143,6 @@ private:
float minPulse;
float maxPulse;
- float resetPulse;
float frequency;
};
diff --git a/src/shared/actuators/Servo/ServoData.h b/src/shared/actuators/Servo/ServoData.h
new file mode 100644
index 000000000..bbed26513
--- /dev/null
+++ b/src/shared/actuators/Servo/ServoData.h
@@ -0,0 +1,45 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <ostream>
+
+namespace Boardcore
+{
+
+struct ServoData
+{
+ uint64_t timestamp;
+ uint8_t timer;
+ uint8_t channel;
+ float position;
+
+ static std::string header() { return "timestamp,position\n"; }
+
+ void print(std::ostream& os) const
+ {
+ os << timestamp << "," << position << "\n";
+ }
+};
+
+} // namespace Boardcore
diff --git a/src/shared/drivers/timer/BasicTimer.h b/src/shared/drivers/timer/BasicTimer.h
index 00428f59a..57c8476a4 100644
--- a/src/shared/drivers/timer/BasicTimer.h
+++ b/src/shared/drivers/timer/BasicTimer.h
@@ -91,6 +91,8 @@ public:
TIM_TypeDef *getTimer();
+ uint8_t getTimerNumber();
+
/**
* @brief Resets the timer configuration to the default state.
*
@@ -211,6 +213,40 @@ inline BasicTimer::~BasicTimer() { ClockUtils::disablePeripheralClock(timer); }
inline TIM_TypeDef *BasicTimer::getTimer() { return timer; }
+inline uint8_t BasicTimer::getTimerNumber()
+{
+ if (timer == TIM1)
+ return 1;
+ else if (timer == TIM2)
+ return 2;
+ else if (timer == TIM3)
+ return 3;
+ else if (timer == TIM4)
+ return 4;
+ else if (timer == TIM5)
+ return 5;
+ else if (timer == TIM6)
+ return 6;
+ else if (timer == TIM7)
+ return 7;
+ else if (timer == TIM8)
+ return 8;
+ else if (timer == TIM9)
+ return 9;
+ else if (timer == TIM10)
+ return 10;
+ else if (timer == TIM11)
+ return 11;
+ else if (timer == TIM12)
+ return 12;
+ else if (timer == TIM13)
+ return 13;
+ else if (timer == TIM14)
+ return 14;
+ else
+ return 0;
+}
+
inline void BasicTimer::reset()
{
timer->CR1 = 0;
diff --git a/src/shared/drivers/timer/PWM.cpp b/src/shared/drivers/timer/PWM.cpp
index b6f8a6aca..f8e2faa10 100644
--- a/src/shared/drivers/timer/PWM.cpp
+++ b/src/shared/drivers/timer/PWM.cpp
@@ -88,6 +88,8 @@ float PWM::getDutyCycle(TimerUtils::Channel channel)
static_cast<float>(timer.readAutoReloadRegister());
}
+GP16bitTimer PWM::getTimer() { return timer; }
+
void PWM::setTimerConfiguration()
{
timer.disable();
diff --git a/src/shared/drivers/timer/PWM.h b/src/shared/drivers/timer/PWM.h
index b25e508ef..d30e5d45c 100644
--- a/src/shared/drivers/timer/PWM.h
+++ b/src/shared/drivers/timer/PWM.h
@@ -95,6 +95,11 @@ public:
*/
float getDutyCycle(TimerUtils::Channel channel);
+ /**
+ * @brief Returns the timer used to generate the pwm signal.
+ */
+ GP16bitTimer getTimer();
+
private:
// This class is not copyable!
PWM& operator=(const PWM&) = delete;
diff --git a/src/shared/drivers/timer/TimerUtils.h b/src/shared/drivers/timer/TimerUtils.h
index 64d7e6801..7d619419f 100644
--- a/src/shared/drivers/timer/TimerUtils.h
+++ b/src/shared/drivers/timer/TimerUtils.h
@@ -241,7 +241,7 @@ enum class OutputComparePolarity : uint16_t
ACTIVE_LOW = 0x1
};
-enum class Channel : int
+enum class Channel : uint8_t
{
CHANNEL_1 = 0,
CHANNEL_2 = 1,
--
GitLab