File content as of revision 4:227f0847a797:

#include <mbed.h>
#include "max32630fthr.h"
#include "bmi160.h"
#include "I2C.h"
#include "QEI.h"
#include "kalman.c"
#include "SDFileSystem.h"
#include <string>
#include "USBSerial.h"

//Defining PI
#ifndef M_PI
#define M_PI 3.1415
#endif

float hitTimes = 0;
/********************** PIN DEFINITIONS ****************************/
PwmOut motorPWM(P4_0);        // Motor PWM output
DigitalOut motorFwd(P5_6);    // Motor forward enable
DigitalOut motorRev(P5_5);    // Motor backward enable

//Analog input
AnalogIn   ain(AIN_5);
Timer t;                    // Timer to measure elapsed time of experiment
QEI encoder(P5_3,P5_4 , NC, 1200 , QEI::X4_ENCODING); // Pins D3, D4, no index, 1200 counts/rev, Quadrature encoding

DigitalOut led1(LED1, 1);

// added by ken //
DigitalIn saveDataButton(P2_3);
SDFileSystem sd(P0_5, P0_6, P0_4, P0_7, "sd"); // the pinout on the mbed Cool Components workshop board
Serial pc(USBTX, USBRX);
// added by ken - ends here //

/********************INSTANTIATING BOARD and SENSORS *************************/
//Board Initialization
MAX32630FTHR pegasus(MAX32630FTHR::VIO_3V3);

// IMU Initialization
I2C i2cBus_acc(P5_7, P6_0);
BMI160_I2C* m_imu;
BMI160::AccConfig accConfig;
BMI160::GyroConfig gyroConfig;
uint32_t failures = 0;

/************** VALUES FOR STORING IMU and KALMAN ANGLE **********************/

// IMU Variables
float imuTemperature;
BMI160::SensorData accData;
BMI160::SensorData gyroData;
BMI160::SensorTime sensorTime;
float accX = 0.0;
float accY = 0.0;
float accZ = 0.0;
float gyroX = 0.0;
float gyroY = 0.0;
float gyroZ = 0.0;

// Kalman Angle
float kalmanAngle = 0.0;

// Collision Detection Variables - originally only for single collisions
bool bool_collision = false; // collision state
float collisionTime = 0.0; //time collision occurred
float collisionTimeRemaining = 0.0; //variable to hold bool_collision for set time
float collisionDuration = 0.2; // duration of collision (rough measured experimentally)


/****************** MOTOR CONTROL VARIABLES FOR STATES  **********************/
//For motor control
//Variables for states
float theta_d = 0.0;
float Kp = 3.0; // proportional gain
float Kd = 1.2; //made up
float Ki = 0.1; //made up

//Declare Parameters
float e = 0.0; //error, theta_d-theta
float theta = 0.0; //present theta
float theta_dot = 0.0; //present angular velocity
float val = 0.0; //

float lastTime = 0.0; //last time step
float presentTime = 0.0; //current time step
float dT = 0.045; //time difference

/** Set gain parameters by log **/
//Default Gains set to avoid problems
float i_d = 0; //target current % changed by code
float r = 3.2; // winding resistance;
float k_b = 0.23; // back emf

float k_th = 4; // proportional theta gain for torque
float b_th = 0.2185; // proportional theta_dot gain for damping
float v_th = 0.0185; //viscous friction coefficient (from experiments)

float torqueCol_d = 0.2; //torque target for collision
float torqueFlight_d = 0.0; //torque target for flight

float i = 0.0; // present current
float e_accum = 0.0;

float torque = 0.0; //present torque
float outputVal;//final compensation value (in rad) --> converted into dutycycle

/********************** ACTUATION STRATEGY INDICATOR ***************************/
float flipMode = 1; //defaulty starts with stiffness strategy

/********************** DEBUG MODE VARIABLES ***********************************/
bool debugModeOn = false;


/********************** FEEDBACK UTILITY FUNCTIONS ******************************/
// A min function (as in min(a,b) or max(a,b)))
float min(float a, float b){
    return a<b ? a : b;
}

