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         FUNCTION_START;
  99
 100         unsigned int pin_1, pin_2, en_ch;
 101
 102         switch (id) {
 103         case MOTOR_ID_1:
 104                 pin_1 = DEFAULT_MOTOR1_PIN1;
 105                 pin_2 = DEFAULT_MOTOR1_PIN2;
 106                 en_ch = DEFAULT_MOTOR1_EN_CH;
 107         break;
 108         case MOTOR_ID_2:
 109                 pin_1 = DEFAULT_MOTOR2_PIN1;
 110                 pin_2 = DEFAULT_MOTOR2_PIN2;
 111                 en_ch = DEFAULT_MOTOR2_EN_CH;
 112         break;
 113         case MOTOR_ID_3:
 114                 pin_1 = DEFAULT_MOTOR3_PIN1;
 115                 pin_2 = DEFAULT_MOTOR3_PIN2;
 116                 en_ch = DEFAULT_MOTOR3_EN_CH;
 117         break;
 118         case MOTOR_ID_4:
 119                 pin_1 = DEFAULT_MOTOR4_PIN1;
 120                 pin_2 = DEFAULT_MOTOR4_PIN2;
 121                 en_ch = DEFAULT_MOTOR4_EN_CH;
 122         break;
 123         case MOTOR_ID_MAX:
 124         default:
 125                 _E("Unkwon ID[%d]", id);
 126                 return -1;
 127         break;
 128         }
 129
 130         g_md_h[id].pin_1 = pin_1;
 131         g_md_h[id].pin_2 = pin_2;
 132         g_md_h[id].en_ch = en_ch;
 133         g_md_h[id].motor_state = MOTOR_STATE_CONFIGURED;
 134
 135         FUNCTION_END;
 136         return 0;
 137 }
 138
 139 static int __fini_motor_by_id(motor_id_e id)
 140 {
 141         retv_if(id == MOTOR_ID_MAX, -1);
 142
 143         if (g_md_h[id].motor_state <= MOTOR_STATE_CONFIGURED)
 144                 return 0;
 145
 146         if (g_md_h[id].motor_state > MOTOR_STATE_STOP)
 147                 __motor_brake_n_stop_by_id(id);
 148
 149         resource_pca9685_fini(g_md_h[id].en_ch);
 150
 151         if (g_md_h[id].pin1_h) {
 152                 peripheral_gpio_close(g_md_h[id].pin1_h);
 153                 g_md_h[id].pin1_h = NULL;
 154         }
 155
 156         if (g_md_h[id].pin2_h) {
 157                 peripheral_gpio_close(g_md_h[id].pin2_h);
 158                 g_md_h[id].pin2_h = NULL;
 159         }
 160
 161         g_md_h[id].motor_state = MOTOR_STATE_CONFIGURED;
 162
 163         return 0;
 164 }
 165
 166 static int __init_motor_by_id(motor_id_e id)
 167 {
 168         int ret = 0;
 169
 170         retv_if(id == MOTOR_ID_MAX, -1);
 171
 172         if (g_md_h[id].motor_state == MOTOR_STATE_NONE)
 173                 __set_default_configuration_by_id(id);
 174
 175         ret = resource_pca9685_init(g_md_h[id].en_ch, 60);
 176         if (ret) {
 177                 _E("failed to init PCA9685");
 178                 return -1;
 179         }
 180
 181         /* open pins for Motor */
 182         ret = peripheral_gpio_open(g_md_h[id].pin_1, &g_md_h[id].pin1_h);
 183         if (ret == PERIPHERAL_ERROR_NONE)
 184                 peripheral_gpio_set_direction(g_md_h[id].pin1_h,
 185                         PERIPHERAL_GPIO_DIRECTION_OUT_INITIALLY_LOW);
 186         else {
 187                 _E("failed to open Motor[%d] gpio pin1[%u]. Error: %s", id, g_md_h[id].pin_1, get_error_message(ret));
 188                 goto ERROR;
 189         }
 190
 191         ret = peripheral_gpio_open(g_md_h[id].pin_2, &g_md_h[id].pin2_h);
 192         if (ret == PERIPHERAL_ERROR_NONE)
 193                 peripheral_gpio_set_direction(g_md_h[id].pin2_h,
 194                         PERIPHERAL_GPIO_DIRECTION_OUT_INITIALLY_LOW);
 195         else {
 196                 _E("failed to open Motor[%d] gpio pin2[%u]. Error: %s", id, g_md_h[id].pin_2, get_error_message(ret));
 197                 goto ERROR;
 198         }
 199
 200         g_md_h[id].motor_state = MOTOR_STATE_STOP;
 201
 202         return 0;
 203
 204 ERROR:
 205         resource_pca9685_fini(g_md_h[id].en_ch);
 206
 207         _E("Motor init error: %s", get_error_message(ret));
 208
 209         if (g_md_h[id].pin1_h) {
 210                 peripheral_gpio_close(g_md_h[id].pin1_h);
 211                 g_md_h[id].pin1_h = NULL;
 212         }
 213
 214         if (g_md_h[id].pin2_h) {
 215                 peripheral_gpio_close(g_md_h[id].pin2_h);
 216                 g_md_h[id].pin2_h = NULL;
 217         }
 218
 219         return -1;
 220 }
 221
 222 void resource_close_motor_driver_L298N(motor_id_e id)
 223 {
 224         __fini_motor_by_id(id);
 225         return;
 226 }
 227
 228 void resource_close_motor_driver_L298N_all(void)
 229 {
 230         int i;
 231         for (i = MOTOR_ID_1; i < MOTOR_ID_MAX; i++)
 232                 __fini_motor_by_id(i);
 233
 234         return;
 235 }
 236
 237 int resource_set_motor_driver_L298N_configuration(motor_id_e id,
 238         unsigned int pin1, unsigned int pin2, unsigned en_ch)
 239 {
 240
 241         if (g_md_h[id].motor_state > MOTOR_STATE_CONFIGURED) {
 242                 _E("cannot set configuration motor[%d] in this state[%d]",
 243                         id, g_md_h[id].motor_state);
 244                 return -1;
 245         }
 246
 247         g_md_h[id].pin_1 = pin1;
 248         g_md_h[id].pin_2 = pin2;
 249         g_md_h[id].en_ch = en_ch;
 250         g_md_h[id].motor_state = MOTOR_STATE_CONFIGURED;
 251
 252         return 0;
 253 }
 254
 255 int resource_set_motor_driver_L298N_speed(motor_id_e id, int speed)
 256 {
 257         int ret = 0;
 258         const int value_max = 4095;
 259         int value = 0;
 260         int e_state = MOTOR_STATE_NONE;
 261         int motor_v_1 = 0;
 262         int motor_v_2 = 0;
 263
 264         if (g_md_h[id].motor_state <= MOTOR_STATE_CONFIGURED) {
 265                 ret = __init_motor_by_id(id);
 266                 if (ret) {
 267                         _E("failed to __init_motor_by_id()");
 268                         return -1;
 269                 }
 270         }
 271
 272         value = abs(speed);
 273
 274         if (value > value_max) {
 275                 value = value_max;
 276                 _D("max speed is %d", value_max);
 277         }
 278         _D("set speed %d", value);
 279
 280         if (speed == 0) {
 281                 /* brake and stop */
 282                 ret = __motor_brake_n_stop_by_id(id);
 283                 if (ret) {
 284                         _E("failed to stop motor[%d]", id);
 285                         return -1;
 286                 }
 287                 return 0; /* done */
 288         }
 289
 290         if (speed > 0)
 291                 e_state = MOTOR_STATE_FORWARD; /* will be set forward */
 292         else
 293                 e_state = MOTOR_STATE_BACKWARD; /* will be set backward */
 294
 295         if (g_md_h[id].motor_state == e_state)
 296                 goto SET_SPEED;
 297         else {
 298                 /* brake and stop */
 299                 ret = __motor_brake_n_stop_by_id(id);
 300                 if (ret) {
 301                         _E("failed to stop motor[%d]", id);
 302                         return -1;
 303                 }
 304         }
 305
 306         switch (e_state) {
 307         case MOTOR_STATE_FORWARD:
 308                 motor_v_1 = 1;
 309                 motor_v_2 = 0;
 310                 break;
 311         case MOTOR_STATE_BACKWARD:
 312                 motor_v_1 = 0;
 313                 motor_v_2 = 1;
 314                 break;
 315         }
 316         ret = peripheral_gpio_write(g_md_h[id].pin1_h, motor_v_1);
 317         if (ret != PERIPHERAL_ERROR_NONE) {
 318                 _E("failed to set value[%d] Motor[%d] pin 1", motor_v_1, id);
 319                 return -1;
 320         }
 321
 322         ret = peripheral_gpio_write(g_md_h[id].pin2_h, motor_v_2);
 323         if (ret != PERIPHERAL_ERROR_NONE) {
 324                 _E("failed to set value[%d] Motor[%d] pin 2", motor_v_2, id);
 325                 return -1;
 326         }
 327
 328 SET_SPEED:
 329         ret = resource_pca9685_set_value_to_channel(g_md_h[id].en_ch, 0, value);
 330         if (ret) {
 331                 _E("failed to set speed - %d", speed);
 332                 return -1;
 333         }
 334
 335         g_md_h[id].motor_state = e_state;
 336
 337         return 0;
 338 }