426 lines
12 KiB
C
426 lines
12 KiB
C
/****************************************************************************************************************************
|
|
RP2040_ISR_Servo_Impl_H.h
|
|
For :
|
|
- MBED RP2040-based boards such as Nano_RP2040_Connect, RASPBERRY_PI_PICO, ADAFRUIT_FEATHER_RP2040 and GENERIC_RP2040.
|
|
- RP2040-based boards such as RASPBERRY_PI_PICO, ADAFRUIT_FEATHER_RP2040 and GENERIC_RP2040 using arduino_pico core
|
|
|
|
Written by Khoi Hoang
|
|
Built by Khoi Hoang https://github.com/khoih-prog/RP2040_ISR_Servo
|
|
Licensed under MIT license
|
|
|
|
Version: 1.1.2
|
|
|
|
Version Modified By Date Comments
|
|
------- ----------- ---------- -----------
|
|
1.0.0 K Hoang 21/08/2021 Initial coding for RP2040 boards using ArduinoCore-mbed or arduino-pico core
|
|
1.0.1 K Hoang 22/10/2021 Fix platform in library.json for PIO
|
|
1.1.0 K Hoang 27/02/2022 Fix setPulseWidth() bug. Convert to h-only style
|
|
1.1.1 K Hoang 08/03/2022 Delete redundant `.cpp` file causing compile error
|
|
1.1.2 K Hoang 08/03/2022 Permit using servos with different pulse ranges simultaneously
|
|
*****************************************************************************************************************************/
|
|
|
|
#pragma once
|
|
|
|
#ifndef RP2040_ISR_Servo_Impl_H
|
|
#define RP2040_ISR_Servo_Impl_H
|
|
|
|
#include "RP2040_ISR_Servo.h"
|
|
#include <string.h>
|
|
|
|
#ifndef ISR_SERVO_DEBUG
|
|
#define ISR_SERVO_DEBUG 1
|
|
#endif
|
|
|
|
static RP2040_ISR_Servo RP2040_ISR_Servos; // create servo object to control up to 16 servos
|
|
|
|
#if defined(ARDUINO_ARCH_MBED)
|
|
|
|
#define TRIM_DURATION 15 //callback overhead (35 us) -> 15 us if toggle() is called after starting the timeout
|
|
|
|
#else
|
|
|
|
#include "servo.pio.h"
|
|
static PIOProgram _servoPgm(&servo_program);
|
|
|
|
#endif
|
|
|
|
/////////////////////////////////////////////////////
|
|
|
|
RP2040_ISR_Servo::RP2040_ISR_Servo()
|
|
: numServos (-1)
|
|
{
|
|
}
|
|
|
|
/////////////////////////////////////////////////////
|
|
|
|
// find the first available slot
|
|
// return -1 if none found
|
|
int8_t RP2040_ISR_Servo::findFirstFreeSlot()
|
|
{
|
|
// all slots are used
|
|
if (numServos >= MAX_SERVOS)
|
|
return -1;
|
|
|
|
// return the first slot with no count (i.e. free)
|
|
for (int8_t servoIndex = 0; servoIndex < MAX_SERVOS; servoIndex++)
|
|
{
|
|
if (servo[servoIndex].enabled == false)
|
|
{
|
|
ISR_SERVO_LOGDEBUG1("Index =", servoIndex);
|
|
|
|
return servoIndex;
|
|
}
|
|
}
|
|
|
|
// no free slots found
|
|
return -1;
|
|
}
|
|
|
|
/////////////////////////////////////////////////////
|
|
|
|
int8_t RP2040_ISR_Servo::setupServo(const uint8_t& pin, const uint16_t& minPulseUs, const uint16_t& maxPulseUs,
|
|
uint16_t value)
|
|
{
|
|
int8_t servoIndex;
|
|
|
|
if (pin > RP2040_MAX_PIN)
|
|
return -1;
|
|
|
|
pinMode(pin, OUTPUT);
|
|
digitalWrite(pin, LOW);
|
|
|
|
if (numServos < 0)
|
|
init();
|
|
|
|
servoIndex = findFirstFreeSlot();
|
|
|
|
if (servoIndex < 0)
|
|
return -1;
|
|
|
|
servo[servoIndex].pin = pin;
|
|
servo[servoIndex].maxPulseUs = maxPulseUs;
|
|
servo[servoIndex].minPulseUs = minPulseUs;
|
|
servo[servoIndex].position = 0;
|
|
servo[servoIndex].enabled = true;
|
|
|
|
numServos++;
|
|
|
|
#if defined(ARDUINO_ARCH_MBED)
|
|
|
|
// Add code here for mbed
|
|
servo[servoIndex].servoImpl = new ServoImpl(digitalPinToPinName(pin));
|
|
|
|
#else
|
|
|
|
if (!_servoPgm.prepare(&servo[servoIndex].pio, &servo[servoIndex].smIdx, &servo[servoIndex].pgmOffset))
|
|
{
|
|
// ERROR, no free slots
|
|
ISR_SERVO_LOGERROR("Error no free slot");
|
|
return -1;
|
|
}
|
|
|
|
servo[servoIndex].enabled = true;
|
|
|
|
servo_program_init(servo[servoIndex].pio, servo[servoIndex].smIdx, servo[servoIndex].pgmOffset, pin);
|
|
pio_sm_set_enabled(servo[servoIndex].pio, servo[servoIndex].smIdx, false);
|
|
pio_sm_put_blocking(servo[servoIndex].pio, servo[servoIndex].smIdx, RP2040::usToPIOCycles(REFRESH_INTERVAL) / 3);
|
|
pio_sm_exec(servo[servoIndex].pio, servo[servoIndex].smIdx, pio_encode_pull(false, false));
|
|
pio_sm_exec(servo[servoIndex].pio, servo[servoIndex].smIdx, pio_encode_out(pio_isr, 32));
|
|
|
|
write(servoIndex, value);
|
|
|
|
pio_sm_exec(servo[servoIndex].pio, servo[servoIndex].smIdx, pio_encode_pull(false, false));
|
|
pio_sm_exec(servo[servoIndex].pio, servo[servoIndex].smIdx, pio_encode_mov(pio_x, pio_osr));
|
|
pio_sm_set_enabled(servo[servoIndex].pio, servo[servoIndex].smIdx, true);
|
|
|
|
/////////////////////////////////////////
|
|
|
|
write(servoIndex, value);
|
|
|
|
#endif
|
|
|
|
ISR_SERVO_LOGDEBUG1("Index =", servoIndex);
|
|
ISR_SERVO_LOGDEBUG3("min =", servo[servoIndex].minPulseUs, ", max =", servo[servoIndex].maxPulseUs);
|
|
|
|
return servoIndex;
|
|
}
|
|
|
|
/////////////////////////////////////////////////////
|
|
|
|
void RP2040_ISR_Servo::write(const uint8_t& servoIndex, uint16_t& value)
|
|
{
|
|
// treat any value less than MIN_PULSE_WIDTH as angle in degrees (values equal or larger are handled as microseconds)
|
|
if (value < MIN_PULSE_WIDTH)
|
|
{
|
|
// assumed to be 0-180 degrees servo
|
|
value = constrain(value, 0, 180);
|
|
value = map(value, 0, 180, servo[servoIndex].minPulseUs, servo[servoIndex].maxPulseUs);
|
|
}
|
|
|
|
writeMicroseconds(servoIndex, value);
|
|
}
|
|
|
|
/////////////////////////////////////////////////////
|
|
|
|
void RP2040_ISR_Servo::writeMicroseconds(const uint8_t& servoIndex, uint16_t value)
|
|
{
|
|
value = constrain(value, servo[servoIndex].minPulseUs, servo[servoIndex].maxPulseUs);
|
|
servo[servoIndex].position = value;
|
|
|
|
if (servo[servoIndex].enabled)
|
|
{
|
|
#if defined(ARDUINO_ARCH_MBED)
|
|
|
|
value = value - TRIM_DURATION;
|
|
|
|
if (servo[servoIndex].servoImpl->duration == -1)
|
|
{
|
|
servo[servoIndex].servoImpl->start(value);
|
|
}
|
|
|
|
servo[servoIndex].servoImpl->duration = value;
|
|
#else
|
|
|
|
// Remove any old updates that haven't yet taken effect
|
|
pio_sm_clear_fifos(servo[servoIndex].pio, servo[servoIndex].smIdx);
|
|
pio_sm_put_blocking(servo[servoIndex].pio, servo[servoIndex].smIdx, RP2040::usToPIOCycles(value) / 3);
|
|
|
|
#endif
|
|
}
|
|
}
|
|
|
|
/////////////////////////////////////////////////////
|
|
|
|
bool RP2040_ISR_Servo::setPosition(const uint8_t& servoIndex, uint16_t position)
|
|
{
|
|
if (servoIndex >= MAX_SERVOS)
|
|
return false;
|
|
|
|
// Updates interval of existing specified servo
|
|
if ( servo[servoIndex].enabled && (servo[servoIndex].pin <= RP2040_MAX_PIN) )
|
|
{
|
|
// treat any value less than MIN_PULSE_WIDTH as angle in degrees (values equal or larger are handled as microseconds)
|
|
if (position < MIN_PULSE_WIDTH)
|
|
{
|
|
// assumed to be 0-180 degrees servo
|
|
position = constrain(position, 0, 180);
|
|
position = map(position, 0, 180, servo[servoIndex].minPulseUs, servo[servoIndex].maxPulseUs);
|
|
}
|
|
|
|
servo[servoIndex].position = position;
|
|
|
|
writeMicroseconds(servoIndex, position);
|
|
|
|
ISR_SERVO_LOGDEBUG3("Idx =", servoIndex, ", pos =", servo[servoIndex].position);
|
|
|
|
return true;
|
|
}
|
|
|
|
// false return for non-used numServo or bad pin
|
|
return false;
|
|
}
|
|
|
|
/////////////////////////////////////////////////////
|
|
|
|
// returns last position in degrees if success, or -1 on wrong servoIndex
|
|
int RP2040_ISR_Servo::getPosition(const uint8_t& servoIndex)
|
|
{
|
|
if (servoIndex >= MAX_SERVOS)
|
|
return -1;
|
|
|
|
// Updates interval of existing specified servo
|
|
if ( servo[servoIndex].enabled && (servo[servoIndex].pin <= RP2040_MAX_PIN) )
|
|
{
|
|
ISR_SERVO_LOGDEBUG3("Idx =", servoIndex, ", pos =", servo[servoIndex].position);
|
|
|
|
return (servo[servoIndex].position);
|
|
}
|
|
|
|
// return 0 for non-used numServo or bad pin
|
|
return -1;
|
|
}
|
|
|
|
/////////////////////////////////////////////////////
|
|
|
|
// setPulseWidth will set servo PWM Pulse Width in microseconds, correcponding to certain position in degrees
|
|
// by using PWM, turn HIGH 'pulseWidth' microseconds within REFRESH_INTERVAL (20000us)
|
|
// min and max for each individual servo are enforced
|
|
// returns true on success or -1 on wrong servoIndex
|
|
bool RP2040_ISR_Servo::setPulseWidth(const uint8_t& servoIndex, uint16_t& pulseWidth)
|
|
{
|
|
if (servoIndex >= MAX_SERVOS)
|
|
return false;
|
|
|
|
// Updates interval of existing specified servo
|
|
if ( servo[servoIndex].enabled && (servo[servoIndex].pin <= RP2040_MAX_PIN) )
|
|
{
|
|
if (pulseWidth < servo[servoIndex].minPulseUs)
|
|
pulseWidth = servo[servoIndex].minPulseUs;
|
|
else if (pulseWidth > servo[servoIndex].maxPulseUs)
|
|
pulseWidth = servo[servoIndex].maxPulseUs;
|
|
|
|
writeMicroseconds(servoIndex, pulseWidth);
|
|
|
|
ISR_SERVO_LOGDEBUG3("Idx =", servoIndex, ", pos =", servo[servoIndex].position);
|
|
|
|
|
|
return true;
|
|
}
|
|
|
|
// false return for non-used numServo or bad pin
|
|
return false;
|
|
}
|
|
|
|
/////////////////////////////////////////////////////
|
|
|
|
// returns pulseWidth in microsecs (within min/max range) if success, or 0 on wrong servoIndex
|
|
uint16_t RP2040_ISR_Servo::getPulseWidth(const uint8_t& servoIndex)
|
|
{
|
|
if (servoIndex >= MAX_SERVOS)
|
|
return 0;
|
|
|
|
// Updates interval of existing specified servo
|
|
if ( servo[servoIndex].enabled && (servo[servoIndex].pin <= RP2040_MAX_PIN) )
|
|
{
|
|
ISR_SERVO_LOGDEBUG3("Idx =", servoIndex, ", pos =", servo[servoIndex].position);
|
|
|
|
return (servo[servoIndex].position);
|
|
}
|
|
|
|
// return 0 for non-used numServo or bad pin
|
|
return 0;
|
|
}
|
|
|
|
/////////////////////////////////////////////////////
|
|
|
|
void RP2040_ISR_Servo::deleteServo(const uint8_t& servoIndex)
|
|
{
|
|
if ( (numServos == 0) || (servoIndex >= MAX_SERVOS) )
|
|
{
|
|
return;
|
|
}
|
|
|
|
// don't decrease the number of servos if the specified slot is already empty
|
|
if (servo[servoIndex].enabled)
|
|
{
|
|
#if defined(ARDUINO_ARCH_MBED)
|
|
|
|
//Must be before memset
|
|
if (servo[servoIndex].servoImpl)
|
|
delete servo[servoIndex].servoImpl;
|
|
|
|
#endif
|
|
|
|
memset((void*) &servo[servoIndex], 0, sizeof (servo_t));
|
|
|
|
servo[servoIndex].enabled = false;
|
|
servo[servoIndex].position = 0;
|
|
// Intentional bad pin, good only from 0-16 for Digital, A0=17
|
|
servo[servoIndex].pin = RP2040_WRONG_PIN;
|
|
|
|
// update number of servos
|
|
numServos--;
|
|
}
|
|
}
|
|
|
|
/////////////////////////////////////////////////////
|
|
|
|
bool RP2040_ISR_Servo::isEnabled(const uint8_t& servoIndex)
|
|
{
|
|
if (servoIndex >= MAX_SERVOS)
|
|
return false;
|
|
|
|
if (servo[servoIndex].pin > RP2040_MAX_PIN)
|
|
{
|
|
// Disable if something wrong
|
|
servo[servoIndex].pin = RP2040_WRONG_PIN;
|
|
servo[servoIndex].enabled = false;
|
|
return false;
|
|
}
|
|
|
|
return servo[servoIndex].enabled;
|
|
}
|
|
|
|
/////////////////////////////////////////////////////
|
|
|
|
bool RP2040_ISR_Servo::enable(const uint8_t& servoIndex)
|
|
{
|
|
if (servoIndex >= MAX_SERVOS)
|
|
return false;
|
|
|
|
if (servo[servoIndex].pin > RP2040_MAX_PIN)
|
|
{
|
|
// Disable if something wrong
|
|
servo[servoIndex].pin = RP2040_WRONG_PIN;
|
|
servo[servoIndex].enabled = false;
|
|
return false;
|
|
}
|
|
|
|
if ( servo[servoIndex].position >= servo[servoIndex].minPulseUs )
|
|
servo[servoIndex].enabled = true;
|
|
|
|
return true;
|
|
}
|
|
|
|
/////////////////////////////////////////////////////
|
|
|
|
bool RP2040_ISR_Servo::disable(const uint8_t& servoIndex)
|
|
{
|
|
if (servoIndex >= MAX_SERVOS)
|
|
return false;
|
|
|
|
if (servo[servoIndex].pin > RP2040_MAX_PIN)
|
|
servo[servoIndex].pin = RP2040_WRONG_PIN;
|
|
|
|
servo[servoIndex].enabled = false;
|
|
|
|
return true;
|
|
}
|
|
|
|
void RP2040_ISR_Servo::enableAll()
|
|
{
|
|
// Enable all servos with a enabled and count != 0 (has PWM) and good pin
|
|
for (int8_t servoIndex = 0; servoIndex < MAX_SERVOS; servoIndex++)
|
|
{
|
|
if ( (servo[servoIndex].position >= servo[servoIndex].minPulseUs) && !servo[servoIndex].enabled
|
|
&& (servo[servoIndex].pin <= RP2040_MAX_PIN) )
|
|
{
|
|
servo[servoIndex].enabled = true;
|
|
}
|
|
}
|
|
}
|
|
|
|
/////////////////////////////////////////////////////
|
|
|
|
void RP2040_ISR_Servo::disableAll()
|
|
{
|
|
// Disable all servos
|
|
for (int8_t servoIndex = 0; servoIndex < MAX_SERVOS; servoIndex++)
|
|
{
|
|
servo[servoIndex].enabled = false;
|
|
}
|
|
}
|
|
|
|
/////////////////////////////////////////////////////
|
|
|
|
bool RP2040_ISR_Servo::toggle(const uint8_t& servoIndex)
|
|
{
|
|
if (servoIndex >= MAX_SERVOS)
|
|
return false;
|
|
|
|
servo[servoIndex].enabled = !servo[servoIndex].enabled;
|
|
|
|
return true;
|
|
}
|
|
|
|
/////////////////////////////////////////////////////
|
|
|
|
int8_t RP2040_ISR_Servo::getNumServos()
|
|
{
|
|
return numServos;
|
|
}
|
|
|
|
/////////////////////////////////////////////////////
|
|
|
|
#endif // RP2040_ISR_Servo_Impl_H
|