float max(float a, float b){
    return a>b ? a : b;
}
// for final angle to pwm duty cycle calc
float toDutyCycle(float inputRad){
    float temp = abs(inputRad);
    if (temp>12){
        return 1.0;
    }else{
        return temp/12.0;
    }
    //abs(min(12.0,inputRad)/12.0);
}
// Calculates angles in radians from position
float getAngleFromPulses(int input){
    return input*(2*M_PI)/1200.0;
}
// Set motor direction
void setMotorDir(char dir){
    if (dir=='f'){
        motorFwd = 1;
        motorRev = 0;
    }else{
        motorFwd = 0;
        motorRev = 1;
    }
}

// gets current
float readCurrent(){
  return 36.7*ain.read()*(1.815)-18.3;
  // return 36.7*ain.read()*(1.815)-18.3;
}

/********************* LED DISPLAY UTILITY FUNCTIONS *************************/
PwmOut rLED(LED1);
PwmOut gLED(LED2);
PwmOut bLED(LED3);

void updateLEDS(float b1,float b2,float b3){

  if(!bool_collision){
    rLED.write(max(1-b1/4,0.0));
    gLED.write(max(1-b2/4,0.0));
    bLED.write(max(1-b3/4,0.0));
  }else{
    rLED.write(0);
    gLED.write(0);
    bLED.write(1);
  }
}

/********************* COLLISION UTILITY FUNCTIONS *************************/
float last_xAcc = 0.0;
float jerk = 0.0;
bool smashHappened = false;

bool checkCollision(float xAccel,float dT){
    jerk = (xAccel-last_xAcc)/dT;
    printf("%f\n",(xAccel-last_xAcc));
    if ((xAccel-last_xAcc)<-1.5){//(jerk>100){ 
     return true;
     }
     else{
      if (collisionTimeRemaining >0.0){
          return true;
      }else{
      return false;
     }
    }
}

//unused function for now
float collisionActuation(float duration){
  //first decide start time
  //then actuate for a set duration
  return duration;
}



/*****************************************************************************/

/******************** SETUP IMU *****************************/
kalman filterRotation;
double refAngle = 0.0;
double qGyro = 0.0;
double qAngle = 0.0;

void setupKalman(){
    kalman_init(&filterRotation, R_matrix,Q_Gyro_matrix,Q_Accel_matrix);
}


void setupIMU()
{
    i2cBus_acc.frequency(400000);
    m_imu = new BMI160_I2C(i2cBus_acc, BMI160_I2C::I2C_ADRS_SDO_LO);
    printf("\033[H");  //home
    printf("\033[0J");  //erase from cursor to end of screen
    if(m_imu->setSensorPowerMode(BMI160::GYRO, BMI160::NORMAL) != BMI160::RTN_NO_ERROR) {
        printf("Failed to set gyroscope power mode\n");
        failures++;
    }
    wait_ms(100);

    if(m_imu->setSensorPowerMode(BMI160::ACC, BMI160::NORMAL) != BMI160::RTN_NO_ERROR) {
        printf("Failed to set accelerometer power mode\n");
        failures++;
    }
    wait_ms(100);


    //example of setting user defined configuration
    accConfig.range = BMI160::SENS_4G;
    accConfig.us = BMI160::ACC_US_OFF;
    accConfig.bwp = BMI160::ACC_BWP_2;
    accConfig.odr = BMI160::ACC_ODR_12; //reads at 160 kHz

    if(m_imu->getSensorConfig(accConfig) == BMI160::RTN_NO_ERROR) {
        printf("ACC Range = %d\n", accConfig.range);
        printf("ACC UnderSampling = %d\n", accConfig.us);
        printf("ACC BandWidthParam = %d\n", accConfig.bwp);
        printf("ACC OutputDataRate = %d\n\n", accConfig.odr);
    } else {
        printf("Failed to get accelerometer configuration\n");
        failures++;
    }

    if(m_imu->getSensorConfig(gyroConfig) == BMI160::RTN_NO_ERROR) {
            printf("GYRO Range = %d\n", gyroConfig.range);
            printf("GYRO BandWidthParam = %d\n", gyroConfig.bwp);
            printf("GYRO OutputDataRate = %d\n\n", gyroConfig.odr);
    } else {
        printf("Failed to get gyroscope configuration\n");
        failures++;
    }

    wait(1.0);

    m_imu->getGyroAccXYZandSensorTime(accData, gyroData, sensorTime, accConfig.range, gyroConfig.range);
    m_imu->getTemperature(&imuTemperature);

    accX = accData.xAxis.scaled;
    accY = accData.yAxis.scaled;
    accZ = accData.zAxis.scaled;

    printf("ACC xAxis = %s%4.3f\n", "\033[K", accX);
    printf("ACC xAxis = %s%4.3f\n", "\033[K", accY);
    printf("ACC xAxis = %s%4.3f\n", "\033[K", accZ);


    updateLEDS(accX,accY,accZ);
}

