Diff: main.cpp

Revision:

3:4dcadafb89a2

Parent:

2:bdfce41aae53

Child:

4:ab834eebd496

--- a/main.cpp	Tue Nov 16 19:10:08 2021 +0000
+++ b/main.cpp	Tue Nov 16 21:21:10 2021 +0000
@@ -10,10 +10,37 @@
 #include "EthernetInterface.h"
 #include "bmp3.h"
 
-// overall loop time
-#define DT 0.005f //0.001f // 0.005f is 200Hz
+/*---------------------- Initial setup ----------------------*/
+// Serial port
+Serial       pc(USBTX, USBRX);
+// Ticker for logging at a fixed frequency
+Ticker datalog;
+bool tickerActivated = false;
+void log_data(){
+    tickerActivated = true;
+} 
 
-//SETUP THE LTC CHIP
+/*---------------------- Ethernet setup ----------------------*/
+const int SERVER_PORT = 2;
+const char* SERVER_ADDRESS = "192.168.1.2";    // Adress of the other Mbed (Mbed B)
+const int LOCAL_PORT = 1;
+const char* ip = "192.168.1.1";     // Mbed A = 1; Mbed B = 2
+const char* mask = "255.255.255.0";
+const char* gateway = "192.168.1.10";
+EthernetInterface eth; // network stack
+SocketAddress local; // local address
+SocketAddress client; // client address (connection to other board)
+UDPSocket server; // UDP socket (peripheral on this board)
+
+// Messages to fill and send over ethernet
+// buffer length is 6*6 + 11*8 + 14 = 138 
+char send_buf[138];
+
+/*---------------------- IMU setup ----------------------*/
+// TODO: add this eventually
+
+
+/*---------------------- LTC chip setup ----------------------*/
 #define LTC_MOSI    PB_2
 #define LTC_MISO    PC_11
 #define LTC_CLK     PC_10
@@ -25,40 +52,118 @@
 #define NUM_CHANNELS   6 //8   // 8 total channels
 #define CH_SIZE   3   // 3 words / channel
 
-SPI LTC_CHIP(LTC_MOSI, LTC_MISO, LTC_CLK);
+SPI LTC_chip(LTC_MOSI, LTC_MISO, LTC_CLK);
 DigitalOut cs_LTC(LTC_CS);
 DigitalIn bsy_LTC(BSY);
 DigitalOut CNV_PIN(CNV);
 
 float CONV_FACTOR = 0.00031294782f; //--> for -10.24 --> 10.24V
-
-//SETUP REGISTERS
+// receive and store data from LTC chip
 uint8_t rx_buff[BUFF_SIZE]; //each is an 8-bit word 
 ltc_spi adc_data;
+int16_t ati_data[6];
 
-//setup SERIAL
-Serial       pc(USBTX, USBRX);
+// calibration matrix for ATI sensor
+float Fxc[6] = {-0.1971322934773, -0.04349257334311, 2.298051028435, -80.35044049387, 1.362983909976, 78.23673392118};
+float Fyc[6] = {-0.855555082028, 90.04004739944, -0.2236363056212, -46.22515556189, 0.4634720862657, -45.33866366008};
+float Fzc[6] = {126.0118743229, -3.400673797001, 125.6239720415, -3.58428375801, 124.6128824882, -3.121863244239};
+float Mxc[6] = {-0.03257086475743, 1.078228404461, -4.281073433774, -0.4388170286617, 4.26206973335, 7 - 0.6391561102933}; // check that these rows are x,y,z too
+float Myc[6] = {5.013689449541, -0.1348267445261, -2.487858919058, 1.036624778844, -2.465023328927, -0.8776820303935};
+float Mzc[6] = {0.03045090196646, -2.681788264229, 0.06994993822276, -2.787067635975, -0.04822780843519, -2.696991001959};
+// bias terms
+float bias[6];
+// convert forces and torques
+float ft_data[6];
+
+// send 6 configuration words with the 6 channel numbers, still the default softspan 7 conversion
+void config_LTC(){
+    uint8_t discard;
+    cs_LTC=0;
+    // set sampling order for channels 0-5
+    discard = LTC_chip.write(0b10000111); // byte is 7:V, 6:0, 5-3:ID, 2-0:mode
+    discard = LTC_chip.write(0b10001111);
+    discard = LTC_chip.write(0b10010111);
+    discard = LTC_chip.write(0b10011111);
+    discard = LTC_chip.write(0b10100111);
+    discard = LTC_chip.write(0b10101111);   
+    cs_LTC=1;
+}
 
