src/resource/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_CHANNER (0)
  23
  24 static peripheral_pwm_h g_pwm_h;
  25
  26 void resource_close_servo_motor(void)
  27 {
  28         if (g_pwm_h) {
  29                 peripheral_pwm_close(g_pwm_h);
  30                 g_pwm_h = NULL;
  31         }
  32 }
  33
  34 int resource_set_servo_motor_value(double duty_cycle_ms)
  35 {
  36         int ret = 0;
  37
  38         if (!g_pwm_h) {
  39                 ret = peripheral_pwm_open(0, SERVO_MOTOR_CHANNER, &g_pwm_h);
  40                 if (ret != PERIPHERAL_ERROR_NONE) {
  41                         _E("failed to open servo motor with ch : %s", get_error_message(ret));
  42                         return -1;
  43                 }
  44         }
  45
  46         ret = peripheral_pwm_set_period(g_pwm_h, 20 * 1000 * 1000);
  47         if (ret != PERIPHERAL_ERROR_NONE) {
  48                 _E("failed to set period : %s", get_error_message(ret));
  49                 return -1;
  50         }
  51
  52         ret = peripheral_pwm_set_duty_cycle(g_pwm_h, duty_cycle_ms * 1000 * 1000);
  53         if (ret != PERIPHERAL_ERROR_NONE) {
  54                 _E("failed to set duty cycle : %s", get_error_message(ret));
  55                 return -1;
  56         }
  57
  58         ret = peripheral_pwm_set_enabled(g_pwm_h, true);
  59         if (ret != PERIPHERAL_ERROR_NONE) {
  60                 _E("failed to enable : %s", get_error_message(ret));
  61                 return -1;
  62         }
  63
  64         return 0;
  65 }