Version 3 is with update to the test rig with a linear actuator
Dependencies: SPTE_10Bar_5V mbed AS5048 SDFileSystem MODSERIAL PinDetect LCM101 LinearActuator
main.cpp@4:1cdce6c6c94e, 2018-10-12 (annotated)
- Committer:
- cnckiwi31
- Date:
- Fri Oct 12 12:12:55 2018 +0000
- Revision:
- 4:1cdce6c6c94e
- Parent:
- 2:84d479fe9b5e
- Child:
- 5:63063a9fa51c
HeRoS: read out and log joint angles and force sensor data from the leg test bench.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
megrootens | 0:3855d4588f76 | 1 | #include "mbed.h" |
cnckiwi31 | 4:1cdce6c6c94e | 2 | #include <string> |
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 |
cnckiwi31 | 4:1cdce6c6c94e | 12 | |
cnckiwi31 | 4:1cdce6c6c94e | 13 | PinDetect forceNull(PTB16,PullUp); |
cnckiwi31 | 4:1cdce6c6c94e | 14 | PinDetect press0Null(PTA4,PullUp); |
cnckiwi31 | 4:1cdce6c6c94e | 15 | |
megrootens | 0:3855d4588f76 | 16 | PinDetect sw2(SW2,PullUp); |
megrootens | 0:3855d4588f76 | 17 | PinDetect sw3(SW3,PullUp); |
megrootens | 0:3855d4588f76 | 18 | DigitalOut led_g(LED_GREEN); |
megrootens | 0:3855d4588f76 | 19 | |
cnckiwi31 | 4:1cdce6c6c94e | 20 | void ResetForce(); |
cnckiwi31 | 4:1cdce6c6c94e | 21 | void ResetPress(); |
megrootens | 0:3855d4588f76 | 22 | void TogglePrinting(); |
megrootens | 0:3855d4588f76 | 23 | void ToggleLogging(); |
megrootens | 0:3855d4588f76 | 24 | void ShowAlive(); |
megrootens | 0:3855d4588f76 | 25 | |
megrootens | 0:3855d4588f76 | 26 | // Bench |
cnckiwi31 | 4:1cdce6c6c94e | 27 | Bench leg(AS5048_MOSI, AS5048_MISO, AS5048_SCLK, AS5048_CS, LCM101,SPTE_0,SPTE_1); |
megrootens | 0:3855d4588f76 | 28 | void Update() |
megrootens | 0:3855d4588f76 | 29 | { |
megrootens | 0:3855d4588f76 | 30 | leg.Update(); |
megrootens | 0:3855d4588f76 | 31 | } |
megrootens | 0:3855d4588f76 | 32 | |
megrootens | 0:3855d4588f76 | 33 | // SD Card |
megrootens | 0:3855d4588f76 | 34 | SDFileSystem sd(SD_MOSI, SD_MISO, SD_SCK, SD_CS, "sd"); |
megrootens | 0:3855d4588f76 | 35 | |
megrootens | 0:3855d4588f76 | 36 | void InitSdCard(); |
megrootens | 0:3855d4588f76 | 37 | void StartLogging(const char * fname_append = "data"); |
megrootens | 0:3855d4588f76 | 38 | void StopLogging(); |
megrootens | 0:3855d4588f76 | 39 | void LogData(); |
megrootens | 0:3855d4588f76 | 40 | |
megrootens | 0:3855d4588f76 | 41 | // Serial |
megrootens | 0:3855d4588f76 | 42 | MODSERIAL pc(USBTX,USBRX); |
megrootens | 0:3855d4588f76 | 43 | |
megrootens | 0:3855d4588f76 | 44 | void PrintStatus(); |
megrootens | 0:3855d4588f76 | 45 | void PrintMenu(); |
megrootens | 0:3855d4588f76 | 46 | |
megrootens | 0:3855d4588f76 | 47 | // Timing |
megrootens | 0:3855d4588f76 | 48 | Ticker tick_update, tick_serial, tick_logging; |
megrootens | 0:3855d4588f76 | 49 | Timer timer; |
cnckiwi31 | 4:1cdce6c6c94e | 50 | int firstReadMS = 0; //first timer value read |
cnckiwi31 | 4:1cdce6c6c94e | 51 | bool startedLogging = false; //in the middle of a logging cycle |
megrootens | 0:3855d4588f76 | 52 | |
megrootens | 0:3855d4588f76 | 53 | /** |
megrootens | 0:3855d4588f76 | 54 | * Main loop/ |
megrootens | 0:3855d4588f76 | 55 | */ |
megrootens | 0:3855d4588f76 | 56 | int main() |
megrootens | 0:3855d4588f76 | 57 | { |
megrootens | 0:3855d4588f76 | 58 | pc.baud(timing::kSerialBaudrate); |
megrootens | 0:3855d4588f76 | 59 | pc.printf("**Hello!**\r\n"); |
megrootens | 0:3855d4588f76 | 60 | |
megrootens | 0:3855d4588f76 | 61 | InitSdCard(); |
megrootens | 0:3855d4588f76 | 62 | |
megrootens | 0:3855d4588f76 | 63 | tick_update.attach_us(&Update,timing::kTimeControlUs); |
megrootens | 0:3855d4588f76 | 64 | tick_serial.attach_us(&PrintStatus,timing::kTimeSerialPrintUs); |
megrootens | 0:3855d4588f76 | 65 | |
megrootens | 0:3855d4588f76 | 66 | PrintMenu(); |
megrootens | 0:3855d4588f76 | 67 | |
cnckiwi31 | 4:1cdce6c6c94e | 68 | forceNull.attach_asserted(&ResetForce); |
cnckiwi31 | 4:1cdce6c6c94e | 69 | press0Null.attach_asserted(&ResetPress); |
cnckiwi31 | 4:1cdce6c6c94e | 70 | |
cnckiwi31 | 4:1cdce6c6c94e | 71 | forceNull.setSampleFrequency(); |
cnckiwi31 | 4:1cdce6c6c94e | 72 | press0Null.setSampleFrequency(); |
cnckiwi31 | 4:1cdce6c6c94e | 73 | |
megrootens | 0:3855d4588f76 | 74 | sw2.attach_asserted(&TogglePrinting); |
megrootens | 0:3855d4588f76 | 75 | sw3.attach_asserted(&ToggleLogging); |
megrootens | 0:3855d4588f76 | 76 | |
megrootens | 0:3855d4588f76 | 77 | sw2.setSampleFrequency(); |
megrootens | 0:3855d4588f76 | 78 | sw3.setSampleFrequency(); |
megrootens | 0:3855d4588f76 | 79 | |
megrootens | 0:3855d4588f76 | 80 | while (true); |
megrootens | 0:3855d4588f76 | 81 | } |
Technical_Muffin | 2:84d479fe9b5e | 82 | // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = |
Technical_Muffin | 2:84d479fe9b5e | 83 | // IMPLEMENTATION torque calculations |
Technical_Muffin | 2:84d479fe9b5e | 84 | // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = |
Technical_Muffin | 2:84d479fe9b5e | 85 | float getTorque(float theta_hip, float theta_knee, float M0) |
Technical_Muffin | 2:84d479fe9b5e | 86 | { |
Technical_Muffin | 2:84d479fe9b5e | 87 | float theta_shin = theta_knee - theta_hip; |
Technical_Muffin | 2:84d479fe9b5e | 88 | //Values for torque calculation |
Technical_Muffin | 2:84d479fe9b5e | 89 | const float Mt = 7.24;//mass of thigh in kg |
Technical_Muffin | 2:84d479fe9b5e | 90 | const float Ms = 3.06;//mass of shank in kg |
Technical_Muffin | 2:84d479fe9b5e | 91 | const float Marm = 2.0;//mass of load carrying arm in kg |
Technical_Muffin | 2:84d479fe9b5e | 92 | const float g = 9.81;//gravity coefficient in m/s2 |
Technical_Muffin | 2:84d479fe9b5e | 93 | const float dt = 0.45;//length of thigh in m |
Technical_Muffin | 2:84d479fe9b5e | 94 | const float dtm = 0.25755;//distance to mass middle point thigh in m |
Technical_Muffin | 2:84d479fe9b5e | 95 | const float ds = 0.45;//length of shank in m |
Technical_Muffin | 2:84d479fe9b5e | 96 | const float dsm = 0.197134;//distance to mass middle point shank in m |
Technical_Muffin | 2:84d479fe9b5e | 97 | const float shank_angle_correction = 6.04;//corrections for the difference in moment arm angle |
Technical_Muffin | 2:84d479fe9b5e | 98 | const float thigh_angle_correction = 3.59;//corrections for the difference in moment arm angle |
Technical_Muffin | 2:84d479fe9b5e | 99 | 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 | 100 | //Calculation of knee torque |
Technical_Muffin | 2:84d479fe9b5e | 101 | 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 | 102 | |
Technical_Muffin | 2:84d479fe9b5e | 103 | return Tk; |
Technical_Muffin | 2:84d479fe9b5e | 104 | } |
megrootens | 0:3855d4588f76 | 105 | // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = |
megrootens | 0:3855d4588f76 | 106 | // IMPLEMENTATION USER IO |
megrootens | 0:3855d4588f76 | 107 | // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = |
megrootens | 0:3855d4588f76 | 108 | |
cnckiwi31 | 4:1cdce6c6c94e | 109 | void ResetForce() |
cnckiwi31 | 4:1cdce6c6c94e | 110 | { |
cnckiwi31 | 4:1cdce6c6c94e | 111 | leg.nullForce(); |
cnckiwi31 | 4:1cdce6c6c94e | 112 | } |
cnckiwi31 | 4:1cdce6c6c94e | 113 | |
cnckiwi31 | 4:1cdce6c6c94e | 114 | void ResetPress() |
cnckiwi31 | 4:1cdce6c6c94e | 115 | { |
cnckiwi31 | 4:1cdce6c6c94e | 116 | leg.nullPressure0(); |
cnckiwi31 | 4:1cdce6c6c94e | 117 | leg.nullPressure1(); |
cnckiwi31 | 4:1cdce6c6c94e | 118 | } |
cnckiwi31 | 4:1cdce6c6c94e | 119 | |
megrootens | 0:3855d4588f76 | 120 | bool is_printing = false; |
megrootens | 0:3855d4588f76 | 121 | void TogglePrinting() |
megrootens | 0:3855d4588f76 | 122 | { |
megrootens | 0:3855d4588f76 | 123 | if (not is_printing) { |
megrootens | 0:3855d4588f76 | 124 | is_printing = true; |
megrootens | 0:3855d4588f76 | 125 | } else { |
megrootens | 0:3855d4588f76 | 126 | is_printing = false; |
megrootens | 0:3855d4588f76 | 127 | PrintMenu(); |
megrootens | 0:3855d4588f76 | 128 | } |
megrootens | 0:3855d4588f76 | 129 | } |
megrootens | 0:3855d4588f76 | 130 | |
megrootens | 0:3855d4588f76 | 131 | bool is_logging = false; |
megrootens | 0:3855d4588f76 | 132 | void ToggleLogging() |
megrootens | 0:3855d4588f76 | 133 | { |
megrootens | 0:3855d4588f76 | 134 | if (not is_logging) { |
megrootens | 0:3855d4588f76 | 135 | StartLogging(); |
megrootens | 0:3855d4588f76 | 136 | } else { |
megrootens | 0:3855d4588f76 | 137 | is_logging = false; |
megrootens | 0:3855d4588f76 | 138 | StopLogging(); |
megrootens | 0:3855d4588f76 | 139 | } |
megrootens | 0:3855d4588f76 | 140 | PrintMenu(); |
megrootens | 0:3855d4588f76 | 141 | } |
megrootens | 0:3855d4588f76 | 142 | |
megrootens | 0:3855d4588f76 | 143 | // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = |
megrootens | 0:3855d4588f76 | 144 | // IMPLEMENTATION SERIAL COM |
megrootens | 0:3855d4588f76 | 145 | // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = |
megrootens | 0:3855d4588f76 | 146 | const int kNumJoints = 4; |
cnckiwi31 | 4:1cdce6c6c94e | 147 | const char *kJointNames[kNumJoints] = {"Toes","Ankle","Knee","Hip"}; |
cnckiwi31 | 4:1cdce6c6c94e | 148 | |
cnckiwi31 | 4:1cdce6c6c94e | 149 | bool sd_card_present = false; |
cnckiwi31 | 4:1cdce6c6c94e | 150 | int fname_prepend = 0; |
Technical_Muffin | 2:84d479fe9b5e | 151 | |
megrootens | 0:3855d4588f76 | 152 | void PrintStatus() |
megrootens | 0:3855d4588f76 | 153 | { |
megrootens | 0:3855d4588f76 | 154 | led_g = !led_g; |
megrootens | 0:3855d4588f76 | 155 | if (is_printing) { |
megrootens | 0:3855d4588f76 | 156 | pc.printf("\r\nLEG STATUS (%s)\r\n",led_g?"+":"*"); |
cnckiwi31 | 4:1cdce6c6c94e | 157 | pc.printf("\tSD card detected? %9s\r\n",sd_card_present?"Yes":"No"); |
cnckiwi31 | 4:1cdce6c6c94e | 158 | if(sd_card_present) |
cnckiwi31 | 4:1cdce6c6c94e | 159 | { |
cnckiwi31 | 4:1cdce6c6c94e | 160 | pc.printf("\tFile number %15i\r\n", fname_prepend); |
cnckiwi31 | 4:1cdce6c6c94e | 161 | pc.printf("\tLogging? %18s\r\n",is_logging?"Yes":"No"); |
cnckiwi31 | 4:1cdce6c6c94e | 162 | } |
cnckiwi31 | 4:1cdce6c6c94e | 163 | pc.printf("\n\t%15s %7.2f\r\n","Pressure0 (kPa)", leg.getPressure0()*100); |
cnckiwi31 | 4:1cdce6c6c94e | 164 | pc.printf("\t%15s %7.2f\r\n","Pressure1 (kPa)", leg.getPressure1()*100); |
cnckiwi31 | 4:1cdce6c6c94e | 165 | |
megrootens | 0:3855d4588f76 | 166 | for (int i=0; i<kNumJoints; ++i) |
cnckiwi31 | 4:1cdce6c6c94e | 167 | { |
cnckiwi31 | 4:1cdce6c6c94e | 168 | string jointName = kJointNames[i]; |
cnckiwi31 | 4:1cdce6c6c94e | 169 | jointName = jointName + " (deg)"; |
cnckiwi31 | 4:1cdce6c6c94e | 170 | pc.printf("\t%15s %7.2f\r\n",jointName, leg.getDegrees(i)); |
cnckiwi31 | 4:1cdce6c6c94e | 171 | } |
cnckiwi31 | 4:1cdce6c6c94e | 172 | pc.printf("\t%15s %7.2f\r\n","Force (N)", leg.getForce()); |
cnckiwi31 | 4:1cdce6c6c94e | 173 | pc.printf("\t%15s %7.2f\r\n","Torque (Nm)", getTorque(leg.getDegrees(3),leg.getDegrees(2),leg.getForce())); |
megrootens | 0:3855d4588f76 | 174 | } |
megrootens | 0:3855d4588f76 | 175 | } |
megrootens | 0:3855d4588f76 | 176 | |
megrootens | 0:3855d4588f76 | 177 | void PrintMenu() |
megrootens | 0:3855d4588f76 | 178 | { |
megrootens | 0:3855d4588f76 | 179 | pc.printf("\r\nMENU\r\n"); |
megrootens | 0:3855d4588f76 | 180 | pc.printf("\t> Press SW2 to toggle printing leg status\r\n"); |
megrootens | 0:3855d4588f76 | 181 | pc.printf("\t> Press SW3 to toggle data logging\r\n"); |
cnckiwi31 | 4:1cdce6c6c94e | 182 | |
cnckiwi31 | 4:1cdce6c6c94e | 183 | pc.printf("\tSD card detected? %9s\r\n",sd_card_present?"Yes":"No"); |
cnckiwi31 | 4:1cdce6c6c94e | 184 | if(sd_card_present) |
cnckiwi31 | 4:1cdce6c6c94e | 185 | { |
cnckiwi31 | 4:1cdce6c6c94e | 186 | pc.printf("\tFile number %15i\r\n", fname_prepend); |
cnckiwi31 | 4:1cdce6c6c94e | 187 | pc.printf("\tLogging? %18s\r\n",is_logging?"Yes":"No"); |
cnckiwi31 | 4:1cdce6c6c94e | 188 | } |
megrootens | 0:3855d4588f76 | 189 | } |
megrootens | 0:3855d4588f76 | 190 | |
megrootens | 0:3855d4588f76 | 191 | // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = |
megrootens | 0:3855d4588f76 | 192 | // IMPLEMENTATION DATA LOGGING |
megrootens | 0:3855d4588f76 | 193 | // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = |
megrootens | 0:3855d4588f76 | 194 | |
megrootens | 0:3855d4588f76 | 195 | FILE * fp_data; |
megrootens | 0:3855d4588f76 | 196 | |
cnckiwi31 | 4:1cdce6c6c94e | 197 | |
megrootens | 0:3855d4588f76 | 198 | |
megrootens | 0:3855d4588f76 | 199 | /** |
megrootens | 0:3855d4588f76 | 200 | * Check contents of SD card and count files in order |
megrootens | 0:3855d4588f76 | 201 | * to ensure unique file name for logging data |
megrootens | 0:3855d4588f76 | 202 | */ |
megrootens | 0:3855d4588f76 | 203 | void InitSdCard() |
megrootens | 0:3855d4588f76 | 204 | { |
megrootens | 0:3855d4588f76 | 205 | pc.printf("INITIALIZING SD CARD\r\n"); |
megrootens | 0:3855d4588f76 | 206 | |
megrootens | 0:3855d4588f76 | 207 | int num_files = 0; |
megrootens | 0:3855d4588f76 | 208 | |
megrootens | 0:3855d4588f76 | 209 | // scan dir |
megrootens | 0:3855d4588f76 | 210 | DIR *d; |
megrootens | 0:3855d4588f76 | 211 | struct dirent *p; |
megrootens | 0:3855d4588f76 | 212 | |
megrootens | 0:3855d4588f76 | 213 | d = opendir("/sd"); |
megrootens | 0:3855d4588f76 | 214 | if (d != NULL) { |
megrootens | 0:3855d4588f76 | 215 | sd_card_present = true; |
megrootens | 0:3855d4588f76 | 216 | |
megrootens | 0:3855d4588f76 | 217 | pc.printf("\t> Contents of SD Card:"); |
megrootens | 0:3855d4588f76 | 218 | while ((p = readdir(d)) != NULL) { |
megrootens | 0:3855d4588f76 | 219 | if (p->d_name[0] != '.') { |
megrootens | 0:3855d4588f76 | 220 | // skip files starting with '.' |
megrootens | 0:3855d4588f76 | 221 | pc.printf("\t %s",p->d_name); |
megrootens | 0:3855d4588f76 | 222 | ++num_files; |
megrootens | 0:3855d4588f76 | 223 | } |
megrootens | 0:3855d4588f76 | 224 | } |
megrootens | 0:3855d4588f76 | 225 | pc.printf("\t> Counted %d visible files.\r\n",num_files); |
megrootens | 0:3855d4588f76 | 226 | |
megrootens | 0:3855d4588f76 | 227 | closedir(d); |
megrootens | 0:3855d4588f76 | 228 | } else { |
megrootens | 0:3855d4588f76 | 229 | sd_card_present = false; |
megrootens | 0:3855d4588f76 | 230 | |
megrootens | 0:3855d4588f76 | 231 | pc.printf("\t> No SD Card present. Data cannot be logged.\r\n"); |
megrootens | 0:3855d4588f76 | 232 | } |
megrootens | 0:3855d4588f76 | 233 | |
megrootens | 0:3855d4588f76 | 234 | // id to be appended to logged data files |
megrootens | 0:3855d4588f76 | 235 | fname_prepend = num_files; |
megrootens | 0:3855d4588f76 | 236 | } |
megrootens | 0:3855d4588f76 | 237 | |
megrootens | 0:3855d4588f76 | 238 | /** |
megrootens | 0:3855d4588f76 | 239 | * Start logging data |
megrootens | 0:3855d4588f76 | 240 | */ |
megrootens | 0:3855d4588f76 | 241 | void StartLogging(const char * fname_append) |
megrootens | 0:3855d4588f76 | 242 | { |
megrootens | 0:3855d4588f76 | 243 | |
megrootens | 0:3855d4588f76 | 244 | pc.printf("DATA LOGGING"); |
megrootens | 0:3855d4588f76 | 245 | if (sd_card_present) { |
megrootens | 0:3855d4588f76 | 246 | |
megrootens | 0:3855d4588f76 | 247 | // create unique file name |
megrootens | 0:3855d4588f76 | 248 | ++fname_prepend; |
megrootens | 0:3855d4588f76 | 249 | char fname[50]; |
megrootens | 0:3855d4588f76 | 250 | sprintf(fname, "/sd/%d_%s.csv",fname_prepend,fname_append); |
megrootens | 0:3855d4588f76 | 251 | |
megrootens | 0:3855d4588f76 | 252 | pc.printf("\t> Opening data log file '%s'...\r\n",fname); |
megrootens | 0:3855d4588f76 | 253 | |
megrootens | 0:3855d4588f76 | 254 | // open file for writing and start logging after success |
megrootens | 0:3855d4588f76 | 255 | fp_data = fopen(fname,"w"); |
megrootens | 0:3855d4588f76 | 256 | if (fp_data==NULL) { |
megrootens | 0:3855d4588f76 | 257 | pc.printf("\t> ERROR: failed to open log file (t=%d ms)\r\n", |
megrootens | 0:3855d4588f76 | 258 | timer.read_ms()); |
megrootens | 0:3855d4588f76 | 259 | } else { |
cnckiwi31 | 4:1cdce6c6c94e | 260 | fprintf(fp_data, "time (s), theta_toe (deg), theta_ankle (deg), theta_knee (deg), theta_hip (deg), force (N), torque (Nm), pressure0 (kPa), pressure1 (kPa)"); |
megrootens | 0:3855d4588f76 | 261 | tick_logging.attach_us(&LogData,timing::kTimeLogDataUs); |
megrootens | 1:417a5b28ac84 | 262 | timer.start(); |
cnckiwi31 | 4:1cdce6c6c94e | 263 | startedLogging = true; |
cnckiwi31 | 4:1cdce6c6c94e | 264 | |
megrootens | 0:3855d4588f76 | 265 | pc.printf("\t> Logging started.\r\n"); |
megrootens | 0:3855d4588f76 | 266 | |
megrootens | 0:3855d4588f76 | 267 | is_logging = true; |
megrootens | 0:3855d4588f76 | 268 | } |
megrootens | 0:3855d4588f76 | 269 | |
megrootens | 0:3855d4588f76 | 270 | } else { |
megrootens | 0:3855d4588f76 | 271 | pc.printf("\t> No SD Card; no data will be logged.\r\n"); |
cnckiwi31 | 4:1cdce6c6c94e | 272 | startedLogging = false; |
megrootens | 0:3855d4588f76 | 273 | } |
megrootens | 0:3855d4588f76 | 274 | } |
megrootens | 0:3855d4588f76 | 275 | |
megrootens | 0:3855d4588f76 | 276 | |
megrootens | 0:3855d4588f76 | 277 | /** |
megrootens | 0:3855d4588f76 | 278 | * Stop logging data |
megrootens | 0:3855d4588f76 | 279 | */ |
megrootens | 0:3855d4588f76 | 280 | void StopLogging() |
megrootens | 0:3855d4588f76 | 281 | { |
megrootens | 0:3855d4588f76 | 282 | pc.printf("DATA LOGGING:"); |
megrootens | 0:3855d4588f76 | 283 | if (sd_card_present) { |
megrootens | 0:3855d4588f76 | 284 | // close data file, stop logging |
megrootens | 0:3855d4588f76 | 285 | fclose(fp_data); |
megrootens | 0:3855d4588f76 | 286 | tick_logging.detach(); |
megrootens | 1:417a5b28ac84 | 287 | timer.stop(); |
megrootens | 1:417a5b28ac84 | 288 | timer.reset(); |
megrootens | 0:3855d4588f76 | 289 | pc.printf("\r> Stopped."); |
megrootens | 0:3855d4588f76 | 290 | } else { |
megrootens | 0:3855d4588f76 | 291 | pc.printf("\t> No data was logged."); |
megrootens | 0:3855d4588f76 | 292 | } |
megrootens | 0:3855d4588f76 | 293 | |
megrootens | 0:3855d4588f76 | 294 | is_logging = false; |
megrootens | 0:3855d4588f76 | 295 | } |
megrootens | 0:3855d4588f76 | 296 | /** |
megrootens | 0:3855d4588f76 | 297 | * Log data |
megrootens | 0:3855d4588f76 | 298 | */ |
megrootens | 0:3855d4588f76 | 299 | void LogData() |
megrootens | 0:3855d4588f76 | 300 | { |
cnckiwi31 | 4:1cdce6c6c94e | 301 | int currTime = timer.read_ms(); |
cnckiwi31 | 4:1cdce6c6c94e | 302 | if(startedLogging) { |
cnckiwi31 | 4:1cdce6c6c94e | 303 | firstReadMS = currTime; |
cnckiwi31 | 4:1cdce6c6c94e | 304 | startedLogging = false; |
cnckiwi31 | 4:1cdce6c6c94e | 305 | } |
cnckiwi31 | 4:1cdce6c6c94e | 306 | double currTimeS = ((double)currTime - firstReadMS)/1000; |
cnckiwi31 | 4:1cdce6c6c94e | 307 | |
megrootens | 0:3855d4588f76 | 308 | // time |
cnckiwi31 | 4:1cdce6c6c94e | 309 | fprintf(fp_data,"\n%+f",currTimeS); |
megrootens | 0:3855d4588f76 | 310 | |
megrootens | 0:3855d4588f76 | 311 | // bench: joint angles and force sensor |
cnckiwi31 | 4:1cdce6c6c94e | 312 | fprintf(fp_data,", %+f, %+f, %+f, %+f, %+f, %+f, %+f, %+f", |
megrootens | 0:3855d4588f76 | 313 | leg.getDegrees(0), |
megrootens | 0:3855d4588f76 | 314 | leg.getDegrees(1), |
megrootens | 0:3855d4588f76 | 315 | leg.getDegrees(2), |
megrootens | 0:3855d4588f76 | 316 | leg.getDegrees(3), |
Technical_Muffin | 2:84d479fe9b5e | 317 | leg.getForce(), |
cnckiwi31 | 4:1cdce6c6c94e | 318 | getTorque(leg.getDegrees(3),leg.getDegrees(2),leg.getForce()), |
cnckiwi31 | 4:1cdce6c6c94e | 319 | leg.getPressure0()*100, |
cnckiwi31 | 4:1cdce6c6c94e | 320 | leg.getPressure1()*100 |
megrootens | 0:3855d4588f76 | 321 | ); |
megrootens | 0:3855d4588f76 | 322 | } |