/******************** READ IMU *****************************/

// ▯  x is up, y is left, z is out of the board (towards me). BLE module is on top.
// R i = sqrt(std::pow(adata.x, 2) + std::pow(adata.y, 2) + std::pow(adata.z, 2));
void getKalmanPrediction(double dT, float gyroReading, float accReading, float R){
    kalman_predict(&filterRotation, gyroReading,  dT);
    kalman_update(&filterRotation, acos(accReading/R));
}


float lastKalTime = 0.0;
float R = 0.0;
float startTimeReference = 0.0;

void readIMU()
{
    m_imu->getGyroAccXYZandSensorTime(accData, gyroData, sensorTime, accConfig.range, gyroConfig.range);
    m_imu->getTemperature(&imuTemperature);

    accX = accData.xAxis.scaled;
    accY = accData.yAxis.scaled;
    accZ = accData.zAxis.scaled;

    gyroX = gyroData.xAxis.scaled;
    gyroY = gyroData.yAxis.scaled;
    gyroZ = gyroData.zAxis.scaled;

    R = sqrt(std::pow(accX, 2) + std::pow(accY, 2) + std::pow(accZ, 2));
    getKalmanPrediction(t.read()-lastKalTime, gyroX, accX, R);
    //printf("%f\n",kalman_get_angle(&filterRotation));
    kalmanAngle = kalman_get_angle(&filterRotation);

    lastKalTime=t.read();

    updateLEDS(accX,accY,accZ);
}

/*********** Load and Update and Save Parameters ***************/
char inputMode = 'n';

void loadParam(){ //from SD
  char buf[15];

  // 0 -- collisionDuration read
  FILE *fp = fopen("/sd/mydir/param/collisionDuration.txt", "r");
  memset(buf, '\0', strlen(buf));
  if(fp == NULL) { error("Could not open file for reading\r\n"); }
  while(fgets (buf, 15, fp) != NULL){
      buf[strlen(buf)-1] = 0;
      }
  fclose(fp);
  collisionDuration = (float)atof(buf);
  printf("collisionDuration is loaded as %f. \r\n", collisionDuration);

    // 1 -- kp read
    fp = fopen("/sd/mydir/param/kp.txt", "r");
    memset(buf, '\0', strlen(buf));
    if(fp == NULL) { error("Could not open file for reading\r\n"); }
    while(fgets (buf, 15, fp) != NULL){
        buf[strlen(buf)-1] = 0;
        }
    fclose(fp);
    Kp = (float)atof(buf);
    printf("Kp is loaded as %f. \r\n", Kp);

    // 2 -- kd read
    fp = fopen("/sd/mydir/param/kd.txt", "r");
    memset(buf, '\0', strlen(buf));
    if(fp == NULL) { error("Could not open file for reading\r\n"); }
    while(fgets (buf, 15, fp) != NULL){
        buf[strlen(buf)-1] = 0;
        }
    fclose(fp);
    Kd = (float)atof(buf);
    printf("Kd is loaded as %f. \r\n", Kd);

    // 3 -- theta_d read
    fp = fopen("/sd/mydir/param/theta_d.txt", "r");
    memset(buf, '\0', strlen(buf));
    if(fp == NULL) { error("Could not open file for reading\r\n"); }
    while(fgets (buf, 15, fp) != NULL){
        buf[strlen(buf)-1] = 0;
        }
    fclose(fp);
    theta_d = (float)atof(buf);
    printf("theta_d is loaded as %f. \r\n", theta_d);

    // 4 -- k_th read
    fp = fopen("/sd/mydir/param/k_th.txt", "r");
    memset(buf, '\0', strlen(buf));
    if(fp == NULL) { error("Could not open file for reading\r\n"); }
    while(fgets (buf, 15, fp) != NULL){
        buf[strlen(buf)-1] = 0;
        }
    fclose(fp);
    k_th = (float)atof(buf);
    printf("k_th is loaded as %f. \r\n", k_th);

      // 5 -- b_th read
      fp = fopen("/sd/mydir/param/b_th.txt", "r");
      memset(buf, '\0', strlen(buf));
      if(fp == NULL) { error("Could not open file for reading\r\n"); }
      while(fgets (buf, 15, fp) != NULL){
          buf[strlen(buf)-1] = 0;
          }
      fclose(fp);
      b_th = (float)atof(buf);
      printf("b_th is loaded as %f. \r\n", b_th);

      // 6 -- v_th read
      fp = fopen("/sd/mydir/param/v_th.txt", "r");
      memset(buf, '\0', strlen(buf));
      if(fp == NULL) { error("Could not open file for reading\r\n"); }
      while(fgets (buf, 15, fp) != NULL){
          buf[strlen(buf)-1] = 0;
          }
      fclose(fp);
      v_th = (float)atof(buf);
      printf("v_th is loaded as %f. \r\n", v_th);

      // 7 -- flipMode read
      fp = fopen("/sd/mydir/param/flipMode.txt", "r");
      memset(buf, '\0', strlen(buf));
      if(fp == NULL) { error("Could not open file for reading\r\n"); }
      while(fgets (buf, 15, fp) != NULL){
          buf[strlen(buf)-1] = 0;
          }
      fclose(fp);
      flipMode = (float)atof(buf);
      printf("flipMode is loaded as %f. \r\n", flipMode);

      // 8 -- torqueCol_d read
      fp = fopen("/sd/mydir/param/torqueCol_d.txt", "r");
      memset(buf, '\0', strlen(buf));
      if(fp == NULL) { error("Could not open file for reading\r\n"); }
      while(fgets (buf, 15, fp) != NULL){
          buf[strlen(buf)-1] = 0;
          }
      fclose(fp);
      torqueCol_d = (float)atof(buf);
      printf("torqueCol_d is loaded as %f. \r\n", torqueCol_d);

      // 9 -- torqueFlight_d read
      fp = fopen("/sd/mydir/param/torqueFlight_d.txt", "r");
      memset(buf, '\0', strlen(buf));
      if(fp == NULL) { error("Could not open file for reading\r\n"); }
      while(fgets (buf, 15, fp) != NULL){
          buf[strlen(buf)-1] = 0;
          }
      fclose(fp);
      torqueFlight_d = (float)atof(buf);
      printf("torqueFlight_d is loaded as %f. \r\n", torqueFlight_d);

}

