Diff: main.cpp

Revision:

3:5696ac47658a

Parent:

2:983a818c6ddf

Child:

4:227f0847a797

--- a/main.cpp	Sat Dec 02 23:33:17 2017 +0000
+++ b/main.cpp	Mon Dec 04 02:26:44 2017 +0000
@@ -7,12 +7,13 @@
 #include "SDFileSystem.h"
 #include <string>
 #include "USBSerial.h"
- 
+
 //Defining PI
 #ifndef M_PI
 #define M_PI 3.1415
 #endif
 
+/********************** PIN DEFINITIONS ****************************/
 PwmOut motorPWM(P4_0);        // Motor PWM output
 DigitalOut motorFwd(P5_6);    // Motor forward enable
 DigitalOut motorRev(P5_5);    // Motor backward enable
@@ -30,41 +31,46 @@
 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;//(i2cBus_acc, BMI160_I2C::I2C_ADRS_SDO_LO);
+BMI160_I2C* m_imu;
 BMI160::AccConfig accConfig;
 BMI160::GyroConfig gyroConfig;
 uint32_t failures = 0;
 
-// for storing values
+/************** VALUES FOR STORING IMU and KALMAN ANGLE **********************/
+
+// IMU Variables
 float imuTemperature;
 BMI160::SensorData accData;
 BMI160::SensorData gyroData;
 BMI160::SensorTime sensorTime;
-
-// IMU Methods
 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.5; // set as variable denoted collision duration.
-                              // the duration of actuation
+float collisionDuration = 0.1; // duration of collision (rough measured experimentally)
+
 
 /****************** MOTOR CONTROL VARIABLES FOR STATES  **********************/
 //For motor control
 //Variables for states
 float theta_d = 0.0;
-//float Kp = 0.0;
+float Kp = 3.0; // proportional gain
 float Kd = 1.2; //made up
 float Ki = 0.1; //made up
 
@@ -78,27 +84,32 @@
 float presentTime = 0.0; //current time step
 float dT = 0.0; //time difference
 
-/** Set gain parameters using bluetooth **/
+/** Set gain parameters by log **/
 //Default Gains set to avoid problems
 float i_d = 0; //target current % changed by code
-float Kp = 3.0; // proportional gain
 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;
+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; //
+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){
@@ -159,22 +170,16 @@
 
 /********************* COLLISION UTILITY FUNCTIONS *************************/
 float last_xAcc = 0.0;
-
+float jerk = 0.0;
 bool checkCollision(float xAccel,float dT){
-  //printf("%f\n",xAccel);
-  //if ((xAccel > 1.85) || (xAccel <-1.85) || (collisionTimeRemaining>0)){
-  //if (last_xAcc<0 && xAccel>0){
-    float floatme = (xAccel-last_xAcc)/dT;
-    if (floatme>200.0){
-   //printf("Collision! %f\n",xAccel);
-    collisionTimeRemaining = max((collisionDuration)-(t.read()-collisionTime),0.0);
-   /* if ((xAccel>1.85) || xAccel <-1.85){
-      collisionTimeRemaining = collisionDuration;
-    }*/
-    return true;
-  }else{
-    return false;
-  }
+    jerk = (xAccel-last_xAcc)/dT;
+    // If above Jerk threshold, register collision
+    if (jerk>200.0){
+      collisionTimeRemaining = max((collisionDuration)-(t.read()-collisionTime),0.0);
+      return true;
+    }else{
+      return false;
+    }
 }
 
 //unused function for now
@@ -272,23 +277,13 @@
 
 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);
 
-    //printf("ACC xAxis = %s%4.3f\n", "\033[K", accData.xAxis.scaled);
-    //printf("ACC yAxis = %s%4.3f\n", "\033[K", accData.yAxis.scaled);
-    //printf("ACC zAxis = %s%4.3f\n\n", "\033[K", accData.zAxis.scaled);
-
-    //    printf("GYRO xAxis = %s%5.1f\n", "\033[K", gyroData.xAxis.scaled);
-    //    printf("GYRO yAxis = %s%5.1f\n", "\033[K", gyroData.yAxis.scaled);
-    //    printf("GYRO zAxis = %s%5.1f\n\n", "\033[K", gyroData.zAxis.scaled);
-
-    //    printf("Sensor Time = %s%f\n", "\033[K", sensorTime.seconds);
-    //    printf("Sensor Temperature = %s%5.3f\n", "\033[K", imuTemperature);
-
     accX = accData.xAxis.scaled;
     accY = accData.yAxis.scaled;
     accZ = accData.zAxis.scaled;
@@ -297,247 +292,553 @@
     gyroY = gyroData.yAxis.scaled;
     gyroZ = gyroData.zAxis.scaled;
 
