File content as of revision 2:bdfce41aae53:

/* mbed Microcontroller Library
 * Copyright (c) 2019 ARM Limited
 * SPDX-License-Identifier: Apache-2.0
 */

#include "mbed.h"
#include "platform/mbed_thread.h"
#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
#define LTC_MISO    PC_11
#define LTC_CLK     PC_10
#define LTC_CS      PA_15
#define BSY         PB_5
#define CNV         PB_6

#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);
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
uint8_t rx_buff[BUFF_SIZE]; //each is an 8-bit word 
ltc_spi adc_data;

//setup SERIAL
Serial       pc(USBTX, USBRX);

// 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
}

// 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;

//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;
    
    }



/*
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];  
    }
    
    }



int main()
{
    // Initialise the digital pin LED1 as an output
    DigitalOut led(LED1);
    CNV_PIN=0;
    
    //setup pc
    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 

    wait_ms(1);
    config_LTC();
    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 FROM OLD SENSOR CODE HERE
    
    // 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);
    
    // 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) {
        
        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
            
            
            
            
         }
        
    }
    
//    // Terminate connection (if you want)
//    server.close();
//    eth.disconnect();
}