+++ /dev/null
-/*
- * Copyright (c) 2017 Samsung Electronics Co., Ltd.
- *
- * Contact: Jeonghoon Park <jh1979.park@samsung.com>
- *
- * Licensed under the Flora License, Version 1.1 (the License);
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://floralicense.org/license/
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an AS IS BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#ifndef __CAR_APP_DC_MOTOR_H__
-#define __CAR_APP_DC_MOTOR_H__
-
-typedef enum {
- DC_MOTOR_ID_L,
- DC_MOTOR_ID_R,
-} dc_motor_id_e;
-
-int dc_motor_init(void);
-int dc_motor_fini(void);
-int dc_motor_speed_set(dc_motor_id_e id, int speed);
-
-#endif /* __CAR_APP_DC_MOTOR_H__ */
+++ /dev/null
-/*
- * Copyright (c) 2017 Samsung Electronics Co., Ltd.
- *
- * Contact: Jeonghoon Park <jh1979.park@samsung.com>
- *
- * Licensed under the Flora License, Version 1.1 (the License);
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://floralicense.org/license/
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an AS IS BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#ifndef __CAR_APP_PCA9685_H__
-#define __CAR_APP_PCA9685_H__
-
-int pca9685_init(void);
-int pca9685_fini(void);
-int pca9685_set_frequency(unsigned int freq_hz);
-int pca9685_set_value_to_channel(unsigned int channel, int on, int off);
-int pca9685_set_value_to_all(int on, int off);
-
-#endif /* __CAR_APP_PCA9685_H__ */
+++ /dev/null
-/*
- * Copyright (c) 2017 Samsung Electronics Co., Ltd.
- *
- * Contact: Jeonghoon Park <jh1979.park@samsung.com>
- *
- * Licensed under the Flora License, Version 1.1 (the License);
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://floralicense.org/license/
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an AS IS BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#ifndef __CAR_APP_SERVO_MOTOR_H__
-#define __CAR_APP_SERVO_MOTOR_H__
-
-int servo_motor_init(void);
-int servo_motor_fini(void);
-
-/*
- * You Must adjust servo motor before using.
- * recommanded center value is 450
- * for left turn, set less then center value(e.g. 400),
- * for right turn, set more then center value(e.g. 500)
- */
-int servo_motor_value_set(int motor_id, int value);
-
-#endif /* __CAR_APP_SERVO_MOTOR_H__ */
\ No newline at end of file
+++ /dev/null
-/*
- * Copyright (c) 2017 Samsung Electronics Co., Ltd.
- *
- * Contact: Jeonghoon Park <jh1979.park@samsung.com>
- *
- * Licensed under the Flora License, Version 1.1 (the License);
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://floralicense.org/license/
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an AS IS BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#include <stdlib.h>
-#include <peripheral_io.h>
-#include "log.h"
-#include "pca9685.h"
-#include "dc_motor.h"
-
-/* connected GPIO pin numbers of raspberry pi 3 with IN pins of L298N */
-#define MotorA_1 19
-#define MotorA_2 16
-#define MotorB_1 26
-#define MotorB_2 20
-
-/* connected channel numbers of PCA9685 with enable pins of L298N */
-#define EnableA_CH 5
-#define EnableB_CH 4
-
-typedef enum {
- MOTOR_STATE_NONE,
- MOTOR_STATE_STOP,
- MOTOR_STATE_FORWARD,
- MOTOR_STATE_BACKWARD,
-} motor_state_e;
-
-peripheral_gpio_h motorA_1_h = NULL;
-peripheral_gpio_h motorA_2_h = NULL;
-peripheral_gpio_h motorB_1_h = NULL;
-peripheral_gpio_h motorB_2_h = NULL;
-
-static int MotorState[2] = {MOTOR_STATE_NONE, };
-
-/* see Principle section in http://wiki.sunfounder.cc/index.php?title=Motor_Driver_Module-L298N */
-
-static int dc_motor_stop(dc_motor_id_e id)
-{
- int ret = PERIPHERAL_ERROR_NONE;
- int motor1_v = 0;
- int motor2_v = 0;
- peripheral_gpio_h motor_1_h = NULL;
- peripheral_gpio_h motor_2_h = NULL;
- int channel = 0;
-
- if (MotorState[id] == MOTOR_STATE_NONE) {
- _E("motor[%d] are not initialized - state(%d)", id, MotorState[id]);
- return -1;
- }
-
- if (MotorState[id] == MOTOR_STATE_STOP) {
- _D("motor[%d] is already stopped", id);
- return 0;
- }
-
- switch (id) {
- case DC_MOTOR_ID_L:
- channel = EnableA_CH;
- motor_1_h = motorA_1_h;
- motor_2_h = motorA_2_h;
- break;
- case DC_MOTOR_ID_R:
- channel = EnableB_CH;
- motor_1_h = motorB_1_h;
- motor_2_h = motorB_2_h;
- break;
- }
-
- switch (MotorState[id]) {
- case MOTOR_STATE_FORWARD:
- motor1_v = 0;
- motor2_v = 0;
- break;
- case MOTOR_STATE_BACKWARD:
- motor1_v = 1;
- motor2_v = 1;
- break;
- }
-
- /* Brake DC motor */
- ret = peripheral_gpio_write(motor_1_h, motor1_v);
- if (ret != PERIPHERAL_ERROR_NONE) {
- _E("Failed to set value[%d] Motor_1[%d]", motor1_v, id);
- return -1;
- }
-
- ret = peripheral_gpio_write(motor_2_h, motor2_v);
- if (ret != PERIPHERAL_ERROR_NONE) {
- _E("Failed to set value[%d] Motor_1[%d]", motor2_v, id);
- return -1;
- }
-
- /* set stop DC motor */
- pca9685_set_value_to_channel(channel, 0, 0);
-
- MotorState[id] = MOTOR_STATE_STOP;
-
- return 0;
-}
-
-static void pins_fini(void)
-{
- MotorState[DC_MOTOR_ID_L] = MOTOR_STATE_NONE;
- MotorState[DC_MOTOR_ID_R] = MOTOR_STATE_NONE;
-
- pca9685_fini();
-
- if (motorA_1_h) {
- peripheral_gpio_close(motorA_1_h);
- motorA_1_h = NULL;
- }
-
- if (motorA_2_h) {
- peripheral_gpio_close(motorA_2_h);
- motorA_2_h = NULL;
- }
-
-
- if (motorB_1_h) {
- peripheral_gpio_close(motorB_1_h);
- motorB_1_h = NULL;
- }
-
- if (motorB_2_h) {
- peripheral_gpio_close(motorB_2_h);
- motorB_2_h = NULL;
- }
-
- return;
-}
-
-static int pins_init(void)
-{
- int ret = 0;
-
- if ((MotorState[DC_MOTOR_ID_L] > MOTOR_STATE_NONE) ||
- (MotorState[DC_MOTOR_ID_R] > MOTOR_STATE_NONE)) {
- _E("current state = %d, %d",
- MotorState[DC_MOTOR_ID_L], MotorState[DC_MOTOR_ID_R]);
- return -1;
- }
-
- ret = pca9685_init();
- if (ret) {
- _E("failed to init PCA9685");
- return -1;
- }
-
- /* open pins for Motor A */
- ret = peripheral_gpio_open(MotorA_1, &motorA_1_h);
- if (ret == PERIPHERAL_ERROR_NONE)
- peripheral_gpio_set_direction(motorA_1_h,
- PERIPHERAL_GPIO_DIRECTION_OUT_INITIALLY_LOW);
- else {
- _E("failed to open MotorA_1 gpio pin(%d)", MotorA_1);
- goto ERROR;
- }
-
- ret = peripheral_gpio_open(MotorA_2, &motorA_2_h);
- if (ret == PERIPHERAL_ERROR_NONE)
- peripheral_gpio_set_direction(motorA_2_h,
- PERIPHERAL_GPIO_DIRECTION_OUT_INITIALLY_LOW);
- else {
- _E("failed to open MotorA_2 gpio pin(%d)", MotorA_2);
- goto ERROR;
- }
-
- MotorState[DC_MOTOR_ID_L] = MOTOR_STATE_STOP;
-
- /* open pins for Motor B */
- ret = peripheral_gpio_open(MotorB_1, &motorB_1_h);
- if (ret == PERIPHERAL_ERROR_NONE)
- peripheral_gpio_set_direction(motorB_1_h,
- PERIPHERAL_GPIO_DIRECTION_OUT_INITIALLY_LOW);
- else {
- _E("failed to open MotorB_1 gpio pin(%d)", MotorB_1);
- goto ERROR;
- }
-
- ret = peripheral_gpio_open(MotorB_2, &motorB_2_h);
- if (ret == PERIPHERAL_ERROR_NONE)
- peripheral_gpio_set_direction(motorB_2_h,
- PERIPHERAL_GPIO_DIRECTION_OUT_INITIALLY_LOW);
- else {
- _E("failed to open MotorB_2 gpio pin(%d)", MotorB_2);
- goto ERROR;
- }
-
- MotorState[DC_MOTOR_ID_R] = MOTOR_STATE_STOP;
-
- return 0;
-
-ERROR:
- pca9685_fini();
- pins_fini();
- return -1;
-}
-
-int dc_motor_init(void)
-{
- int ret = 0;
-
- ret = pins_init();
-
- return ret;
-}
-
-int dc_motor_fini(void)
-{
- int ret = 0;
-
- pins_fini();
-
- return ret;
-}
-
-int dc_motor_speed_set(dc_motor_id_e id, int speed)
-{
- int ret = 0;
- const int value_max = 4095;
- int value = 0;
- int channel = 0;
- peripheral_gpio_h motor_1_h = NULL;
- peripheral_gpio_h motor_2_h = NULL;
- int motor_v_1 = 0;
- int motor_v_2 = 0;
- int e_state = MOTOR_STATE_NONE;
-
- value = abs(speed);
-
- if (value > value_max) {
- value = value_max;
- _D("max speed is %d", value_max);
- }
- _D("set speed %d", value);
-
- if (speed == 0) {
- /* brake and stop */
- ret = dc_motor_stop(id);
- if (ret) {
- _E("failed to stop motor[%d]", id);
- return -1;
- }
- return 0; /* done */
- }
-
- switch (id) {
- case DC_MOTOR_ID_L:
- channel = EnableA_CH;
- motor_1_h = motorA_1_h;
- motor_2_h = motorA_2_h;
- break;
- case DC_MOTOR_ID_R:
- channel = EnableB_CH;
- motor_1_h = motorB_1_h;
- motor_2_h = motorB_2_h;
- break;
- }
-
- if (speed > 0)
- e_state = MOTOR_STATE_FORWARD; /* will be set forward */
- else
- e_state = MOTOR_STATE_BACKWARD; /* will be set backward */
-
- if (MotorState[id] == e_state)
- goto SET_SPEED;
- else {
- /* brake and stop */
- ret = dc_motor_stop(id);
- if (ret) {
- _E("failed to stop motor[%d]", id);
- return -1;
- }
- }
-
- switch (e_state) {
- case MOTOR_STATE_FORWARD:
- motor_v_1 = 1;
- motor_v_2 = 0;
- break;
- case MOTOR_STATE_BACKWARD:
- motor_v_1 = 0;
- motor_v_2 = 1;
- break;
- }
- ret = peripheral_gpio_write(motor_1_h, motor_v_1);
- if (ret != PERIPHERAL_ERROR_NONE) {
- _E("failed to set value[%d] Motor_1[%d]", motor_v_1, id);
- return -1;
- }
-
- ret = peripheral_gpio_write(motor_2_h, motor_v_2);
- if (ret != PERIPHERAL_ERROR_NONE) {
- _E("failed to set value[%d] Motor_2[%d]", motor_v_2, id);
- return -1;
- }
-
-SET_SPEED:
- ret = pca9685_set_value_to_channel(channel, 0, value);
- if (ret) {
- _E("failed to set speed - %d", speed);
- return -1;
- }
-
- MotorState[id] = e_state;
-
- return 0;
-}
+++ /dev/null
-/*
- * Copyright (c) 2017 Samsung Electronics Co., Ltd.
- *
- * Contact: Jeonghoon Park <jh1979.park@samsung.com>
- *
- * Licensed under the Flora License, Version 1.1 (the License);
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://floralicense.org/license/
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an AS IS BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#include <stdio.h>
-#include <unistd.h>
-#include <math.h>
-#include <peripheral_io.h>
-#include "log.h"
-
-#define RPI3_I2C_BUS 1
-
-/* Registers/etc: */
-#define PCA9685_ADDRESS 0x40
-#define MODE1 0x00
-#define MODE2 0x01
-#define SUBADR1 0x02
-#define SUBADR2 0x03
-#define SUBADR3 0x04
-#define PRESCALE 0xFE
-#define LED0_ON_L 0x06
-#define LED0_ON_H 0x07
-#define LED0_OFF_L 0x08
-#define LED0_OFF_H 0x09
-#define ALL_LED_ON_L 0xFA
-#define ALL_LED_ON_H 0xFB
-#define ALL_LED_OFF_L 0xFC
-#define ALL_LED_OFF_H 0xFD
-
-/* Bits: */
-#define RESTART 0x80
-#define SLEEP 0x10
-#define ALLCALL 0x01
-#define INVRT 0x10
-#define OUTDRV 0x04
-
-static peripheral_i2c_h g_i2c_h = NULL;
-static unsigned int ref_count = 0;
-
-int pca9685_set_frequency(unsigned int freq_hz)
-{
- int ret = PERIPHERAL_ERROR_NONE;
- double prescale_value = 0.0;
- int prescale = 0;
- uint8_t oldmode = 0;
- uint8_t newmode = 0;
-
- prescale_value = 25000000.0; // 25MHz
- prescale_value /= 4096.0; // 12-bit
- prescale_value /= (double)freq_hz;
- prescale_value -= 1.0;
-
- prescale = (int)floor(prescale_value + 0.5);
-
- ret = peripheral_i2c_read_register_byte(g_i2c_h, MODE1, &oldmode);
- retvm_if(ret != PERIPHERAL_ERROR_NONE, -1, "failed to read register");
-
- newmode = (oldmode & 0x7F) | 0x10; // sleep
- ret = peripheral_i2c_write_register_byte(g_i2c_h, MODE1, newmode); // go to sleep
- retvm_if(ret != PERIPHERAL_ERROR_NONE, -1, "failed to write register");
-
- ret = peripheral_i2c_write_register_byte(g_i2c_h, PRESCALE, prescale);
- retvm_if(ret != PERIPHERAL_ERROR_NONE, -1, "failed to write register");
-
- ret = peripheral_i2c_write_register_byte(g_i2c_h, MODE1, oldmode);
- retvm_if(ret != PERIPHERAL_ERROR_NONE, -1, "failed to write register");
-
- usleep(500);
-
- ret = peripheral_i2c_write_register_byte(g_i2c_h, MODE1, (oldmode | 0x80));
- retvm_if(ret != PERIPHERAL_ERROR_NONE, -1, "failed to write register");
-
- return 0;
-}
-
-int pca9685_set_value_to_channel(unsigned int channel, int on, int off)
-{
- int ret = PERIPHERAL_ERROR_NONE;
- retvm_if(g_i2c_h == NULL, -1, "Not initialized yet");
-
- ret = peripheral_i2c_write_register_byte(g_i2c_h,
- LED0_ON_L + 4*channel, on & 0xFF);
- retvm_if(ret != PERIPHERAL_ERROR_NONE, -1, "failed to write register");
-
- ret = peripheral_i2c_write_register_byte(g_i2c_h,
- LED0_ON_H + 4*channel, on >> 8);
- retvm_if(ret != PERIPHERAL_ERROR_NONE, -1, "failed to write register");
-
- ret = peripheral_i2c_write_register_byte(g_i2c_h,
- LED0_OFF_L + 4*channel, off & 0xFF);
- retvm_if(ret != PERIPHERAL_ERROR_NONE, -1, "failed to write register");
-
- ret = peripheral_i2c_write_register_byte(g_i2c_h,
- LED0_OFF_H + 4*channel, off >> 8);
- retvm_if(ret != PERIPHERAL_ERROR_NONE, -1, "failed to write register");
-
- return 0;
-}
-
-int pca9685_set_value_to_all(int on, int off)
-{
- int ret = PERIPHERAL_ERROR_NONE;
- retvm_if(g_i2c_h == NULL, -1, "Not initialized yet");
-
- ret = peripheral_i2c_write_register_byte(g_i2c_h,
- ALL_LED_ON_L, on & 0xFF);
- retvm_if(ret != PERIPHERAL_ERROR_NONE, -1, "failed to write register");
-
- ret = peripheral_i2c_write_register_byte(g_i2c_h,
- ALL_LED_ON_H, on >> 8);
- retvm_if(ret != PERIPHERAL_ERROR_NONE, -1, "failed to write register");
-
- ret = peripheral_i2c_write_register_byte(g_i2c_h,
- ALL_LED_OFF_L, off & 0xFF);
- retvm_if(ret != PERIPHERAL_ERROR_NONE, -1, "failed to write register");
-
- ret = peripheral_i2c_write_register_byte(g_i2c_h,
- ALL_LED_OFF_H, off >> 8);
- retvm_if(ret != PERIPHERAL_ERROR_NONE, -1, "failed to write register");
-
- return 0;
-}
-
-int pca9685_init(void)
-{
- uint8_t mode1 = 0;
- int ret = PERIPHERAL_ERROR_NONE;
-
- if (g_i2c_h) {
- ref_count++;
- _D("Already initialized - ref_count[%u]\n", ref_count);
- return 0;
- }
-
- ret = peripheral_i2c_open(RPI3_I2C_BUS, PCA9685_ADDRESS, &g_i2c_h);
- if (ret != PERIPHERAL_ERROR_NONE) {
- _E("failed to open pca9685-[bus:%d, addr:%d]",
- RPI3_I2C_BUS, PCA9685_ADDRESS);
- return -1;
- }
- ret = pca9685_set_value_to_all(0, 0);
- retvm_if(ret, -1, "failed to set value to register");
-
- ret = peripheral_i2c_write_register_byte(g_i2c_h, MODE2, OUTDRV);
- retvm_if(ret != PERIPHERAL_ERROR_NONE, -1, "failed to write register");
-
- ret = peripheral_i2c_write_register_byte(g_i2c_h, MODE1, ALLCALL);
- retvm_if(ret != PERIPHERAL_ERROR_NONE, -1, "failed to write register");
-
- usleep(500); // wait for oscillator
-
- ret = peripheral_i2c_read_register_byte(g_i2c_h, MODE1, &mode1);
- retvm_if(ret != PERIPHERAL_ERROR_NONE, -1, "failed to read register");
-
- mode1 = mode1 & (~SLEEP); // # wake up (reset sleep)
- ret = peripheral_i2c_write_register_byte(g_i2c_h, MODE1, mode1);
- retvm_if(ret != PERIPHERAL_ERROR_NONE, -1, "failed to write register");
-
- usleep(500); // wait for oscillator
-
- ret = pca9685_set_frequency(60);
- if (ret) {
- _E("failed to set frequency");
- peripheral_i2c_close(g_i2c_h);
- g_i2c_h = NULL;
- return -1;
- }
- ref_count++;
-
- return 0;
-}
-
-int pca9685_fini(void)
-{
- ref_count--;
-
- _D("ref count - %u", ref_count);
-
- if (g_i2c_h) {
- _D("finalizing pca9685");
- pca9685_set_value_to_all(0, 0);
- peripheral_i2c_close(g_i2c_h);
- g_i2c_h = NULL;
- }
-
- return 0;
-}
+++ /dev/null
-/*
- * Copyright (c) 2017 Samsung Electronics Co., Ltd.
- *
- * Contact: Jeonghoon Park <jh1979.park@samsung.com>
- *
- * Licensed under the Flora License, Version 1.1 (the License);
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://floralicense.org/license/
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an AS IS BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#include "log.h"
-#include "pca9685.h"
-
-int servo_motor_init(void)
-{
- int ret = 0;
- ret = pca9685_init();
- if (ret) {
- _E("failed to init PCA9685");
- return -1;
- }
-
- return 0;
-}
-
-int servo_motor_fini(void)
-{
- pca9685_fini();
- return 0;
-}
-
-int servo_motor_value_set(int motor_id, int value)
-{
- int ret = 0;
- ret = pca9685_set_value_to_channel(motor_id, 0, value);
- return ret;
-}
projects / apps / native / gear-racing-car.git / commitdiff