void saveParam(){ //to SD
  char buf[10];

  // 0 -- collisionDuration
  FILE *fp = fopen("/sd/mydir/param/collisionDuration.txt", "w");
  if(fp == NULL) {
      error("Could not open file for write\n");
  }
  memset(buf, '\0', strlen(buf));
  sprintf(buf, "%.3f", collisionDuration);
  fprintf(fp, buf);
  fprintf(fp, "\r\n");
  fclose(fp);
  printf("collisionDuration saved as %f.\r\n", collisionDuration);

    // 1 -- kp
    fp = fopen("/sd/mydir/param/kp.txt", "w");
    if(fp == NULL) {
        error("Could not open file for write\n");
    }
    memset(buf, '\0', strlen(buf));
    sprintf(buf, "%.3f", Kp);
    fprintf(fp, buf);
    fprintf(fp, "\r\n");
    fclose(fp);
    printf("Kp saved as %f.\r\n", Kp);

    // 2 -- kd
    fp = fopen("/sd/mydir/param/kd.txt", "w");
    if(fp == NULL) {
        error("Could not open file for write\n");
    }
    memset(buf, '\0', strlen(buf));
    sprintf(buf, "%.3f", Kd);
    fprintf(fp, buf);
    fprintf(fp, "\r\n");
    fclose(fp);
    printf("Kd saved as %f.\r\n", Kd);

    // 3 -- theta_d
    fp = fopen("/sd/mydir/param/theta_d.txt", "w");
    if(fp == NULL) {
        error("Could not open file for write\n");
    }
    memset(buf, '\0', strlen(buf));
    sprintf(buf, "%.3f", theta_d);
    fprintf(fp, buf);
    fprintf(fp, "\r\n");
    fclose(fp);
    printf("theta_d saved as %f.\r\n", theta_d);

    // 4 -- k_th
    fp = fopen("/sd/mydir/param/k_th.txt", "w");
    if(fp == NULL) {
        error("Could not open file for write\n");
    }
    memset(buf, '\0', strlen(buf));
    sprintf(buf, "%.3f", k_th);
    fprintf(fp, buf);
    fprintf(fp, "\r\n");
    fclose(fp);
    printf("k_th saved as %f.\r\n", k_th);

      // 5 -- b_th
      fp = fopen("/sd/mydir/param/b_th.txt", "w");
      if(fp == NULL) {
          error("Could not open file for write\n");
      }
      memset(buf, '\0', strlen(buf));
      sprintf(buf, "%.3f", b_th);
      fprintf(fp, buf);
      fprintf(fp, "\r\n");
      fclose(fp);
      printf("b_th saved as %f.\r\n", b_th);

      // 6 -- v_th
      fp = fopen("/sd/mydir/param/v_th.txt", "w");
      if(fp == NULL) {
          error("Could not open file for write\n");
      }
      memset(buf, '\0', strlen(buf));
      sprintf(buf, "%.3f", v_th);
      fprintf(fp, buf);
      fprintf(fp, "\r\n");
      fclose(fp);
      printf("v_th saved as %f.\r\n", v_th);

      // 7 -- flipMode
      fp = fopen("/sd/mydir/param/flipMode.txt", "w");
      if(fp == NULL) {
          error("Could not open file for write\n");
      }
      memset(buf, '\0', strlen(buf));
      sprintf(buf, "%.3f", flipMode);
      fprintf(fp, buf);
      fprintf(fp, "\r\n");
      fclose(fp);
      printf("flipMode saved as %f.\r\n", flipMode);

      // 8 -- torqueCol_d
      fp = fopen("/sd/mydir/param/torqueCol_d.txt", "w");
      if(fp == NULL) {
          error("Could not open file for write\n");
      }
      memset(buf, '\0', strlen(buf));
      sprintf(buf, "%.3f", torqueCol_d);
      fprintf(fp, buf);
      fprintf(fp, "\r\n");
      fclose(fp);
      printf("torqueCol_d saved as %f.\r\n", torqueCol_d);

      // 9 -- torqueFlight_d
      fp = fopen("/sd/mydir/param/torqueFlight_d.txt", "w");
      if(fp == NULL) {
          error("Could not open file for write\n");
      }
      memset(buf, '\0', strlen(buf));
      sprintf(buf, "%.3f", torqueFlight_d);
      fprintf(fp, buf);
      fprintf(fp, "\r\n");
      fclose(fp);
      printf("torqueFlight_d saved as %f.\r\n", torqueFlight_d);
}

