Diff: main.cpp

Revision:

2:bdfce41aae53

Parent:

1:466049963f1f

Child:

3:4dcadafb89a2

--- a/main.cpp	Thu Nov 04 21:09:53 2021 +0000
+++ b/main.cpp	Tue Nov 16 19:10:08 2021 +0000
@@ -8,6 +8,10 @@
 #include "ltc_message.h"
 #include <SPI.h>
 #include "EthernetInterface.h"
+#include "bmp3.h"
+
+// overall loop time
+#define DT 0.005f //0.001f // 0.005f is 200Hz
 
 //SETUP THE LTC CHIP
 #define LTC_MOSI    PB_2
@@ -17,8 +21,8 @@
 #define BSY         PB_5
 #define CNV         PB_6
 
-#define BUFF_SIZE   24   // 24, 8-bit words will come through
-#define NUM_CHANNELS   8   // 8 total channels
+#define BUFF_SIZE   18 //24   // 24, 8-bit words will come through
+#define NUM_CHANNELS   6 //8   // 8 total channels
 #define CH_SIZE   3   // 3 words / channel
 
 SPI LTC_CHIP(LTC_MOSI, LTC_MISO, LTC_CLK);
@@ -26,7 +30,7 @@
 DigitalIn bsy_LTC(BSY);
 DigitalOut CNV_PIN(CNV);
 
-float CONV_FACTOR = 0.00031294782; //--> for -10.24 --> 10.24V
+float CONV_FACTOR = 0.00031294782f; //--> for -10.24 --> 10.24V
 
 //SETUP REGISTERS
 uint8_t rx_buff[BUFF_SIZE]; //each is an 8-bit word 
@@ -35,21 +39,195 @@
 //setup SERIAL
 Serial       pc(USBTX, USBRX);
 
-//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)
+// set up IMU SPI bus?
+
+
+////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)
+
+
+/// INITIALIZE SENSOR FOR TESTING HERE
+Timer timer;
+int begin, end;
+struct bmp3_dev s1; // sets up dev as a 'bmp3_dev structure' w/ associated variables
+struct bmp3_dev s2;
+struct bmp3_dev s3;
+struct bmp3_dev s4;
+struct bmp3_dev s5;
+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;
+} 
+
+void writeLow(uint8_t pin){ // modified for just 2 sensors
+    dec_enable = 0;
+    if (pin == 1){
+        dec_bit0 = 0;
+        dec_bit1 = 0;
+        dec_bit2 = 0;
+    }
+    else if (pin == 2){
+        dec_bit0 = 1;
+        dec_bit1 = 0;
+        dec_bit2 = 0;
+    }
+    else if (pin == 3){
+        dec_bit0 = 0;
+        dec_bit1 = 1;
+        dec_bit2 = 0;
+    }
+    else if (pin == 4){
+        dec_bit0 = 1;
+        dec_bit1 = 1;
+        dec_bit2 = 0;
+    }
+    else if (pin == 5){
+        dec_bit0 = 0;
+        dec_bit1 = 0;
+        dec_bit2 = 1;
+    }
+    else if (pin == 6){
+        dec_bit0 = 1;
+        dec_bit1 = 0;
+        dec_bit2 = 1;
+    }
+    else if (pin == 7){
+        dec_bit0 = 0;
+        dec_bit1 = 1;
+        dec_bit2 = 1;
+    }
+    else if (pin == 8){
+        dec_bit0 = 1;
+        dec_bit1 = 1;
+        dec_bit2 = 1;
+    }
+}
+
+void writeHigh(){
+    dec_enable = 1; // write all pins high by disabling the decoder
+}
 
-// overall loop time
-#define DT 0.001f // 0.005f is 200Hz
+// General Read and Write functions
+// read function: |0x80 done in library, dummy byte taken care of in library
+static int8_t user_spi_read(uint8_t cspin, uint8_t reg_addr, uint8_t *reg_data, uint16_t len) {
+    writeLow(cspin);
+    sn_spi.write(reg_addr); // send read command to chip_id register (reg 0x00)
+    for(int i = 0; i < len; i++){
+        *(reg_data+i) = sn_spi.write(0x00); // read in 2nd byte = chip_id
+    }
+    writeHigh();
+    return 0;
+}
+
+static int8_t user_spi_write(uint8_t cspin, uint8_t reg_addr, uint8_t *reg_data, uint16_t len) {
+    writeLow(cspin);
+    sn_spi.write(reg_addr);
+    if (len>1) {
+        for(int i = 0; i < len-1; i++){
+            sn_spi.write(*(reg_data+i)); // send alternating register address and register bytes in multi write
+        }
+    }
+    else{
+        sn_spi.write(reg_data[0]);
+    }    
+    writeHigh();
+    return 0;
+}
+
+void user_delay_ms(uint32_t msec){ //delay in milliseconds
+    wait_ms(msec); 
+}
+
+void config_dev(struct bmp3_dev *dev){
+    int8_t rslt=0;//BMP3_OK; // get error with rslt = BMP3_OK;
+    
+    dev -> intf = BMP3_SPI_INTF;
+    dev -> read = &user_spi_read; // what to set here??? should be pointer to read function? &spi_read
+    dev -> write = &user_spi_write;// what to set here??? should be pointer to write function? &spi_write
+    dev -> delay_ms = &user_delay_ms;// what to set here???  delay in ms
+    rslt = bmp3_init(dev);
+    pc.printf("* initialize sensor result = 0x%x *\r\n", rslt);
+    wait(0.25);
+    
+    // ***** Configuring settings of sensor
+    // Normal Mode - bmp3_set_op_mode
+    // Temp En, Press En 
+    // OSR = no oversampling temp, press
+    // ODR = 200Hz temp, press
+    // IRR = no IRR filter
+    // ^^^all 4 above =  bmp3_set_sensor_settings
+    
+    // Set sensor settings (press en, temp en, OSR, ODR, IRR)
+    dev -> settings.press_en = 0x01; // BMP3_ENABLE
+    dev -> settings.temp_en = 0x01; //BMP3_ENABLE
+    dev -> settings.odr_filter.press_os = 0x00; //BMP3_NO_OVERSAMPLING
+    dev -> settings.odr_filter.temp_os = 0x00; //BMP3_NO_OVERSAMPLING
+    dev -> settings.odr_filter.odr = 0x00; //BMP3_ODR_200_HZ
+    dev -> settings.odr_filter.iir_filter = 0x00; //BMP3_IIR_Filter_disable
+    
+    uint16_t settings_sel;
+    //settings_sel = BMP3_PRESS_EN_SEL | BMP3_TEMP_EN_SEL | BMP3_PRESS_OS_SEL | BMP3_TEMP_OS_SEL | BMP3_IIR_FILTER_SEL | BMP3_ODR_SEL;
+    settings_sel = uint16_t(1 << 1) | uint16_t(1 << 2) | uint16_t(1 << 4) | uint16_t(1 << 5) | uint16_t(1 << 6) | uint16_t(1 << 7);
+    //settings_sel = uint16_t(1 << 1) | uint16_t(1 << 2);
+    rslt = bmp3_set_sensor_settings(settings_sel, dev);
+    
+    // Set operating (power) mode
+    dev -> settings.op_mode = 0x03; /// normal mode = 0x03
+    rslt = bmp3_set_op_mode(dev);
+    
+    // Check settings
+    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;
 
@@ -66,6 +244,28 @@
         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;
+    
+    }
+
+
+
 /*
 We need to 
 - pull CS low
@@ -76,6 +276,10 @@
 */
 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;
