diff --git a/software/firmware/libraries/comm_protocol/examples/rover20_comms.py b/software/firmware/libraries/comm_protocol/examples/rover20_comms.py new file mode 100644 index 0000000..e16cd59 --- /dev/null +++ b/software/firmware/libraries/comm_protocol/examples/rover20_comms.py @@ -0,0 +1,129 @@ +#pothos V1.0 +#author: Anthony Grana +#date: 1/10/2019 +#OSURC Mars Rover 2019-2020 + +import sys +import serial +import time +import io +import struct +import timeit +from serial import tools + +class rover20_comms(): + num_slaves = 0 + adr = [] + data = [] + port = '' + + def __init__(self,num_slaves, data, port,TO): + self.num_slaves = num_slaves + self.data = data + if (not (num_slaves == len(self.data))): + raise ValueError('number of slaves does not match initial data') + for i in range(self.num_slaves): + self.adr.append([]) + for j in range(len(self.data[i])): + self.adr[i].append(j) + self.port = serial.Serial(port, 115200,timeout = TO) + + def list_types(self): + for i in range(self.num_slaves): + for j in range(len(self.data[i])): + print("Slave ID: " + str(i+1) + " adr: " + str(self.adr[i][j]) + " data type: " + str(self.data[i][j])) + + def write_single(self, ID, adr, data): + self.port.write(bytes([0x00])) + self.port.write(bytes([ID])) + self.port.write(bytes([0x02])) + self.port.write(bytes([adr])) + if(isinstance(data, int) and data >= -32768 and data <= 32767 and self.data[ID-1][adr] == 'int'): + self.port.write((data).to_bytes(2, byteorder='big')) + elif(isinstance(data,float) and self.data[ID-1][adr] == 'float'): + self.port.write(bytearray(struct.pack("f",data))) + elif(isinstance(data,str) and self.data[ID-1][adr] == 'chr'): + self.port.write(data.encode('utf-8')) + elif(isinstance(data, int) and data >= -2147483648 and data <= 2147483647 and self.data[ID-1][adr] == 'long'): + self.port.write((data).to_bytes(4, byteorder='big')) + else: + raise TypeError("Transmit Error\nThe data you're trying to send does not mach the type stored in the register\nor, the pothos protocol does not support that data type\n or the data you're trying to send is too large or too small") + self.port.write(bytes([0xff])) + while(self.port.out_waiting): + time.sleep(0.0001) + acknowledge = self.port.read(4) + if (not(acknowledge == (bytes(b'\xff\xff\xff\xff')))): + print("Bad acknowledge!!!!") + + def read(self,ID,address): + good_comms = True + self.port.write(bytes([0x00])) + self.port.write(bytes([ID])) + self.port.write(bytes([0x01])) + for i in address: + self.port.write(bytes([i])) + self.port.write(bytes([0xff])) + while(self.port.out_waiting): + time.sleep(0.0001) + data_list = [] + for i in address: + if(self.data[ID-1][i] == 'float'): + while(self.port.in_waiting < 4): time.sleep(0.0001) + data_list.append(struct.unpack('f',self.port.read(4))) + elif(self.data[ID-1][i] == 'int'): + while(self.port.in_waiting < 2): time.sleep(0.0001) + data_list.append(struct.unpack('h',self.port.read(2))) + elif(self.data[ID-1][i] == 'long'): + while(self.port.in_waiting < 4): time.sleep(0.0001) + data_list.append(struct.unpack('i',self.port.read(4))) + elif(self.data[ID-1][i] == 'chr'): + while(self.port.in_waiting < 1): time.sleep(0.0001) + data_list.append(struct.unpack('c',self.port.read(1))) + while(self.port.in_waiting < 1): time.sleep(0.0001) + if(not(self.port.read(1) == b'\x00')): + print("Type Error when reading data from a slave\nThis will also cause a \"Bad acknowledge\" Error") + time.sleep(0.020) + self.port.reset_input_buffer() + break + sync = self.port.read(5) + if(not(sync == b'\xff\xff\xff\xff\xff')): + print("Bad acknowledge!!!!") + return(data_list) + + + def write_multiple(self,ID,adr,data): + self.port.write(bytes([0x00])) + self.port.write(bytes([ID])) + self.port.write(bytes([0x03])) + for i in range(len(adr)): + self.port.write(bytes([adr[i]])) + if(isinstance(data[i], int) and data[i] >= -32768 and data[i] <= 32767 and self.data[ID-1][adr[i]] == 'int'): + self.port.write((data[i]).to_bytes(2, byteorder='big')) + elif(isinstance(data[i],float) and self.data[ID-1][adr[i]] == 'float'): + self.port.write(bytearray(struct.pack("f",data[i]))) + elif(isinstance(data[i],str) and self.data[ID-1][adr[i]] == 'chr'): + self.port.write(data[i].encode('utf-8')) + elif(isinstance(data[i], int) and data[i] >= -2147483648 and data[i] <= 2147483647 and self.data[ID-1][adr[i]] == 'long'): + self.port.write((data[i]).to_bytes(4, byteorder='big')) + else: + raise TypeError("Transmit Error\nThe data you're trying to send does not mach the type stored in the register\nor, the pothos protocol does not support that data type\n or the data you're trying to send is too large or too small") + self.port.write(bytes([0xff])) + while(self.port.out_waiting): + time.sleep(0.0001) + acknowledge = self.port.read(4) + if (not(acknowledge == (bytes(b'\xff\xff\xff\xff')))): + print("Bad acknowledge!!!!mult") + + + + + +data_types = [['chr','int','long','float'], #slave 1 + ['float','int','int']] #slave 2 + +pothos = rover20_comms(2, data_types,'/dev/ttyUSB0',0.030) #class instantiation + +pothos.list_types() #list stored data types +pothos.write_single(1,3,12398.09234) #write slave 1, register 3, (which is a float) to 12398.09234 +pothos.write_multiple(1,[1,2,3],[1589,34567899,3453.0956]) #write to registers 1,2,&3 of slave 1 +print(pothos.read(1,[0, 1, 0x02, 0x03])) #read from all of the registers of slave 1 (can be done in hex or dec) diff --git a/software/firmware/libraries/comm_protocol/pothos/data_store.h b/software/firmware/libraries/comm_protocol/pothos/data_store.h new file mode 100644 index 0000000..7ef1515 --- /dev/null +++ b/software/firmware/libraries/comm_protocol/pothos/data_store.h @@ -0,0 +1,119 @@ +class data_store{ + private: + char char_data[255]; + int int_data[255]; + long long_data[255]; + float float_data[255]; + int type[255]; + public: + + data_store(){ + for(int i=0;i<255;i++){ + char_data[i] = 0x00; + int_data[i] = 0; + long_data[i] = 0; + float_data[i] = 0.0; + type[i] = 0; + } + } + + void set_type(int reg, String dType){ + if(dType == "int") + type[reg] = 2; + else if(dType == "char") + type[reg] = 1; + else if(dType == "long") + type[reg] = 3; + else if(dType == "float") + type[reg] = 4; + } + + int get_type(int reg){ + return type[reg]; + } + + void set_data(int reg, byte* data){ + switch(type[reg]){ + case(0): + break; + case(1): + char_data[reg] = data[0]; + #ifdef POTHOS_DEBUG + Serial.print("The data that was stored is: "); + Serial.println(char_data[reg]); + #endif + break; + case(2): + int_data[reg] = 0; + int_data[reg] = int_data[reg] | data[0]; + int_data[reg] = int_data[reg] << 8; + int_data[reg] = int_data[reg] | data[1]; + #ifdef POTHOS_DEBUG + Serial.print("The data that was stored is: "); + Serial.println(int_data[reg]); + #endif + break; + case(3): + long_data[reg] = 0; + long_data[reg] = long_data[reg] | data[0]; + for(int i=1;i<4;i++){ + long_data[reg] = long_data[reg] << 8; + long_data[reg] = long_data[reg] | data[i]; + } + #ifdef POTHOS_DEBUG + Serial.print("The data that was stored is: "); + Serial.println(long_data[reg]); + #endif + break; + case(4): + float_data[reg] = 0; + memcpy(&(float_data[reg]), data, sizeof(float)); + #ifdef POTHOS_DEBUG + Serial.print("The data that was stored is: "); + Serial.println(float_data[reg],7); + #endif + break; + } + } + + + void set_data(int reg, int data){ + if(type[reg] == 2){ + int_data[reg] = data; + } + } + + void set_data(int reg, float data){ + if(type[reg] == 4){ + float_data[reg] = data; + } + } + + void set_data(int reg, long data){ + if(type[reg] == 3){ + long_data[reg] = data; + } + } + + void set_data(int reg, char data){ + if(type[reg] == 1){ + char_data[reg] = data; + } + } + + int get_int_data(int reg){ + return int_data[reg]; + } + + char get_char_data(int reg){ + return char_data[reg]; + } + + long get_long_data(int reg){ + return long_data[reg]; + } + + float get_float_data(int reg){ + return float_data[reg]; + } +}; diff --git a/software/firmware/libraries/comm_protocol/pothos/examples/Basic_Single_Slave/Basic_Single_Slave.ino b/software/firmware/libraries/comm_protocol/pothos/examples/Basic_Single_Slave/Basic_Single_Slave.ino new file mode 100644 index 0000000..12d34b6 --- /dev/null +++ b/software/firmware/libraries/comm_protocol/pothos/examples/Basic_Single_Slave/Basic_Single_Slave.ino @@ -0,0 +1,46 @@ +//#define POTHOS_DEBUG +#include + +unsigned long blink_timer = 0; +unsigned long blinkTime = 1000; +bool blinkState = false; + +int EnablePin = 2; + +enum DATA{ + CHAR_DATA = 0, + ALT_DATA = 1, + TIME_DATA = 2, + TMP_DATA = 3 +}; + +pothos comms(1, EnablePin); + +void setup() { + Serial1.begin(115200); + comms.setPort(&Serial1); + + Serial.begin(115200); + pinMode(13,OUTPUT); + digitalWrite(13,HIGH); + + set_data_types(); +} + +void loop() { + comms.update(); + + if(millis() - blink_timer >= blinkTime){ + blinkState = !blinkState; + digitalWrite(13,blinkState); + blink_timer = millis(); + } +} + + +void set_data_types(){ + comms.data.set_type(DATA::CHAR_DATA, "char"); + comms.data.set_type(DATA::ALT_DATA, "int"); + comms.data.set_type(DATA::TIME_DATA, "long"); + comms.data.set_type(DATA::TMP_DATA, "float"); +} diff --git a/software/firmware/libraries/comm_protocol/pothos/pothos.h b/software/firmware/libraries/comm_protocol/pothos/pothos.h new file mode 100644 index 0000000..10cb923 --- /dev/null +++ b/software/firmware/libraries/comm_protocol/pothos/pothos.h @@ -0,0 +1,229 @@ +/****************************************** +*Pothos V1 +*Author: Anthony Grana +*1/10/2020 +*OSURC Mars Rover 2019 - 2020 +******************************************/ + + +#include + +class pothos{ + private: + int ID; + HardwareSerial serial; + int sync_counter; + int enPin; + void read(); + void write(); + void write_multiple(); + void sync(); + public: + data_store data; + pothos(int, int); + void setPort(HardwareSerial* s){ serial = *s;} + void update(); + bool synced; +}; + +pothos::pothos(int SlaveID, int en){ + ID = SlaveID; + enPin = en; + pinMode(en,OUTPUT); + synced = true; +} + +void pothos::update(){ + + if(synced){ + if(serial.available()>4){ + if(serial.read() == 0x00){ + if(serial.read() == ID){ + #ifdef POTHOS_DEBUG + Serial.println("slave ID Match"); + #endif + int fc = serial.read(); + switch(fc){ + case(1): + read(); + break; + case(2): + write(); + break; + case(3): + write_multiple(); + break; + } + serial.read(); + digitalWrite(enPin,HIGH); + serial.write(0xff); + serial.write(0xff); + serial.write(0xff); + serial.write(0xff); + while(serial.availableForWrite() != 63) delayMicroseconds(1); + delayMicroseconds(800); + digitalWrite(enPin,LOW); + }else{ + sync_counter = 0; + sync(); + } + }else { + sync_counter = 0; + sync(); + } + } + }else sync(); +} + +void pothos::read(){ + #ifdef POTHOS_DEBUG + Serial.println("Reading!"); + #endif + byte adrs[255]; + int length=0; + adrs[length] = serial.read(); + while(serial.peek() != 0xff){ + length++; + adrs[length] = serial.read(); + } + serial.read(); + digitalWrite(enPin,HIGH); + for(int i=0;i<=length;i++){ + #ifdef POTHOS_DEBUG + Serial.print("Transmitting at adr "); + Serial.print(adrs[i]); + Serial.print(" : "); + #endif + int type = data.get_type(adrs[i]); + + switch(type){ + case(1): + #ifdef POTHOS_DEBUG + Serial.println(data.get_char_data(adrs[i])); + #endif + serial.write(data.get_char_data(adrs[i])); + break; + case(2): + #ifdef POTHOS_DEBUG + Serial.println(data.get_int_data(adrs[i])); + #endif + for(int j=1;j>=0;j = j-1) + serial.write((byte)(0 | (data.get_int_data(adrs[i]) >> 8*(1-j)))); + break; + case(3): + #ifdef POTHOS_DEBUG + Serial.println(data.get_long_data(adrs[i])); + #endif + for(int j=3;j>=0;j = j-1) + serial.write((byte)(0 | ((data.get_long_data(adrs[i]) >> 8*(3-j))))); + break; + case(4): + #ifdef POTHOS_DEBUG + Serial.println(data.get_float_data(adrs[i]),9); + #endif + float datf = data.get_float_data(adrs[i]); + byte* dat_raw = (byte*)(&datf); + for(int j=0;j<4;j++) + serial.write(dat_raw[j]); + break; + } + serial.write(0x00); + } + serial.write(0xff); + #ifdef POTHOS_DEBUG + Serial.println("\n"); + #endif +} + +void pothos::write(){ + int reg = serial.read(); + int len; + int type = data.get_type(reg); + #ifdef POTHOS_DEBUG + Serial.println("Writing single!"); + Serial.print("data type: "); + Serial.println(type); + #endif + if(type <= 2) len = type; //determine length of data + else len = 4; + #ifdef POTHOS_DEBUG + Serial.print("register is : "); + Serial.println(reg); + Serial.print("length in bytes: "); + Serial.println(len); + if(len == 0) Serial.println("You are trying to access a register that has an unset type!!!!!!!!!!!! ERROR!!!"); + #endif + while(serial.available()<=len) delayMicroseconds(1); //get trapped in a loop until all of the data has arived + byte data_in[len]; + for(int i=0;i +#define RS485 Serial3 //Select RS-485 Serial Port. Reference https://www.pjrc.com/teensy/card7a_rev1.png to find serial port for your pinout. +//#define POTHOS_DEBUG // Enables Debug Message Functionality +#include //#include #include //#include @@ -12,45 +14,54 @@ FASTLED_USING_NAMESPACE ////////// Hardware / Data Enumerations ////////// enum HARDWARE { - GPS_IMU_RS485_EN = 22, + //IMU/GPS Comms + GPS_IMU_RS485_EN = 15, GPS_IMU_RS485_RX = 0, GPS_IMU_RS485_TX = 1, - + + //Comms COMMS_RS485_EN = 2, COMMS_RS485_RX = 7, COMMS_RS485_TX = 8, - + + //IMU/GPS Pinouts GPS_UART_RX = 9, GPS_UART_TX = 10, - IMU_SDA = 18, IMU_SCL = 19, - - NEO_PIXEL = 6, - - LED_BLUE_EXTRA = 13 + + //LED Strip Pinout + NEO_PIXEL = 23, + + //RGB LED pinouts + RGB_R = 5, + RGB_G = 32, + RGB_B = 6, + LED_BLUE_EXTRA = 13, + + //Laser MOSFET Pinout + LASER_SIGNAL = 14 }; -enum MODBUS_REGISTERS { - LED_CONTROL = 0, // Input +enum DATA { + LED_LIGHTS_STATE = 0, + LED_DRIVE_STATE = 1, + LED_WAYPOINT_STATE = 2, + LED_COMMS_STATE = 3, + LASERS = 4 }; #define GPS_SERIAL_PORT Serial2 #define GPS_IMU_STREAMING_PORT Serial1 #define Num_Pixels 60 -#define Num_rainbow_pixles 57 +#define Num_rainbow_pixels 57 ////////// Global Variables ////////// -///// Modbus -const uint8_t node_id = 1; -const uint8_t mobus_serial_port_number = 3; - -uint16_t modbus_data[] = {0}; -uint8_t num_modbus_registers = 0; +///// POTHOS +const uint8_t slave_id = 7; +int pothos_timeout = 50; -int8_t poll_state = 0; bool communication_good = false; -uint8_t message_count = 0; ///// GPS char current_byte = '$'; @@ -72,39 +83,28 @@ CRGB leds[Num_Pixels]; uint8_t gHue = 0; unsigned long pixel_clock = 0 ; int pixel_timer = 15; +uint8_t flash_duration = 1000; - -////////// Class Instantiations ////////// -Modbus slave(node_id, mobus_serial_port_number, HARDWARE::COMMS_RS485_EN); +///// LASERS +uint8_t laser_last_state = 0; +uint8_t laser_state = 0; -//Adafruit_NeoPixel strip(Num_Pixels, HARDWARE::NEO_PIXEL, NEO_GRB + NEO_KHZ800); +////////// Class Instantiations ////////// +pothos comms(slave_id, HARDWARE::COMMS_RS485_EN, pothos_timeout, HARDWARE::RGB_R, HARDWARE::RGB_G, HARDWARE::RGB_B); const char baud115200[] = "PUBX,41,1,3,3,115200,0"; void setup() { - // Debugging - Serial.begin(9600); - // while (!Serial); - - // Raw pin setup - setup_hardware(); - - // Setup modbus serial communication - num_modbus_registers = sizeof(modbus_data) / sizeof(modbus_data[0]); - slave.begin(115200); // baud-rate at 115200 - slave.setTimeOut(1750); - - // GPS & IMU serial streaming setup - GPS_IMU_STREAMING_PORT.begin(115200); - GPS_IMU_STREAMING_PORT.transmitterEnable(HARDWARE::GPS_IMU_RS485_EN); - - - // GPS Setup - GPS_SERIAL_PORT.begin(9600); - + comms.setup(115200); + #ifndef POTHOS_DEBUG + Serial.begin(115200); + #endif + + setup_hardware(); + set_data_types(); } void loop() { @@ -113,9 +113,10 @@ void loop() { JsonObject& root = jsonBuffer.createObject(); // Do normal polling - poll_modbus(); + comms.update(); set_leds(); + laser_zone(); process_gps_and_send_if_ready(root); // Print JSON and newline @@ -135,6 +136,17 @@ void setup_hardware() { FastLED.setBrightness(120); } +/** + * @description: Sets the Datatypes for Pothos registers + **/ +void set_data_types(){ + comms.data.set_type(DATA::LED_WAYPOINT_STATE, "int"); + comms.data.set_type(DATA::LED_DRIVE_STATE, "int"); + comms.data.set_type(DATA::LED_COMMS_STATE, "int"); + comms.data.set_type(DATA::LED_LIGHTS_STATE, "int"); + comms.data.set_type(DATA::LASERS, "char"); +} + void process_gps_and_send_if_ready(JsonObject &root) { root["gps"] = ""; @@ -160,68 +172,102 @@ void process_gps_and_send_if_ready(JsonObject &root) { } -void poll_modbus() { - poll_state = slave.poll(modbus_data, num_modbus_registers); - communication_good = !slave.getTimeOutState(); +/** + * @description: Sets the LASERS based on pothos register DATA + * + **/ +void laser_zone() { + + laser_state = DATA::LASERS; + + if(laser_last_state != laser_state) { + + laser_last_state = laser_state; + + if(laser_state) { + digitalWrite(HARDWARE::LASER_SIGNAL, HIGH); + } else { + digitalWrite(HARDWARE::LASER_SIGNAL, LOW); + } + } } -void rainbow() -{ +void rainbow() { // FastLED's built-in rainbow generator - fill_rainbow( leds, Num_rainbow_pixles , gHue, 7); + fill_rainbow( leds, Num_rainbow_pixels , gHue, 7); } -void rainbowWithGlitter() -{ +void rainbowWithGlitter() { // built-in FastLED rainbow, plus some random sparkly glitter rainbow(); addGlitter(80); } -void addGlitter( fract8 chanceOfGlitter) -{ +void addGlitter( fract8 chanceOfGlitter) { if( random8() < chanceOfGlitter) { - leds[ random16(Num_rainbow_pixles) ] += CRGB::White; + leds[ random16(Num_rainbow_pixels) ] += CRGB::White; } } -void confetti() -{ +void confetti() { // random colored speckles that blink in and fade smoothly - fadeToBlackBy( leds,Num_rainbow_pixles, 10); - int pos = random16(Num_rainbow_pixles); - leds[pos] += CHSV( gHue + random8(Num_rainbow_pixles), 200, 255); + fadeToBlackBy( leds,Num_rainbow_pixels, 10); + int pos = random16(Num_rainbow_pixels); + leds[pos] += CHSV( gHue + random8(Num_rainbow_pixels), 200, 255); } -void sinelon() -{ +void sinelon() { // a colored dot sweeping back and forth, with fadingtrails - fadeToBlackBy( leds, Num_rainbow_pixles, 20); - int pos = beatsin16( 13, 0, Num_rainbow_pixles-1 ); + fadeToBlackBy( leds, Num_rainbow_pixels, 20); + int pos = beatsin16( 13, 0, Num_rainbow_pixels-1 ); leds[pos] += CHSV( gHue, 255, 192); } -void bpm() -{ +void bpm() { // colored stripes pulsing at a defined Beats-Per-Minute (BPM) uint8_t BeatsPerMinute = 120; CRGBPalette16 palette = PartyColors_p; uint8_t beat = beatsin8( BeatsPerMinute, 64, 255); - for( int i = 0; i < Num_rainbow_pixles; i++) { //9948 + for( int i = 0; i < Num_rainbow_pixels; i++) { //9948 leds[i] = ColorFromPalette(palette, gHue+(i*2), beat-gHue+(i*10)); } } void juggle() { // eight colored dots, weaving in and out of sync with each other - fadeToBlackBy( leds, Num_rainbow_pixles, 20); + fadeToBlackBy( leds, Num_rainbow_pixels, 20); byte dothue = 0; for( int i = 0; i < 8; i++) { - leds[beatsin16( i+7, 0, Num_rainbow_pixles-1 )] |= CHSV(dothue, 200, 255); + leds[beatsin16( i+7, 0, Num_rainbow_pixels-1 )] |= CHSV(dothue, 200, 255); dothue += 32; } } +void autonomous() { + for(int i = 0; i < Num_rainbow_pixels; i++) { + leds[i] = red; + } +} + +void teleoperation() { + for(int i = 0; i < Num_rainbow_pixels; i++) { + leds[i] = blue; + } +} + +void complete() { + if(millis() - pixel_clock > flash_duration) { + for(int i = 0; i < Num_rainbow_pixels; i++) { + if(leds[i] != green) { + leds[i] = green; + } else { + leds[i] = CHSV(0, 0, 0); + } + } + + pixel_clock = millis(); + } +} void run_leds() { if (millis() - pixel_clock > pixel_timer) { @@ -229,10 +275,10 @@ void run_leds() { static uint8_t led = 0; static uint8_t dir = 1; leds[led] = CHSV(gHue, 240, 170); - for (int i = 0; i < Num_rainbow_pixles; i++) { + for (int i = 0; i < Num_rainbow_pixels; i++) { leds[i].nscale8(237 ); } - if (led == Num_rainbow_pixles - 1) { + if (led == Num_rainbow_pixels - 1) { dir = -1; } else if (led == 0) { dir = 1; @@ -243,7 +289,7 @@ void run_leds() { } } -CRGB status_scale(int scale, int val){ +CRGB status_scale(int scale, int val) { if(val < scale/2) return CRGB(254,70*val*2/scale,0); else if(val > scale/2) @@ -252,12 +298,15 @@ CRGB status_scale(int scale, int val){ return yellow; } -void set_leds(){ + +/** + * @Description: Updates the LEDs for NEOPIXEL Display via Register Data from Pothos + **/ +void set_leds() { EVERY_N_MILLISECONDS( 20 ) { gHue++; } - - // registers to store data - bool IMU_cals[] = {0,0,0}; + + bool comms_status = communication_good; uint8_t drive_state = 0; uint8_t waypoint_state = 0; @@ -266,20 +315,14 @@ void set_leds(){ //int * color = blue; - //collect data from modbus registers - for(int i=0;i<16;i++){ - if(i < 3) - IMU_cals[i] = bitRead(modbus_data[MODBUS_REGISTERS::LED_CONTROL], i); - else if(i>2 && i<5) - bitWrite(drive_state, i-3, bitRead(modbus_data[MODBUS_REGISTERS::LED_CONTROL], i)); - else if(i>4 && i<7) - bitWrite(waypoint_state, i-5, bitRead(modbus_data[MODBUS_REGISTERS::LED_CONTROL], i)); - else if(i>6 && i<11) - bitWrite(lights, i-7, bitRead(modbus_data[MODBUS_REGISTERS::LED_CONTROL], i)); - else if(i>10 && i<16) - bitWrite(comms, i-11, bitRead(modbus_data[MODBUS_REGISTERS::LED_CONTROL], i)); - } + //collect data from pothos registers + drive_state = DATA::LED_DRIVE_STATE; + waypoint_state = DATA::LED_WAYPOINT_STATE; + comms = DATA::LED_COMMS_STATE; + lights = DATA::LED_LIGHTS_STATE; + + //Calling LED Control functions switch(lights){ case 1: rainbow(); @@ -302,6 +345,12 @@ void set_leds(){ case 0: run_leds(); break; + case 7: + autonomous(); + case 8: + teleoperation(); + case 9: + complete(); } if(!comms_status)