src/resource/resource_motor_driver_L298N.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 <stdlib.h>
  20 #include <peripheral_io.h>
  21 #include "log.h"
  22 #include "resource/resource_PCA9685.h"
  23 #include "resource/resource_motor_driver_L298N.h"
  24
  25 typedef enum {
  26         MOTOR_STATE_NONE,
  27         MOTOR_STATE_CONFIGURED,
  28         MOTOR_STATE_STOP,
  29         MOTOR_STATE_FORWARD,
  30         MOTOR_STATE_BACKWARD,
  31 } motor_state_e;
  32
  33 typedef struct __motor_driver_s {
  34         unsigned int pin_1;
  35         unsigned int pin_2;
  36         unsigned int en_ch;
  37         motor_state_e motor_state;
  38         peripheral_gpio_h pin1_h;
  39         peripheral_gpio_h pin2_h;
  40 } motor_driver_s;
  41
  42 static motor_driver_s g_md_h[MOTOR_ID_MAX] = {
  43         {0, 0, 0, MOTOR_STATE_NONE, NULL, NULL},
  44 };
  45
  46
  47 /* see Principle section in http://wiki.sunfounder.cc/index.php?title=Motor_Driver_Module-L298N */
  48
  49 static int __motor_brake_n_stop_by_id(motor_id_e id)
  50 {
  51         int ret = PERIPHERAL_ERROR_NONE;
  52         int motor1_v = 0;
  53         int motor2_v = 0;
  54
  55         if (g_md_h[id].motor_state <= MOTOR_STATE_CONFIGURED) {
  56                 _E("motor[%d] are not initialized - state(%d)",
  57                         id, g_md_h[id].motor_state);
  58                 return -1;
  59         }
  60
  61         if (g_md_h[id].motor_state == MOTOR_STATE_STOP) {
  62                 _D("motor[%d] is already stopped", id);
  63                 return 0;
  64         }
  65
  66         if (g_md_h[id].motor_state == MOTOR_STATE_FORWARD) {
  67                 motor1_v = 0;
  68                 motor2_v = 0;
  69         } else if (g_md_h[id].motor_state == MOTOR_STATE_BACKWARD) {
  70                 motor1_v = 1;
  71                 motor2_v = 1;
  72         }
  73
  74         /* Brake DC motor */
  75         ret = peripheral_gpio_write(g_md_h[id].pin1_h, motor1_v);
  76         if (ret != PERIPHERAL_ERROR_NONE) {
  77                 _E("Failed to set value[%d] Motor[%d] pin 1", motor1_v, id);
  78                 return -1;
  79         }
  80
  81         ret = peripheral_gpio_write(g_md_h[id].pin2_h, motor2_v);
  82         if (ret != PERIPHERAL_ERROR_NONE) {
  83                 _E("Failed to set value[%d] Motor[%d] pin 2", motor2_v, id);
  84                 return -1;
  85         }
  86
  87         /* set stop DC motor */
  88         // need to stop motor or not?, it may stop motor to free running
  89         resource_pca9685_set_value_to_channel(g_md_h[id].en_ch, 0, 0);
  90
  91         g_md_h[id].motor_state = MOTOR_STATE_STOP;
  92
  93         return 0;
  94 }
  95
  96 static int __set_default_configuration_by_id(motor_id_e id)
  97 {
  98         unsigned int pin_1, pin_2, en_ch;
  99
 100         switch (id) {
 101         case MOTOR_ID_1:
 102                 pin_1 = DEFAULT_MOTOR1_PIN1;
 103                 pin_2 = DEFAULT_MOTOR1_PIN2;
 104                 en_ch = DEFAULT_MOTOR1_EN_CH;
 105         break;
 106         case MOTOR_ID_2:
 107                 pin_1 = DEFAULT_MOTOR2_PIN1;
 108                 pin_2 = DEFAULT_MOTOR2_PIN2;
 109                 en_ch = DEFAULT_MOTOR2_EN_CH;
 110         break;
 111         case MOTOR_ID_3:
 112                 pin_1 = DEFAULT_MOTOR3_PIN1;
 113                 pin_2 = DEFAULT_MOTOR3_PIN2;
 114                 en_ch = DEFAULT_MOTOR3_EN_CH;
 115         break;
 116         case MOTOR_ID_4:
 117                 pin_1 = DEFAULT_MOTOR4_PIN1;
 118                 pin_2 = DEFAULT_MOTOR4_PIN2;
 119                 en_ch = DEFAULT_MOTOR4_EN_CH;
 120         break;
 121         case MOTOR_ID_MAX:
 122         default:
 123                 _E("Unkwon ID[%d]", id);
 124                 return -1;
 125         break;
 126         }
 127
 128         g_md_h[id].pin_1 = pin_1;
 129         g_md_h[id].pin_2 = pin_2;
 130         g_md_h[id].en_ch = en_ch;
 131         g_md_h[id].motor_state = MOTOR_STATE_CONFIGURED;
 132
 133         return 0;
 134 }
 135
 136 static int __fini_motor_by_id(motor_id_e id)
 137 {
 138         retv_if(id == MOTOR_ID_MAX, -1);
 139
 140         if (g_md_h[id].motor_state <= MOTOR_STATE_CONFIGURED)
 141                 return 0;
 142
 143         if (g_md_h[id].motor_state > MOTOR_STATE_STOP)
 144                 __motor_brake_n_stop_by_id(id);
 145
 146         resource_pca9685_fini(g_md_h[id].en_ch);
 147
 148         if (g_md_h[id].pin1_h) {
 149                 peripheral_gpio_close(g_md_h[id].pin1_h);
 150                 g_md_h[id].pin1_h = NULL;
 151         }
 152
 153         if (g_md_h[id].pin2_h) {
 154                 peripheral_gpio_close(g_md_h[id].pin2_h);
 155                 g_md_h[id].pin2_h = NULL;
 156         }
 157
 158         g_md_h[id].motor_state = MOTOR_STATE_CONFIGURED;
 159
 160         return 0;
 161 }
 162
 163 static int __init_motor_by_id(motor_id_e id)
 164 {
 165         int ret = 0;
 166
 167         retv_if(id == MOTOR_ID_MAX, -1);
 168
 169         if (g_md_h[id].motor_state == MOTOR_STATE_NONE)
 170                 __set_default_configuration_by_id(id);
 171
 172         ret = resource_pca9685_init(g_md_h[id].en_ch);
 173         if (ret) {
 174                 _E("failed to init PCA9685");
 175                 return -1;
 176         }
 177
 178         /* open pins for Motor */
 179         ret = peripheral_gpio_open(g_md_h[id].pin_1, &g_md_h[id].pin1_h);
 180         if (ret == PERIPHERAL_ERROR_NONE)
 181                 peripheral_gpio_set_direction(g_md_h[id].pin1_h,
 182                         PERIPHERAL_GPIO_DIRECTION_OUT_INITIALLY_LOW);
 183         else {
 184                 _E("failed to open Motor[%d] gpio pin1[%u]", id, g_md_h[id].pin_1);
 185                 goto ERROR;
 186         }
 187
 188         ret = peripheral_gpio_open(g_md_h[id].pin_2, &g_md_h[id].pin2_h);
 189         if (ret == PERIPHERAL_ERROR_NONE)
 190                 peripheral_gpio_set_direction(g_md_h[id].pin2_h,
 191                         PERIPHERAL_GPIO_DIRECTION_OUT_INITIALLY_LOW);
 192         else {
 193                 _E("failed to open Motor[%d] gpio pin2[%u]", id, g_md_h[id].pin_2);
 194                 goto ERROR;
 195         }
 196
 197         g_md_h[id].motor_state = MOTOR_STATE_STOP;
 198
 199         return 0;
 200
 201 ERROR:
 202         resource_pca9685_fini(g_md_h[id].en_ch);
 203
 204         if (g_md_h[id].pin1_h) {
 205                 peripheral_gpio_close(g_md_h[id].pin1_h);
 206                 g_md_h[id].pin1_h = NULL;
 207         }
 208
 209         if (g_md_h[id].pin2_h) {
 210                 peripheral_gpio_close(g_md_h[id].pin2_h);
 211                 g_md_h[id].pin2_h = NULL;
 212         }
 213
 214         return -1;
 215 }
 216
 217 void resource_close_motor_driver_L298N(motor_id_e id)
 218 {
 219         __fini_motor_by_id(id);
 220         return;
 221 }
 222
 223 void resource_close_motor_driver_L298N_all(void)
 224 {
 225         int i;
 226         for (i = MOTOR_ID_1; i < MOTOR_ID_MAX; i++)
 227                 __fini_motor_by_id(i);
 228
 229         return;
 230 }
 231
 232 int resource_set_motor_driver_L298N_configuration(motor_id_e id,
 233         unsigned int pin1, unsigned int pin2, unsigned en_ch)
 234 {
 235
 236         if (g_md_h[id].motor_state > MOTOR_STATE_CONFIGURED) {
 237                 _E("cannot set configuration motor[%d] in this state[%d]",
 238                         id, g_md_h[id].motor_state);
 239                 return -1;
 240         }
 241
 242         g_md_h[id].pin_1 = pin1;
 243         g_md_h[id].pin_2 = pin2;
 244         g_md_h[id].en_ch = en_ch;
 245         g_md_h[id].motor_state = MOTOR_STATE_CONFIGURED;
 246
 247         return 0;
 248 }
 249
 250 int resource_set_motor_driver_L298N_speed(motor_id_e id, int speed)
 251 {
 252         int ret = 0;
 253         const int value_max = 4095;
 254         int value = 0;
 255         int e_state = MOTOR_STATE_NONE;
 256         int motor_v_1 = 0;
 257         int motor_v_2 = 0;
 258
 259         if (g_md_h[id].motor_state <= MOTOR_STATE_CONFIGURED) {
 260                 ret = __init_motor_by_id(id);
 261                 if (ret) {
 262                         _E("failed to __init_motor_by_id()");
 263                         return -1;
 264                 }
 265         }
 266
 267         value = abs(speed);
 268
 269         if (value > value_max) {
 270                 value = value_max;
 271                 _D("max speed is %d", value_max);
 272         }
 273         _D("set speed %d", value);
 274
 275         if (speed == 0) {
 276                 /* brake and stop */
 277                 ret = __motor_brake_n_stop_by_id(id);
 278                 if (ret) {
 279                         _E("failed to stop motor[%d]", id);
 280                         return -1;
 281                 }
 282                 return 0; /* done */
 283         }
 284
 285         if (speed > 0)
 286                 e_state = MOTOR_STATE_FORWARD; /* will be set forward */
 287         else
 288                 e_state = MOTOR_STATE_BACKWARD; /* will be set backward */
 289
 290         if (g_md_h[id].motor_state == e_state)
 291                 goto SET_SPEED;
 292         else {
 293                 /* brake and stop */
 294                 ret = __motor_brake_n_stop_by_id(id);
 295                 if (ret) {
 296                         _E("failed to stop motor[%d]", id);
 297                         return -1;
 298                 }
 299         }
 300
 301         switch (e_state) {
 302         case MOTOR_STATE_FORWARD:
 303                 motor_v_1 = 1;
 304                 motor_v_2 = 0;
 305                 break;
 306         case MOTOR_STATE_BACKWARD:
 307                 motor_v_1 = 0;
 308                 motor_v_2 = 1;
 309                 break;
 310         }
 311         ret = peripheral_gpio_write(g_md_h[id].pin1_h, motor_v_1);
 312         if (ret != PERIPHERAL_ERROR_NONE) {
 313                 _E("failed to set value[%d] Motor[%d] pin 1", motor_v_1, id);
 314                 return -1;
 315         }
 316
 317         ret = peripheral_gpio_write(g_md_h[id].pin2_h, motor_v_2);
 318         if (ret != PERIPHERAL_ERROR_NONE) {
 319                 _E("failed to set value[%d] Motor[%d] pin 2", motor_v_2, id);
 320                 return -1;
 321         }
 322
 323 SET_SPEED:
 324         ret = resource_pca9685_set_value_to_channel(g_md_h[id].en_ch, 0, value);
 325         if (ret) {
 326                 _E("failed to set speed - %d", speed);
 327                 return -1;
 328         }
 329
 330         g_md_h[id].motor_state = e_state;
 331
 332         return 0;
 333 }