 #include <Wire.h>
-//#define FSR_PIN     0      
+#define FSR_PIN     0      
 #define NUM_MODULES 2
 #define DATA_SIZE   4
 #define ADDR1       0xAA

// }


// //get instantaneous FSR mass
// int massFSR(){
//     int sensorValue = 0;
//     int output = 2;
//     sensorValue = analogRead(FSR_PIN);
//     //scale from [0,1023] to [0,100]
//     if(sensorValue>=threshold)
//         output = (sensorValue-threshold)/10.23;
//     else
//         output = 0;
//     if(output<0 || output>1023)
//         return error();
//     return output;
// }
// int massChange(int initMass){
//     return massFSR()-initMass;
// }

    if(!initialize()) {
        Serial.println("Initialization Failed");
    }
    send(ADDR1, test, 4);
    delay(1000);
    send(ADDR1, test2, 4);
    delay(1000);
    send(ADDR2, test, 4);
    delay(1000);
    send(ADDR2, test2, 4);
    delay(1000);
    test[1] = 0;
    test2[1] = 0;
    send(ADDR1, test, 4);
    delay(1000);
    send(ADDR1, test2, 4);
    delay(1000);
    send(ADDR2, test, 4);
    delay(1000);
    send(ADDR2, test2, 4);
    delay(1000);
    threshold = massFSR();
}

void loop() {

    // sendTest(0, massFSR());
    Serial.println(massFSR());
    // delay(100);
}

