2 * Copyright (c) 2017 Samsung Electronics Co., Ltd.
4 * Contact: Jeonghoon Park <jh1979.park@samsung.com>
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
10 * http://floralicense.org/license/
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.
20 #include <peripheral_io.h>
25 /* connected GPIO pin numbers of raspberry pi 3 with IN pins of L298N */
31 /* connected channel numbers of PCA9685 with enable pins of L298N */
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;
47 static int MotorState[2] = {MOTOR_STATE_NONE, };
49 /* see Principle section in http://wiki.sunfounder.cc/index.php?title=Motor_Driver_Module-L298N */
51 static int dc_motor_stop(dc_motor_id_e id)
53 int ret = PERIPHERAL_ERROR_NONE;
56 peripheral_gpio_h motor_1_h = NULL;
57 peripheral_gpio_h motor_2_h = NULL;
60 if (MotorState[id] == MOTOR_STATE_NONE) {
61 _E("motor[%d] are not initialized - state(%d)", id, MotorState[id]);
65 if (MotorState[id] == MOTOR_STATE_STOP) {
66 _D("motor[%d] is already stopped", id);
73 motor_1_h = motorA_1_h;
74 motor_2_h = motorA_2_h;
78 motor_1_h = motorB_1_h;
79 motor_2_h = motorB_2_h;
83 switch (MotorState[id]) {
84 case MOTOR_STATE_FORWARD:
88 case MOTOR_STATE_BACKWARD:
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);
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);
107 /* set stop DC motor */
108 pca9685_set_value_to_channel(channel, 0, 0);
110 MotorState[id] = MOTOR_STATE_STOP;
115 static void pins_fini(void)
117 MotorState[DC_MOTOR_ID_L] = MOTOR_STATE_NONE;
118 MotorState[DC_MOTOR_ID_R] = MOTOR_STATE_NONE;
123 peripheral_gpio_close(motorA_1_h);
128 peripheral_gpio_close(motorA_2_h);
134 peripheral_gpio_close(motorB_1_h);
139 peripheral_gpio_close(motorB_2_h);
146 static int pins_init(void)
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]);
157 ret = pca9685_init();
159 _E("failed to init PCA9685");
163 ret = pca9685_set_frequency(60);
165 _E("failed to set frequency to PCA9685");
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);
175 _E("failed to open MotorA_1 gpio pin(%d)", MotorA_1);
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);
184 _E("failed to open MotorA_2 gpio pin(%d)", MotorA_2);
188 MotorState[DC_MOTOR_ID_L] = MOTOR_STATE_STOP;
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);
196 _E("failed to open MotorB_1 gpio pin(%d)", MotorB_1);
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);
205 _E("failed to open MotorB_2 gpio pin(%d)", MotorB_2);
209 MotorState[DC_MOTOR_ID_R] = MOTOR_STATE_STOP;
219 int dc_motor_init(void)
228 int dc_motor_fini(void)
237 int dc_motor_speed_set(dc_motor_id_e id, int speed)
240 const int value_max = 4095;
243 peripheral_gpio_h motor_1_h = NULL;
244 peripheral_gpio_h motor_2_h = NULL;
247 int e_state = MOTOR_STATE_NONE;
251 if (value > value_max) {
253 _D("max speed is %d", value_max);
255 _D("set speed %d", value);
259 ret = dc_motor_stop(id);
261 _E("failed to stop motor[%d]", id);
269 channel = EnableA_CH;
270 motor_1_h = motorA_1_h;
271 motor_2_h = motorA_2_h;
274 channel = EnableB_CH;
275 motor_1_h = motorB_1_h;
276 motor_2_h = motorB_2_h;
281 e_state = MOTOR_STATE_FORWARD; /* will be set forward */
283 e_state = MOTOR_STATE_BACKWARD; /* will be set backward */
285 if (MotorState[id] == e_state)
289 ret = dc_motor_stop(id);
291 _E("failed to stop motor[%d]", id);
297 case MOTOR_STATE_FORWARD:
301 case MOTOR_STATE_BACKWARD:
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);
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);
319 ret = pca9685_set_value_to_channel(channel, 0, value);
321 _E("failed to set speed - %d", speed);
325 MotorState[id] = e_state;