From 00ab5344c54c4580384b159503c862156f22d1f8 Mon Sep 17 00:00:00 2001 From: Alex Mikhalev Date: Thu, 10 Jan 2019 14:00:45 -0800 Subject: [PATCH] start adding ugv_io --- main/CMakeLists.txt | 4 ++-- main/ugv_io.c | 45 +++++++++++++++++++++++++++++++++++++++++++++ main/ugv_io.h | 44 ++++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 91 insertions(+), 2 deletions(-) create mode 100644 main/ugv_io.c create mode 100644 main/ugv_io.h diff --git a/main/CMakeLists.txt b/main/CMakeLists.txt index 0f3514b..191dcd7 100644 --- a/main/CMakeLists.txt +++ b/main/CMakeLists.txt @@ -2,17 +2,17 @@ include(${CMAKE_CURRENT_LIST_DIR}/../components/nanopb/functions.cmake) set(PB_OUT ${CMAKE_CURRENT_SOURCE_DIR}/pb_out) +make_directory(${PB_OUT}) nanopb_generate(messages.proto ${PB_OUT} PROTO_HDRS PROTO_SRCS) list(APPEND COMPONENT_SRCS "ugv_main.c" "ugv_comms.c" + "ugv_io.c" "u8g2_esp32_hal.c" ${PROTO_SRCS}) set(COMPONENT_PRIV_INCLUDEDIRS "." ${PB_OUT}) set(COMPONENT_REQUIRES "u8g2" "sx127x_driver" "nanopb") -make_directory(${PB_OUT}) - register_component() component_compile_options("-Werror=incompatible-pointer-types") diff --git a/main/ugv_io.c b/main/ugv_io.c new file mode 100644 index 0000000..50631d8 --- /dev/null +++ b/main/ugv_io.c @@ -0,0 +1,45 @@ +#include "ugv_io.h" + +#include +#include +#include + +#define IO_MCPWM_UNIT MCPWM_UNIT_0 +#define IO_MOTOR_LEFT_PWM 0 +#define IO_MOTOR_LEFT_DIR 0 +#define IO_MOTOR_RIGHT_PWM 0 +#define IO_MOTOR_RIGHT_DIR 0 + +typedef struct ugv_io_s { +} ugv_io_t; + +static ugv_io_t io; + +void ugv_io_init() { + gpio_set_direction(IO_MOTOR_LEFT_PWM, GPIO_MODE_OUTPUT); + gpio_set_direction(IO_MOTOR_LEFT_DIR, GPIO_MODE_OUTPUT); + gpio_set_direction(IO_MOTOR_RIGHT_PWM, GPIO_MODE_OUTPUT); + gpio_set_direction(IO_MOTOR_RIGHT_DIR, GPIO_MODE_OUTPUT); + mcpwm_gpio_init(IO_MCPWM_UNIT, MCPWM0A, IO_MOTOR_LEFT_PWM); + mcpwm_gpio_init(IO_MCPWM_UNIT, MCPWM0B, IO_MOTOR_RIGHT_PWM); + + mcpwm_config_t mcpwm_config; + mcpwm_config.frequency = 20000; // 20khz + mcpwm_config.cmpr_a = 0; + mcpwm_config.cmpr_b = 0; + mcpwm_config.counter_mode = MCPWM_UP_DOWN_COUNTER; // for symmetric pwm + mcpwm_config.duty_mode = MCPWM_DUTY_MODE_0; // active high + mcpwm_init(IO_MCPWM_UNIT, MCPWM_TIMER_0, &mcpwm_config); +} + +void ugv_io_write_outputs(ugv_outputs_t *outputs) { + mcpwm_set_duty(IO_MCPWM_UNIT, MCPWM_TIMER_0, MCPWM_OPR_A, + fabs(outputs->left_motor)); + mcpwm_set_duty(IO_MCPWM_UNIT, MCPWM_TIMER_0, MCPWM_OPR_B, + fabs(outputs->right_motor)); + bool left_dir, right_dir; + left_dir = outputs->left_motor < 0.f; + right_dir = outputs->right_motor < 0.f; + gpio_set_level(IO_MOTOR_LEFT_DIR, left_dir); + gpio_set_level(IO_MOTOR_RIGHT_DIR, right_dir); +} \ No newline at end of file diff --git a/main/ugv_io.h b/main/ugv_io.h new file mode 100644 index 0000000..d36bcac --- /dev/null +++ b/main/ugv_io.h @@ -0,0 +1,44 @@ +#pragma once + +#include + +typedef enum gps_fix_qual_e { + GPS_FIX_INVALID = 0, // invalid gps fix + GPS_FIX_GPS = 1, // GPS fix + GPS_FIX_DGPS = 2, // differential GPS fix + GPS_FIX_PPS = 3, // PPS fix + GPS_FIX_RTK = 4, // Real Time Kinematic fix + GPS_FIX_FRTK = 5, // Float Real Time Kinematic fix + GPS_FIX_ESTIMATED = 6, // Estimated fix + GPS_FIX_MANUAL = 7, // Manual fix + GPS_FIX_SIMULATED = 8, // Simulated fix +} gps_fix_qual_t; + +typedef struct ugv_inputs_s { + float gyro_x; // degrees + float gyro_y; // degrees + float gyro_z; // degrees + float accel_x; // gs + float accel_y; // gs + float accel_z; // gs + float mag_x; // flux density uT + float mag_y; // flux density uT + float mag_z; // flux density uT + + gps_fix_qual_t gps_fix_quality; + uint8_t gps_num_satellites; + + double gps_lat; // degrees +/- + double gps_long; // degrees +/- + double gps_altitude; // meters +} ugv_inputs_t; + +typedef struct ugv_outputs_s { + float left_motor; // left motor power -1.0 to +1.0 + float right_motor; // right motor power -1.0 to +1.0 +} ugv_outputs_t; + +void ugv_io_init(); + +void ugv_io_read_inputs(ugv_inputs_t *inputs); +void ugv_io_write_outputs(ugv_outputs_t *outputs);