src/resource_servo_motor_sg90.c

   1  /*
   2  * Copyright (c) 2018 Samsung Electronics Co., Ltd.
   3  *
   4  * Licensed under the Apache License, Version 2.0 (the "License");
   5  * you may not use this file except in compliance with the License.
   6  * You may obtain a copy of the License at
   7  *
   8  * http://www.apache.org/licenses/LICENSE-2.0
   9  *
  10  * Unless required by applicable law or agreed to in writing, software
  11  * distributed under the License is distributed on an AS IS BASIS,
  12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  13  * See the License for the specific language governing permissions and
  14  * limitations under the License.
  15  */
  16
  17 #include <peripheral_io.h>
  18 #include <glib.h>
  19 #include "log.h"
  20
  21 #define ENABLE_SERVO_TIMEOUT
  22
  23 /* This value is only for Servo Motor - SG90 */
  24 #define SERVO_MOTOR_DEFAULT_PERIOD 20.0
  25 #define SERVO_MOTOR_DUTY_CYCLE_COUNTER_CLOCKWISE 0.9
  26 #define SERVO_MOTOR_DUTY_CYCLE_CLOCKWISE 2.4
  27
  28 #ifdef ENABLE_SERVO_TIMEOUT
  29 #define SERVO_TIMEOUT_INTERVAL 150
  30 #endif /* ENABLE_SERVO_TIMEOUT */
  31
  32 /* This APIs are only made for Eagleye530s */
  33 /* There are 2 pins for PWM */
  34 static peripheral_pwm_h g_pwm0_h;
  35 static peripheral_pwm_h g_pwm2_h;
  36
  37 #ifdef ENABLE_SERVO_TIMEOUT
  38 static guint g_timer_id0;
  39 static guint g_timer_id2;
  40
  41 static gboolean __timeout_cb(void *data)
  42 {
  43         int channel = GPOINTER_TO_INT(data);
  44
  45         _D("pwm channel[%d] disable", channel);
  46         if (channel == 0 && g_pwm0_h) {
  47                 peripheral_pwm_set_enabled(g_pwm0_h, false);
  48                 g_timer_id0 = 0;
  49         } else if (channel == 2 && g_pwm2_h) {
  50                 peripheral_pwm_set_enabled(g_pwm2_h, false);
  51                 g_timer_id2 = 0;
  52         }
  53         return FALSE;
  54 }
  55
  56 static void __remove_timeout_cb(int channel)
  57 {
  58         if (channel == 0) {
  59                 if (g_timer_id0) {
  60                         g_source_remove(g_timer_id0);
  61                         g_timer_id0 = 0;
  62                 }
  63         } else if (channel == 2) {
  64                 if (g_timer_id2) {
  65                         g_source_remove(g_timer_id0);
  66                         g_timer_id2 = 0;
  67                 }
  68         }
  69 }
  70
  71 static void __add_timeout_cb(int channel)
  72 {
  73         if (channel == 0) {
  74                 g_timer_id0 = g_timeout_add(SERVO_TIMEOUT_INTERVAL,
  75                                                 __timeout_cb, GINT_TO_POINTER(channel));
  76         } else if (channel == 2) {
  77                 g_timer_id2 = g_timeout_add(SERVO_TIMEOUT_INTERVAL,
  78                                                 __timeout_cb, GINT_TO_POINTER(channel));
  79         }
  80 }
  81 #endif /* ENABLE_SERVO_TIMEOUT */
  82
  83 void resource_close_servo_motor(int channel)
  84 {
  85 #ifdef ENABLE_SERVO_TIMEOUT
  86         __remove_timeout_cb(channel);
  87 #endif /* ENABLE_SERVO_TIMEOUT */
  88
  89         if (channel == 0 && g_pwm0_h) {
  90                 peripheral_pwm_close(g_pwm0_h);
  91                 g_pwm0_h = NULL;
  92         } else if (channel == 2 && g_pwm2_h) {
  93                 peripheral_pwm_close(g_pwm2_h);
  94                 g_pwm2_h = NULL;
  95         }
  96 }
  97
  98 int resource_set_servo_motor_sg90_value(int channel, double duty_cycle_ms)
  99 {
 100         int ret = 0;
 101         peripheral_pwm_h *pwm_h = NULL;
 102
 103         if (duty_cycle_ms >= SERVO_MOTOR_DEFAULT_PERIOD) {
 104                 _E("Too large duty cycle");
 105                 return -1;
 106         }
 107
 108 #ifdef ENABLE_SERVO_TIMEOUT
 109         __remove_timeout_cb(channel);
 110 #endif /* ENABLE_SERVO_TIMEOUT */
 111
 112         if (channel == 0) {
 113                 pwm_h = &g_pwm0_h;
 114         } else if (channel == 2) {
 115                 pwm_h = &g_pwm2_h;
 116         } else {
 117                 _E("unsupported channel - %d", channel);
 118                 return -1;
 119         }
 120
 121         if (!*pwm_h) {
 122                 ret = peripheral_pwm_open(0, channel, pwm_h);
 123                 if (ret != PERIPHERAL_ERROR_NONE) {
 124                         _E("failed to open servo motor with channel(%d) : %s", channel, get_error_message(ret));
 125                         return -1;
 126                 }
 127         }
 128
 129         ret = peripheral_pwm_set_period(*pwm_h, SERVO_MOTOR_DEFAULT_PERIOD * 1000 * 1000);
 130         if (ret != PERIPHERAL_ERROR_NONE) {
 131                 _E("failed to set period : %s", get_error_message(ret));
 132                 return -1;
 133         }
 134
 135         ret = peripheral_pwm_set_duty_cycle(*pwm_h, duty_cycle_ms * 1000 * 1000);
 136         if (ret != PERIPHERAL_ERROR_NONE) {
 137                 _E("failed to set duty cycle : %s", get_error_message(ret));
 138                 return -1;
 139         }
 140
 141         ret = peripheral_pwm_set_enabled(*pwm_h, true);
 142         if (ret != PERIPHERAL_ERROR_NONE) {
 143                 _E("failed to enable : %s", get_error_message(ret));
 144                 return -1;
 145         }
 146
 147 #ifdef ENABLE_SERVO_TIMEOUT
 148         __add_timeout_cb(channel);
 149 #endif /* ENABLE_SERVO_TIMEOUT */
 150
 151         return 0;
 152 }
 153
 154 int resource_rotate_servo_motor_by_percent(int channel, double percent)
 155 {
 156         int ret = 0;
 157         double duty_cycle = 0.0f;
 158
 159         duty_cycle = ((SERVO_MOTOR_DUTY_CYCLE_CLOCKWISE - SERVO_MOTOR_DUTY_CYCLE_COUNTER_CLOCKWISE)
 160                         * (percent / 100.0))
 161                         + SERVO_MOTOR_DUTY_CYCLE_COUNTER_CLOCKWISE;
 162
 163         ret = resource_set_servo_motor_sg90_value(channel, duty_cycle);
 164
 165         return ret;
 166 }