char buffer_serial[20];
void serialUpdateVal(){

      pc.scanf("%s", &buffer_serial);
      pc.printf("I received ");
      pc.printf(buffer_serial);
      pc.printf("\n");

      if(inputMode == 'n'){
        if(buffer_serial[0] == 'a'){
            pc.printf("input mode is set to collisionDuration, currently %f. Please enter value. \r\n", collisionDuration);
            inputMode = 'a';
         } else if(buffer_serial[0] == 'p'){
             pc.printf("input mode is set to Kp, currently %f. Please enter value. \r\n", Kp);
             inputMode = 'p';
          } else if(buffer_serial[0] == 'd'){
            pc.printf("input mode is set to Kd, currently %f. Please enter value. \r\n", Kd);
            inputMode = 'd';
         } else if(buffer_serial[0] == 't'){
            pc.printf("input mode is set to theta_d, currently %f. Please enter value. \r\n", theta_d);
            inputMode = 't';
         } else if(buffer_serial[0] == 'e'){ //k_th
             pc.printf("input mode is set to k_th, currently %f. Please enter value. \r\n", k_th);
             inputMode = 'e';
          } else if(buffer_serial[0] == 'b'){ //b_th
            pc.printf("input mode is set to b_th, currently %f. Please enter value. \r\n", b_th);
            inputMode = 'b';
         } else if(buffer_serial[0] == 'v'){ //v_th
            pc.printf("input mode is set to v_th, currently %f. Please enter value. \r\n", v_th);
            inputMode = 'v';
         } else if(buffer_serial[0] == 'f'){ //flipMode
             pc.printf("input mode is set to flipMode, currently %f. Please enter value. \r\n", flipMode);
             inputMode = 'f';
          } else if(buffer_serial[0] == 'g'){ //torqueCol_d
            pc.printf("input mode is set to torqueCol_d, currently %f. Please enter value. \r\n", torqueCol_d);
            inputMode = 'g';
         } else if(buffer_serial[0] == 'h'){ //torqueFlight_d
            pc.printf("input mode is set to torqueFlight_d, currently %f. Please enter value. \r\n", torqueFlight_d);
            inputMode = 'h';
         }


       } else if(inputMode == 'a'){
                collisionDuration = (float)atof(buffer_serial);
                inputMode = 'n';
                pc.printf("collisionDuration is set to %f \r\n", collisionDuration);
                saveParam();
        } else if(inputMode == 'p'){
                Kp = (float)atof(buffer_serial);
                inputMode = 'n';
                pc.printf("Kp is set to %f \r\n", Kp);
                saveParam();
        } else if(inputMode == 'd'){
                Kd = (float)atof(buffer_serial);
                inputMode = 'n';
                pc.printf("Kd is set to %f \r\n", Kd);
                saveParam();
        } else if(inputMode == 't'){
                theta_d = (float)atof(buffer_serial);
                inputMode = 'n';
                pc.printf("theta_d is set to %f \r\n", theta_d);
                saveParam();
        } else if(inputMode == 'e'){
                 k_th = (float)atof(buffer_serial);
                 inputMode = 'n';
                 pc.printf("k_th is set to %f \r\n", k_th);
                 saveParam();
         } else if(inputMode == 'b'){
                 b_th = (float)atof(buffer_serial);
                 inputMode = 'n';
                 pc.printf("b_th is set to %f \r\n", b_th);
                 saveParam();
         } else if(inputMode == 'v'){
                 v_th = (float)atof(buffer_serial);
                 inputMode = 'n';
                 pc.printf("v_th is set to %f \r\n", v_th);
                 saveParam();
         } else if(inputMode == 'f'){
                 flipMode = (float)atof(buffer_serial);
                 inputMode = 'n';
                 pc.printf("flipMode is set to %f \r\n", flipMode);
                 saveParam();
         }  else if(inputMode == 'g'){
                 torqueCol_d = (float)atof(buffer_serial);
                 inputMode = 'n';
                 pc.printf("torqueCol_d is set to %f \r\n", torqueCol_d);
                 saveParam();
         } else if(inputMode == 'h'){
                 torqueFlight_d = (float)atof(buffer_serial);
                 inputMode = 'n';
                 pc.printf("torqueFlight_d is set to %f \r\n", torqueFlight_d);
                 saveParam();
         }




        if(buffer_serial[0] == 'c'){
            pc.printf("collisionDuration: % f | Kp: %f | Kd: %f | theta_d: %f | k_th: %f | b_th: %f | v_th: %f | flipMode: %f | torqueCol_d: %f | torqueFlight_d: %f \r\n",
            collisionDuration, Kp, Kd, theta_d, k_th, b_th, v_th, flipMode, torqueCol_d, torqueFlight_d);
        }

}

