src/resource/resource_servo_motor.c

   1 /*
   2  * Copyright (c) 2017 Samsung Electronics Co., Ltd.
   3  *
   4  * Contact: Jeonghoon Park <jh1979.park@samsung.com>
   5  *
   6  * Licensed under the Flora License, Version 1.1 (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://floralicense.org/license/
  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 "log.h"
  20 #include "resource/resource_PCA9685.h"
  21
  22 #define SERVO_MOTOR_MAX PCA9685_CH_MAX
  23
  24 static int servo_motor_index[SERVO_MOTOR_MAX + 1] = {0, };
  25
  26 static int resource_servo_motor_init(unsigned int ch)
  27 {
  28         int ret = 0;
  29         ret = resource_pca9685_init(ch);
  30         if (ret) {
  31                 _E("failed to init PCA9685 with ch[%u]", ch);
  32                 return -1;
  33         }
  34         servo_motor_index[ch] = 1;
  35
  36         return 0;
  37 }
  38
  39 void resource_close_servo_motor(unsigned int ch)
  40 {
  41         if (servo_motor_index[ch] == 1) {
  42                 resource_pca9685_fini(ch);
  43                 servo_motor_index[ch] = 0;
  44         }
  45
  46         return;
  47 }
  48
  49 void resource_close_servo_motor_all(void)
  50 {
  51         unsigned int i;
  52
  53         for (i = 0 ; i <= SERVO_MOTOR_MAX; i++)
  54                 resource_close_servo_motor(i);
  55
  56         return;
  57 }
  58
  59 int resource_set_servo_motor_value(unsigned int motor_id, int value)
  60 {
  61         int ret = 0;
  62
  63         if (motor_id > SERVO_MOTOR_MAX)
  64                 return -1;
  65
  66         if (servo_motor_index[motor_id] == 0) {
  67                 ret = resource_servo_motor_init(motor_id);
  68                 if (ret)
  69                         return -1;
  70         }
  71
  72         ret = resource_pca9685_set_value_to_channel(motor_id, 0, value);
  73
  74         return ret;
  75 }