Skip to content

Commit

Permalink
Pc connection (#24)
Browse files Browse the repository at this point in the history 
Detecting whether Micro ROS Agent is connected over an SBC pin header (RaspberryPi 4, o UpBoard) or over the USB-B on the rear panel.
  • Loading branch information
@DominikN
DominikN authored Dec 8, 2023
1 parent e7003f0 commit 537af1b
Show file tree
Hide file tree
Showing 3 changed files with 73 additions and 46 deletions.
3 changes: 2 additions & 1 deletion include/main.hpp
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,8 @@
#pragma once
#include <microros.hpp>

static UARTSerial microros_serial(RPI_SERIAL_TX, RPI_SERIAL_RX);
static UARTSerial microros_serial_rpi(RPI_SERIAL_TX, RPI_SERIAL_RX);
static UARTSerial microros_serial_ftdi(FT_SERIAL_TX, FT_SERIAL_RX);

static volatile bool distance_sensors_enabled = false;
static DigitalOut sens_power(SENS_POWER_ON, 0);
5 changes: 4 additions & 1 deletion platformio.ini
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,4 +21,7 @@ board_microros_user_meta = colcon.meta

lib_deps =
https://github.com/husarion/micro_ros_platformio
lib_compat_mode = off
lib_compat_mode = off

upload_protocol = stlink
debug_tool = stlink
111 changes: 67 additions & 44 deletions src/main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,9 @@

#include <main.hpp>

#include <cstdlib>
#include <ctime>

#define MAIN_LOOP_INTERVAL_MS 10
#define IMU_I2C_FREQUENCY 100000L
#define IMU_I2C_SCL SENS2_PIN3
Expand Down Expand Up @@ -43,13 +46,13 @@ void range_sensors_msg_handler()
osEvent evt1 = distance_sensor_mail_box.get(0);
if (evt1.status == osEventMail)
{
SensorsMeasurement* message = (SensorsMeasurement*)evt1.value.p;
SensorsMeasurement *message = (SensorsMeasurement *)evt1.value.p;
if (message->status == MultiDistanceSensor::ERR_I2C_FAILURE)
{
err_msg++;
if (distance_sensor_commands.empty() && err_msg == 3)
{
uint8_t* data = distance_sensor_commands.alloc();
uint8_t *data = distance_sensor_commands.alloc();
*data = 2;
distance_sensor_commands.put(data);
data = distance_sensor_commands.alloc();
Expand All @@ -70,7 +73,7 @@ void range_sensors_msg_handler()
}
}

void publish_range_sensors(rcl_timer_t* timer, int64_t last_call_time)
void publish_range_sensors(rcl_timer_t *timer, int64_t last_call_time)
{
RCLC_UNUSED(last_call_time);
if (timer != NULL)
Expand All @@ -89,7 +92,7 @@ void imu_msg_handler()

if (evt2.status == osEventMail)
{
ImuDriver::ImuMeasurement* message = (ImuDriver::ImuMeasurement*)evt2.value.p;
ImuDriver::ImuMeasurement *message = (ImuDriver::ImuMeasurement *)evt2.value.p;
fill_imu_msg(&imu_msg);
fill_imu_msg_with_measurements(&imu_msg, message);
publish_imu_msg(&imu_msg);
Expand Down Expand Up @@ -127,7 +130,7 @@ void wheels_state_msg_handler()
}
}

void timer_callback(rcl_timer_t* timer, int64_t last_call_time)
void timer_callback(rcl_timer_t *timer, int64_t last_call_time)
{
RCLC_UNUSED(last_call_time);
if (timer != NULL)
Expand All @@ -141,7 +144,7 @@ void timer_callback(rcl_timer_t* timer, int64_t last_call_time)
}
}

bool on_parameter_changed(const Parameter* old_param, const Parameter* new_param, void* context)
bool on_parameter_changed(const Parameter *old_param, const Parameter *new_param, void *context)
{
(void)context;
if (old_param == NULL && new_param == NULL)
Expand All @@ -153,40 +156,40 @@ bool on_parameter_changed(const Parameter* old_param, const Parameter* new_param
std::map<double, uint8_t>::iterator it;
switch (old_param->value.type)
{
case RCLC_PARAMETER_DOUBLE:
it = servo_voltage_configuration.find(new_param->value.double_value);
if (it == servo_voltage_configuration.end())
{
return false;
}
servo_manager.setPowerMode(it->second);
break;
case RCLC_PARAMETER_BOOL:
if (not strcmp(new_param->name.data, "servo_enable_power"))
{
servo_manager.enablePower(new_param->value.bool_value);
}
else if (isdigit(new_param->name.data[5]))
{
servo_manager.enableOutput(new_param->name.data[5] - '0', new_param->value.bool_value);
}
else
{
return false;
}
break;
case RCLC_PARAMETER_INT:
if (isdigit(new_param->name.data[5]))
{
servo_manager.setPeriod(new_param->name.data[5] - '0', new_param->value.integer_value);
}
else
{
return false;
}
break;
default:
break;
case RCLC_PARAMETER_DOUBLE:
it = servo_voltage_configuration.find(new_param->value.double_value);
if (it == servo_voltage_configuration.end())
{
return false;
}
servo_manager.setPowerMode(it->second);
break;
case RCLC_PARAMETER_BOOL:
if (not strcmp(new_param->name.data, "servo_enable_power"))
{
servo_manager.enablePower(new_param->value.bool_value);
}
else if (isdigit(new_param->name.data[5]))
{
servo_manager.enableOutput(new_param->name.data[5] - '0', new_param->value.bool_value);
}
else
{
return false;
}
break;
case RCLC_PARAMETER_INT:
if (isdigit(new_param->name.data[5]))
{
servo_manager.setPeriod(new_param->name.data[5] - '0', new_param->value.integer_value);
}
else
{
return false;
}
break;
default:
break;
}
}
return true;
Expand All @@ -195,33 +198,53 @@ bool on_parameter_changed(const Parameter* old_param, const Parameter* new_param
int main()
{
ThisThread::sleep_for(100);
sens_power = 1; // sensors power on
sens_power = 1; // sensors power on
ThisThread::sleep_for(100);
init_battery();
init_wheels();
init_button_and_attach_to_callbacks(&button1, button1_rise_callback, button1_fall_callback);
init_button_and_attach_to_callbacks(&button2, button2_rise_callback, button2_fall_callback);

I2C* i2c_ptr = new I2C(IMU_I2C_SDA, IMU_I2C_SCL);
I2C *i2c_ptr = new I2C(IMU_I2C_SDA, IMU_I2C_SCL);
i2c_ptr->frequency(IMU_I2C_FREQUENCY);
init_imu(i2c_ptr);
init_servos();
init_ranges();

set_microros_serial_transports(&microros_serial);
std::srand(std::time(nullptr)); // Seed the random number generator
if (std::rand() % 2 == 0)
{
led2=0;
led3=1;
set_microros_serial_transports(&microros_serial_rpi);
}
else
{
led2=1;
led3=0;
set_microros_serial_transports(&microros_serial_ftdi);
}

uint32_t cnt=0;
while (not rmw_uros_ping_agent(100, 1) == RMW_RET_OK)
{
ThisThread::sleep_for(100);
cnt++;
if (cnt == 10) {
NVIC_SystemReset();
}
}

if (not microros_init())
{
microros_deinit();
ThisThread::sleep_for(2000);

NVIC_SystemReset();
}

led2=0;
led3=0;

AgentStates state = AGENT_CONNECTED;
while (state == AGENT_CONNECTED)
{
Expand Down

0 comments on commit 537af1b

Please sign in to comment.