Diff: main.cpp

Revision:

4:227f0847a797

Parent:

3:5696ac47658a

--- a/main.cpp	Mon Dec 04 02:26:44 2017 +0000
+++ b/main.cpp	Tue Mar 13 04:00:09 2018 +0000
@@ -13,6 +13,7 @@
 #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
@@ -63,7 +64,7 @@
 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.1; // duration of collision (rough measured experimentally)
+float collisionDuration = 0.2; // duration of collision (rough measured experimentally)
 
 
 /****************** MOTOR CONTROL VARIABLES FOR STATES  **********************/
@@ -82,7 +83,7 @@
 
 float lastTime = 0.0; //last time step
 float presentTime = 0.0; //current time step
-float dT = 0.0; //time difference
+float dT = 0.045; //time difference
 
 /** Set gain parameters by log **/
 //Default Gains set to avoid problems
@@ -171,14 +172,20 @@
 /********************* 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;
-    // If above Jerk threshold, register collision
-    if (jerk>200.0){
-      collisionTimeRemaining = max((collisionDuration)-(t.read()-collisionTime),0.0);
-      return true;
-    }else{
+    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;
+     }
     }
 }
 
@@ -711,7 +718,7 @@
     logVal[13][currentLogNum] = gyroY;
     logVal[14][currentLogNum] = gyroZ;
 
-   // printf("logged data for %i. t.read() = %f \r\n", currentLogNum, t.read());
+    //printf("logged data for %i. t.read() = %f \r\n", currentLogNum, t.read());
 
     currentLogNum++;
 }
@@ -774,13 +781,24 @@
 /*******************************************************************/
 
 /******************** FLIP STRATEGIES *****************************/
-float stiffnessStrategy(void){
+
+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
-  e = theta_d-theta;
+  //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);
@@ -789,10 +807,10 @@
   }
  // 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;*/
+  //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);
@@ -805,7 +823,12 @@
   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
@@ -813,20 +836,25 @@
   i_d = torque/k_b;
   i = readCurrent();
   e = i_d-i;
-  printf("%f\n",outputVal);
+  //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()+torqueStrategy(collisionState);
+  return stiffnessStrategy(collisionState)+torqueStrategy(collisionState);
 }
 
 float executeFlipStrategy(int flipmode, bool collisionState){
 
   // Mode 1: Stiffness only
   if (flipmode==1){
-    return stiffnessStrategy();
+    return stiffnessStrategy(collisionState);
   }else{
     // Mode 2: Torque Profile only
     if (flipmode==2){
@@ -844,7 +872,6 @@
 /******************** EXECUTE MOTOR LOOP *****************************/
 
 float pikeAngle = 0.0;
-float hitTimes = 0;
 
 /** Core Motor Code Loop**/
 void executeMotorLoop(){
@@ -878,17 +905,25 @@
       /* Checking collisions */
 
       // Determines whether new hit.
-      if ((!bool_collision) & checkCollision(accX,dT)){
-    collisionTime = t.read();
-    collisionTimeRemaining = collisionDuration;
-    bool_collision = true;
-    hitTimes = hitTimes + 1;
+      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{
-    bool_collision = checkCollision(accX,dT);
-      }
+        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