-// set up IMU SPI bus?
+void read_LTC_data() {
+    
+    // right now, programmed LTC chip to only sample 6 channels, starting at channel 0 on next conversion (in config_LTC)...also changed number of channels variables
+    // TODO: could also decode channel ID from bits 4-6 of the info byte in message packet, assign to correct adc channel
+    // TODO: could also include next channel for sampling in each conversion message
+    
+    // sample from all channels
+    for (int i = 0; i<NUM_CHANNELS; i++) {
+        //request conversion
+        CNV_PIN=1;
+        wait_ns(60); // wait for 60ns
+        CNV_PIN=0;
+        //WAIT FOR BSY --> bsy_LTC
+        while(bsy_LTC==1){}
+        //debugging ONLY
+        wait_us(1);
+        //then ask for data
+        cs_LTC=0;   
+        //read data
+        int bytecount = CH_SIZE*i;
+        while(bytecount < CH_SIZE*(1+i)){
+            rx_buff[bytecount] = LTC_chip.write(0x00);
+            bytecount++;
+        }
+        cs_LTC=1; //lift CS
+    }
+    // fill adc data struct with received bytes    
+    for(int i = 0; i < BUFF_SIZE; i++){
+        ((uint8_t*)(&adc_data))[i] = rx_buff[i];
+    }
+    // pull out voltage data here
+    ati_data[0] = adc_data.channel[0].cnv_upper<<8 | adc_data.channel[0].cnv_lower;
+    ati_data[1] = adc_data.channel[1].cnv_upper<<8 | adc_data.channel[1].cnv_lower;
+    ati_data[2] = adc_data.channel[2].cnv_upper<<8 | adc_data.channel[2].cnv_lower;
+    ati_data[3] = adc_data.channel[3].cnv_upper<<8 | adc_data.channel[3].cnv_lower;
+    ati_data[4] = adc_data.channel[4].cnv_upper<<8 | adc_data.channel[4].cnv_lower;
+    ati_data[5] = adc_data.channel[5].cnv_upper<<8 | adc_data.channel[5].cnv_lower;
+}
+
+// convert integer readings to voltages to f/t values
+void convert_LTC_data(){
+    // dummy buffer to store converted ADC vals
+    float buff[6];
+    for(int i=0; i<6; i++){
+        buff[i] = CONV_FACTOR*((float)ati_data[i]-bias[i]); // bias[] is in same units as msg[]
+        ft_data[i] = 0; // also zero out ft_data here
+    }
+    // convert each f/t value separately
+    for(int i=0; i<6; i++){
+        ft_data[0] += Fxc[i]*buff[i];   
+        ft_data[1] += Fyc[i]*buff[i];  
+        ft_data[2] += Fzc[i]*buff[i];  
+        ft_data[3] += Mxc[i]*buff[i];  
+        ft_data[4] += Myc[i]*buff[i];  
+        ft_data[5] += Mzc[i]*buff[i];  
+    }
+}
 
 
-////Setup ETHERNET
-//// Set up ethernet connection
-//const int SERVER_PORT = 2;
-//const char* SERVER_ADDRESS = "192.168.1.2";    // Adress of the other Mbed (Mbed B)
-//const int LOCAL_PORT = 1;
-//const char* ip = "192.168.1.1";     // Mbed A = 1; Mbed B = 2
-//const char* mask = "255.255.255.0";
-//const char* gateway = "192.168.1.10";
-//EthernetInterface eth; // network stack
-//SocketAddress local; // local address
-//SocketAddress client; // client address (connection to other board)
-//UDPSocket server; // UDP socket (peripheral on this board)
+/*---------------------- Force sensor setup ----------------------*/
+#define SN_MOSI     PC_3
+#define SN_MISO     PC_2
+#define SN_CLK      PB_10
+#define SN_CS       PB_12
+#define SN_A        PG_15
+#define SN_B        PG_10
+#define SN_C        PG_12
 
