K K / Mbed 2 deprecated heros_leg_readout_torque_addition

Torque calculation added to leg readout

Dependencies:   AS5048 LCM101 MODSERIAL PinDetect SDFileSystem mbed

Fork of heros_leg_readout by Martijn Grootens

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 }