Skip to content

Instantly share code, notes, and snippets.

@zsup
Last active Dec 30, 2015
Embed
What would you like to do?
Spark Servo, first commit
/**
******************************************************************************
* @file spark_wiring_servo.h
* @author Zach Supalla
* @version V1.0.0
* @date 06-December-2013
* @brief Header for spark_wiring_servo.h module
******************************************************************************
Copyright (c) 2013 Spark Labs, Inc. All rights reserved.
Copyright (c) 2010 LeafLabs, LLC.
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation, either
version 3 of the License, or (at your option) any later version.
This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
Lesser General Public License for more details.
You should have received a copy of the GNU Lesser General Public
License along with this library; if not, see <http://www.gnu.org/licenses/>.
******************************************************************************
*/
/*
* Note on Arduino compatibility:
*
* In the Arduino implementation, PWM is done "by hand" in the sense
* that timer channels are hijacked in groups and an ISR is set which
* toggles Servo::attach()ed pins using digitalWrite().
*
* While this scheme allows any pin to drive a servo, it chews up
* cycles and complicates the programmer's notion of when a particular
* timer channel will be in use.
*
* This implementation only allows Servo instances to attach() to pins
* that already have a timer channel associated with them, and just
* uses pwmWrite() to drive the wave.
*
* This introduces an incompatibility: while the Arduino
* implementation of attach() returns the affected channel on success
* and 0 on failure, this one returns true on success and false on
* failure.
*
* RC Servos expect a pulse every 20ms. Since periods are set for
* entire timers, rather than individual channels, attach()ing a Servo
* to a pin can interfere with other pins associated with the same
* timer. As always, your board's pin map is your friend.
*/
// Pin number of unattached pins
#define NOT_ATTACHED (-1)
// Default min/max pulse widths (in microseconds) and angles (in
// degrees). Values chosen for Arduino compatibility. These values
// are part of the public API; DO NOT CHANGE THEM.
#define SERVO_DEFAULT_MIN_PW 544
#define SERVO_DEFAULT_MAX_PW 2400
#define SERVO_DEFAULT_MIN_ANGLE 0
#define SERVO_DEFAULT_MAX_ANGLE 180
/** Class for interfacing with RC servomotors. */
class Servo {
public:
/**
* @brief Construct a new Servo instance.
*
* The new instance will not be attached to any pin.
*/
Servo();
/**
* @brief Associate this instance with a servomotor whose input is
* connected to pin.
*
* If this instance is already attached to a pin, it will be
* detached before being attached to the new pin. This function
* doesn't detach any interrupt attached with the pin's timer
* channel.
*
* @param pin Pin connected to the servo pulse wave input. This
* pin must be capable of PWM output.
*
* @param minPulseWidth Minimum pulse width to write to pin, in
* microseconds. This will be associated
* with a minAngle degree angle. Defaults to
* SERVO_DEFAULT_MIN_PW = 544.
*
* @param maxPulseWidth Maximum pulse width to write to pin, in
* microseconds. This will be associated
* with a maxAngle degree angle. Defaults to
* SERVO_DEFAULT_MAX_PW = 2400.
*
* @param minAngle Target angle (in degrees) associated with
* minPulseWidth. Defaults to
* SERVO_DEFAULT_MIN_ANGLE = 0.
*
* @param maxAngle Target angle (in degrees) associated with
* maxPulseWidth. Defaults to
* SERVO_DEFAULT_MAX_ANGLE = 180.
*
* @sideeffect May set pinMode(pin, PWM).
*
* @return true if successful, false when pin doesn't support PWM.
*/
bool attach(uint16_t pin,
uint16_t minPulseWidth=SERVO_DEFAULT_MIN_PW,
uint16_t maxPulseWidth=SERVO_DEFAULT_MAX_PW,
int16_t minAngle=SERVO_DEFAULT_MIN_ANGLE,
int16_t maxAngle=SERVO_DEFAULT_MAX_ANGLE);
/**
* @brief Check if this instance is attached to a servo.
* @return true if this instance is attached to a servo, false otherwise.
* @see Servo::attachedPin()
*/
bool attached() const { return this->pin != NOT_ATTACHED; }
/**
* @brief Get the pin this instance is attached to.
* @return Pin number if currently attached to a pin, NOT_ATTACHED
* otherwise.
* @see Servo::attach()
*/
int attachedPin() const { return this->pin; }
/**
* @brief Stop driving the servo pulse train.
*
* If not currently attached to a motor, this function has no effect.
*
* @return true if this call did anything, false otherwise.
*/
bool detach();
/**
* @brief Set the servomotor target angle.
*
* @param angle Target angle, in degrees. If the target angle is
* outside the range specified at attach() time, it
* will be clamped to lie in that range.
*
* @see Servo::attach()
*/
void write(int angle);
/**
* Get the servomotor's target angle, in degrees. This will
* lie inside the range specified at attach() time.
*
* @see Servo::attach()
*/
int read() const;
/**
* @brief Set the pulse width, in microseconds.
*
* @param pulseWidth Pulse width to send to the servomotor, in
* microseconds. If outside of the range
* specified at attach() time, it is clamped to
* lie in that range.
*
* @see Servo::attach()
*/
void writeMicroseconds(uint16_t pulseWidth);
/**
* Get the current pulse width, in microseconds. This will
* lie within the range specified at attach() time.
*
* @see Servo::attach()
*/
uint16_t readMicroseconds() const;
private:
int16_t pin;
uint16_t minPW;
uint16_t maxPW;
int16_t minAngle;
int16_t maxAngle;
void resetFields(void);
};
/**
******************************************************************************
* @file spark_wiring_spi.h
* @author Zach Supalla
* @version V1.0.0
* @date 06-December-2013
* @brief Header for spark_wiring_servo.cpp module
******************************************************************************
Copyright (c) 2013 Spark Labs, Inc. All rights reserved.
Copyright (c) 2010 LeafLabs, LLC.
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation, either
version 3 of the License, or (at your option) any later version.
This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
Lesser General Public License for more details.
You should have received a copy of the GNU Lesser General Public
License along with this library; if not, see <http://www.gnu.org/licenses/>.
******************************************************************************
*/
// 20 millisecond period config. For a 1-based prescaler,
//
// (prescaler * overflow / CYC_MSEC) msec = 1 timer cycle = 20 msec
// => prescaler * overflow = 20 * CYC_MSEC
//
// This picks the smallest prescaler that allows an overflow < 2^16.
#define MAX_OVERFLOW ((1 << 16) - 1)
#define CYC_MSEC (SystemCoreClock / 1000)
#define TAU_MSEC 20
#define TAU_USEC (TAU_MSEC * 1000)
#define TAU_CYC (TAU_MSEC * CYC_MSEC)
#define SERVO_PRESCALER (TAU_CYC / MAX_OVERFLOW + 1)
#define SERVO_OVERFLOW ((uint16_t)round((double)TAU_CYC / SERVO_PRESCALER))
// Unit conversions
#define US_TO_COMPARE(us) ((uint16_t)map((us), 0, TAU_USEC, 0, SERVO_OVERFLOW))
#define CAPTURE_TO_US(c) ((uint32_t)map((c), 0, SERVO_OVERFLOW, 0, TAU_USEC))
#define ANGLE_TO_US(a) ((uint16_t)(map((a), this->minAngle, this->maxAngle, \
this->minPW, this->maxPW)))
#define US_TO_ANGLE(us) ((int16_t)(map((us), this->minPW, this->maxPW, \
this->minAngle, this->maxAngle)))
Servo::Servo() {
this->resetFields();
}
bool Servo::attach(uint16_t pin,
uint16_t minPW,
uint16_t maxPW,
int16_t minAngle,
int16_t maxAngle) {
if (pin >= TOTAL_PINS || PIN_MAP[pin].timer_peripheral == NULL)
{
return false;
}
// SPI safety check
if (SPI.isEnabled() == true && (pin == SCK || pin == MOSI || pin == MISO))
{
return false;
}
// I2C safety check
if (Wire.isEnabled() == true && (pin == SCL || pin == SDA))
{
return false;
}
// Serial1 safety check
if (Serial1.isEnabled() == true && (pin == RX || pin == TX))
{
return false;
}
if (this->attached()) {
this->detach();
}
this->pin = pin;
this->minPW = minPW;
this->maxPW = maxPW;
this->minAngle = minAngle;
this->maxAngle = maxAngle;
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
TIM_OCInitTypeDef TIM_OCInitStructure;
// AFIO clock enable
RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE);
pinMode(pin, AF_OUTPUT_PUSHPULL);
// TIM clock enable
if(PIN_MAP[pin].timer_peripheral == TIM2)
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);
else if(PIN_MAP[pin].timer_peripheral == TIM3)
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
else if(PIN_MAP[pin].timer_peripheral == TIM4)
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);
// Time base configuration
TIM_TimeBaseStructure.TIM_Period = SERVO_OVERFLOW;
TIM_TimeBaseStructure.TIM_Prescaler = SERVO_PRESCALER - 1;
TIM_TimeBaseStructure.TIM_ClockDivision = 0;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInit(PIN_MAP[pin].timer_peripheral, &TIM_TimeBaseStructure);
// PWM1 Mode configuration
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
TIM_OCInitStructure.TIM_Pulse = 0x0000;
if(PIN_MAP[this->pin].timer_ch == TIM_Channel_1)
{
// PWM1 Mode configuration: Channel1
TIM_OC1Init(PIN_MAP[this->pin].timer_peripheral, &TIM_OCInitStructure);
TIM_OC1PreloadConfig(PIN_MAP[this->pin].timer_peripheral, TIM_OCPreload_Enable);
}
else if(PIN_MAP[this->pin].timer_ch == TIM_Channel_2)
{
// PWM1 Mode configuration: Channel2
TIM_OC2Init(PIN_MAP[this->pin].timer_peripheral, &TIM_OCInitStructure);
TIM_OC2PreloadConfig(PIN_MAP[this->pin].timer_peripheral, TIM_OCPreload_Enable);
}
else if(PIN_MAP[this->pin].timer_ch == TIM_Channel_3)
{
// PWM1 Mode configuration: Channel3
TIM_OC3Init(PIN_MAP[this->pin].timer_peripheral, &TIM_OCInitStructure);
TIM_OC3PreloadConfig(PIN_MAP[this->pin].timer_peripheral, TIM_OCPreload_Enable);
}
else if(PIN_MAP[this->pin].timer_ch == TIM_Channel_4)
{
// PWM1 Mode configuration: Channel4
TIM_OC4Init(PIN_MAP[this->pin].timer_peripheral, &TIM_OCInitStructure);
TIM_OC4PreloadConfig(PIN_MAP[this->pin].timer_peripheral, TIM_OCPreload_Enable);
}
TIM_ARRPreloadConfig(PIN_MAP[this->pin].timer_peripheral, ENABLE);
// TIM enable counter
TIM_Cmd(PIN_MAP[this->pin].timer_peripheral, ENABLE);
return true;
}
bool Servo::detach() {
if (!this->attached()) {
return false;
}
// TIM disable counter
TIM_Cmd(PIN_MAP[this->pin].timer_peripheral, DISABLE);
this->resetFields();
return true;
}
void Servo::write(int degrees) {
degrees = constrain(degrees, this->minAngle, this->maxAngle);
this->writeMicroseconds(ANGLE_TO_US(degrees));
}
int Servo::read() const {
int a = US_TO_ANGLE(this->readMicroseconds());
// map() round-trips in a weird way we mostly correct for here;
// the round-trip is still sometimes off-by-one for write(1) and
// write(179).
return a == this->minAngle || a == this->maxAngle ? a : a + 1;
}
void Servo::writeMicroseconds(uint16_t pulseWidth) {
if (!this->attached()) {
return;
}
pulseWidth = constrain(pulseWidth, this->minPW, this->maxPW);
uint16_t Compare_Value = US_TO_COMPARE(pulseWidth);
if(PIN_MAP[this->pin].timer_ch == TIM_Channel_1)
{
TIM_SetCompare1(PIN_MAP[this->pin].timer_peripheral, Compare_Value);
}
else if(PIN_MAP[this->pin].timer_ch == TIM_Channel_2)
{
TIM_SetCompare2(PIN_MAP[this->pin].timer_peripheral, Compare_Value);
}
else if(PIN_MAP[this->pin].timer_ch == TIM_Channel_3)
{
TIM_SetCompare3(PIN_MAP[this->pin].timer_peripheral, Compare_Value);
}
else if(PIN_MAP[this->pin].timer_ch == TIM_Channel_4)
{
TIM_SetCompare4(PIN_MAP[this->pin].timer_peripheral, Compare_Value);
}
}
uint16_t Servo::readMicroseconds() const {
if (!this->attached()) {
return 0;
}
uint16_t Capture_Value = 0x0000;
if(PIN_MAP[this->pin].timer_ch == TIM_Channel_1)
{
Capture_Value = TIM_GetCapture1(PIN_MAP[this->pin].timer_peripheral);
}
else if(PIN_MAP[this->pin].timer_ch == TIM_Channel_2)
{
Capture_Value = TIM_GetCapture2(PIN_MAP[this->pin].timer_peripheral);
}
else if(PIN_MAP[this->pin].timer_ch == TIM_Channel_3)
{
Capture_Value = TIM_GetCapture3(PIN_MAP[this->pin].timer_peripheral);
}
else if(PIN_MAP[this->pin].timer_ch == TIM_Channel_4)
{
Capture_Value = TIM_GetCapture4(PIN_MAP[this->pin].timer_peripheral);
}
return CAPTURE_TO_US(Capture_Value);
}
void Servo::resetFields(void) {
this->pin = NOT_ATTACHED;
this->minAngle = SERVO_DEFAULT_MIN_ANGLE;
this->maxAngle = SERVO_DEFAULT_MAX_ANGLE;
this->minPW = SERVO_DEFAULT_MIN_PW;
this->maxPW = SERVO_DEFAULT_MAX_PW;
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment