K K / Mbed 2 deprecated heros_leg_readout_torque_addition

Torque calculation added to leg readout

Dependencies:   AS5048 LCM101 MODSERIAL PinDetect SDFileSystem mbed

Fork of heros_leg_readout by Martijn Grootens

main.cpp@3:3fd9f1a00381, 2018-06-05 (annotated)

Committer:
Technical_Muffin
Date:
Tue Jun 05 08:51:45 2018 +0000
Revision:
3:3fd9f1a00381
Parent:
2:84d479fe9b5e 
added torque calculation to the main file of the code. New calibration values were also added to correctly display the values.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
megrootens 0:3855d4588f76 1 #include "mbed.h"
megrootens 0:3855d4588f76 2 #include "string.h"
megrootens 0:3855d4588f76 3
megrootens 0:3855d4588f76 4 #include "bench.h"
megrootens 0:3855d4588f76 5 #include "PinDetect.h"
megrootens 0:3855d4588f76 6 #include "SDFileSystem.h"
megrootens 0:3855d4588f76 7 #include "MODSERIAL.h"
megrootens 0:3855d4588f76 8
Technical_Muffin 2:84d479fe9b5e 9 #define PI 3.14159265
megrootens 0:3855d4588f76 10
megrootens 0:3855d4588f76 11 // User io
megrootens 0:3855d4588f76 12 PinDetect sw2(SW2,PullUp);
megrootens 0:3855d4588f76 13 PinDetect sw3(SW3,PullUp);
megrootens 0:3855d4588f76 14 DigitalOut led_g(LED_GREEN);
megrootens 0:3855d4588f76 15
megrootens 0:3855d4588f76 16 void TogglePrinting();
megrootens 0:3855d4588f76 17 void ToggleLogging();
megrootens 0:3855d4588f76 18 void ShowAlive();
megrootens 0:3855d4588f76 19
megrootens 0:3855d4588f76 20 // Bench
megrootens 0:3855d4588f76 21 Bench leg(AS5048_MOSI, AS5048_MISO, AS5048_SCLK, AS5048_CS, LCM101);
megrootens 0:3855d4588f76 22 void Update()
megrootens 0:3855d4588f76 23 {
megrootens 0:3855d4588f76 24 leg.Update();
megrootens 0:3855d4588f76 25 }
megrootens 0:3855d4588f76 26
megrootens 0:3855d4588f76 27 // SD Card
megrootens 0:3855d4588f76 28 SDFileSystem sd(SD_MOSI, SD_MISO, SD_SCK, SD_CS, "sd");
megrootens 0:3855d4588f76 29
megrootens 0:3855d4588f76 30 void InitSdCard();
megrootens 0:3855d4588f76 31 void StartLogging(const char * fname_append = "data");
megrootens 0:3855d4588f76 32 void StopLogging();
megrootens 0:3855d4588f76 33 void LogData();
megrootens 0:3855d4588f76 34
megrootens 0:3855d4588f76 35 // Serial
megrootens 0:3855d4588f76 36 MODSERIAL pc(USBTX,USBRX);
megrootens 0:3855d4588f76 37
megrootens 0:3855d4588f76 38 void PrintStatus();
megrootens 0:3855d4588f76 39 void PrintMenu();
megrootens 0:3855d4588f76 40
megrootens 0:3855d4588f76 41 // Timing
megrootens 0:3855d4588f76 42 Ticker tick_update, tick_serial, tick_logging;
megrootens 0:3855d4588f76 43 Timer timer;
megrootens 0:3855d4588f76 44
megrootens 0:3855d4588f76 45 /**
megrootens 0:3855d4588f76 46 * Main loop/
megrootens 0:3855d4588f76 47 */
megrootens 0:3855d4588f76 48 int main()
megrootens 0:3855d4588f76 49 {
megrootens 0:3855d4588f76 50 pc.baud(timing::kSerialBaudrate);
megrootens 0:3855d4588f76 51 pc.printf("**Hello!**\r\n");
megrootens 0:3855d4588f76 52
megrootens 0:3855d4588f76 53 InitSdCard();
megrootens 0:3855d4588f76 54
megrootens 0:3855d4588f76 55 tick_update.attach_us(&Update,timing::kTimeControlUs);
megrootens 0:3855d4588f76 56 tick_serial.attach_us(&PrintStatus,timing::kTimeSerialPrintUs);
megrootens 0:3855d4588f76 57
megrootens 0:3855d4588f76 58 PrintMenu();
megrootens 0:3855d4588f76 59
megrootens 0:3855d4588f76 60 sw2.attach_asserted(&TogglePrinting);
megrootens 0:3855d4588f76 61 sw3.attach_asserted(&ToggleLogging);
megrootens 0:3855d4588f76 62
megrootens 0:3855d4588f76 63 sw2.setSampleFrequency();
megrootens 0:3855d4588f76 64 sw3.setSampleFrequency();
megrootens 0:3855d4588f76 65
megrootens 0:3855d4588f76 66 while (true);
megrootens 0:3855d4588f76 67 }
Technical_Muffin 2:84d479fe9b5e 68 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
Technical_Muffin 2:84d479fe9b5e 69 // IMPLEMENTATION torque calculations
Technical_Muffin 2:84d479fe9b5e 70 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
Technical_Muffin 2:84d479fe9b5e 71 float getTorque(float theta_hip, float theta_knee, float M0)
Technical_Muffin 2:84d479fe9b5e 72 {
Technical_Muffin 2:84d479fe9b5e 73 float theta_shin = theta_knee - theta_hip;
Technical_Muffin 2:84d479fe9b5e 74 //Values for torque calculation
Technical_Muffin 2:84d479fe9b5e 75 const float Mt = 7.24;//mass of thigh in kg
Technical_Muffin 2:84d479fe9b5e 76 const float Ms = 3.06;//mass of shank in kg
Technical_Muffin 2:84d479fe9b5e 77 const float Marm = 2.0;//mass of load carrying arm in kg
Technical_Muffin 2:84d479fe9b5e 78 const float g = 9.81;//gravity coefficient in m/s2
Technical_Muffin 2:84d479fe9b5e 79 const float dt = 0.45;//length of thigh in m
Technical_Muffin 2:84d479fe9b5e 80 const float dtm = 0.25755;//distance to mass middle point thigh in m
Technical_Muffin 2:84d479fe9b5e 81 const float ds = 0.45;//length of shank in m
Technical_Muffin 2:84d479fe9b5e 82 const float dsm = 0.197134;//distance to mass middle point shank in m
Technical_Muffin 2:84d479fe9b5e 83 const float shank_angle_correction = 6.04;//corrections for the difference in moment arm angle
Technical_Muffin 2:84d479fe9b5e 84 const float thigh_angle_correction = 3.59;//corrections for the difference in moment arm angle
Technical_Muffin 2:84d479fe9b5e 85 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.
Technical_Muffin 2:84d479fe9b5e 86 //Calculation of knee torque
Technical_Muffin 2:84d479fe9b5e 87 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));
Technical_Muffin 2:84d479fe9b5e 88
Technical_Muffin 2:84d479fe9b5e 89 return Tk;
Technical_Muffin 2:84d479fe9b5e 90 }
megrootens 0:3855d4588f76 91 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
megrootens 0:3855d4588f76 92 // IMPLEMENTATION USER IO
megrootens 0:3855d4588f76 93 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
megrootens 0:3855d4588f76 94
megrootens 0:3855d4588f76 95 bool is_printing = false;
megrootens 0:3855d4588f76 96 void TogglePrinting()
megrootens 0:3855d4588f76 97 {
megrootens 0:3855d4588f76 98 if (not is_printing) {
megrootens 0:3855d4588f76 99 is_printing = true;
megrootens 0:3855d4588f76 100 } else {
megrootens 0:3855d4588f76 101 is_printing = false;
megrootens 0:3855d4588f76 102 PrintMenu();
megrootens 0:3855d4588f76 103 }
megrootens 0:3855d4588f76 104 }
megrootens 0:3855d4588f76 105
megrootens 0:3855d4588f76 106 bool is_logging = false;
megrootens 0:3855d4588f76 107 void ToggleLogging()
megrootens 0:3855d4588f76 108 {
megrootens 0:3855d4588f76 109 if (not is_logging) {
megrootens 0:3855d4588f76 110 StartLogging();
megrootens 0:3855d4588f76 111 } else {
megrootens 0:3855d4588f76 112 is_logging = false;
megrootens 0:3855d4588f76 113 StopLogging();
megrootens 0:3855d4588f76 114 }
megrootens 0:3855d4588f76 115 PrintMenu();
megrootens 0:3855d4588f76 116 }
megrootens 0:3855d4588f76 117
megrootens 0:3855d4588f76 118 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
megrootens 0:3855d4588f76 119 // IMPLEMENTATION SERIAL COM
megrootens 0:3855d4588f76 120 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
megrootens 0:3855d4588f76 121 const int kNumJoints = 4;
megrootens 0:3855d4588f76 122 const char *kJointNames[kNumJoints] = {"Toes","Ankle","knee","Hip"};
Technical_Muffin 2:84d479fe9b5e 123
megrootens 0:3855d4588f76 124 void PrintStatus()
megrootens 0:3855d4588f76 125 {
megrootens 0:3855d4588f76 126 led_g = !led_g;
megrootens 0:3855d4588f76 127 if (is_printing) {
megrootens 0:3855d4588f76 128 pc.printf("\r\nLEG STATUS (%s)\r\n",led_g?"+":"*");
megrootens 0:3855d4588f76 129 for (int i=0; i<kNumJoints; ++i)
megrootens 0:3855d4588f76 130 pc.printf("\t%5s %7.2f\r\n",kJointNames[i], leg.getDegrees(i));
megrootens 0:3855d4588f76 131 pc.printf("\t%5s %7.2f\r\n","Force", leg.getForce());
Technical_Muffin 2:84d479fe9b5e 132 pc.printf("\t%5s %7.2f\r\n","Torque", getTorque(leg.getDegrees(3),leg.getDegrees(2),leg.getForce()));
megrootens 0:3855d4588f76 133 }
megrootens 0:3855d4588f76 134 }
megrootens 0:3855d4588f76 135
megrootens 0:3855d4588f76 136 void PrintMenu()
megrootens 0:3855d4588f76 137 {
megrootens 0:3855d4588f76 138 pc.printf("\r\nMENU\r\n");
megrootens 0:3855d4588f76 139 pc.printf("\t> Press SW2 to toggle printing leg status\r\n");
megrootens 0:3855d4588f76 140 pc.printf("\t> Press SW3 to toggle data logging\r\n");
megrootens 0:3855d4588f76 141 }
megrootens 0:3855d4588f76 142
megrootens 0:3855d4588f76 143 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
megrootens 0:3855d4588f76 144 // IMPLEMENTATION DATA LOGGING
megrootens 0:3855d4588f76 145 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
megrootens 0:3855d4588f76 146
megrootens 0:3855d4588f76 147 FILE * fp_data;
megrootens 0:3855d4588f76 148
megrootens 0:3855d4588f76 149 bool sd_card_present = false;
megrootens 0:3855d4588f76 150 int fname_prepend = 0;
megrootens 0:3855d4588f76 151
megrootens 0:3855d4588f76 152 /**
megrootens 0:3855d4588f76 153 * Check contents of SD card and count files in order
megrootens 0:3855d4588f76 154 * to ensure unique file name for logging data
megrootens 0:3855d4588f76 155 */
megrootens 0:3855d4588f76 156 void InitSdCard()
megrootens 0:3855d4588f76 157 {
megrootens 0:3855d4588f76 158 pc.printf("INITIALIZING SD CARD\r\n");
megrootens 0:3855d4588f76 159
megrootens 0:3855d4588f76 160 int num_files = 0;
megrootens 0:3855d4588f76 161
megrootens 0:3855d4588f76 162 // scan dir
megrootens 0:3855d4588f76 163 DIR *d;
megrootens 0:3855d4588f76 164 struct dirent *p;
megrootens 0:3855d4588f76 165
megrootens 0:3855d4588f76 166 d = opendir("/sd");
megrootens 0:3855d4588f76 167 if (d != NULL) {
megrootens 0:3855d4588f76 168 sd_card_present = true;
megrootens 0:3855d4588f76 169
megrootens 0:3855d4588f76 170 pc.printf("\t> Contents of SD Card:");
megrootens 0:3855d4588f76 171 while ((p = readdir(d)) != NULL) {
megrootens 0:3855d4588f76 172 if (p->d_name[0] != '.') {
megrootens 0:3855d4588f76 173 // skip files starting with '.'
megrootens 0:3855d4588f76 174 pc.printf("\t %s",p->d_name);
megrootens 0:3855d4588f76 175 ++num_files;
megrootens 0:3855d4588f76 176 }
megrootens 0:3855d4588f76 177 }
megrootens 0:3855d4588f76 178 pc.printf("\t> Counted %d visible files.\r\n",num_files);
megrootens 0:3855d4588f76 179
megrootens 0:3855d4588f76 180 closedir(d);
megrootens 0:3855d4588f76 181 } else {
megrootens 0:3855d4588f76 182 sd_card_present = false;
megrootens 0:3855d4588f76 183
megrootens 0:3855d4588f76 184 pc.printf("\t> No SD Card present. Data cannot be logged.\r\n");
megrootens 0:3855d4588f76 185 }
megrootens 0:3855d4588f76 186
megrootens 0:3855d4588f76 187 // id to be appended to logged data files
megrootens 0:3855d4588f76 188 fname_prepend = num_files;
megrootens 0:3855d4588f76 189 }
megrootens 0:3855d4588f76 190
megrootens 0:3855d4588f76 191 /**
megrootens 0:3855d4588f76 192 * Start logging data
megrootens 0:3855d4588f76 193 */
megrootens 0:3855d4588f76 194 void StartLogging(const char * fname_append)
megrootens 0:3855d4588f76 195 {
megrootens 0:3855d4588f76 196
megrootens 0:3855d4588f76 197 pc.printf("DATA LOGGING");
megrootens 0:3855d4588f76 198 if (sd_card_present) {
megrootens 0:3855d4588f76 199
megrootens 0:3855d4588f76 200 // create unique file name
megrootens 0:3855d4588f76 201 ++fname_prepend;
megrootens 0:3855d4588f76 202 char fname[50];
megrootens 0:3855d4588f76 203 sprintf(fname, "/sd/%d_%s.csv",fname_prepend,fname_append);
megrootens 0:3855d4588f76 204
megrootens 0:3855d4588f76 205 pc.printf("\t> Opening data log file '%s'...\r\n",fname);
megrootens 0:3855d4588f76 206
megrootens 0:3855d4588f76 207 // open file for writing and start logging after success
megrootens 0:3855d4588f76 208 fp_data = fopen(fname,"w");
megrootens 0:3855d4588f76 209 if (fp_data==NULL) {
megrootens 0:3855d4588f76 210 pc.printf("\t> ERROR: failed to open log file (t=%d ms)\r\n",
megrootens 0:3855d4588f76 211 timer.read_ms());
megrootens 0:3855d4588f76 212 } else {
Technical_Muffin 2:84d479fe9b5e 213 fprintf(fp_data, "time_ms, theta_toe, theta_ankle, theta_knee, theta_hip, force, torque");
megrootens 0:3855d4588f76 214 tick_logging.attach_us(&LogData,timing::kTimeLogDataUs);
megrootens 1:417a5b28ac84 215 timer.start();
megrootens 0:3855d4588f76 216 pc.printf("\t> Logging started.\r\n");
megrootens 0:3855d4588f76 217
megrootens 0:3855d4588f76 218 is_logging = true;
megrootens 0:3855d4588f76 219 }
megrootens 0:3855d4588f76 220
megrootens 0:3855d4588f76 221 } else {
megrootens 0:3855d4588f76 222 pc.printf("\t> No SD Card; no data will be logged.\r\n");
megrootens 0:3855d4588f76 223 }
megrootens 0:3855d4588f76 224 }
megrootens 0:3855d4588f76 225
megrootens 0:3855d4588f76 226
megrootens 0:3855d4588f76 227 /**
megrootens 0:3855d4588f76 228 * Stop logging data
megrootens 0:3855d4588f76 229 */
megrootens 0:3855d4588f76 230 void StopLogging()
megrootens 0:3855d4588f76 231 {
megrootens 0:3855d4588f76 232 pc.printf("DATA LOGGING:");
megrootens 0:3855d4588f76 233 if (sd_card_present) {
megrootens 0:3855d4588f76 234 // close data file, stop logging
megrootens 0:3855d4588f76 235 fclose(fp_data);
megrootens 0:3855d4588f76 236 tick_logging.detach();
megrootens 1:417a5b28ac84 237 timer.stop();
megrootens 1:417a5b28ac84 238 timer.reset();
megrootens 0:3855d4588f76 239 pc.printf("\r> Stopped.");
megrootens 0:3855d4588f76 240 } else {
megrootens 0:3855d4588f76 241 pc.printf("\t> No data was logged.");
megrootens 0:3855d4588f76 242 }
megrootens 0:3855d4588f76 243
megrootens 0:3855d4588f76 244 is_logging = false;
megrootens 0:3855d4588f76 245 }
megrootens 0:3855d4588f76 246
megrootens 0:3855d4588f76 247 /**
megrootens 0:3855d4588f76 248 * Log data
megrootens 0:3855d4588f76 249 */
megrootens 0:3855d4588f76 250 void LogData()
megrootens 0:3855d4588f76 251 {
megrootens 0:3855d4588f76 252 // time
megrootens 0:3855d4588f76 253 fprintf(fp_data,"\n%d", timer.read_ms());
megrootens 0:3855d4588f76 254
megrootens 0:3855d4588f76 255 // bench: joint angles and force sensor
Technical_Muffin 2:84d479fe9b5e 256 fprintf(fp_data,", %+f, %+f, %+f, %+f, %+f, %+f",
megrootens 0:3855d4588f76 257 leg.getDegrees(0),
megrootens 0:3855d4588f76 258 leg.getDegrees(1),
megrootens 0:3855d4588f76 259 leg.getDegrees(2),
megrootens 0:3855d4588f76 260 leg.getDegrees(3),
Technical_Muffin 2:84d479fe9b5e 261 leg.getForce(),
Technical_Muffin 2:84d479fe9b5e 262 getTorque(leg.getDegrees(3),leg.getDegrees(2),leg.getForce())
megrootens 0:3855d4588f76 263 );
megrootens 0:3855d4588f76 264 }