-    //printf("%4.3f\n", accX);
+    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);
 
-    //printf("ACC xAxis = %s%4.3f\n", "\033[K", accX);
-    //printf("ACC yAxis = %s%4.3f\n", "\033[K", accY);
-    //printf("ACC zAxis = %s%4.3f\n\n", "\033[K", accZ);
-    R = sqrt(std::pow(accX, 2) + std::pow(accY, 2) + std::pow(accZ, 2));
-        getKalmanPrediction(t.read()-lastKalTime, gyroY, accY, R);
-        //printf("%f\n",kalman_get_angle(&filterRotation));
-        lastKalTime=t.read();
+    lastKalTime=t.read();
 
     updateLEDS(accX,accY,accZ);
 }
 
-/*********** Load and Update and Save Parameters ***************/ 
+/*********** Load and Update and Save Parameters ***************/
 char inputMode = 'n';
 
 void loadParam(){ //from SD
-    //kp read
-    FILE *fp_kp = fopen("/sd/mydir/param/kp.txt", "r");
-    char Buf_p[30];    
-    if(fp_kp == NULL) { error("Could not open file for reading\r\n"); }
-    while(fgets (Buf_p, 30, fp_kp) != NULL){
-        Buf_p[strlen(Buf_p)-1] = 0;
-        printf("String = \"%s\" \r\n", Buf_p);
+  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);
-    
-    Kp = (float)atof(Buf_p);
+    fclose(fp);
+    Kp = (float)atof(buf);
     printf("Kp is loaded as %f. \r\n", Kp);
-    
-    //kd read
-    FILE *fp_kd = fopen("/sd/mydir/param/kd.txt", "r");
-    char Buf_d[30];    
-    if(fp_kd == NULL) { error("Could not open file for reading\r\n"); }
-    while(fgets (Buf_d, 30, fp_kd) != NULL){
-        Buf_d[strlen(Buf_d)-1] = 0;
-        printf("String = \"%s\" \r\n", Buf_d);
+
+    // 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);
-    
-    Kd = (float)atof(Buf_d);
+    fclose(fp);
+    Kd = (float)atof(buf);
     printf("Kd is loaded as %f. \r\n", Kd);
-    
-    //theta_d read
-    FILE *fp_th = fopen("/sd/mydir/param/theta_d.txt", "r");
-    char Buf_t[30];    
-    if(fp_th == NULL) { error("Could not open file for reading\r\n"); }
-    while(fgets (Buf_t, 30, fp_th) != NULL){
-        Buf_t[strlen(Buf_t)-1] = 0;
-        printf("String = \"%s\" \r\n", Buf_t);
+
+    // 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_th);
-    
-    theta_d = (float)atof(Buf_t);
-    printf("theta_d is loaded as %f. \r\n", fp_th);
-    
+    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];
 
-    // -- kp
-    FILE *fp_kp = fopen("/sd/mydir/param/kp.txt", "w");
-    if(fp_kp == NULL) {
+  // 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");
     }
-    char buf_p[10];
-    sprintf(buf_p, "%.3f", Kp);
-    fprintf(fp_kp, buf_p);
-    fprintf(fp_kp, "\r\n");
-    fclose(fp_kp); 
+    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);
-    
-    // -- kd
-    FILE *fp_kd = fopen("/sd/mydir/param/kd.txt", "w");
-    if(fp_kd == NULL) {
+
+    // 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");
     }
-    char buf_d[10];
-    sprintf(buf_d, "%.3f", Kd);
-    fprintf(fp_kd, buf_d);
-    fprintf(fp_kd, "\r\n");
-    fclose(fp_kd); 
-    printf("Kd saved as %f.\r\n", Kd);
-    
-    // -- theta_d
-    FILE *fp_th = fopen("/sd/mydir/param/theta_d.txt", "w");
-    if(fp_th == NULL) {
-        error("Could not open file for write\n");
-    }
-    char buf_t[10];
-    sprintf(buf_t, "%.3f", theta_d);
-    fprintf(fp_th, buf_t);
-    fprintf(fp_th, "\r\n");
-    fclose(fp_th); 
-    printf("theta_d saved as %f.\r\n", theta_d);
-    
+    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] == 'p'){
-            pc.printf("input mode is set to Kp, Please enter value. \n");
-            inputMode = 'p';
-         } if(buffer_serial[0] == 'd'){
-            pc.printf("input mode is set to Kd, Please enter value. \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';
-         } if(buffer_serial[0] == 't'){
-            pc.printf("input mode is set to theta_d, Please enter value. \n");
+         } 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 \n\n", Kp);
+                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 \n\n", Kd);
+                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 \n\n", theta_d);
+                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);
         }