@@ -91,6 +295,9 @@
         //LTC_CHIP.write(0x00); // --> do we need this? 
         //read 144 bytes
         
+        
+        
+        
         //read data
         int bytecount = CH_SIZE*i;
         while(bytecount < CH_SIZE*(1+i)){
@@ -109,10 +316,44 @@
         ((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];  
+    }
+    
+    }
+
+
 
 int main()
 {
@@ -122,66 +363,148 @@
     
     //setup pc
     pc.baud(115200);
-    pc.printf("------STARTUP------\n\n\n");
+    pc.printf("------STARTUP------\n\n\n\r");
     
     //setup SPI
     LTC_CHIP.format(8, 0);
-    LTC_CHIP.frequency(10000); //60Mhz clock frequency 
+    LTC_CHIP.frequency(10000); //10MHz? //60Mhz clock frequency 
+
+    wait_ms(1);
+    config_LTC();
+    wait_ms(1);
     
-    //setup ETHERNET
-    eth.set_network(ip, mask, gateway);
-    eth.connect();
+//    //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); }
     
-    //more ETHERNET
+    
+    
+    // SET UP FROM OLD SENSOR CODE HERE
     
-    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 sensor pins
+    dec_enable = 1;    
+    // Initialize dev 1
+    s1.dev_id = 1;  // tells which cs pin associated with device
+    config_dev(&s1);
+    //Initialize dev 2
+    s2.dev_id = 2;  // tells which cs pin associated with device
+    config_dev(&s2);
+    //Initialize dev 3
+    s3.dev_id = 3;  // tells which cs pin associated with device
+    config_dev(&s3);
+    //Initialize dev 4
+    s4.dev_id = 4;  // tells which cs pin associated with device
+    config_dev(&s4);
+    //Initialize dev 5
+    s5.dev_id = 5;  // tells which cs pin associated with device
+    config_dev(&s5);
+    //Initialize dev 6
+    s6.dev_id = 6;  // tells which cs pin associated with device
+    config_dev(&s6);
+    //Initialize dev 7
+    s7.dev_id = 7;  // tells which cs pin associated with device
+    config_dev(&s7);
+    //Initialize dev 8
+    s8.dev_id = 8;  // tells which cs pin associated with device
+    config_dev(&s8);
     
-    //start timer
+    // 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;
+    
+    
+
+    
+    
+    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();  
+        for(int j=0; j<6; j++){
+            bias[j] += 0.01*(float)msg[j];
+        }
+        while(t.read()<0.001){;}
+    }
+    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("Starting to sample...\n\n\r");
+    
+    // attach sampling interrupt
+    datalog.attach_us(&log_data,10000); // 1000us = 1ms (10000 = 10 ms = 100 Hz)
 
     while (true) {
-        //timer reset
-        t.reset();
-        //read and print data to serial terminal
-        read_data();
-        //print the spi bytes
-        
-        
-        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;
         
-        //uint16_t conv_1_z = adc_data.channel[3].info;
-        //int16_t output = adc_data.channel[3].cnv_upper<<8 | adc_data.channel[3].cnv_lower;
-        
-        //convert to float --> 2's Complement Number from -10.24V --> 10.24V
-        //float cnv1 = CONV_FACTOR*(float)output; //--> CONVERT ON THE PYTHON SIDE 
+        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);
+            
+            // 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;
+            
+            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
+            
+            
+            
+            
+         }
         
-        //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
-        
-        
-        //pc.printf("conversion: %x vs %f\n", output, cnv1); //just check the first "word" for now b/c it should be correct
-        //pc.printf("zero_check: %d\n", conv_1_z);
-        
-        //wait for DT second
-        //wait(1);
-        while(t.read()<DT){;}
     }
     
-    // Terminate connection (if you want)
-    server.close();
-    eth.disconnect();
+//    // Terminate connection (if you want)
+//    server.close();
+//    eth.disconnect();
 }