Allan Veale / Mbed 2 deprecated heros_leg_readout_torque_addition

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 K K

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?

UserRevisionLine numberNew 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 }