From cfdc8cff1da3dc4e069c4c162c726feb52906024 Mon Sep 17 00:00:00 2001 From: Abhishek Malik Date: Fri, 9 Sep 2016 19:14:33 -0700 Subject: [PATCH] Servo: Adding C Source Signed-off-by: Abhishek Malik --- src/servo/CMakeLists.txt | 14 ++-- src/servo/es08a.c | 107 ++++++++++++++++++++++++++++++ src/servo/es08a.h | 137 +++++++++++++++++++++++++++++++++++++++ src/servo/es08a_fti.c | 89 +++++++++++++++++++++++++ src/servo/es9257.c | 107 ++++++++++++++++++++++++++++++ src/servo/es9257.h | 137 +++++++++++++++++++++++++++++++++++++++ src/servo/es9257_fti.c | 89 +++++++++++++++++++++++++ 7 files changed, 675 insertions(+), 5 deletions(-) create mode 100644 src/servo/es08a.c create mode 100644 src/servo/es08a.h create mode 100644 src/servo/es08a_fti.c create mode 100644 src/servo/es9257.c create mode 100644 src/servo/es9257.h create mode 100644 src/servo/es9257_fti.c diff --git a/src/servo/CMakeLists.txt b/src/servo/CMakeLists.txt index 9cd02270..41f85e78 100644 --- a/src/servo/CMakeLists.txt +++ b/src/servo/CMakeLists.txt @@ -1,5 +1,9 @@ -set (libname "servo") -set (libdescription "upm servo") -set (module_src servo.cxx es08a.cxx es9257.cxx) -set (module_hpp servo.hpp es08a.hpp es9257.hpp) -upm_module_init() +upm_mixed_module_init (NAME servo + DESCRIPTION "upm servo module" + C_HDR es08a.h es9257.h + C_SRC es08a.c es9257.c + CPP_HDR servo.hpp es08a.hpp es9257.hpp + CPP_SRC servo.cxx es08a.cxx es9257.cxx + FTI_SRC es08a_fti.c es9257_fti.c + CPP_WRAPS_C + REQUIRES mraa) diff --git a/src/servo/es08a.c b/src/servo/es08a.c new file mode 100644 index 00000000..aa7256fc --- /dev/null +++ b/src/servo/es08a.c @@ -0,0 +1,107 @@ +/* + * Author: Yevgeniy Kiveisha + * Abhishek Malik + * Copyright (c) 2014 Intel Corporation. + * + * 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. + */ +#include "es08a.h" + +es08a_context es08a_init(int32_t pin, int32_t min_pulse_width, + int32_t max_pulse_width){ + es08a_context dev = (es08a_context) malloc(sizeof(struct _es08a_context)); + + if(dev == NULL){ + printf("Unable to assign memory to the Servo motor structure"); + return NULL; + } + + dev->servo_pin = pin; + + // second is the min pulse width + dev->min_pulse_width = min_pulse_width; + // third is the max pulse width + dev->max_pulse_width = max_pulse_width; + + dev->pwm = mraa_pwm_init(dev->servo_pin); + if(dev->pwm == NULL){ + printf("Unable to initialize the PWM pin"); + } + + es08a_set_angle(dev, 0); + return dev; +} + +void es08a_halt(es08a_context dev){ + mraa_pwm_enable(dev->pwm, 0); + free(dev); +} + +upm_result_t es08a_set_angle(es08a_context dev, int32_t angle){ + + if(ES08A_MAX_ANGLE < angle || angle < 0){ + printf("The angle specified is either above the max angle or below 0"); + return UPM_ERROR_UNSPECIFIED; + } + printf("setting angle to: %d\n", angle); + + mraa_pwm_enable(dev->pwm, 1); + mraa_pwm_period_us(dev->pwm, ES08A_PERIOD); + int32_t val = 0; + + es08a_calc_pulse_travelling(dev, &val, angle); + mraa_pwm_pulsewidth_us(dev->pwm, val); + + upm_delay(1); + mraa_pwm_enable(dev->pwm, 0); + + return UPM_SUCCESS; +} + +upm_result_t es08a_calc_pulse_travelling(const es08a_context dev, + int32_t* ret_val, int32_t value){ + if (value > dev->max_pulse_width) { + return dev->max_pulse_width; + } + + // if less than the boundaries + if (value < 0) { + return dev->min_pulse_width; + } + + *ret_val = (int) ((float)dev->min_pulse_width + ((float)value / ES08A_MAX_ANGLE) * ((float)dev->max_pulse_width - (float)dev->min_pulse_width)); + return UPM_SUCCESS; +} + +void es08a_set_min_pulse_width (es08a_context dev, int width){ + dev->min_pulse_width = width; +} + +void es08a_set_max_pulse_width (es08a_context dev, int width){ + dev->max_pulse_width = width; +} + +int es08a_get_min_pulse_width (es08a_context dev){ + return dev->min_pulse_width; +} + +int es08a_get_max_pulse_width (es08a_context dev){ + return dev->max_pulse_width; +} \ No newline at end of file diff --git a/src/servo/es08a.h b/src/servo/es08a.h new file mode 100644 index 00000000..e3d772b4 --- /dev/null +++ b/src/servo/es08a.h @@ -0,0 +1,137 @@ +/* + * Author: Yevgeniy Kiveisha + * Abhishek Malik + * Copyright (c) 2016 Intel Corporation. + * + * 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. + */ + +#ifndef ES08A_H_ +#define ES08A_H_ + +#include +#include +#include + +#include "upm.h" +#include "mraa/pwm.h" + +/** + * @library servo + * @sensor es08a + * @comname ES08A Servo + * @altname Grove Servo + * @type servos + * @man emax + * @web http://www.seeedstudio.com/wiki/Grove_-_Servo + * @con pwm + * @kit gsk + * + * @brief API for the ES08A Servo + * + * This module defines the ES08A interface for ES08A servos. + * Like other servos, the ES08A servo has a shaft that can be controlled + * by setting the desired angle. There are also routines for setting + * and getting the minimum and maximum pulse width as well as the + * maximum period. + * + * @image html es08a.jpg + * @snippet es08a.c Interesting + */ + +#define ES08A_MIN_PULSE_WIDTH 600 +#define ES08A_MAX_PULSE_WIDTH 2200 +#define ES08A_PERIOD 20000 +#define ES08A_MAX_ANGLE 180.0 + +/** + * device context + */ +typedef struct _es08a_context { + mraa_pwm_context pwm; + uint16_t servo_pin; + uint32_t max_pulse_width; + uint32_t min_pulse_width; +} *es08a_context; + +/** + * Instantiates a the servo at the given pin + * + * @param pin Servo pin number + * @param minPulseWidth Minimum pulse width, in microseconds + * @param maxPulseWidth Maximum pulse width, in microseconds + * @param waitAndDisablePwm If 1, PWM is enabled only during the + * setAngle() execution for a period of 1 second, and then turned back + * off. If 0, PWM remains on afterward. + */ + + +es08a_context es08a_init(int32_t pin, int32_t min_pulse_width, + int32_t max_pulse_width); + +/** + * Halts PWM for this servo and allows it to move freely. + */ +void es08a_halt(es08a_context dev); + +/** + * Sets the angle of the servo engine. + * + * @param angle Number between 0 and 180 + * @return 0 if successful, non-zero otherwise + */ +upm_result_t es08a_set_angle(es08a_context dev, int32_t angle); + +/* + * Calculating relative pulse time to the value. + * */ +upm_result_t es08a_calc_pulse_travelling(const es08a_context dev, + int32_t* ret_val, + int32_t value); + +/** + * Sets the minimum pulse width + * + * @param width Minimum HIGH signal width + */ +void es08a_set_min_pulse_width (es08a_context dev, int width); + +/** + * Sets the maximum pulse width + * + * @param width Maximum HIGH signal width + */ +void es08a_set_max_pulse_width (es08a_context dev, int width); + +/** + * Returns the minimum pulse width + * + * @return Minimum pulse width + */ +int es08a_get_min_pulse_width (es08a_context dev); + +/** + * Returns the maximum pulse width + * + * @return Maximum pulse width + */ +int es08a_get_max_pulse_width (es08a_context dev); + +#endif /* ES08A_H_ */ diff --git a/src/servo/es08a_fti.c b/src/servo/es08a_fti.c new file mode 100644 index 00000000..b635c11d --- /dev/null +++ b/src/servo/es08a_fti.c @@ -0,0 +1,89 @@ +/* + * Author: Yevgeniy Kiveisha + * Abhishek Malik + * Copyright (c) 2014 Intel Corporation. + * + * 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. + */ + +#include "es08a.h" + +#include "upm_fti.h" + +/** + * This file implements the Function Table Interface (FTI) for this sensor + */ + +const char upm_es08a_name[] = "ES08A"; +const char upm_es08a_description[] = "Grove Servo Motor"; +const upm_protocol_t upm_es08a_protocol[] = {UPM_PWM}; +const upm_sensor_t upm_es08a_category[] = {UPM_SERVO}; + +// forward declarations +const upm_sensor_descriptor_t upm_es08a_get_descriptor (); +const void* upm_es08a_get_ft(upm_sensor_t sensor_type); +void* upm_es08a_init_name(); +upm_result_t upm_es08a_set_angle(void* dev, int32_t angle); +void upm_es08a_close(void* dev); + +const upm_sensor_descriptor_t upm_es08a_get_descriptor (){ + upm_sensor_descriptor_t usd; + usd.name = upm_es08a_name; + usd.description = upm_es08a_description; + usd.protocol_size = 1; + usd.protocol = upm_es08a_protocol; + usd.category_size = 1; + usd.category = upm_es08a_category; + return usd; +} + +static const upm_sensor_ft ft = +{ + .upm_sensor_init_name = &upm_es08a_init_name, + .upm_sensor_close = &upm_es08a_close, + .upm_sensor_get_descriptor = &upm_es08a_get_descriptor +}; + +static const upm_servo_ft sft = +{ + .upm_servo_set_angle = &upm_es08a_set_angle +}; + +const void* upm_es08a_get_ft(upm_sensor_t sensor_type){ + if(sensor_type == UPM_SERVO){ + return &sft; + } + else if(sensor_type == UPM_SENSOR){ + return &ft; + } + return NULL; +} + +void* upm_es08a_init_name(){ + return NULL; +} + +void upm_es08a_close(void* dev){ + es08a_halt((es08a_context)dev); +} + +upm_result_t upm_es08a_set_angle(void* dev, int32_t angle){ + return es08a_set_angle((es08a_context)dev, angle); +} \ No newline at end of file diff --git a/src/servo/es9257.c b/src/servo/es9257.c new file mode 100644 index 00000000..d5fe0d7a --- /dev/null +++ b/src/servo/es9257.c @@ -0,0 +1,107 @@ +/* + * Author: Yevgeniy Kiveisha + * Abhishek Malik + * Copyright (c) 2014 Intel Corporation. + * + * 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. + */ +#include "es9257.h" + +es9257_context es9257_init(int32_t pin, int32_t min_pulse_width, + int32_t max_pulse_width){ + es9257_context dev = (es9257_context) malloc(sizeof(struct _es9257_context)); + + if(dev == NULL){ + printf("Unable to assign memory to the Servo motor structure"); + return NULL; + } + + dev->servo_pin = pin; + + // second is the min pulse width + dev->min_pulse_width = min_pulse_width; + // third is the max pulse width + dev->max_pulse_width = max_pulse_width; + + dev->pwm = mraa_pwm_init(dev->servo_pin); + if(dev->pwm == NULL){ + printf("Unable to initialize the PWM pin"); + } + + es9257_set_angle(dev, 0); + return dev; +} + +void es9257_halt(es9257_context dev){ + mraa_pwm_enable(dev->pwm, 0); + free(dev); +} + +upm_result_t es9257_set_angle(es9257_context dev, int32_t angle){ + + if(ES9257_MAX_ANGLE < angle || angle < 0){ + printf("The angle specified is either above the max angle or below 0"); + return UPM_ERROR_UNSPECIFIED; + } + printf("setting angle to: %d\n", angle); + + mraa_pwm_enable(dev->pwm, 1); + mraa_pwm_period_us(dev->pwm, ES9257_PERIOD); + int32_t val = 0; + + es9257_calc_pulse_travelling(dev, &val, angle); + mraa_pwm_pulsewidth_us(dev->pwm, val); + + upm_delay(1); + mraa_pwm_enable(dev->pwm, 0); + + return UPM_SUCCESS; +} + +upm_result_t es9257_calc_pulse_travelling(const es9257_context dev, + int32_t* ret_val, int32_t value){ + if (value > dev->max_pulse_width) { + return dev->max_pulse_width; + } + + // if less than the boundaries + if (value < 0) { + return dev->min_pulse_width; + } + + *ret_val = (int) ((float)dev->min_pulse_width + ((float)value / ES9257_MAX_ANGLE) * ((float)dev->max_pulse_width - (float)dev->min_pulse_width)); + return UPM_SUCCESS; +} + +void es9257_set_min_pulse_width (es9257_context dev, int width){ + dev->min_pulse_width = width; +} + +void es9257_set_max_pulse_width (es9257_context dev, int width){ + dev->max_pulse_width = width; +} + +int es9257_get_min_pulse_width (es9257_context dev){ + return dev->min_pulse_width; +} + +int es9257_get_max_pulse_width (es9257_context dev){ + return dev->max_pulse_width; +} diff --git a/src/servo/es9257.h b/src/servo/es9257.h new file mode 100644 index 00000000..c8722974 --- /dev/null +++ b/src/servo/es9257.h @@ -0,0 +1,137 @@ +/* + * Author: Yevgeniy Kiveisha + * Abhishek Malik + * Copyright (c) 2016 Intel Corporation. + * + * 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. + */ + +#ifndef ES9257_H_ +#define ES9257_H_ + +#include +#include +#include + +#include "upm.h" +#include "mraa/pwm.h" + +/** + * @library servo + * @sensor es9257 + * @comname ES9257 Servo + * @altname Grove Servo + * @type servos + * @man emax + * @web http://www.seeedstudio.com/wiki/Grove_-_Servo + * @con pwm + * @kit gsk + * + * @brief API for the ES9257 Servo + * + * This module defines the ES9257 interface for ES9257 servos. + * Like other servos, the ES9257 servo has a shaft that can be controlled + * by setting the desired angle. There are also routines for setting + * and getting the minimum and maximum pulse width as well as the + * maximum period. + * + * @image html es9257.jpg + * @snippet es9257.c Interesting + */ + +#define ES9257_MIN_PULSE_WIDTH 600 +#define ES9257_MAX_PULSE_WIDTH 2200 +#define ES9257_PERIOD 20000 +#define ES9257_MAX_ANGLE 180.0 + +/* + * device context + */ +typedef struct _es9257_context { + mraa_pwm_context pwm; + uint16_t servo_pin; + uint32_t max_pulse_width; + uint32_t min_pulse_width; +} *es9257_context; + +/** + * Instantiates a the servo at the given pin + * + * @param pin Servo pin number + * @param minPulseWidth Minimum pulse width, in microseconds + * @param maxPulseWidth Maximum pulse width, in microseconds + * @param waitAndDisablePwm If 1, PWM is enabled only during the + * setAngle() execution for a period of 1 second, and then turned back + * off. If 0, PWM remains on afterward. + */ + + +es9257_context es9257_init(int32_t pin, int32_t min_pulse_width, + int32_t max_pulse_width); + +/** + * Halts PWM for this servo and allows it to move freely. + */ +void es9257_halt(es9257_context dev); + +/** + * Sets the angle of the servo engine. + * + * @param angle Number between 0 and 180 + * @return 0 if successful, non-zero otherwise + */ +upm_result_t es9257_set_angle(es9257_context dev, int32_t angle); + +/* + * Calculating relative pulse time to the value. + * */ +upm_result_t es9257_calc_pulse_travelling(const es9257_context dev, + int32_t* ret_val, + int32_t value); + +/** + * Sets the minimum pulse width + * + * @param width Minimum HIGH signal width + */ +void es9257_set_min_pulse_width (es9257_context dev, int width); + +/** + * Sets the maximum pulse width + * + * @param width Maximum HIGH signal width + */ +void es9257_set_max_pulse_width (es9257_context dev, int width); + +/** + * Returns the minimum pulse width + * + * @return Minimum pulse width + */ +int es9257_get_min_pulse_width (es9257_context dev); + +/** + * Returns the maximum pulse width + * + * @return Maximum pulse width + */ +int es9257_get_max_pulse_width (es9257_context dev); + +#endif /* ES9257_H_ */ \ No newline at end of file diff --git a/src/servo/es9257_fti.c b/src/servo/es9257_fti.c new file mode 100644 index 00000000..2599987b --- /dev/null +++ b/src/servo/es9257_fti.c @@ -0,0 +1,89 @@ +/* + * Author: Yevgeniy Kiveisha + * Abhishek Malik + * Copyright (c) 2014 Intel Corporation. + * + * 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. + */ + +#include "es9257.h" + +#include "upm_fti.h" + +/** + * This file implements the Function Table Interface (FTI) for this sensor + */ + +const char upm_es9257_name[] = "ES9257"; +const char upm_es9257_description[] = "Grove Servo Motor"; +const upm_protocol_t upm_es9257_protocol[] = {UPM_PWM}; +const upm_sensor_t upm_es9257_category[] = {UPM_SERVO}; + +// forward declarations +const upm_sensor_descriptor_t upm_es9257_get_descriptor (); +const void* upm_es9257_get_ft(upm_sensor_t sensor_type); +void* upm_es9257_init_name(); +upm_result_t upm_es9257_set_angle(void* dev, int32_t angle); +void upm_es9257_close(void* dev); + +const upm_sensor_descriptor_t upm_es9257_get_descriptor (){ + upm_sensor_descriptor_t usd; + usd.name = upm_es9257_name; + usd.description = upm_es9257_description; + usd.protocol_size = 1; + usd.protocol = upm_es9257_protocol; + usd.category_size = 1; + usd.category = upm_es9257_category; + return usd; +} + +static const upm_sensor_ft ft = +{ + .upm_sensor_init_name = &upm_es9257_init_name, + .upm_sensor_close = &upm_es9257_close, + .upm_sensor_get_descriptor = &upm_es9257_get_descriptor +}; + +static const upm_servo_ft sft = +{ + .upm_servo_set_angle = &upm_es9257_set_angle +}; + +const void* upm_es9257_get_ft(upm_sensor_t sensor_type){ + if(sensor_type == UPM_SERVO){ + return &sft; + } + else if(sensor_type == UPM_SENSOR){ + return &ft; + } + return NULL; +} + +void* upm_es9257_init_name(){ + return NULL; +} + +void upm_es9257_close(void* dev){ + es9257_halt((es9257_context)dev); +} + +upm_result_t upm_es9257_set_angle(void* dev, int32_t angle){ + return es9257_set_angle((es9257_context)dev, angle); +} \ No newline at end of file