src/dc_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 <stdlib.h>
  20 #include <peripheral_io.h>
  21 #include "log.h"
  22 #include "pca9685.h"
  23 #include "dc_motor.h"
  24
  25 /* connected GPIO pin numbers of raspberry pi 3 with IN pins of L298N */
  26 #define MotorA_1 19
  27 #define MotorA_2 16
  28 #define MotorB_1 26
  29 #define MotorB_2 20
  30
  31 /* connected channel numbers of PCA9685 with enable pins of L298N */
  32 #define EnableA_CH 5
  33 #define EnableB_CH 4
  34
  35 typedef enum {
  36         MOTOR_STATE_NONE,
  37         MOTOR_STATE_STOP,
  38         MOTOR_STATE_FORWARD,
  39         MOTOR_STATE_BACKWARD,
  40 } motor_state_e;
  41
  42 peripheral_gpio_h motorA_1_h = NULL;
  43 peripheral_gpio_h motorA_2_h = NULL;
  44 peripheral_gpio_h motorB_1_h = NULL;
  45 peripheral_gpio_h motorB_2_h = NULL;
  46
  47 static int MotorState[2] = {MOTOR_STATE_NONE, };
  48
  49 /* see Principle section in http://wiki.sunfounder.cc/index.php?title=Motor_Driver_Module-L298N */
  50
  51 static int dc_motor_stop(dc_motor_id_e id)
  52 {
  53         int ret = PERIPHERAL_ERROR_NONE;
  54         int motor1_v = 0;
  55         int motor2_v = 0;
  56         peripheral_gpio_h motor_1_h = NULL;
  57         peripheral_gpio_h motor_2_h = NULL;
  58         int channel = 0;
  59
  60         if (MotorState[id] == MOTOR_STATE_NONE) {
  61                 _E("motor[%d] are not initialized - state(%d)", id, MotorState[id]);
  62                 return -1;
  63         }
  64
  65         if (MotorState[id] == MOTOR_STATE_STOP) {
  66                 _D("motor[%d] is already stopped", id);
  67                 return 0;
  68         }
  69
  70         switch (id) {
  71         case DC_MOTOR_ID_L:
  72                 channel = EnableA_CH;
  73                 motor_1_h = motorA_1_h;
  74                 motor_2_h = motorA_2_h;
  75                 break;
  76         case DC_MOTOR_ID_R:
  77                 channel = EnableB_CH;
  78                 motor_1_h = motorB_1_h;
  79                 motor_2_h = motorB_2_h;
  80                 break;
  81         }
  82
  83         switch (MotorState[id]) {
  84         case MOTOR_STATE_FORWARD:
  85                 motor1_v = 0;
  86                 motor2_v = 0;
  87                 break;
  88         case MOTOR_STATE_BACKWARD:
  89                 motor1_v = 1;
  90                 motor2_v = 1;
  91                 break;
  92         }
  93
  94         /* Brake DC motor */
  95         ret = peripheral_gpio_write(motor_1_h, motor1_v);
  96         if (ret != PERIPHERAL_ERROR_NONE) {
  97                 _E("Failed to set value[%d] Motor_1[%d]", motor1_v, id);
  98                 return -1;
  99         }
 100
 101         ret = peripheral_gpio_write(motor_2_h, motor2_v);
 102         if (ret != PERIPHERAL_ERROR_NONE) {
 103                 _E("Failed to set value[%d] Motor_1[%d]", motor2_v, id);
 104                 return -1;
 105         }
 106
 107         /* set stop DC motor */
 108         pca9685_set_value_to_channel(channel, 0, 0);
 109
 110         MotorState[id] = MOTOR_STATE_STOP;
 111
 112         return 0;
 113 }
 114
 115 static void pins_fini(void)
 116 {
 117         MotorState[DC_MOTOR_ID_L] = MOTOR_STATE_NONE;
 118         MotorState[DC_MOTOR_ID_R] = MOTOR_STATE_NONE;
 119
 120         pca9685_fini();
 121
 122         if (motorA_1_h) {
 123                 peripheral_gpio_close(motorA_1_h);
 124                 motorA_1_h = NULL;
 125         }
 126
 127         if (motorA_2_h) {
 128                 peripheral_gpio_close(motorA_2_h);
 129                 motorA_2_h = NULL;
 130         }
 131
 132
 133         if (motorB_1_h) {
 134                 peripheral_gpio_close(motorB_1_h);
 135                 motorB_1_h = NULL;
 136         }
 137
 138         if (motorB_2_h) {
 139                 peripheral_gpio_close(motorB_2_h);
 140                 motorB_2_h = NULL;
 141         }
 142
 143         return;
 144 }
 145
 146 static int pins_init(void)
 147 {
 148         int ret = 0;
 149
 150         if ((MotorState[DC_MOTOR_ID_L] > MOTOR_STATE_NONE) ||
 151                 (MotorState[DC_MOTOR_ID_R] > MOTOR_STATE_NONE)) {
 152                 _E("current state = %d, %d",
 153                         MotorState[DC_MOTOR_ID_L], MotorState[DC_MOTOR_ID_R]);
 154                 return -1;
 155         }
 156
 157         ret = pca9685_init();
 158         if (ret) {
 159                 _E("failed to init PCA9685");
 160                 return -1;
 161         }
 162
 163         ret = pca9685_set_frequency(60);
 164         if (ret) {
 165                 _E("failed to set frequency to PCA9685");
 166                 goto ERROR;
 167         }
 168
 169         /* open pins for Motor A */
 170         ret = peripheral_gpio_open(MotorA_1, &motorA_1_h);
 171         if (ret == PERIPHERAL_ERROR_NONE)
 172                 peripheral_gpio_set_direction(motorA_1_h,
 173                         PERIPHERAL_GPIO_DIRECTION_OUT_INITIALLY_LOW);
 174         else {
 175                 _E("failed to open MotorA_1 gpio pin(%d)", MotorA_1);
 176                 goto ERROR;
 177         }
 178
 179         ret = peripheral_gpio_open(MotorA_2, &motorA_2_h);
 180         if (ret == PERIPHERAL_ERROR_NONE)
 181                 peripheral_gpio_set_direction(motorA_2_h,
 182                         PERIPHERAL_GPIO_DIRECTION_OUT_INITIALLY_LOW);
 183         else {
 184                 _E("failed to open MotorA_2 gpio pin(%d)", MotorA_2);
 185                 goto ERROR;
 186         }
 187
 188         MotorState[DC_MOTOR_ID_L] = MOTOR_STATE_STOP;
 189
 190         /* open pins for Motor B */
 191         ret = peripheral_gpio_open(MotorB_1, &motorB_1_h);
 192         if (ret == PERIPHERAL_ERROR_NONE)
 193                 peripheral_gpio_set_direction(motorB_1_h,
 194                         PERIPHERAL_GPIO_DIRECTION_OUT_INITIALLY_LOW);
 195         else {
 196                 _E("failed to open MotorB_1 gpio pin(%d)", MotorB_1);
 197                 goto ERROR;
 198         }
 199
 200         ret = peripheral_gpio_open(MotorB_2, &motorB_2_h);
 201         if (ret == PERIPHERAL_ERROR_NONE)
 202                 peripheral_gpio_set_direction(motorB_2_h,
 203                         PERIPHERAL_GPIO_DIRECTION_OUT_INITIALLY_LOW);
 204         else {
 205                 _E("failed to open MotorB_2 gpio pin(%d)", MotorB_2);
 206                 goto ERROR;
 207         }
 208
 209         MotorState[DC_MOTOR_ID_R] = MOTOR_STATE_STOP;
 210
 211         return 0;
 212
 213 ERROR:
 214         pca9685_fini();
 215         pins_fini();
 216         return -1;
 217 }
 218
 219 int dc_motor_init(void)
 220 {
 221         int ret = 0;
 222
 223         ret = pins_init();
 224
 225         return ret;
 226 }
 227
 228 int dc_motor_fini(void)
 229 {
 230         int ret = 0;
 231
 232         pins_fini();
 233
 234         return ret;
 235 }
 236
 237 int dc_motor_speed_set(dc_motor_id_e id, int speed)
 238 {
 239         int ret = 0;
 240         const int value_max = 4095;
 241         int value = 0;
 242         int channel = 0;
 243         peripheral_gpio_h motor_1_h = NULL;
 244         peripheral_gpio_h motor_2_h = NULL;
 245         int motor_v_1 = 0;
 246         int motor_v_2 = 0;
 247         int e_state = MOTOR_STATE_NONE;
 248
 249         value = abs(speed);
 250
 251         if (value > value_max) {
 252                 value = value_max;
 253                 _D("max speed is %d", value_max);
 254         }
 255         _D("set speed %d", value);
 256
 257         if (speed == 0) {
 258                 /* brake and stop */
 259                 ret = dc_motor_stop(id);
 260                 if (ret) {
 261                         _E("failed to stop motor[%d]", id);
 262                         return -1;
 263                 }
 264                 return 0; /* done */
 265         }
 266
 267         switch (id) {
 268         case DC_MOTOR_ID_L:
 269                 channel = EnableA_CH;
 270                 motor_1_h = motorA_1_h;
 271                 motor_2_h = motorA_2_h;
 272                 break;
 273         case DC_MOTOR_ID_R:
 274                 channel = EnableB_CH;
 275                 motor_1_h = motorB_1_h;
 276                 motor_2_h = motorB_2_h;
 277         break;
 278         }
 279
 280         if (speed > 0)
 281                 e_state = MOTOR_STATE_FORWARD; /* will be set forward */
 282         else
 283                 e_state = MOTOR_STATE_BACKWARD; /* will be set backward */
 284
 285         if (MotorState[id] == e_state)
 286                 goto SET_SPEED;
 287         else {
 288                 /* brake and stop */
 289                 ret = dc_motor_stop(id);
 290                 if (ret) {
 291                         _E("failed to stop motor[%d]", id);
 292                         return -1;
 293                 }
 294         }
 295
 296         switch (e_state) {
 297         case MOTOR_STATE_FORWARD:
 298                 motor_v_1 = 1;
 299                 motor_v_2 = 0;
 300                 break;
 301         case MOTOR_STATE_BACKWARD:
 302                 motor_v_1 = 0;
 303                 motor_v_2 = 1;
 304                 break;
 305         }
 306         ret = peripheral_gpio_write(motor_1_h, motor_v_1);
 307         if (ret != PERIPHERAL_ERROR_NONE) {
 308                 _E("failed to set value[%d] Motor_1[%d]", motor_v_1, id);
 309                 return -1;
 310         }
 311
 312         ret = peripheral_gpio_write(motor_2_h, motor_v_2);
 313         if (ret != PERIPHERAL_ERROR_NONE) {
 314                 _E("failed to set value[%d] Motor_2[%d]", motor_v_2, id);
 315                 return -1;
 316         }
 317
 318 SET_SPEED:
 319         ret = pca9685_set_value_to_channel(channel, 0, value);
 320         if (ret) {
 321                 _E("failed to set speed - %d", speed);
 322                 return -1;
 323         }
 324
 325         MotorState[id] = e_state;
 326
 327         return 0;
 328 }