Rough single process multi-thread robot control

example.cpp

#include <boost/circular_buffer.hpp>
#include <thread>
#include <mutex>

struct sense_packet {
	uint16_t pressure;
	uint8_t  bus_voltage;
};
boost::circular_buffer<sense_packet> sense_buff(2);
std::mutex sense_mutex;

struct imu_packet {
  //.....
};
boost::circular_buffer<imu_packet> imu_buff(2);
std::mutex imu_mutex;

struct act_packet {
	uint8_t pwm1;
    uint8_t pwm2;
};
boost::circular_buffer<act_packet> act_buff(2);
std::mutex act_mutex;


int stop_threads = 0;

void sense_thread_1()
{
  //open device 
  while(!stop_threads){
	sense_packet incoming;
    //read device, convert to sense_packet. Might be a nested while loop depending on communication style.
    //Protect data access from here to end of block
	std::lock_guard<std::mutex> guard(sense_mutex); 
	sense_buff.push_back(incoming);	
  } 
  //close device
}

void sense_thread_2()
{
  //open device 
  while(!stop_threads){
	imu_packet incoming;
    //read device, convert to sense_packet. Might be a nested while loop depending on communication style.
	std::lock_guard<std::mutex> guard(imu_mutex); 
	imu_buff.push_back(incoming);	
  } 
  //close device
}  

void act_thread_1()
{
  //open device 
  while(!stop_threads){
    if (act_buff.size() > 0){
		act_packet outgoing;
		std::lock_guard<std::mutex> guard(act_mutex); 
		//only send the most recent command
		outgoing = act_buff.back();
		act_buff.clear();
		//convert, write device
    }
	usleep(10000); //check every 10ms
  } 
  //close device
}  

void behavior_1()
{

    //Protect data access from here to end of block
	std::lock_guard<std::mutex> guard(sense_mutex); 
	std::lock_guard<std::mutex> guard(imu_mutex); 
    act_packet command;
    command.pwm1 = sense_buff.back().pressure * 2;
    //other control calculations

	std::lock_guard<std::mutex> guard(act_mutex); 
    act_buff.push_back(command);

}

void behaviors_thread()
{
	
    while(!stop_threads){
	    switch(robot_state){
	        case STATE1:
	          behavior1();
	          behavior2();
	          if(some_condition){
	              robot_state = DIFFERENT_STATE;
	          }
	          break;
	        case STATE2:
	          behavior2();
	          break;
	        default:
	          default_behavior();
	          
	    }
	    usleep(10000);
    }
}

int main()
{

  //start up threads
  std::thread sense1(sense_thread_1);
  std::thread sense2(sense_thread_2);
  std::thread act1(act_thread_1);
  std::thread behaviors(behaviors_thread());
  
  //wait for threads to shut down
  sense1.join();
  sense2.join();
  act1.join();
  behaviors.join();
  
  return 0; 
}