-    
-        
-        if(buffer_serial[0] == 'c'){
-            pc.printf("Kp is %f | Kd is %f | theta_d is %f \n\n", Kp, Kd, theta_d);
-            }
-         
+
 }
 
 /******************** LogData to SD card  *****************************/
 
 int trialTimeCount = 0;
 
-const int logValNum = 4;
+const int logValNum = 16;
 char logValName[logValNum][30];
-float logVal[logValNum][10000];
+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); 
+    fclose(fp3);
     printf("trial time saved\n");
 }
 
 void logData(){ // log data time
+
+
     logVal[0][currentLogNum] = t.read();
-    logVal[1][currentLogNum] = accX;
-    logVal[2][currentLogNum] = accY;
-    logVal[3][currentLogNum] = accZ;
-    
-    printf("logged data for %i. t.read() = %f \r\n", currentLogNum, t.read());
-    
-    currentLogNum++;   
+    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], "accX");
-   sprintf( logValName[2], "accY");
-   sprintf( logValName[3], "accZ");
-    
+   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); 
-    
+    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];  
- 
+
+        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); 
-    
+
+
+    fclose(f_sv);
+
     printf("Log Data file is saved as 'flipper_logData_%i.txt'.\n", trialTimeCount);
 }
 
 /*******************************************************************/
 
+/******************** FLIP STRATEGIES *****************************/
+float stiffnessStrategy(void){
+  // depends on global variables
+  // theta_d: reference angle
+  // theta: measured angle at joint B
+  // theta_dot: measured angular velocity at joint B
+  e = theta_d-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{
+    torque = torqueFlight_d;
+  }
+  // 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);
+  outputVal = r*i_d+Kp*(e)-k_b*theta_dot;
+  return outputVal;
+}
+
+float stiffnessAndtorqueStrategy(bool collisionState){
+  return stiffnessStrategy()+torqueStrategy(collisionState);
+}
+
+float executeFlipStrategy(int flipmode, bool collisionState){
+
+  // Mode 1: Stiffness only
+  if (flipmode==1){
+    return stiffnessStrategy();
+  }else{
+    // Mode 2: Torque Profile only
+    if (flipmode==2){
+      return torqueStrategy(collisionState);
+    }
+    // Mode 3: Stiffness and Torque
+    else{
+      return stiffnessAndtorqueStrategy(collisionState);
+    }
+  }
+
+}
 
 
 /******************** EXECUTE MOTOR LOOP *****************************/
@@ -556,140 +857,73 @@
     setMotorDir('f'); //begin with motor driving forward
     motorPWM.write(0);
     lastTime=t.read();
