arduino/libraries
2
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity

Servo

Browse source
This commit is contained in:
Roald Batts 2023-01-26 20:22:43 -05:00
parent 064a8ffd26
commit a334d75815
20 changed files with 2326 additions and 0 deletions
Download patch file Download diff file Expand all files Collapse all files

194
Servo/src/stm32f4/Servo.cpp Normal file
View file

@ -0,0 +1,194 @@
/******************************************************************************
* The MIT License
*
* Copyright (c) 2010, LeafLabs, LLC.
*
* 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.
*****************************************************************************/
#if defined(ARDUINO_ARCH_STM32F4)
#include "ServoTimers.h"
#include "boards.h"
#include "io.h"
#include "pwm.h"
#include "math.h"
// 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 (1000 * CYCLES_PER_MICROSECOND)
#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)round((double)TAU_CYC / SERVO_PRESCALER))
// Unit conversions
#define US_TO_COMPARE(us) ((uint16)map((us), 0, TAU_USEC, 0, SERVO_OVERFLOW))
#define COMPARE_TO_US(c) ((uint32)map((c), 0, SERVO_OVERFLOW, 0, TAU_USEC))
#define ANGLE_TO_US(a) ((uint16)(map((a), this->minAngle, this->maxAngle, \
this->minPW, this->maxPW)))
#define US_TO_ANGLE(us) ((int16)(map((us), this->minPW, this->maxPW, \
this->minAngle, this->maxAngle)))
Servo::Servo() {
this->resetFields();
}
bool Servo::attach(uint8 pin, uint16 minPW, uint16 maxPW, int16 minAngle, int16 maxAngle)
{
// SerialUSB.begin(115200);
// SerialUSB.println(MAX_OVERFLOW);
timer_dev *tdev = PIN_MAP[pin].timer_device;
analogWriteResolution(16);
int prescaler = 6;
int overflow = 65400;
int minPW_correction = 300;
int maxPW_correction = 300;
pinMode(pin, OUTPUT);
if (tdev == NULL) {
// don't reset any fields or ASSERT(0), to keep driving any
// previously attach()ed servo.
return false;
}
if ( (tdev == TIMER1) || (tdev == TIMER8) || (tdev == TIMER10) || (tdev == TIMER11))
{
prescaler = 54;
overflow = 65400;
minPW_correction = 40;
maxPW_correction = 50;
}
if ( (tdev == TIMER2) || (tdev == TIMER3) || (tdev == TIMER4) || (tdev == TIMER5) )
{
prescaler = 6;
overflow = 64285;
minPW_correction = 370;
maxPW_correction = 350;
}
if ( (tdev == TIMER6) || (tdev == TIMER7) )
{
prescaler = 6;
overflow = 65400;
minPW_correction = 0;
maxPW_correction = 0;
}
if ( (tdev == TIMER9) || (tdev == TIMER12) || (tdev == TIMER13) || (tdev == TIMER14) )
{
prescaler = 6;
overflow = 65400;
minPW_correction = 30;
maxPW_correction = 0;
}
if (this->attached()) {
this->detach();
}
this->pin = pin;
this->minPW = (minPW + minPW_correction);
this->maxPW = (maxPW + maxPW_correction);
this->minAngle = minAngle;
this->maxAngle = maxAngle;
timer_pause(tdev);
timer_set_prescaler(tdev, prescaler); // prescaler is 1-based
timer_set_reload(tdev, overflow);
timer_generate_update(tdev);
timer_resume(tdev);
return true;
}
bool Servo::detach() {
if (!this->attached()) {
return false;
}
timer_dev *tdev = PIN_MAP[this->pin].timer_device;
uint8 tchan = PIN_MAP[this->pin].timer_channel;
timer_set_mode(tdev, tchan, TIMER_DISABLED);
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 pulseWidth) {
if (!this->attached()) {
ASSERT(0);
return;
}
pulseWidth = constrain(pulseWidth, this->minPW, this->maxPW);
analogWrite(this->pin, US_TO_COMPARE(pulseWidth));
}
uint16 Servo::readMicroseconds() const {
if (!this->attached()) {
ASSERT(0);
return 0;
}
stm32_pin_info pin_info = PIN_MAP[this->pin];
uint16 compare = timer_get_compare(pin_info.timer_device,
pin_info.timer_channel);
return COMPARE_TO_US(compare);
}
void Servo::resetFields(void) {
this->pin = NOT_ATTACHED;
this->minAngle = MIN_ANGLE;
this->maxAngle = MAX_ANGLE;
this->minPW = MIN_PULSE_WIDTH;
this->maxPW = MAX_PULSE_WIDTH;
}
#endif

207
Servo/src/stm32f4/ServoTimers.h Normal file
View file

@ -0,0 +1,207 @@
/******************************************************************************
* The MIT License
*
* Copyright (c) 2010, LeafLabs, LLC.
*
* 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.
*****************************************************************************/
/*
* Arduino srl - www.arduino.org
* 2017 Feb 23: Edited by Francesco Alessi (alfran) - francesco@arduino.org
*/
#ifndef _SERVO_H_
#define _SERVO_H_
#include "types.h"
#include "timer.h"
#include "wiring.h" /* hack for IDE compile */
/*
* 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)
#define _Nbr_16timers 14 // mumber of STM32F469 Timers
#define SERVOS_PER_TIMER 4 // Number of timer channels
// 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 MIN_ANGLE 0
#define MAX_ANGLE 180
#define MIN_PULSE_WIDTH 544 // the shortest pulse sent to a servo
#define MAX_PULSE_WIDTH 2400 // the longest pulse sent to a servo
/** 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(uint8 pin,
uint16 minPulseWidth=MIN_PULSE_WIDTH,
uint16 maxPulseWidth=MAX_PULSE_WIDTH,
int16 minAngle=MIN_ANGLE,
int16 maxAngle=MAX_ANGLE);
/**
* @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);
/**
* @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 pulseWidth);
/**
* Get the servomotor's target angle, in degrees. This will
* lie inside the range specified at attach() time.
*
* @see Servo::attach()
*/
int read() const;
/**
* Get the current pulse width, in microseconds. This will
* lie within the range specified at attach() time.
*
* @see Servo::attach()
*/
uint16 readMicroseconds() const;
/**
* @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; }
private:
int16 pin;
uint16 minPW;
uint16 maxPW;
int16 minAngle;
int16 maxAngle;
void resetFields(void);
};
#endif /* _SERVO_H_ */