/******************** LogData to SD card  *****************************/

int trialTimeCount = 0;

const int logValNum = 16;
char logValName[logValNum][30];
float logVal[logValNum][2500];
int currentLogNum = 0;

void updateTrialTime(){
    //trial time read
    FILE *fp_tr = fopen("/sd/mydir/trialtime.txt", "r");

    char Buffer_t[512];

    if(fp_tr == NULL) { error("Could not open file for reading\r\n"); }

    while(fgets (Buffer_t, 512, fp_tr) != NULL){
        Buffer_t[strlen(Buffer_t)-1] = 0;
        printf("String = \"%s\" \r\n", Buffer_t);
        }
    fclose(fp_tr);

    trialTimeCount = (int)atof(Buffer_t);

    printf("last trialTimeCount was %i. \n", trialTimeCount);

    trialTimeCount++; //count up trial time
    printf("current trialTimeCount updated to %i. \n", trialTimeCount);

    FILE *fp3 = fopen("/sd/mydir/trialtime.txt", "w");
    if(fp3 == NULL) {
        error("Could not open file for write\n");
    }
    char buf[10];

    sprintf(buf, "%d", trialTimeCount);

    fprintf(fp3, buf);
    fprintf(fp3, "\r\n");
    fclose(fp3);
    printf("trial time saved\n");
}

