src/resource_servo_motor.c

   1 /*
   2  * Copyright (c) 2018 Samsung Electronics Co., Ltd.
   3  *
   4  * Contact: Jin Yoon <jinny.yoon@samsung.com>
   5  *
   6  * Licensed under the Apache License, Version 2.0 (the "License");
   7  * you may not use this file except in compliance with the License.
   8  * You may obtain a copy of the License at
   9  *
  10  * http://www.apache.org/licenses/LICENSE-2.0
  11  *
  12  * Unless required by applicable law or agreed to in writing, software
  13  * distributed under the License is distributed on an AS IS BASIS,
  14  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  15  * See the License for the specific language governing permissions and
  16  * limitations under the License.
  17  */
  18
  19 #include <peripheral_io.h>
  20 #include "log.h"
  21
  22 #define SERVO_MOTOR_CHANNEL (0)
  23 #define SERVO_MOTOR_DEFAULT_PERIOD 20.0
  24
  25 static peripheral_pwm_h g_pwm_h;
  26
  27 void resource_close_servo_motor(void)
  28 {
  29         if (g_pwm_h) {
  30                 peripheral_pwm_close(g_pwm_h);
  31                 g_pwm_h = NULL;
  32         }
  33 }
  34
  35 int resource_set_servo_motor_value(double duty_cycle_ms)
  36 {
  37         int ret = 0;
  38
  39         if (duty_cycle_ms >= SERVO_MOTOR_DEFAULT_PERIOD) {
  40                 _E("Too large duty cycle");
  41                 return -1;
  42         }
  43
  44         if (!g_pwm_h) {
  45                 ret = peripheral_pwm_open(0, SERVO_MOTOR_CHANNEL, &g_pwm_h);
  46                 if (ret != PERIPHERAL_ERROR_NONE) {
  47                         _E("failed to open servo motor with ch : %s", get_error_message(ret));
  48                         return -1;
  49                 }
  50         }
  51
  52         ret = peripheral_pwm_set_period(g_pwm_h, SERVO_MOTOR_DEFAULT_PERIOD * 1000 * 1000);
  53         if (ret != PERIPHERAL_ERROR_NONE) {
  54                 _E("failed to set period : %s", get_error_message(ret));
  55                 return -1;
  56         }
  57
  58         ret = peripheral_pwm_set_duty_cycle(g_pwm_h, duty_cycle_ms * 1000 * 1000);
  59         if (ret != PERIPHERAL_ERROR_NONE) {
  60                 _E("failed to set duty cycle : %s", get_error_message(ret));
  61                 return -1;
  62         }
  63
  64         ret = peripheral_pwm_set_enabled(g_pwm_h, true);
  65         if (ret != PERIPHERAL_ERROR_NONE) {
  66                 _E("failed to enable : %s", get_error_message(ret));
  67                 return -1;
  68         }
  69
  70         return 0;
  71 }