Code for the COVR project DROPSAW project test rig (previously used for the Heros testing).
Dependencies: SPTE_10Bar_5V mbed AS5048 SDFileSystem MODSERIAL LCM101_DROPSAW LinearActuator
main.cpp@2:84d479fe9b5e, 2018-06-05 (annotated)
- Committer:
- Technical_Muffin
- Date:
- Tue Jun 05 08:49:21 2018 +0000
- Revision:
- 2:84d479fe9b5e
- Parent:
- 1:417a5b28ac84
- Child:
- 4:1cdce6c6c94e
Added torque calculation to the main file of the code. The constant calibration values were also changed to correctly display the desired 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 | } |