void logData(){ // log data time


    logVal[0][currentLogNum] = t.read();
    logVal[1][currentLogNum] = (int)bool_collision;
    logVal[2][currentLogNum] = theta;
    logVal[3][currentLogNum] = theta_dot;
    logVal[4][currentLogNum] = dT;
    logVal[5][currentLogNum] = i;
    logVal[6][currentLogNum] = outputVal;
    logVal[7][currentLogNum] = torque;
    logVal[8][currentLogNum] = kalmanAngle;
    logVal[9][currentLogNum] = accX;
    logVal[10][currentLogNum] = accY;
    logVal[11][currentLogNum] = accZ;
    logVal[12][currentLogNum] = gyroX;
    logVal[13][currentLogNum] = gyroY;
    logVal[14][currentLogNum] = gyroZ;

    //printf("logged data for %i. t.read() = %f \r\n", currentLogNum, t.read());

    currentLogNum++;
}

void saveData(){ // call when the while loop ends or the button pressed
   sprintf( logValName[0], "time");
   sprintf( logValName[1], "bool_collision");
   sprintf( logValName[2], "theta");
   sprintf( logValName[3], "theta_dot");
   sprintf( logValName[4], "dT");
   sprintf( logValName[5], "current");
   sprintf( logValName[6], "outputVal");
   sprintf( logValName[7], "torque");
   sprintf( logValName[8], "kalmanAngle");
   sprintf( logValName[9], "accX");
   sprintf( logValName[10], "accY");
   sprintf( logValName[11], "accZ");
   sprintf( logValName[12], "gyroX");
   sprintf( logValName[13], "gyroY");
   sprintf( logValName[14], "gyroZ");


    char filename[256];
    sprintf(filename, "/sd/mydir/log/flipper_logData_%i.txt",trialTimeCount);

    FILE *f_sv = fopen(filename, "w");
    if(f_sv == NULL) {
        error("Could not open file for write\n");
    }

    for(int i = 0; i < logValNum; i++){
        fprintf(f_sv, logValName[i]);
        fprintf(f_sv, ",");
    }
    fprintf(f_sv, "\r\n");

    for(int j = 0; j < currentLogNum; j++){
    for(int i = 0; i < logValNum; i++){
        //char buf_temp[10];
        //int a = 5;

        char cVal[8];

         sprintf(cVal,"%.3f", logVal[i][j]);

        //sprintf(buf_temp, "%f", logVal[i][j]);

        fprintf(f_sv, cVal);
        fprintf(f_sv, ",");
    }
    fprintf(f_sv, "\r\n");
    }


    fclose(f_sv);

    printf("Log Data file is saved as 'flipper_logData_%i.txt'.\n", trialTimeCount);
}

/*******************************************************************/

/******************** FLIP STRATEGIES *****************************/

float targetTheta = 0.0;
float stiffnessStrategy(bool collision){
  // depends on global variables
  // theta_d: reference angle
  // theta: measured angle at joint B
  // theta_dot: measured angular velocity at joint B
  //if (collisionHappened<1){
  if (!collision){
    targetTheta = 0.0;
  }else{
    targetTheta = theta_d;
  }

  //printf("TARGET THETA %f\n",targetTheta);


  e = targetTheta-theta;
  /*torque = -k_th*(theta-theta_d)-b_th*theta_dot+v_th*theta_dot;
  if (torque<0){
    torque = max(-1.4, torque);
  }else{
    torque = min(1.4, torque);
  }
 // printf("torque %f\n", torque);

  //i_d = torque/k_b;
  //i = readCurrent();
  //e = i_d-i;
  //outputVal = r*i_d+Kp*(e)-k_b*theta_dot;*/
  //printf("%f\n",e);
  outputVal = k_th*e+b_th*(-theta_dot);
  //outputVal = Kp*e + Kd*(-theta_dot);
  return outputVal;
}

float torqueStrategy(bool collision){
  //uses global variables i,r
  //strategy begins at collisions
  if (collision){
    torque = torqueCol_d; //predefined value
  }else{
    if (hitTimes>0){
    torque = torqueFlight_d;
    }
    else{
    torque = 0;    
    }
  }
  // computation for output from torque
  // where these torques are discerned from
  // simulation profile
  i_d = torque/k_b;
  i = readCurrent();
  e = i_d-i;
  //printf("%f\n",outputVal);
  if (torque==0){
      outputVal=0;
  }
  else{
  outputVal = r*i_d+Kp*(e)-k_b*theta_dot;
  }
  return outputVal;
}

