Diff: main.cpp

Revision:

2:84d479fe9b5e

Parent:

1:417a5b28ac84

--- a/main.cpp	Wed Dec 06 11:22:54 2017 +0000
+++ b/main.cpp	Tue Jun 05 08:49:21 2018 +0000
@@ -6,6 +6,7 @@
 #include "SDFileSystem.h"
 #include "MODSERIAL.h"
 
+#define PI 3.14159265
 
 // User io
 PinDetect sw2(SW2,PullUp);
@@ -64,8 +65,29 @@
 
     while (true);
 }
-
-
+// = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
+// IMPLEMENTATION torque calculations
+// = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
+float getTorque(float theta_hip, float theta_knee, float M0)
+{
+    float theta_shin = theta_knee - theta_hip;
+    //Values for torque calculation
+    const float Mt = 7.24;//mass of thigh in kg
+    const float Ms = 3.06;//mass of shank in kg
+    const float Marm = 2.0;//mass of load carrying arm in kg
+    const float g = 9.81;//gravity coefficient in m/s2
+    const float dt = 0.45;//length of thigh in m
+    const float dtm = 0.25755;//distance to mass middle point thigh in m
+    const float ds = 0.45;//length of shank in m
+    const float dsm = 0.197134;//distance to mass middle point shank in m
+    const float shank_angle_correction = 6.04;//corrections for the difference in moment arm angle
+    const float thigh_angle_correction = 3.59;//corrections for the difference in moment arm angle
+    float Mw=M0/g;//M0 is measured in N, but it is calculated as kg in the equation. So instead of changing the whole equation, this is a quick fix.
+    //Calculation of knee torque
+    float Tk = -1*(Ms*g)*(dsm*sin((theta_shin+shank_angle_correction)*PI/180))-((-1*(Ms*g)*(dsm*sin((theta_shin+shank_angle_correction)*PI/180))+(Mw+Marm+Mt+Ms)*g*(ds*sin(theta_shin*PI/180))-(Mt*g)*(dtm*sin((theta_hip-thigh_angle_correction)*PI/180))-(Mw+Marm)*g*(dt*sin(theta_hip*PI/180)))/(ds*cos(theta_shin*PI/180)+dt*cos(theta_hip*PI/180)))*(ds*cos(theta_shin*PI/180))+(Mw+Marm+Mt+Ms)*g*(ds*sin(theta_shin*PI/180));
+    
+    return Tk;
+}
 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
 // IMPLEMENTATION USER IO
 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
@@ -98,7 +120,7 @@
 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
 const int kNumJoints = 4;
 const char *kJointNames[kNumJoints] = {"Toes","Ankle","knee","Hip"};
-
+    
 void PrintStatus()
 {
     led_g = !led_g;
@@ -107,6 +129,7 @@
         for (int i=0; i<kNumJoints; ++i)
             pc.printf("\t%5s %7.2f\r\n",kJointNames[i], leg.getDegrees(i));
         pc.printf("\t%5s %7.2f\r\n","Force",  leg.getForce());
+        pc.printf("\t%5s %7.2f\r\n","Torque",  getTorque(leg.getDegrees(3),leg.getDegrees(2),leg.getForce()));
     }
 }
 
@@ -187,7 +210,7 @@
             pc.printf("\t> ERROR: failed to open log file (t=%d ms)\r\n",
                       timer.read_ms());
         } else {
-            fprintf(fp_data, "time_ms, theta_toe, theta_ankle, theta_knee, theta_hip, force");
+            fprintf(fp_data, "time_ms, theta_toe, theta_ankle, theta_knee, theta_hip, force, torque");
             tick_logging.attach_us(&LogData,timing::kTimeLogDataUs);
             timer.start();
             pc.printf("\t> Logging started.\r\n");
@@ -230,11 +253,12 @@
     fprintf(fp_data,"\n%d", timer.read_ms());
 
     // bench: joint angles and force sensor
-    fprintf(fp_data,", %+f, %+f, %+f, %+f, %+f",
+    fprintf(fp_data,", %+f, %+f, %+f, %+f, %+f, %+f",
             leg.getDegrees(0),
             leg.getDegrees(1),
             leg.getDegrees(2),
             leg.getDegrees(3),
-            leg.getForce()
+            leg.getForce(),
+            getTorque(leg.getDegrees(3),leg.getDegrees(2),leg.getForce())
            );
 }
\ No newline at end of file