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         /* open pins for Motor A */
 164         ret = peripheral_gpio_open(MotorA_1, &motorA_1_h);
 165         if (ret == PERIPHERAL_ERROR_NONE)
 166                 peripheral_gpio_set_direction(motorA_1_h,
 167                         PERIPHERAL_GPIO_DIRECTION_OUT_INITIALLY_LOW);
 168         else {
 169                 _E("failed to open MotorA_1 gpio pin(%d)", MotorA_1);
 170                 goto ERROR;
 171         }
 172
 173         ret = peripheral_gpio_open(MotorA_2, &motorA_2_h);
 174         if (ret == PERIPHERAL_ERROR_NONE)
 175                 peripheral_gpio_set_direction(motorA_2_h,
 176                         PERIPHERAL_GPIO_DIRECTION_OUT_INITIALLY_LOW);
 177         else {
 178                 _E("failed to open MotorA_2 gpio pin(%d)", MotorA_2);
 179                 goto ERROR;
 180         }
 181
 182         MotorState[DC_MOTOR_ID_L] = MOTOR_STATE_STOP;
 183
 184         /* open pins for Motor B */
 185         ret = peripheral_gpio_open(MotorB_1, &motorB_1_h);
 186         if (ret == PERIPHERAL_ERROR_NONE)
 187                 peripheral_gpio_set_direction(motorB_1_h,
 188                         PERIPHERAL_GPIO_DIRECTION_OUT_INITIALLY_LOW);
 189         else {
 190                 _E("failed to open MotorB_1 gpio pin(%d)", MotorB_1);
 191                 goto ERROR;
 192         }
 193
 194         ret = peripheral_gpio_open(MotorB_2, &motorB_2_h);
 195         if (ret == PERIPHERAL_ERROR_NONE)
 196                 peripheral_gpio_set_direction(motorB_2_h,
 197                         PERIPHERAL_GPIO_DIRECTION_OUT_INITIALLY_LOW);
 198         else {
 199                 _E("failed to open MotorB_2 gpio pin(%d)", MotorB_2);
 200                 goto ERROR;
 201         }
 202
 203         MotorState[DC_MOTOR_ID_R] = MOTOR_STATE_STOP;
 204
 205         return 0;
 206
 207 ERROR:
 208         pca9685_fini();
 209         pins_fini();
 210         return -1;
 211 }
 212
 213 int dc_motor_init(void)
 214 {
 215         int ret = 0;
 216
 217         ret = pins_init();
 218
 219         return ret;
 220 }
 221
 222 int dc_motor_fini(void)
 223 {
 224         int ret = 0;
 225
 226         pins_fini();
 227
 228         return ret;
 229 }
 230
 231 int dc_motor_speed_set(dc_motor_id_e id, int speed)
 232 {
 233         int ret = 0;
 234         const int value_max = 4095;
 235         int value = 0;
 236         int channel = 0;
 237         peripheral_gpio_h motor_1_h = NULL;
 238         peripheral_gpio_h motor_2_h = NULL;
 239         int motor_v_1 = 0;
 240         int motor_v_2 = 0;
 241         int e_state = MOTOR_STATE_NONE;
 242
 243         value = abs(speed);
 244
 245         if (value > value_max) {
 246                 value = value_max;
 247                 _D("max speed is %d", value_max);
 248         }
 249         _D("set speed %d", value);
 250
 251         if (speed == 0) {
 252                 /* brake and stop */
 253                 ret = dc_motor_stop(id);
 254                 if (ret) {
 255                         _E("failed to stop motor[%d]", id);
 256                         return -1;
 257                 }
 258                 return 0; /* done */
 259         }
 260
 261         switch (id) {
 262         case DC_MOTOR_ID_L:
 263                 channel = EnableA_CH;
 264                 motor_1_h = motorA_1_h;
 265                 motor_2_h = motorA_2_h;
 266                 break;
 267         case DC_MOTOR_ID_R:
 268                 channel = EnableB_CH;
 269                 motor_1_h = motorB_1_h;
 270                 motor_2_h = motorB_2_h;
 271         break;
 272         }
 273
 274         if (speed > 0)
 275                 e_state = MOTOR_STATE_FORWARD; /* will be set forward */
 276         else
 277                 e_state = MOTOR_STATE_BACKWARD; /* will be set backward */
 278
 279         if (MotorState[id] == e_state)
 280                 goto SET_SPEED;
 281         else {
 282                 /* brake and stop */
 283                 ret = dc_motor_stop(id);
 284                 if (ret) {
 285                         _E("failed to stop motor[%d]", id);
 286                         return -1;
 287                 }
 288         }
 289
 290         switch (e_state) {
 291         case MOTOR_STATE_FORWARD:
 292                 motor_v_1 = 1;
 293                 motor_v_2 = 0;
 294                 break;
 295         case MOTOR_STATE_BACKWARD:
 296                 motor_v_1 = 0;
 297                 motor_v_2 = 1;
 298                 break;
 299         }
 300         ret = peripheral_gpio_write(motor_1_h, motor_v_1);
 301         if (ret != PERIPHERAL_ERROR_NONE) {
 302                 _E("failed to set value[%d] Motor_1[%d]", motor_v_1, id);
 303                 return -1;
 304         }
 305
 306         ret = peripheral_gpio_write(motor_2_h, motor_v_2);
 307         if (ret != PERIPHERAL_ERROR_NONE) {
 308                 _E("failed to set value[%d] Motor_2[%d]", motor_v_2, id);
 309                 return -1;
 310         }
 311
 312 SET_SPEED:
 313         ret = pca9685_set_value_to_channel(channel, 0, value);
 314         if (ret) {
 315                 _E("failed to set speed - %d", speed);
 316                 return -1;
 317         }
 318
 319         MotorState[id] = e_state;
 320
 321         return 0;
 322 }