+SPI sn_spi(SN_MOSI, SN_MISO, SN_CLK); //Sensor SPI - mosi, miso, sclk
+DigitalOut dec_enable(SN_CS);
+DigitalOut dec_bit0(SN_A); 
+DigitalOut dec_bit1(SN_B); 
+DigitalOut dec_bit2(SN_C);
 
-/// INITIALIZE SENSOR FOR TESTING HERE
-Timer timer;
-int begin, end;
+// structs for individual sensor devices
 struct bmp3_dev s1; // sets up dev as a 'bmp3_dev structure' w/ associated variables
 struct bmp3_dev s2;
 struct bmp3_dev s3;
@@ -67,24 +172,20 @@
 struct bmp3_dev s6;
 struct bmp3_dev s7;
 struct bmp3_dev s8;
-// Sensor pins
-SPI sn_spi(PC_3, PC_2, PB_10); //Sensor SPI - mosi, miso, sclk
-DigitalOut dec_enable(PB_12);
-DigitalOut dec_bit0(PG_15); 
-DigitalOut dec_bit1(PG_10); 
-DigitalOut dec_bit2(PG_12);
-
-
-Ticker datalog;
-
-int sn_data[8];
-
-bool tickerActivated = false;
-
-void log_data(){
-    tickerActivated = true;
-} 
-
+// Structs for sensor data
+struct bmp3_data sn_data1;
+struct bmp3_data sn_data2;
+struct bmp3_data sn_data3;
+struct bmp3_data sn_data4;
+struct bmp3_data sn_data5;
+struct bmp3_data sn_data6;
+struct bmp3_data sn_data7;
+struct bmp3_data sn_data8;
+// Store sensor output data (pressures)
+int pr_data[8];
+// Configure data from sensor
+uint8_t sensor_comp = uint8_t(1)| uint8_t(1<<1); // sensor_comp = BMP3_PRESS | BMP3_TEMP;
+    
 void writeLow(uint8_t pin){ // modified for just 2 sensors
     dec_enable = 0;
     if (pin == 1){
@@ -164,7 +265,7 @@
     wait_ms(msec); 
 }
 
-void config_dev(struct bmp3_dev *dev){
+void config_bmp_dev(struct bmp3_dev *dev){
     int8_t rslt=0;//BMP3_OK; // get error with rslt = BMP3_OK;
     
     dev -> intf = BMP3_SPI_INTF;
@@ -205,306 +306,129 @@
     rslt = bmp3_get_sensor_settings(dev);  
 }
 
-
-
-
-
-
-
-
-
-// calibration matrix for ATI sensor
-float Fxc[6] = {-0.1971322934773, -0.04349257334311, 2.298051028435, -80.35044049387, 1.362983909976, 78.23673392118};
-float Fyc[6] = {-0.855555082028, 90.04004739944, -0.2236363056212, -46.22515556189, 0.4634720862657, -45.33866366008};
-float Fzc[6] = {126.0118743229, -3.400673797001, 125.6239720415, -3.58428375801, 124.6128824882, -3.121863244239};
-float Mxc[6] = {-0.03257086475743, 1.078228404461, -4.281073433774, -0.4388170286617, 4.26206973335, 7 - 0.6391561102933}; // check that these rows are x,y,z too
-float Myc[6] = {5.013689449541, -0.1348267445261, -2.487858919058, 1.036624778844, -2.465023328927, -0.8776820303935};
-float Mzc[6] = {0.03045090196646, -2.681788264229, 0.06994993822276, -2.787067635975, -0.04822780843519, -2.696991001959};
-
-// bias terms
-float bias[6];
-
-// convert forces and torques
-float ftdata[6];
-
-
-//Setup Timer
-Timer t;
-
-//MESSAGE TO SEND 
-//float msg[6];
-int16_t msg[6];
-char send_buf[36];
-//server.sendto(client, send_buf, sizeof(send_buf)-1); --> command to send
-/*msg[0] = thumb_prox.read_u16()/64;
-        msg[1] = thumb_dist.read_u16()/64; 
-        msg[2] = index_prox.read_u16()/64; 
-        msg[3] = index_dist.read_u16()/64; 
-        msg[4] = mid_prox.read_u16()/64; 
-        msg[5] = mid_dist.read_u16()/64; */ //--> populating the message
-
-
-
-// send 6 configuration words with the 6 channel numbers, still the default softspan 7 conversion
-void config_LTC(){
-    
-    uint8_t discard;
-    
-    cs_LTC=0;
-    
-    // set sampling order for channels 0-5
-    discard = LTC_CHIP.write(0b10000111); // byte is 7:V, 6:0, 5-3:ID, 2-0:mode
-    discard = LTC_CHIP.write(0b10001111);
-    discard = LTC_CHIP.write(0b10010111);
-    discard = LTC_CHIP.write(0b10011111);
-    discard = LTC_CHIP.write(0b10100111);
-    discard = LTC_CHIP.write(0b10101111);
-        
-    cs_LTC=1;
-    
-    }
+void read_bmp_data(){
+    // read data from sensors
+    bmp3_get_sensor_data(sensor_comp, &sn_data1, &s1); // todo: combine into a read_data function        
+    bmp3_get_sensor_data(sensor_comp, &sn_data2, &s2);
+    bmp3_get_sensor_data(sensor_comp, &sn_data3, &s3);
+    bmp3_get_sensor_data(sensor_comp, &sn_data4, &s4);
+    bmp3_get_sensor_data(sensor_comp, &sn_data5, &s5);
+    bmp3_get_sensor_data(sensor_comp, &sn_data6, &s6);
+    bmp3_get_sensor_data(sensor_comp, &sn_data7, &s7);
+    bmp3_get_sensor_data(sensor_comp, &sn_data8, &s8);
+}
 
 
 
-/*
-We need to 
-- pull CS low
-- read the data into a register
-- release CS
-- decode the data
-- repeat
-*/
-void read_data() {
-    
-    // right now, programmed LTC chip to only sample 6 channels, starting at channel 0 on next conversion (in config_LTC)...also changed number of channels variables
-    // TODO: could also decode channel ID from bits 4-6 of the info byte in message packet, assign to correct adc channel
-    // TODO: could also include next channel for sampling in each conversion message
-    
-    for (int i = 0; i<NUM_CHANNELS; i++) {
-        //request conversion
-        CNV_PIN=1;
-        wait_ns(60); // wait for 60ns
-        CNV_PIN=0;
-        //WAIT FOR BSY --> bsy_LTC
-        while(bsy_LTC==1){}
-        //debugging ONLY
-        wait_us(1);
-        //then ask for data
-        cs_LTC=0;
-        //spi id register
-        //LTC_CHIP.write(0x00); // --> do we need this? 
-        //read 144 bytes
-        
-        
-        
-        
-        //read data
-        int bytecount = CH_SIZE*i;
-        while(bytecount < CH_SIZE*(1+i)){
-            rx_buff[bytecount] = LTC_CHIP.write(0x00);
-            bytecount++;
-        }
-        
-        //lift CS
-        cs_LTC=1;
-    }
-    
-    //PACK THE STRUCT
-    
-    for(int i = 0; i < BUFF_SIZE; i++)
-    {
-        ((uint8_t*)(&adc_data))[i] = rx_buff[i];
-    }
-    
-    
-    // fill msg[] here
-    msg[0] = adc_data.channel[0].cnv_upper<<8 | adc_data.channel[0].cnv_lower;
-    msg[1] = adc_data.channel[1].cnv_upper<<8 | adc_data.channel[1].cnv_lower;
-    msg[2] = adc_data.channel[2].cnv_upper<<8 | adc_data.channel[2].cnv_lower;
-    msg[3] = adc_data.channel[3].cnv_upper<<8 | adc_data.channel[3].cnv_lower;
-    msg[4] = adc_data.channel[4].cnv_upper<<8 | adc_data.channel[4].cnv_lower;
-    msg[5] = adc_data.channel[5].cnv_upper<<8 | adc_data.channel[5].cnv_lower;
-    
-    // set a flag to add bias here after first 100 samples?
-    
-    
-    //WRITE THIS TO A STRUCT OF SOME SORT but we need to delete the bits we don't care about
-}
-
-// convert integer readings to voltages to f/t values
-void convert_data(){
-    
-    // dummy buffer to store converted ADC vals
-    float buff[6];
-    for(int i=0; i<6; i++){
-        buff[i] = CONV_FACTOR*((float)msg[i]-bias[i]); // bias[] is in same units as msg[]
-        ftdata[i] = 0; // also zero out ftdata here
-    }
-    
-    // convert each f/t value separately
-    for(int i=0; i<6; i++){
-        ftdata[0] += Fxc[i]*buff[i];   
-        ftdata[1] += Fyc[i]*buff[i];  
-        ftdata[2] += Fzc[i]*buff[i];  
-        ftdata[3] += Mxc[i]*buff[i];  
-        ftdata[4] += Myc[i]*buff[i];  
-        ftdata[5] += Mzc[i]*buff[i];  
-    }
-    
-    }
-
-
-
+/*---------------------- Main function ----------------------*/
 int main()
 {
     // Initialise the digital pin LED1 as an output
     DigitalOut led(LED1);
     CNV_PIN=0;
     
-    //setup pc
+    // Set up serial port
     pc.baud(115200);
-    pc.printf("------STARTUP------\n\n\n\r");
     
-    //setup SPI
-    LTC_CHIP.format(8, 0);
-    LTC_CHIP.frequency(10000); //10MHz? //60Mhz clock frequency 
-
+    // Set up LTC chip
+    LTC_chip.format(8, 0);
+    LTC_chip.frequency(10000); //10MHz? //60Mhz clock frequency 
     wait_ms(1);
-    config_LTC();
+    config_LTC(); // programs sample order
     wait_ms(1);
     
-//    //setup ETHERNET
-//    eth.set_network(ip, mask, gateway);
-//    eth.connect();
-//    //more ETHERNET
-//    client.set_port(SERVER_PORT);
-//    client.set_ip_address(SERVER_ADDRESS);
-//    local.set_port(LOCAL_PORT);
-//    local.set_ip_address(ip);
-//    int code = server.open(&eth);
-//    if(code!=0) { pc.printf("Error from opening server = %d\n\r",code); }    
-//    code = server.bind(local);
-//    if(code!=0) { pc.printf("Error from binding socket = %d\n\r",code); }
+    // Set up ethernet
+    eth.set_network(ip, mask, gateway);
+    eth.connect();
+    wait_ms(1);
+    client.set_port(SERVER_PORT);
+    client.set_ip_address(SERVER_ADDRESS);
+    local.set_port(LOCAL_PORT);
+    local.set_ip_address(ip);
+    int code = server.open(&eth);
+    if(code!=0) { pc.printf("Error from opening server = %d\n\r",code); }    
+    code = server.bind(local);
+    if(code!=0) { pc.printf("Error from binding socket = %d\n\r",code); }
     
-    
-    
-    // SET UP FROM OLD SENSOR CODE HERE
-    
-    // Set up sensor pins
+    // Set up force sensor and decoder
     dec_enable = 1;    
-    // Initialize dev 1
+    // Initialize bmp device 1
     s1.dev_id = 1;  // tells which cs pin associated with device
-    config_dev(&s1);
-    //Initialize dev 2
+    config_bmp_dev(&s1);
+    //Initialize bmp device 2
     s2.dev_id = 2;  // tells which cs pin associated with device
-    config_dev(&s2);
-    //Initialize dev 3
+    config_bmp_dev(&s2);
+    //Initialize bmp device 3
     s3.dev_id = 3;  // tells which cs pin associated with device
-    config_dev(&s3);
-    //Initialize dev 4
+    config_bmp_dev(&s3);
+    //Initialize bmp device 4
     s4.dev_id = 4;  // tells which cs pin associated with device
-    config_dev(&s4);
-    //Initialize dev 5
+    config_bmp_dev(&s4);
+    //Initialize bmp device 5
     s5.dev_id = 5;  // tells which cs pin associated with device
-    config_dev(&s5);
-    //Initialize dev 6
+    config_bmp_dev(&s5);
+    //Initialize bmp device 6
     s6.dev_id = 6;  // tells which cs pin associated with device
-    config_dev(&s6);
-    //Initialize dev 7
+    config_bmp_dev(&s6);
+    //Initialize bmp device 7
     s7.dev_id = 7;  // tells which cs pin associated with device
-    config_dev(&s7);
-    //Initialize dev 8
+    config_bmp_dev(&s7);
+    //Initialize bmp device 8
     s8.dev_id = 8;  // tells which cs pin associated with device
-    config_dev(&s8);
-    
-    // Getting data from sensor
-    struct bmp3_data data1;
-    struct bmp3_data data2;
-    struct bmp3_data data3;
-    struct bmp3_data data4;
-    struct bmp3_data data5;
-    struct bmp3_data data6;
-    struct bmp3_data data7;
-    struct bmp3_data data8;
-    
-    uint8_t sensor_comp;
-    sensor_comp = uint8_t(1)| uint8_t(1<<1); // sensor_comp = BMP3_PRESS | BMP3_TEMP;
-    
-    
-
+    config_bmp_dev(&s8);
     
-    
-    pc.printf("Calibrating ATI sensor...");
-    t.start();
-    // read 100 times to calculate bias voltages
-    for(int i=0; i<100; i++){
-        t.reset();
-        read_data();  
+    // Calculate bias voltages for ATI sensor
+    for(int i=0; i<100; i++){ // read 100 times and take average
+        read_LTC_data();  
         for(int j=0; j<6; j++){
-            bias[j] += 0.01*(float)msg[j];
+            bias[j] += 0.01*(float)ati_data[j];
         }
-        while(t.read()<0.001){;}
+        wait_ms(1);
     }
-    t.stop();
-    pc.printf("done!\n\r");
-//    pc.printf("%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\n\r", bias[0],bias[1],bias[2],bias[3],bias[4],bias[5]);
+    pc.printf("ATI sensor is calibrated with biases of: %.2f, %.2f, %.2f, %.2f, %.2f, %.2f \n\r", bias[0],bias[1],bias[2],bias[3],bias[4],bias[5]);
+      
+    // Attach sampling interrupt
+//    datalog.attach_us(&log_data,50000); // 1000us = 1ms (10000 = 10 ms = 100 Hz)
+    datalog.attach(&log_data,0.1); // 10Hz
     
-    pc.printf("Starting to sample...\n\n\r");
-    
-    // attach sampling interrupt
-    datalog.attach_us(&log_data,10000); // 1000us = 1ms (10000 = 10 ms = 100 Hz)
-
     while (true) {
         
         if(tickerActivated == true) { 
 
             tickerActivated = false;
             
-            // LTC chip for ATI sensor
-            read_data();
-            convert_data(); // remove this eventually, convert after sending over ethernet
-            // print received data
-//            pc.printf("%d,%d,%d,%d,%d,%d\n\r", msg[0],msg[1],msg[2],msg[3],msg[4],msg[5]);
-            // print converted data
-            pc.printf("%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\t", ftdata[0],ftdata[1],ftdata[2],ftdata[3],ftdata[4],ftdata[5]);
-            
-            // Custom force sensor
-                       
-            bmp3_get_sensor_data(sensor_comp, &data1, &s1); // todo: combine into a read_data function        
-            bmp3_get_sensor_data(sensor_comp, &data2, &s2);
-            bmp3_get_sensor_data(sensor_comp, &data3, &s3);
-            bmp3_get_sensor_data(sensor_comp, &data4, &s4);
-            bmp3_get_sensor_data(sensor_comp, &data5, &s5);
-            bmp3_get_sensor_data(sensor_comp, &data6, &s6);
-            bmp3_get_sensor_data(sensor_comp, &data7, &s7);
-            bmp3_get_sensor_data(sensor_comp, &data8, &s8);
+            // Sample from LTC chip 
+            read_LTC_data();
+            convert_LTC_data(); // TODO: remove this eventually, since we'll convert the data after sending over ethernet
+            // Print received data
+            //pc.printf("%6d,%6d,%6d,%6d,%6d,%6d\n\r", ati_data[0],ati_data[1],ati_data[2],ati_data[3],ati_data[4],ati_data[5]);
+            // Print converted data
+            pc.printf("%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,  ", ft_data[0],ft_data[1],ft_data[2],ft_data[3],ft_data[4],ft_data[5]);
             
-            // todo: combine into a convert_data function, or inlcude in the read_data function...at least have d[] instead of d1,d2,d3,etc.
-            sn_data[0] = int(data1.pressure)-100000; // pressure is returned in Pa, could subtract actual sea level pressure here
-            sn_data[1] = int(data2.pressure)-100000;
-            sn_data[2] = int(data3.pressure)-100000;
-            sn_data[3] = int(data4.pressure)-100000;
-            sn_data[4] = int(data5.pressure)-100000;
-            sn_data[5] = int(data6.pressure)-100000;
-            sn_data[6] = int(data7.pressure)-100000;
-            sn_data[7] = int(data8.pressure)-100000;
+            // Sample from force sensor
+            read_bmp_data();
+            // Convert to relative pressures
+            pr_data[0] = int(sn_data1.pressure)-100000; // pressure is returned in Pa, could subtract actual sea level pressure here
+            pr_data[1] = int(sn_data2.pressure)-100000;
+            pr_data[2] = int(sn_data3.pressure)-100000;
+            pr_data[3] = int(sn_data4.pressure)-100000;
+            pr_data[4] = int(sn_data5.pressure)-100000;
+            pr_data[5] = int(sn_data6.pressure)-100000;
+            pr_data[6] = int(sn_data7.pressure)-100000;
+            pr_data[7] = int(sn_data8.pressure)-100000;
+            // Print received data
+            pc.printf("%d,%d,%d,%d,%d,%d,%d,%d\n\r", pr_data[0],pr_data[1],pr_data[2],pr_data[3],pr_data[4],pr_data[5],pr_data[6],pr_data[7]);      
             
-            pc.printf("%03d,%03d,%03d,%03d,%03d,%03d,%03d,%03d \n\r", sn_data[0],sn_data[1],sn_data[2],sn_data[3],sn_data[4],sn_data[5],sn_data[6],sn_data[7]);      
-            
-//            pc.printf("/n/r");
-            
-            //POPULATE THE ETHERNET MESSAGE
-//            sprintf(send_buf, "%d,%d,%d,%d,%d,%d", msg[0],msg[1],msg[2],msg[3],msg[4],msg[5]);       
-//            server.sendto(client, send_buf, sizeof(send_buf)-1); // send message
-            
-            
-            
+            // Pack and send the ethernet message
+            sprintf(send_buf, "%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n", ati_data[0],ati_data[1],ati_data[2],ati_data[3],ati_data[4],ati_data[5], pr_data[0],pr_data[1],pr_data[2],pr_data[3],pr_data[4],pr_data[5],pr_data[6],pr_data[7]);       
+            server.sendto(client, send_buf, sizeof(send_buf)); // send message, look for '\n' character when decoding the string
             
          }
         
     }
+        
+    // Terminate connection (if you want)
+    server.close();
+    eth.disconnect();
     
-//    // Terminate connection (if you want)
-//    server.close();
-//    eth.disconnect();
 }