Torque calculation added to leg readout
Dependencies: AS5048 LCM101 MODSERIAL PinDetect SDFileSystem mbed
Fork of heros_leg_readout by
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?
User | Revision | Line number | New 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 | } |