Andrew Olguin
/
comms_packet_test
Comms prototype for the robosub
main.cpp
- Committer:
- aolgu003
- Date:
- 2016-07-22
- Revision:
- 1:3e33729a7a92
- Parent:
- 0:b7d6e190f091
File content as of revision 1:3e33729a7a92:
#include "mbed.h" #include "rtos.h" #include <string> #include <vector> // Need to create sensor library to handle all the sensor readings DigitalOut led1(LED1); DigitalOut led2(LED2); Serial pc(USBTX, USBRX); typedef struct { string direction; //Forward or Downward facing gives context for the values of x and y float x; float y; float yaw; float depth; } message; typedef struct { float w_yaw; float w_pitch; float w_roll; float yaw; float pitch; float roll; } orientation; typedef struct { float depth; //From pressure sensor float velsurge; //from flow meter float accelsurge; //forward float accelsway; //side to side float accelheave; //downwards } translation; orientation vehicle_att; translation vehicle_trans; message vehicle_cmd; /////////////////////////////////////////////////////////////////////////////////////////////// Thread *rx_handler_pointer; Mutex command_mutex; //lock for contrlling access to command variable Mutex sensor_mutex; //lock for controlling access to sensor variables orientation and translation Mutex stdio_mutex; #define BUFFER_SIZE 255 char buffer[BUFFER_SIZE] = {' '}; int buffer_iter = 0; void callback() { char c = 0; string packet; // Note: you need to actually read from the serial to clear the RX interrupt if (pc.readable()) { //led1 = !led1; do { c = pc.getc(); buffer[buffer_iter] = c; buffer_iter++; } while (buffer_iter < BUFFER_SIZE && c != '\n'); (*rx_handler_pointer).signal_set(0x8); } } void handle_packet(void const *argument) { string command; float x,y,yaw; std::string packet; while (true) { Thread::signal_wait(0x8); command = strtok (buffer," ,\n"); x = atof(strtok (NULL, " ,\n")); y = atof(strtok (NULL, " ,\n")); yaw = atof(strtok (NULL, " ,\n")); depth = atof(strtok (NULL, " ,\n")); printf("Command: %s, X: %f, Y: %f, Z: %f\n", command, x, y, yaw); memset(buffer, ' ', sizeof(buffer)); buffer_iter = 0; fflush(stdout); command_mutex.lock(); vehicle_cmd->direction = command; vehicle_cmd->x = x; vehicle_cmd->y = y; vehicle_cmd->yaw = yaw; vehicle_cmd->depth = depth; command_mutex.unlock(); } } void handle_sensors (void const *arg) { //run through sensor initialization while (true) { //read sensor values //... //first lock using mutex or wait to lock sensor_mutex.lock(); //store to global variables //... //Unlock mutex sensor_mutex.unlock(); Thread::wait(10); // set sensor update rate } } int main() { pc.attach(&callback); Thread service_serial(handle_packet, NULL, osPriorityHigh); Thread sensor_thread(handle_sensors, NULL, osPriorityHigh); //Thread att_cntrl_thread(); //Thread trans_cntrl_thread(); rx_handler_pointer = &service_serial; while (1) { led1 = !led1; Thread::wait(500); } }