float stiffnessAndtorqueStrategy(bool collisionState){
  return stiffnessStrategy(collisionState)+torqueStrategy(collisionState);
}

float executeFlipStrategy(int flipmode, bool collisionState){

  // Mode 1: Stiffness only
  if (flipmode==1){
    return stiffnessStrategy(collisionState);
  }else{
    // Mode 2: Torque Profile only
    if (flipmode==2){
      return torqueStrategy(collisionState);
    }
    // Mode 3: Stiffness and Torque
    else{
      return stiffnessAndtorqueStrategy(collisionState);
    }
  }

}


/******************** EXECUTE MOTOR LOOP *****************************/

float pikeAngle = 0.0;

/** Core Motor Code Loop**/
void executeMotorLoop(){

    printf("Entering Motor Loop\n");
    // initial setup
    t.reset();
    t.start();
    encoder.reset();
    setMotorDir('f'); //begin with motor driving forward
    motorPWM.write(0);
    lastTime=t.read();
    startTimeReference = t.read(); //set reference time for Kalman Angle

    /* Run experiment */
    while( t.read() < 10.0 && !debugModeOn) {
      //Update IMU and Kalman
      readIMU();

      //read present time and calculate dt
      presentTime = t.read();
      dT = presentTime - lastTime;

      // set last time to determine next time step
      lastTime = presentTime;

      // Perform control loop logic
      theta = getAngleFromPulses(encoder.getPulses());
      theta_dot = getAngleFromPulses(encoder.getVelocity());

      /* Checking collisions */

      // Determines whether new hit.
      if ((!bool_collision) && checkCollision(accX,dT) && (hitTimes<1)){
        printf("i am dumb\n");
        collisionTime = t.read();
        collisionTimeRemaining = collisionDuration;
        bool_collision = true;
        hitTimes = hitTimes + 1;
      }
      // If not a new hit, see if we are still in "hit state" from
      // collision duration parameter for actuation
      else{
        if ((bool_collision)&&(hitTimes<=1)){
          collisionTimeRemaining = (max((collisionDuration)-(t.read()-collisionTime),0.0));
          if (collisionTimeRemaining<=0.0){
            bool_collision = false;
          }
          }
          }
        //bool_collision = checkCollision(accX,dT);


      /* actuation strategy */
      // flip mode 1: stiffness only
      // flip mode 2: torque driving only
      // flip mode 3: torque driving with stiffness


      // Initial State, zero torque or other affects.
      torque = 0.0;

      /* Compute Actuation Force */
      outputVal = executeFlipStrategy(flipMode, bool_collision);

      /* Actuate Motors */
      if (outputVal<0){
    //negative difference, need to move motor backward to correct
    setMotorDir('f');
      }
      if (outputVal>0){
    //positive difference, need to move motor forward to correct
    setMotorDir('r');
      }
      motorPWM.write(toDutyCycle(abs(outputVal)));
      //printf("Serial: %f\n",outputVal);

      /* Log Data */
      logData();
      if(!saveDataButton){
    saveData();
    debugModeOn = true;
      }

    }
    // End of action cycle, turn motor "off"
    motorPWM.write(0);
}




/****************************** MAIN *************************************/

int main()
{

  printf("test!!\n");
  updateTrialTime();
  loadParam();

  /* setup imu and kalman filter for rotation */
  setupIMU();
  setupKalman();

  /* run core motor loop */
  executeMotorLoop();

  /* save trial data to sd card */
  saveData();

  printf("collisionDuration: % f | Kp: %f | Kd: %f | theta_d: %f | k_th: %f | b_th: %f | v_th: %f | flipMode: %f | torqueCol_d: %f | torqueFlight_d: %f \r\n",
  collisionDuration, Kp, Kd, theta_d, k_th, b_th, v_th, flipMode, torqueCol_d, torqueFlight_d);
  printf("Now you can set parameters by typing; 'a' - collisionDuration, 'p' - Kp, 'd' - Kd, 't' - theta_d, 'e' - k_th, 'b' - b_th, 'v' - v_th, 'f' - flipMode, 'g' - torqueCol_d, 'h' - torqueFlight_d.\n You can check the values with 'c'.");

  while(1){ // debugMode
    serialUpdateVal();
  }

}