HeRoS: read out and log joint angles and force sensor data from the leg test bench. Now with additional features to read pressure sensors and set the null values of the pressure and force sensors
Dependencies: SPTE_10Bar_5V mbed AS5048 SDFileSystem MODSERIAL PinDetect Valve LCM101
Fork of heros_leg_readout_torque_addition by
main.cpp@1:417a5b28ac84, 2017-12-06 (annotated)
- Committer:
- megrootens
- Date:
- Wed Dec 06 11:22:54 2017 +0000
- Revision:
- 1:417a5b28ac84
- Parent:
- 0:3855d4588f76
- Child:
- 2:84d479fe9b5e
starting, stopping and resetting timer for data logging t column
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 | |
megrootens | 0:3855d4588f76 | 9 | |
megrootens | 0:3855d4588f76 | 10 | // User io |
megrootens | 0:3855d4588f76 | 11 | PinDetect sw2(SW2,PullUp); |
megrootens | 0:3855d4588f76 | 12 | PinDetect sw3(SW3,PullUp); |
megrootens | 0:3855d4588f76 | 13 | DigitalOut led_g(LED_GREEN); |
megrootens | 0:3855d4588f76 | 14 | |
megrootens | 0:3855d4588f76 | 15 | void TogglePrinting(); |
megrootens | 0:3855d4588f76 | 16 | void ToggleLogging(); |
megrootens | 0:3855d4588f76 | 17 | void ShowAlive(); |
megrootens | 0:3855d4588f76 | 18 | |
megrootens | 0:3855d4588f76 | 19 | // Bench |
megrootens | 0:3855d4588f76 | 20 | Bench leg(AS5048_MOSI, AS5048_MISO, AS5048_SCLK, AS5048_CS, LCM101); |
megrootens | 0:3855d4588f76 | 21 | void Update() |
megrootens | 0:3855d4588f76 | 22 | { |
megrootens | 0:3855d4588f76 | 23 | leg.Update(); |
megrootens | 0:3855d4588f76 | 24 | } |
megrootens | 0:3855d4588f76 | 25 | |
megrootens | 0:3855d4588f76 | 26 | // SD Card |
megrootens | 0:3855d4588f76 | 27 | SDFileSystem sd(SD_MOSI, SD_MISO, SD_SCK, SD_CS, "sd"); |
megrootens | 0:3855d4588f76 | 28 | |
megrootens | 0:3855d4588f76 | 29 | void InitSdCard(); |
megrootens | 0:3855d4588f76 | 30 | void StartLogging(const char * fname_append = "data"); |
megrootens | 0:3855d4588f76 | 31 | void StopLogging(); |
megrootens | 0:3855d4588f76 | 32 | void LogData(); |
megrootens | 0:3855d4588f76 | 33 | |
megrootens | 0:3855d4588f76 | 34 | // Serial |
megrootens | 0:3855d4588f76 | 35 | MODSERIAL pc(USBTX,USBRX); |
megrootens | 0:3855d4588f76 | 36 | |
megrootens | 0:3855d4588f76 | 37 | void PrintStatus(); |
megrootens | 0:3855d4588f76 | 38 | void PrintMenu(); |
megrootens | 0:3855d4588f76 | 39 | |
megrootens | 0:3855d4588f76 | 40 | // Timing |
megrootens | 0:3855d4588f76 | 41 | Ticker tick_update, tick_serial, tick_logging; |
megrootens | 0:3855d4588f76 | 42 | Timer timer; |
megrootens | 0:3855d4588f76 | 43 | |
megrootens | 0:3855d4588f76 | 44 | /** |
megrootens | 0:3855d4588f76 | 45 | * Main loop/ |
megrootens | 0:3855d4588f76 | 46 | */ |
megrootens | 0:3855d4588f76 | 47 | int main() |
megrootens | 0:3855d4588f76 | 48 | { |
megrootens | 0:3855d4588f76 | 49 | pc.baud(timing::kSerialBaudrate); |
megrootens | 0:3855d4588f76 | 50 | pc.printf("**Hello!**\r\n"); |
megrootens | 0:3855d4588f76 | 51 | |
megrootens | 0:3855d4588f76 | 52 | InitSdCard(); |
megrootens | 0:3855d4588f76 | 53 | |
megrootens | 0:3855d4588f76 | 54 | tick_update.attach_us(&Update,timing::kTimeControlUs); |
megrootens | 0:3855d4588f76 | 55 | tick_serial.attach_us(&PrintStatus,timing::kTimeSerialPrintUs); |
megrootens | 0:3855d4588f76 | 56 | |
megrootens | 0:3855d4588f76 | 57 | PrintMenu(); |
megrootens | 0:3855d4588f76 | 58 | |
megrootens | 0:3855d4588f76 | 59 | sw2.attach_asserted(&TogglePrinting); |
megrootens | 0:3855d4588f76 | 60 | sw3.attach_asserted(&ToggleLogging); |
megrootens | 0:3855d4588f76 | 61 | |
megrootens | 0:3855d4588f76 | 62 | sw2.setSampleFrequency(); |
megrootens | 0:3855d4588f76 | 63 | sw3.setSampleFrequency(); |
megrootens | 0:3855d4588f76 | 64 | |
megrootens | 0:3855d4588f76 | 65 | while (true); |
megrootens | 0:3855d4588f76 | 66 | } |
megrootens | 0:3855d4588f76 | 67 | |
megrootens | 0:3855d4588f76 | 68 | |
megrootens | 0:3855d4588f76 | 69 | // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = |
megrootens | 0:3855d4588f76 | 70 | // IMPLEMENTATION USER IO |
megrootens | 0:3855d4588f76 | 71 | // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = |
megrootens | 0:3855d4588f76 | 72 | |
megrootens | 0:3855d4588f76 | 73 | bool is_printing = false; |
megrootens | 0:3855d4588f76 | 74 | void TogglePrinting() |
megrootens | 0:3855d4588f76 | 75 | { |
megrootens | 0:3855d4588f76 | 76 | if (not is_printing) { |
megrootens | 0:3855d4588f76 | 77 | is_printing = true; |
megrootens | 0:3855d4588f76 | 78 | } else { |
megrootens | 0:3855d4588f76 | 79 | is_printing = false; |
megrootens | 0:3855d4588f76 | 80 | PrintMenu(); |
megrootens | 0:3855d4588f76 | 81 | } |
megrootens | 0:3855d4588f76 | 82 | } |
megrootens | 0:3855d4588f76 | 83 | |
megrootens | 0:3855d4588f76 | 84 | bool is_logging = false; |
megrootens | 0:3855d4588f76 | 85 | void ToggleLogging() |
megrootens | 0:3855d4588f76 | 86 | { |
megrootens | 0:3855d4588f76 | 87 | if (not is_logging) { |
megrootens | 0:3855d4588f76 | 88 | StartLogging(); |
megrootens | 0:3855d4588f76 | 89 | } else { |
megrootens | 0:3855d4588f76 | 90 | is_logging = false; |
megrootens | 0:3855d4588f76 | 91 | StopLogging(); |
megrootens | 0:3855d4588f76 | 92 | } |
megrootens | 0:3855d4588f76 | 93 | PrintMenu(); |
megrootens | 0:3855d4588f76 | 94 | } |
megrootens | 0:3855d4588f76 | 95 | |
megrootens | 0:3855d4588f76 | 96 | // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = |
megrootens | 0:3855d4588f76 | 97 | // IMPLEMENTATION SERIAL COM |
megrootens | 0:3855d4588f76 | 98 | // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = |
megrootens | 0:3855d4588f76 | 99 | const int kNumJoints = 4; |
megrootens | 0:3855d4588f76 | 100 | const char *kJointNames[kNumJoints] = {"Toes","Ankle","knee","Hip"}; |
megrootens | 0:3855d4588f76 | 101 | |
megrootens | 0:3855d4588f76 | 102 | void PrintStatus() |
megrootens | 0:3855d4588f76 | 103 | { |
megrootens | 0:3855d4588f76 | 104 | led_g = !led_g; |
megrootens | 0:3855d4588f76 | 105 | if (is_printing) { |
megrootens | 0:3855d4588f76 | 106 | pc.printf("\r\nLEG STATUS (%s)\r\n",led_g?"+":"*"); |
megrootens | 0:3855d4588f76 | 107 | for (int i=0; i<kNumJoints; ++i) |
megrootens | 0:3855d4588f76 | 108 | pc.printf("\t%5s %7.2f\r\n",kJointNames[i], leg.getDegrees(i)); |
megrootens | 0:3855d4588f76 | 109 | pc.printf("\t%5s %7.2f\r\n","Force", leg.getForce()); |
megrootens | 0:3855d4588f76 | 110 | } |
megrootens | 0:3855d4588f76 | 111 | } |
megrootens | 0:3855d4588f76 | 112 | |
megrootens | 0:3855d4588f76 | 113 | void PrintMenu() |
megrootens | 0:3855d4588f76 | 114 | { |
megrootens | 0:3855d4588f76 | 115 | pc.printf("\r\nMENU\r\n"); |
megrootens | 0:3855d4588f76 | 116 | pc.printf("\t> Press SW2 to toggle printing leg status\r\n"); |
megrootens | 0:3855d4588f76 | 117 | pc.printf("\t> Press SW3 to toggle data logging\r\n"); |
megrootens | 0:3855d4588f76 | 118 | } |
megrootens | 0:3855d4588f76 | 119 | |
megrootens | 0:3855d4588f76 | 120 | // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = |
megrootens | 0:3855d4588f76 | 121 | // IMPLEMENTATION DATA LOGGING |
megrootens | 0:3855d4588f76 | 122 | // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = |
megrootens | 0:3855d4588f76 | 123 | |
megrootens | 0:3855d4588f76 | 124 | FILE * fp_data; |
megrootens | 0:3855d4588f76 | 125 | |
megrootens | 0:3855d4588f76 | 126 | bool sd_card_present = false; |
megrootens | 0:3855d4588f76 | 127 | int fname_prepend = 0; |
megrootens | 0:3855d4588f76 | 128 | |
megrootens | 0:3855d4588f76 | 129 | /** |
megrootens | 0:3855d4588f76 | 130 | * Check contents of SD card and count files in order |
megrootens | 0:3855d4588f76 | 131 | * to ensure unique file name for logging data |
megrootens | 0:3855d4588f76 | 132 | */ |
megrootens | 0:3855d4588f76 | 133 | void InitSdCard() |
megrootens | 0:3855d4588f76 | 134 | { |
megrootens | 0:3855d4588f76 | 135 | pc.printf("INITIALIZING SD CARD\r\n"); |
megrootens | 0:3855d4588f76 | 136 | |
megrootens | 0:3855d4588f76 | 137 | int num_files = 0; |
megrootens | 0:3855d4588f76 | 138 | |
megrootens | 0:3855d4588f76 | 139 | // scan dir |
megrootens | 0:3855d4588f76 | 140 | DIR *d; |
megrootens | 0:3855d4588f76 | 141 | struct dirent *p; |
megrootens | 0:3855d4588f76 | 142 | |
megrootens | 0:3855d4588f76 | 143 | d = opendir("/sd"); |
megrootens | 0:3855d4588f76 | 144 | if (d != NULL) { |
megrootens | 0:3855d4588f76 | 145 | sd_card_present = true; |
megrootens | 0:3855d4588f76 | 146 | |
megrootens | 0:3855d4588f76 | 147 | pc.printf("\t> Contents of SD Card:"); |
megrootens | 0:3855d4588f76 | 148 | while ((p = readdir(d)) != NULL) { |
megrootens | 0:3855d4588f76 | 149 | if (p->d_name[0] != '.') { |
megrootens | 0:3855d4588f76 | 150 | // skip files starting with '.' |
megrootens | 0:3855d4588f76 | 151 | pc.printf("\t %s",p->d_name); |
megrootens | 0:3855d4588f76 | 152 | ++num_files; |
megrootens | 0:3855d4588f76 | 153 | } |
megrootens | 0:3855d4588f76 | 154 | } |
megrootens | 0:3855d4588f76 | 155 | pc.printf("\t> Counted %d visible files.\r\n",num_files); |
megrootens | 0:3855d4588f76 | 156 | |
megrootens | 0:3855d4588f76 | 157 | closedir(d); |
megrootens | 0:3855d4588f76 | 158 | } else { |
megrootens | 0:3855d4588f76 | 159 | sd_card_present = false; |
megrootens | 0:3855d4588f76 | 160 | |
megrootens | 0:3855d4588f76 | 161 | pc.printf("\t> No SD Card present. Data cannot be logged.\r\n"); |
megrootens | 0:3855d4588f76 | 162 | } |
megrootens | 0:3855d4588f76 | 163 | |
megrootens | 0:3855d4588f76 | 164 | // id to be appended to logged data files |
megrootens | 0:3855d4588f76 | 165 | fname_prepend = num_files; |
megrootens | 0:3855d4588f76 | 166 | } |
megrootens | 0:3855d4588f76 | 167 | |
megrootens | 0:3855d4588f76 | 168 | /** |
megrootens | 0:3855d4588f76 | 169 | * Start logging data |
megrootens | 0:3855d4588f76 | 170 | */ |
megrootens | 0:3855d4588f76 | 171 | void StartLogging(const char * fname_append) |
megrootens | 0:3855d4588f76 | 172 | { |
megrootens | 0:3855d4588f76 | 173 | |
megrootens | 0:3855d4588f76 | 174 | pc.printf("DATA LOGGING"); |
megrootens | 0:3855d4588f76 | 175 | if (sd_card_present) { |
megrootens | 0:3855d4588f76 | 176 | |
megrootens | 0:3855d4588f76 | 177 | // create unique file name |
megrootens | 0:3855d4588f76 | 178 | ++fname_prepend; |
megrootens | 0:3855d4588f76 | 179 | char fname[50]; |
megrootens | 0:3855d4588f76 | 180 | sprintf(fname, "/sd/%d_%s.csv",fname_prepend,fname_append); |
megrootens | 0:3855d4588f76 | 181 | |
megrootens | 0:3855d4588f76 | 182 | pc.printf("\t> Opening data log file '%s'...\r\n",fname); |
megrootens | 0:3855d4588f76 | 183 | |
megrootens | 0:3855d4588f76 | 184 | // open file for writing and start logging after success |
megrootens | 0:3855d4588f76 | 185 | fp_data = fopen(fname,"w"); |
megrootens | 0:3855d4588f76 | 186 | if (fp_data==NULL) { |
megrootens | 0:3855d4588f76 | 187 | pc.printf("\t> ERROR: failed to open log file (t=%d ms)\r\n", |
megrootens | 0:3855d4588f76 | 188 | timer.read_ms()); |
megrootens | 0:3855d4588f76 | 189 | } else { |
megrootens | 0:3855d4588f76 | 190 | fprintf(fp_data, "time_ms, theta_toe, theta_ankle, theta_knee, theta_hip, force"); |
megrootens | 0:3855d4588f76 | 191 | tick_logging.attach_us(&LogData,timing::kTimeLogDataUs); |
megrootens | 1:417a5b28ac84 | 192 | timer.start(); |
megrootens | 0:3855d4588f76 | 193 | pc.printf("\t> Logging started.\r\n"); |
megrootens | 0:3855d4588f76 | 194 | |
megrootens | 0:3855d4588f76 | 195 | is_logging = true; |
megrootens | 0:3855d4588f76 | 196 | } |
megrootens | 0:3855d4588f76 | 197 | |
megrootens | 0:3855d4588f76 | 198 | } else { |
megrootens | 0:3855d4588f76 | 199 | pc.printf("\t> No SD Card; no data will be logged.\r\n"); |
megrootens | 0:3855d4588f76 | 200 | } |
megrootens | 0:3855d4588f76 | 201 | } |
megrootens | 0:3855d4588f76 | 202 | |
megrootens | 0:3855d4588f76 | 203 | |
megrootens | 0:3855d4588f76 | 204 | /** |
megrootens | 0:3855d4588f76 | 205 | * Stop logging data |
megrootens | 0:3855d4588f76 | 206 | */ |
megrootens | 0:3855d4588f76 | 207 | void StopLogging() |
megrootens | 0:3855d4588f76 | 208 | { |
megrootens | 0:3855d4588f76 | 209 | pc.printf("DATA LOGGING:"); |
megrootens | 0:3855d4588f76 | 210 | if (sd_card_present) { |
megrootens | 0:3855d4588f76 | 211 | // close data file, stop logging |
megrootens | 0:3855d4588f76 | 212 | fclose(fp_data); |
megrootens | 0:3855d4588f76 | 213 | tick_logging.detach(); |
megrootens | 1:417a5b28ac84 | 214 | timer.stop(); |
megrootens | 1:417a5b28ac84 | 215 | timer.reset(); |
megrootens | 0:3855d4588f76 | 216 | pc.printf("\r> Stopped."); |
megrootens | 0:3855d4588f76 | 217 | } else { |
megrootens | 0:3855d4588f76 | 218 | pc.printf("\t> No data was logged."); |
megrootens | 0:3855d4588f76 | 219 | } |
megrootens | 0:3855d4588f76 | 220 | |
megrootens | 0:3855d4588f76 | 221 | is_logging = false; |
megrootens | 0:3855d4588f76 | 222 | } |
megrootens | 0:3855d4588f76 | 223 | |
megrootens | 0:3855d4588f76 | 224 | /** |
megrootens | 0:3855d4588f76 | 225 | * Log data |
megrootens | 0:3855d4588f76 | 226 | */ |
megrootens | 0:3855d4588f76 | 227 | void LogData() |
megrootens | 0:3855d4588f76 | 228 | { |
megrootens | 0:3855d4588f76 | 229 | // time |
megrootens | 0:3855d4588f76 | 230 | fprintf(fp_data,"\n%d", timer.read_ms()); |
megrootens | 0:3855d4588f76 | 231 | |
megrootens | 0:3855d4588f76 | 232 | // bench: joint angles and force sensor |
megrootens | 0:3855d4588f76 | 233 | fprintf(fp_data,", %+f, %+f, %+f, %+f, %+f", |
megrootens | 0:3855d4588f76 | 234 | leg.getDegrees(0), |
megrootens | 0:3855d4588f76 | 235 | leg.getDegrees(1), |
megrootens | 0:3855d4588f76 | 236 | leg.getDegrees(2), |
megrootens | 0:3855d4588f76 | 237 | leg.getDegrees(3), |
megrootens | 0:3855d4588f76 | 238 | leg.getForce() |
megrootens | 0:3855d4588f76 | 239 | ); |
megrootens | 0:3855d4588f76 | 240 | } |