-    
-    float timeSinceCollision=0.0;
-
-    // Run experiment
-    while( t.read() < 10.0 && !debugModeOn) {
-           //Update IMU and Kalman
-            readIMU();
+    startTimeReference = t.read(); //set reference time for Kalman Angle
 
-            //R = sqrt(std::pow(accX, 2) + std::pow(accY, 2) + std::pow(accZ, 2));
-            //getKalmanPrediction(t.read()-lastTime, gyroY, accY, R);
-            //printf("kalman angle: %f\n",kalman_get_angle(&filterRotation));
-
-            //read present time and calculate dt
-            presentTime = t.read();
-            dT = presentTime - lastTime;
-
-            float floatme = (accX-last_xAcc)/dT;
+    /* Run experiment */
+    while( t.read() < 10.0 && !debugModeOn) {
+      //Update IMU and Kalman
+      readIMU();
 
-            //printf("last %f\n",last_xAcc);
-            //printf("now %f\n", accX);
-            //printf("%f\n",floatme);
-            lastTime = presentTime;
-
-            // Perform control loop logic
-                theta = getAngleFromPulses(encoder.getPulses());
-                theta_dot = getAngleFromPulses(encoder.getVelocity());
-
-                //check collisions
+      //read present time and calculate dt
+      presentTime = t.read();
+      dT = presentTime - lastTime;
 
-                //If new hit. Otherwise bool_collision will do the work.
-                if ((!bool_collision) & checkCollision(accX,dT)){
-                  collisionTime = t.read();
-                  collisionTimeRemaining = collisionDuration;
-                  bool_collision = true;
-                  hitTimes = hitTimes + 1;
-                }else{
-                  //see if actuation is maintained:
-                  bool_collision = checkCollision(accX,dT);
-                }
+      // set last time to determine next time step
+      lastTime = presentTime;
+
+      // Perform control loop logic
+      theta = getAngleFromPulses(encoder.getPulses());
+      theta_dot = getAngleFromPulses(encoder.getVelocity());
 
-                //originally default state (drop)
-                //torque is defaulty 0
-                torque = 0.0;
-                
-                // state when in stance (bool collision)
-                 if (bool_collision){
-                    torque = -0.2;
-                    i_d = torque/k_b;
+      /* Checking collisions */
 
-                     //Update Error and accumalated error term
-                    i = readCurrent();
-                    e = i_d-i;
-                    outputVal = r*i_d+Kp*(e)-k_b*theta_dot;
-                    timeSinceCollision = t.read();
-                }
-                
-                // state in the air (after bool collision duration off)
-                else{
-                    
-                    if (hitTimes >1){
-                        if ((t.read()-timeSinceCollision)<0.3){
-                            theta_d = -0.3;
-                            }
-                        if (  ((t.read()-timeSinceCollision)>0.3) && ((t.read()-timeSinceCollision)<0.7) ){
-                            theta_d = 0.31;
-                            }
-                        if ((t.read()-timeSinceCollision)>=1.0){
-                            theta_d = 0.64;
-                            }
-                            
-                            //at hit -1.7  t=-.3
-                            //peak -0.7    t=.31
-                            //end is 0.2   t=.64
-                            e = theta_d-theta;
-                            //e_accum = e_accum+e*dT;
-                            Kp = 5;
-                            Kd = 0.5;
-                            outputVal = Kp*e + Kd*(-theta_dot);
-                        }
-                }
-                
-                if (hitTimes<=1){
-                    outputVal = 0;    
-                }
-               // printf("%f",hitTimes);
+      // Determines whether new hit.
+      if ((!bool_collision) & checkCollision(accX,dT)){
+    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{
+    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
 
 
-                //direct input for torque
-         /*       if (bool_collision){
-                    torque = 1.4;
-                }else{
-                    torque = 0.0;
-                }*/
-                last_xAcc = accX;
-                //printf("HEY\n");
+      // Initial State, zero torque or other affects.
+      torque = 0.0;
 
-                //set reference current for torque
-                //theta_d = 0.0;//M_PI; // target at 180 degrees;
-                //torque = -k_th*(theta_d-theta)-b_th*-theta_dot+v_th*-theta_dot;
-             
-
-               // printf("currentError: %f\n",e);
-                //temp removal of force for non collision
-                //printf("%f\n",ain.read());
-
+      /* Compute Actuation Force */
+      outputVal = executeFlipStrategy(flipMode, bool_collision);
 
-               //regular pid
-                //Update Error and accumalated error term
-                /*e = theta_d-theta;
-                e_accum = e_accum+e*dT;
-                outputVal = Kp*e + Kd*(-theta_dot) + Ki*e_accum;*/
+      /* 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);
 
-               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);
-                 
-                 logData();
-                 if(!saveDataButton){
-                     saveData();
-                     debugModeOn = true;
-                 }
-                 
-            }
-            motorPWM.write(0);
+      /* Log Data */
+      logData();
+      if(!saveDataButton){
+    saveData();
+    debugModeOn = true;
+      }
+
+    }
+    // End of action cycle, turn motor "off"
+    motorPWM.write(0);
 }
 
 
@@ -699,42 +933,27 @@
 
 int main()
 {
-    
-    printf("test!!\n");
-    updateTrialTime();
-    loadParam();
-    
-    /* setup imu and kalman filter for rotation */
-    setupIMU();
-    setupKalman();
+
+  printf("test!!\n");
+  updateTrialTime();
+  loadParam();
+
+  /* setup imu and kalman filter for rotation */
+  setupIMU();
+  setupKalman();
+
+  /* run core motor loop */
+  executeMotorLoop();
 
-    //while( t.read() < 60.0) {
-    /* run core program */
-    executeMotorLoop();
-    
-    
-    //}
-    saveData();
-    
-    printf("Now you can set parameters by typing 'p', 'd', or 't'. You can check the values with 'c'.");
-    while(1){ // debugMode
-        serialUpdateVal();
-    }
+  /* save trial data to sd card */
+  saveData();
 
-    /* extra code for threading accelerometer kalman data */
-    //Thread eventThread;
-    //eventQueue.call_every(0.00001, periodicIMUCallback);
-    //eventQueue.dispatch_forever();
-    //eventThread.start(callback(&eventQueue, &EventQueue::dispatch_forever));
-
-
-    /*eventQueue.call_every(0.2, periodicCallback);
+  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'.");
 
-    BLE &ble = BLE::Instance();
-    ble.onEventsToProcess(scheduleBleEventsProcessing);
-    ble.init(bleInitComplete);
-    /*eventQueue.call_every(0.2, periodicCallback);
-    eventQueue.dispatch_forever();
+  while(1){ // debugMode
+    serialUpdateVal();
+  }
 
-    return 0;*/
-}
\ No newline at end of file
+}