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@0:3855d4588f76, 2017-12-01 (annotated)
- Committer:
- megrootens
- Date:
- Fri Dec 01 11:14:38 2017 +0000
- Revision:
- 0:3855d4588f76
- Child:
- 1:417a5b28ac84
serial printing and SD card logging of joint angles and force sensor
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 | 0:3855d4588f76 | 192 | pc.printf("\t> Logging started.\r\n"); |
megrootens | 0:3855d4588f76 | 193 | |
megrootens | 0:3855d4588f76 | 194 | is_logging = true; |
megrootens | 0:3855d4588f76 | 195 | } |
megrootens | 0:3855d4588f76 | 196 | |
megrootens | 0:3855d4588f76 | 197 | } else { |
megrootens | 0:3855d4588f76 | 198 | pc.printf("\t> No SD Card; no data will be logged.\r\n"); |
megrootens | 0:3855d4588f76 | 199 | } |
megrootens | 0:3855d4588f76 | 200 | } |
megrootens | 0:3855d4588f76 | 201 | |
megrootens | 0:3855d4588f76 | 202 | |
megrootens | 0:3855d4588f76 | 203 | /** |
megrootens | 0:3855d4588f76 | 204 | * Stop logging data |
megrootens | 0:3855d4588f76 | 205 | */ |
megrootens | 0:3855d4588f76 | 206 | void StopLogging() |
megrootens | 0:3855d4588f76 | 207 | { |
megrootens | 0:3855d4588f76 | 208 | pc.printf("DATA LOGGING:"); |
megrootens | 0:3855d4588f76 | 209 | if (sd_card_present) { |
megrootens | 0:3855d4588f76 | 210 | // close data file, stop logging |
megrootens | 0:3855d4588f76 | 211 | fclose(fp_data); |
megrootens | 0:3855d4588f76 | 212 | tick_logging.detach(); |
megrootens | 0:3855d4588f76 | 213 | pc.printf("\r> Stopped."); |
megrootens | 0:3855d4588f76 | 214 | } else { |
megrootens | 0:3855d4588f76 | 215 | pc.printf("\t> No data was logged."); |
megrootens | 0:3855d4588f76 | 216 | } |
megrootens | 0:3855d4588f76 | 217 | |
megrootens | 0:3855d4588f76 | 218 | is_logging = false; |
megrootens | 0:3855d4588f76 | 219 | } |
megrootens | 0:3855d4588f76 | 220 | |
megrootens | 0:3855d4588f76 | 221 | /** |
megrootens | 0:3855d4588f76 | 222 | * Log data |
megrootens | 0:3855d4588f76 | 223 | */ |
megrootens | 0:3855d4588f76 | 224 | void LogData() |
megrootens | 0:3855d4588f76 | 225 | { |
megrootens | 0:3855d4588f76 | 226 | // time |
megrootens | 0:3855d4588f76 | 227 | fprintf(fp_data,"\n%d", timer.read_ms()); |
megrootens | 0:3855d4588f76 | 228 | |
megrootens | 0:3855d4588f76 | 229 | // bench: joint angles and force sensor |
megrootens | 0:3855d4588f76 | 230 | fprintf(fp_data,", %+f, %+f, %+f, %+f, %+f", |
megrootens | 0:3855d4588f76 | 231 | leg.getDegrees(0), |
megrootens | 0:3855d4588f76 | 232 | leg.getDegrees(1), |
megrootens | 0:3855d4588f76 | 233 | leg.getDegrees(2), |
megrootens | 0:3855d4588f76 | 234 | leg.getDegrees(3), |
megrootens | 0:3855d4588f76 | 235 | leg.getForce() |
megrootens | 0:3855d4588f76 | 236 | ); |
megrootens | 0:3855d4588f76 | 237 | } |