Suraj Giri / Mbed 2 deprecated heros_leg_readout_torque_addition_V3

Version 3 is with update to the test rig with a linear actuator

Dependencies:   SPTE_10Bar_5V mbed AS5048 SDFileSystem MODSERIAL PinDetect LCM101 LinearActuator

bench.cpp@5:63063a9fa51c, 2019-12-09 (annotated)

Committer:
cnckiwi31
Date:
Mon Dec 09 10:51:46 2019 +0000
Revision:
5:63063a9fa51c
Parent:
4:1cdce6c6c94e
Child:
6:02507d7a6f51
Ready to run on test rig

Who changed what in which revision?

UserRevisionLine numberNew contents of line
megrootens 0:3855d4588f76 1 #include "bench.h"
megrootens 0:3855d4588f76 2
megrootens 0:3855d4588f76 3 /**
megrootens 0:3855d4588f76 4 * Create an object representing the testbench; the 4 AS5048 sensors
megrootens 0:3855d4588f76 5 * with the offsets specified by the constants kOffsetsDegrees[4]
megrootens 0:3855d4588f76 6 * @param mosi: mosi pin for sensor chain
megrootens 0:3855d4588f76 7 * @param miso: miso pin for sensor chain
megrootens 0:3855d4588f76 8 * @param sck: clock pin for sensor chain
megrootens 0:3855d4588f76 9 * @param cs: chip select pin for sensor chain
cnckiwi31 5:63063a9fa51c 10 * @param use5kNLoadCell: if 5kN load cell is used (if false, 1kN sensor is used)
cnckiwi31 4:1cdce6c6c94e 11 * @param p_lcm101: analog input pin for load cell
cnckiwi31 4:1cdce6c6c94e 12 * @param p_spte0: analog input pin for pressure sensor 0
cnckiwi31 4:1cdce6c6c94e 13 * @param p_spte1: analog input pin for pressure sensor 1
cnckiwi31 5:63063a9fa51c 14 * @param p_valve: digital output for valve (on/off) that inflates leg actuator
cnckiwi31 5:63063a9fa51c 15 * @param mosi: mosi pin for sd card
cnckiwi31 5:63063a9fa51c 16 * @param miso: miso pin for sd card
cnckiwi31 5:63063a9fa51c 17 * @param sck: clock pin for sd card
cnckiwi31 5:63063a9fa51c 18 * @param cs: chip select pin for sd card
cnckiwi31 5:63063a9fa51c 19 * @param tx: serial transmission line
cnckiwi31 5:63063a9fa51c 20 * @param rx: serial receive line
cnckiwi31 5:63063a9fa51c 21 * @param but0: first input button
cnckiwi31 5:63063a9fa51c 22 * @param but1: second input button
megrootens 0:3855d4588f76 23 */
megrootens 0:3855d4588f76 24 Bench::Bench(PinName mosi, PinName miso, PinName sck, PinName cs,
cnckiwi31 5:63063a9fa51c 25 bool use5kNLoadCell,PinName p_lcm101, PinName p_spte0, PinName p_spte1, PinName p_valve,
cnckiwi31 5:63063a9fa51c 26 PinName sd_mosi, PinName sd_miso, PinName sd_sck, PinName sd_cs,
cnckiwi31 5:63063a9fa51c 27 PinName tx, PinName rx,
cnckiwi31 5:63063a9fa51c 28 PinName but0, PinName but1) :
cnckiwi31 5:63063a9fa51c 29 pc(tx,rx),
cnckiwi31 5:63063a9fa51c 30 lowerBut(but0,PullUp),
cnckiwi31 5:63063a9fa51c 31 upperBut(but1,PullUp),
megrootens 0:3855d4588f76 32 as5048_(mosi, miso, sck, cs, sensors::kNumJoints),
cnckiwi31 5:63063a9fa51c 33 loadCell5kN(p_lcm101, sensors::kGen5kNOffset, sensors::kGen5kNFactor),
cnckiwi31 5:63063a9fa51c 34 loadCell1kN(p_lcm101, sensors::kLcm101Offset, sensors::kLcm101Factor),
cnckiwi31 5:63063a9fa51c 35 use5kN(use5kNLoadCell),
cnckiwi31 4:1cdce6c6c94e 36 spte0(p_spte0, sensors::kSPTE0Offset, sensors::kSPTE0Factor),
cnckiwi31 5:63063a9fa51c 37 spte1(p_spte1, sensors::kSPTE1Offset, sensors::kSPTE1Factor),
cnckiwi31 5:63063a9fa51c 38 valveFesto(p_valve),
cnckiwi31 5:63063a9fa51c 39 sd(sd_mosi, sd_miso, sd_sck, sd_cs, "sd")
cnckiwi31 5:63063a9fa51c 40 {
cnckiwi31 5:63063a9fa51c 41 //init serial things
cnckiwi31 5:63063a9fa51c 42 pc.baud(timing::kSerialBaudrate);//set the serial rate
cnckiwi31 5:63063a9fa51c 43
cnckiwi31 5:63063a9fa51c 44 sd_card_present = false;
cnckiwi31 5:63063a9fa51c 45 fname_prepend = 0;
cnckiwi31 5:63063a9fa51c 46 is_logging = false;
cnckiwi31 5:63063a9fa51c 47 is_printing = false;
cnckiwi31 5:63063a9fa51c 48 was_printing = false;
cnckiwi31 5:63063a9fa51c 49
cnckiwi31 5:63063a9fa51c 50 usedExtraCols = 0;
cnckiwi31 5:63063a9fa51c 51
cnckiwi31 5:63063a9fa51c 52 firstReadMS = 0; //first timer value read
cnckiwi31 5:63063a9fa51c 53 startedLogging = false; //in the middle of a logging cycle
cnckiwi31 5:63063a9fa51c 54
cnckiwi31 5:63063a9fa51c 55 //set data logging frequency
cnckiwi31 5:63063a9fa51c 56 loggingUS = timing::kTimeLogDataUs;
cnckiwi31 5:63063a9fa51c 57 loggingHz = (int)(1000000/timing::kTimeLogDataUs);
cnckiwi31 5:63063a9fa51c 58
megrootens 0:3855d4588f76 59 for (int i=0; i<sensors::kNumJoints; ++i) {
megrootens 0:3855d4588f76 60 as5048_.setOffsetDegrees(i,sensors::kOffsetsDegrees[i]);
megrootens 0:3855d4588f76 61 as5048_.setDirection(i,sensors::kDirections[i]);
cnckiwi31 5:63063a9fa51c 62 }
cnckiwi31 5:63063a9fa51c 63 }
cnckiwi31 5:63063a9fa51c 64
cnckiwi31 5:63063a9fa51c 65 /**
cnckiwi31 5:63063a9fa51c 66 * Initialises: timers, SD card
cnckiwi31 5:63063a9fa51c 67 */
cnckiwi31 5:63063a9fa51c 68 void Bench::initialise()
cnckiwi31 5:63063a9fa51c 69 {
cnckiwi31 5:63063a9fa51c 70 //setup the timing
cnckiwi31 5:63063a9fa51c 71 tick_update.attach_us(this,&Bench::update,timing::kTimeControlUs);
cnckiwi31 5:63063a9fa51c 72
cnckiwi31 5:63063a9fa51c 73
cnckiwi31 5:63063a9fa51c 74 // set rate at which data is printed
cnckiwi31 5:63063a9fa51c 75 tick_serial.attach_us(this,&Bench::PrintStatus,timing::kTimeSerialPrintUs);
cnckiwi31 5:63063a9fa51c 76
cnckiwi31 5:63063a9fa51c 77 //setup the buttons with debouncing
cnckiwi31 5:63063a9fa51c 78 lowerBut.attach_asserted(this,&Bench::TogglePrinting);
cnckiwi31 5:63063a9fa51c 79 upperBut.attach_asserted(this,&Bench::ToggleLogging);
cnckiwi31 5:63063a9fa51c 80
cnckiwi31 5:63063a9fa51c 81 lowerBut.setSampleFrequency();
cnckiwi31 5:63063a9fa51c 82 upperBut.setSampleFrequency();
cnckiwi31 5:63063a9fa51c 83
cnckiwi31 5:63063a9fa51c 84 //display welcome message
cnckiwi31 5:63063a9fa51c 85 pc.printf("\r\n**Hello!**\r\n");
cnckiwi31 5:63063a9fa51c 86 pc.printf("Version: 3/12/2019 - 00:00\r\n\n");
cnckiwi31 5:63063a9fa51c 87
cnckiwi31 5:63063a9fa51c 88 Bench::pc.printf("5kN load cell? %s\r\n\n",sensors::use5kN?"Yes":"No");
cnckiwi31 5:63063a9fa51c 89
cnckiwi31 5:63063a9fa51c 90 pc.printf("Bench update rate (Hz): %i\r\n",timing::TimeControlHertz);
cnckiwi31 5:63063a9fa51c 91 pc.printf("Logging rate (Hz): %i\r\n\n",(int)loggingHz);
cnckiwi31 5:63063a9fa51c 92
cnckiwi31 5:63063a9fa51c 93 //startup the SD card
cnckiwi31 5:63063a9fa51c 94 InitSdCard();
cnckiwi31 5:63063a9fa51c 95
cnckiwi31 5:63063a9fa51c 96 //Display the menu
cnckiwi31 5:63063a9fa51c 97 PrintMenu();
cnckiwi31 5:63063a9fa51c 98 }
cnckiwi31 5:63063a9fa51c 99
cnckiwi31 5:63063a9fa51c 100 /**
cnckiwi31 5:63063a9fa51c 101 * Sets the rate at which data is logged in an experiment
cnckiwi31 5:63063a9fa51c 102 * @param hertz: logging frequency in Hz
cnckiwi31 5:63063a9fa51c 103 */
cnckiwi31 5:63063a9fa51c 104 void Bench::setLoggingFrequency(float hertz)
cnckiwi31 5:63063a9fa51c 105 {
cnckiwi31 5:63063a9fa51c 106 //set data logging frequency
cnckiwi31 5:63063a9fa51c 107 if (hertz > 0) {
cnckiwi31 5:63063a9fa51c 108 loggingUS = 1000000/hertz;
cnckiwi31 5:63063a9fa51c 109 loggingHz = hertz;
cnckiwi31 5:63063a9fa51c 110 } else {
cnckiwi31 5:63063a9fa51c 111 loggingUS = timing::kTimeLogDataUs;
cnckiwi31 5:63063a9fa51c 112 loggingHz = (int)(1000000/timing::kTimeLogDataUs);
megrootens 0:3855d4588f76 113 }
megrootens 0:3855d4588f76 114 }
megrootens 0:3855d4588f76 115
megrootens 0:3855d4588f76 116 /**
cnckiwi31 5:63063a9fa51c 117 * Sets names of extra columns of data to be recorded
cnckiwi31 5:63063a9fa51c 118 * @param extraColumnNames: string array of the column names
cnckiwi31 5:63063a9fa51c 119 * @param numCols: number of elements in the string array
cnckiwi31 5:63063a9fa51c 120 */
cnckiwi31 5:63063a9fa51c 121 void Bench::setExtraColumns(string extraColumnNames[], int numCols)
cnckiwi31 5:63063a9fa51c 122 {
cnckiwi31 5:63063a9fa51c 123 usedExtraCols = numCols;
cnckiwi31 5:63063a9fa51c 124 //pc.printf("Length: %i\r\n",usedExtraCols);
cnckiwi31 5:63063a9fa51c 125 // save the names
cnckiwi31 5:63063a9fa51c 126 for(int i=0; i<maxCols; i++) {
cnckiwi31 5:63063a9fa51c 127 if (i<numCols) {
cnckiwi31 5:63063a9fa51c 128 extraColNames[i] = extraColumnNames[i];
cnckiwi31 5:63063a9fa51c 129 //pc.printf("\r\nS%d: %s\r\n",i,extraColNames[i]);
cnckiwi31 5:63063a9fa51c 130 } else {
cnckiwi31 5:63063a9fa51c 131 extraColNames[i] = ""; //less columns than the max possible are filled with a null space
cnckiwi31 5:63063a9fa51c 132 }
cnckiwi31 5:63063a9fa51c 133 }
cnckiwi31 5:63063a9fa51c 134
cnckiwi31 5:63063a9fa51c 135 }
cnckiwi31 5:63063a9fa51c 136
cnckiwi31 5:63063a9fa51c 137 /**
cnckiwi31 5:63063a9fa51c 138 * Sets values of extra columns of data to be recorded
cnckiwi31 5:63063a9fa51c 139 * @param data: data to be logged with the extra columns
cnckiwi31 5:63063a9fa51c 140 */
cnckiwi31 5:63063a9fa51c 141 void Bench::setExtraData(float data[])
cnckiwi31 5:63063a9fa51c 142 {
cnckiwi31 5:63063a9fa51c 143 for (int i=0; i<usedExtraCols; i++) {
cnckiwi31 5:63063a9fa51c 144 extraColValues[i] = data[i];
cnckiwi31 5:63063a9fa51c 145 //pc.printf("\r\nD%d: %f\r\n",i,extraColValues[i]);
cnckiwi31 5:63063a9fa51c 146 }
cnckiwi31 5:63063a9fa51c 147
cnckiwi31 5:63063a9fa51c 148 }
cnckiwi31 5:63063a9fa51c 149
cnckiwi31 5:63063a9fa51c 150 /**
megrootens 0:3855d4588f76 151 * Update routine for the testbench.
megrootens 0:3855d4588f76 152 * - Updates the angle buffer of the sensor array
megrootens 0:3855d4588f76 153 * - ... that's it for now (add filtering?)
megrootens 0:3855d4588f76 154 * Note that angles lag one Update() behind, due to the way the SPI
megrootens 0:3855d4588f76 155 * protocol works.
megrootens 0:3855d4588f76 156 */
cnckiwi31 5:63063a9fa51c 157 void Bench::update()
megrootens 0:3855d4588f76 158 {
megrootens 0:3855d4588f76 159 as5048_.UpdateAngleBuffer();
megrootens 0:3855d4588f76 160 }
megrootens 0:3855d4588f76 161
megrootens 0:3855d4588f76 162 float Bench::getDegrees(int i_joint)
megrootens 0:3855d4588f76 163 {
megrootens 0:3855d4588f76 164 float ang = as5048_.getAngleDegrees(i_joint);
megrootens 0:3855d4588f76 165 if (ang>kCutOffDegrees) {
megrootens 0:3855d4588f76 166 return ang-As5048::kDegPerRev;
megrootens 0:3855d4588f76 167 }
megrootens 0:3855d4588f76 168 return ang;
megrootens 0:3855d4588f76 169 }
megrootens 0:3855d4588f76 170
cnckiwi31 5:63063a9fa51c 171 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
cnckiwi31 5:63063a9fa51c 172 // IMPLEMENTATION DATA LOGGING
cnckiwi31 5:63063a9fa51c 173 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
cnckiwi31 5:63063a9fa51c 174
cnckiwi31 5:63063a9fa51c 175 /**
cnckiwi31 5:63063a9fa51c 176 * Check contents of SD card and count files in order
cnckiwi31 5:63063a9fa51c 177 * to ensure unique file name for logging data
cnckiwi31 5:63063a9fa51c 178 */
cnckiwi31 5:63063a9fa51c 179 void Bench::InitSdCard()
cnckiwi31 5:63063a9fa51c 180 {
cnckiwi31 5:63063a9fa51c 181 pc.printf("\r\nINITIALISING SD CARD\r\n");
cnckiwi31 5:63063a9fa51c 182
cnckiwi31 5:63063a9fa51c 183 int num_files = 0;
cnckiwi31 5:63063a9fa51c 184
cnckiwi31 5:63063a9fa51c 185 // scan dir
cnckiwi31 5:63063a9fa51c 186 DIR *d;
cnckiwi31 5:63063a9fa51c 187 struct dirent *p;
cnckiwi31 5:63063a9fa51c 188
cnckiwi31 5:63063a9fa51c 189 d = opendir("/sd");
cnckiwi31 5:63063a9fa51c 190 if (d != NULL) {
cnckiwi31 5:63063a9fa51c 191 sd_card_present = true;
cnckiwi31 5:63063a9fa51c 192
cnckiwi31 5:63063a9fa51c 193 pc.printf("\t> Contents of SD Card:\r\n");
cnckiwi31 5:63063a9fa51c 194 while ((p = readdir(d)) != NULL) {
cnckiwi31 5:63063a9fa51c 195 if (p->d_name[0] != '.') {
cnckiwi31 5:63063a9fa51c 196 // skip files starting with '.'
cnckiwi31 5:63063a9fa51c 197 pc.printf("\t %s",p->d_name);
cnckiwi31 5:63063a9fa51c 198 ++num_files;
cnckiwi31 5:63063a9fa51c 199 }
cnckiwi31 5:63063a9fa51c 200 }
cnckiwi31 5:63063a9fa51c 201 pc.printf("\r\n\t> Counted %d visible files.\r\n",num_files);
cnckiwi31 5:63063a9fa51c 202
cnckiwi31 5:63063a9fa51c 203 closedir(d);
cnckiwi31 5:63063a9fa51c 204 } else {
cnckiwi31 5:63063a9fa51c 205 sd_card_present = false;
cnckiwi31 5:63063a9fa51c 206
cnckiwi31 5:63063a9fa51c 207 pc.printf("\t> No SD Card present. Data cannot be logged.\r\n");
cnckiwi31 5:63063a9fa51c 208 }
cnckiwi31 5:63063a9fa51c 209
cnckiwi31 5:63063a9fa51c 210 // id to be appended to logged data files
cnckiwi31 5:63063a9fa51c 211 fname_prepend = num_files;
cnckiwi31 5:63063a9fa51c 212 }
cnckiwi31 5:63063a9fa51c 213
cnckiwi31 5:63063a9fa51c 214 /**
cnckiwi31 5:63063a9fa51c 215 * Start logging data
cnckiwi31 5:63063a9fa51c 216 */
cnckiwi31 5:63063a9fa51c 217 void Bench::StartLogging(const char * fname_append)
cnckiwi31 5:63063a9fa51c 218 {
cnckiwi31 5:63063a9fa51c 219 pc.printf("\r\nDATA LOGGING");
cnckiwi31 5:63063a9fa51c 220 if (sd_card_present) {
cnckiwi31 5:63063a9fa51c 221
cnckiwi31 5:63063a9fa51c 222 // create unique file name
cnckiwi31 5:63063a9fa51c 223 ++fname_prepend;
cnckiwi31 5:63063a9fa51c 224 char fname[50];
cnckiwi31 5:63063a9fa51c 225 sprintf(fname, "/sd/%d_%s.csv",fname_prepend,fname_append);
cnckiwi31 5:63063a9fa51c 226
cnckiwi31 5:63063a9fa51c 227 pc.printf("\t> Opening data log file '%s'...\r\n",fname);
cnckiwi31 5:63063a9fa51c 228
cnckiwi31 5:63063a9fa51c 229 // open file for writing and start logging after success
cnckiwi31 5:63063a9fa51c 230 fp_data = fopen(fname,"w");
cnckiwi31 5:63063a9fa51c 231 if (fp_data==NULL) {
cnckiwi31 5:63063a9fa51c 232 pc.printf("\t> ERROR: failed to open log file (t=%d ms)\r\n",
cnckiwi31 5:63063a9fa51c 233 timer.read_ms());
cnckiwi31 5:63063a9fa51c 234 } else {
cnckiwi31 5:63063a9fa51c 235 string fHeader = "time (s),theta_knee (deg),force (N),pressure (kPa)";
cnckiwi31 5:63063a9fa51c 236
cnckiwi31 5:63063a9fa51c 237 for (int i=0; i<usedExtraCols; i++) {
cnckiwi31 5:63063a9fa51c 238 if (extraColNames[i] != "") {
cnckiwi31 5:63063a9fa51c 239 fHeader = fHeader + "," + extraColNames[i];
cnckiwi31 5:63063a9fa51c 240 }
cnckiwi31 5:63063a9fa51c 241 }
cnckiwi31 5:63063a9fa51c 242 //pc.printf("%s", fHeader.c_str());
cnckiwi31 5:63063a9fa51c 243 fprintf(fp_data, "%s", fHeader.c_str());
cnckiwi31 5:63063a9fa51c 244 tick_logging.attach_us(this,&Bench::LogData,loggingUS);
cnckiwi31 5:63063a9fa51c 245 timer.start();
cnckiwi31 5:63063a9fa51c 246 startedLogging = true;
cnckiwi31 5:63063a9fa51c 247
cnckiwi31 5:63063a9fa51c 248 pc.printf("\t> Logging started.\r\n");
cnckiwi31 5:63063a9fa51c 249
cnckiwi31 5:63063a9fa51c 250 is_logging = true;
cnckiwi31 5:63063a9fa51c 251 }
cnckiwi31 5:63063a9fa51c 252 } else {
cnckiwi31 5:63063a9fa51c 253 pc.printf("\t> No SD Card; no data will be logged.\r\n");
cnckiwi31 5:63063a9fa51c 254 startedLogging = false;
cnckiwi31 5:63063a9fa51c 255 }
cnckiwi31 5:63063a9fa51c 256 }
cnckiwi31 5:63063a9fa51c 257
cnckiwi31 5:63063a9fa51c 258
cnckiwi31 5:63063a9fa51c 259 /**
cnckiwi31 5:63063a9fa51c 260 * Stop logging data
cnckiwi31 5:63063a9fa51c 261 */
cnckiwi31 5:63063a9fa51c 262 void Bench::StopLogging()
cnckiwi31 5:63063a9fa51c 263 {
cnckiwi31 5:63063a9fa51c 264 Bench::pc.printf("\r\nDATA LOGGING:");
cnckiwi31 5:63063a9fa51c 265 if (sd_card_present) {
cnckiwi31 5:63063a9fa51c 266 // close data file, stop logging
cnckiwi31 5:63063a9fa51c 267 fclose(fp_data);
cnckiwi31 5:63063a9fa51c 268 tick_logging.detach();
cnckiwi31 5:63063a9fa51c 269 timer.stop();
cnckiwi31 5:63063a9fa51c 270 timer.reset();
cnckiwi31 5:63063a9fa51c 271 Bench::pc.printf("\r> Stopped.");
cnckiwi31 5:63063a9fa51c 272 } else {
cnckiwi31 5:63063a9fa51c 273 Bench::pc.printf("\t> No data was logged.");
cnckiwi31 5:63063a9fa51c 274 }
cnckiwi31 5:63063a9fa51c 275
cnckiwi31 5:63063a9fa51c 276 is_logging = false;
cnckiwi31 5:63063a9fa51c 277 }
cnckiwi31 5:63063a9fa51c 278
cnckiwi31 5:63063a9fa51c 279 /**
cnckiwi31 5:63063a9fa51c 280 * Stop logging data and print the menu
cnckiwi31 5:63063a9fa51c 281 */
cnckiwi31 5:63063a9fa51c 282 void Bench::stopLogging()
cnckiwi31 5:63063a9fa51c 283 {
cnckiwi31 5:63063a9fa51c 284 if(is_logging) {
cnckiwi31 5:63063a9fa51c 285 is_logging = false;
cnckiwi31 5:63063a9fa51c 286 StopLogging();
cnckiwi31 5:63063a9fa51c 287 PrintMenu();
cnckiwi31 5:63063a9fa51c 288 }
cnckiwi31 5:63063a9fa51c 289 }
cnckiwi31 5:63063a9fa51c 290
cnckiwi31 5:63063a9fa51c 291 /**
cnckiwi31 5:63063a9fa51c 292 * Log data
cnckiwi31 5:63063a9fa51c 293 */
cnckiwi31 5:63063a9fa51c 294 void Bench::LogData()
cnckiwi31 5:63063a9fa51c 295 {
cnckiwi31 5:63063a9fa51c 296 int currTime = timer.read_ms();
cnckiwi31 5:63063a9fa51c 297 if(startedLogging) {
cnckiwi31 5:63063a9fa51c 298 firstReadMS = currTime;
cnckiwi31 5:63063a9fa51c 299 startedLogging = false;
cnckiwi31 5:63063a9fa51c 300 }
cnckiwi31 5:63063a9fa51c 301 double currTimeS = ((double)currTime - firstReadMS)/1000;
cnckiwi31 5:63063a9fa51c 302
cnckiwi31 5:63063a9fa51c 303 // time
cnckiwi31 5:63063a9fa51c 304 fprintf(fp_data,"\n%f",currTimeS);
cnckiwi31 5:63063a9fa51c 305
cnckiwi31 5:63063a9fa51c 306 // bench: joint angles and force sensor and pressure sensor values
cnckiwi31 5:63063a9fa51c 307 fprintf(fp_data,",%f,%f,%f",
cnckiwi31 5:63063a9fa51c 308 getDegrees(0),
cnckiwi31 5:63063a9fa51c 309 getForce(),
cnckiwi31 5:63063a9fa51c 310 getPressure0()*100
cnckiwi31 5:63063a9fa51c 311 );
cnckiwi31 5:63063a9fa51c 312 for (int i=0; i<usedExtraCols; i++) {
cnckiwi31 5:63063a9fa51c 313 fprintf(fp_data,",%f",extraColValues[i]);
cnckiwi31 5:63063a9fa51c 314 }
cnckiwi31 5:63063a9fa51c 315 }
cnckiwi31 5:63063a9fa51c 316
cnckiwi31 5:63063a9fa51c 317 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
cnckiwi31 5:63063a9fa51c 318 // IMPLEMENTATION SERIAL COM
cnckiwi31 5:63063a9fa51c 319 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
cnckiwi31 5:63063a9fa51c 320 /**
cnckiwi31 5:63063a9fa51c 321 * Prints the values of the key measurements in the setup
cnckiwi31 5:63063a9fa51c 322 */
cnckiwi31 5:63063a9fa51c 323 void Bench::PrintStatus()
cnckiwi31 5:63063a9fa51c 324 {
cnckiwi31 5:63063a9fa51c 325 if (is_printing) {
cnckiwi31 5:63063a9fa51c 326 if(sd_card_present)
cnckiwi31 5:63063a9fa51c 327 {
cnckiwi31 5:63063a9fa51c 328 Bench::pc.printf("\tFile number %15i\r\n", fname_prepend);
cnckiwi31 5:63063a9fa51c 329 Bench::pc.printf("\tLogging? %18s\r\n",is_logging?"Yes":"No");
cnckiwi31 5:63063a9fa51c 330 }
cnckiwi31 5:63063a9fa51c 331
cnckiwi31 5:63063a9fa51c 332 for (int i=0; i<sensors::kNumJoints; ++i)
cnckiwi31 5:63063a9fa51c 333 {
cnckiwi31 5:63063a9fa51c 334 string jointName = sensors::kJointNames[i];
cnckiwi31 5:63063a9fa51c 335 jointName = jointName + " (deg)";
cnckiwi31 5:63063a9fa51c 336 Bench::pc.printf("\t%15s %7.2f\r\n",jointName, getDegrees(i));
cnckiwi31 5:63063a9fa51c 337 }
cnckiwi31 5:63063a9fa51c 338 Bench::pc.printf("\t%15s %7.2f\r\n","Force (N)", getForce());
cnckiwi31 5:63063a9fa51c 339
cnckiwi31 5:63063a9fa51c 340 Bench::pc.printf("\t%15s %7.2f\r\n","Pressure0 (kPa)", getPressure0()*100);
cnckiwi31 5:63063a9fa51c 341 }
cnckiwi31 5:63063a9fa51c 342 }
cnckiwi31 5:63063a9fa51c 343
cnckiwi31 5:63063a9fa51c 344 /**
cnckiwi31 5:63063a9fa51c 345 * Prints the user choices to be made
cnckiwi31 5:63063a9fa51c 346 */
cnckiwi31 5:63063a9fa51c 347 void Bench::PrintMenu()
cnckiwi31 5:63063a9fa51c 348 {
cnckiwi31 5:63063a9fa51c 349 Bench::pc.printf("\r\nMENU\r\n");
cnckiwi31 5:63063a9fa51c 350 Bench::pc.printf("\t> Press SW2 to toggle printing leg status\r\n");
cnckiwi31 5:63063a9fa51c 351 Bench::pc.printf("\t> Press SW3 to toggle data logging\r\n");
cnckiwi31 5:63063a9fa51c 352
cnckiwi31 5:63063a9fa51c 353 Bench::pc.printf("\tSD card detected? %9s\r\n",sd_card_present?"Yes":"No");
cnckiwi31 5:63063a9fa51c 354 if(sd_card_present)
cnckiwi31 5:63063a9fa51c 355 {
cnckiwi31 5:63063a9fa51c 356 Bench::pc.printf("\tFile number %15i\r\n", fname_prepend);
cnckiwi31 5:63063a9fa51c 357 Bench::pc.printf("\tLogging? %18s\r\n",is_logging?"Yes":"No");
cnckiwi31 5:63063a9fa51c 358 }
cnckiwi31 5:63063a9fa51c 359 }
cnckiwi31 5:63063a9fa51c 360
cnckiwi31 5:63063a9fa51c 361 /**
cnckiwi31 5:63063a9fa51c 362 * Stops the Bench class from printing data if it is
cnckiwi31 5:63063a9fa51c 363 */
cnckiwi31 5:63063a9fa51c 364 void Bench::pausePrint()
cnckiwi31 5:63063a9fa51c 365 {
cnckiwi31 5:63063a9fa51c 366 was_printing = is_printing;
cnckiwi31 5:63063a9fa51c 367 is_printing = false;
cnckiwi31 5:63063a9fa51c 368 }
cnckiwi31 5:63063a9fa51c 369
cnckiwi31 5:63063a9fa51c 370 /**
cnckiwi31 5:63063a9fa51c 371 * Resumes printing in the Bench class
cnckiwi31 5:63063a9fa51c 372 */
cnckiwi31 5:63063a9fa51c 373 void Bench::resumePrint()
cnckiwi31 5:63063a9fa51c 374 {
cnckiwi31 5:63063a9fa51c 375 is_printing = was_printing;
cnckiwi31 5:63063a9fa51c 376 }
cnckiwi31 5:63063a9fa51c 377
cnckiwi31 5:63063a9fa51c 378 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
cnckiwi31 5:63063a9fa51c 379 // IMPLEMENTATION USER IO
cnckiwi31 5:63063a9fa51c 380 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
cnckiwi31 5:63063a9fa51c 381
cnckiwi31 5:63063a9fa51c 382 /**
cnckiwi31 5:63063a9fa51c 383 * button lower toggles printing of data
cnckiwi31 5:63063a9fa51c 384 */
cnckiwi31 5:63063a9fa51c 385 void Bench::TogglePrinting()
cnckiwi31 5:63063a9fa51c 386 {
cnckiwi31 5:63063a9fa51c 387 if (not is_printing) {
cnckiwi31 5:63063a9fa51c 388 is_printing = true;
cnckiwi31 5:63063a9fa51c 389 } else {
cnckiwi31 5:63063a9fa51c 390 is_printing = false;
cnckiwi31 5:63063a9fa51c 391 PrintMenu();
cnckiwi31 5:63063a9fa51c 392 }
cnckiwi31 5:63063a9fa51c 393 was_printing = is_printing;
cnckiwi31 5:63063a9fa51c 394 }
cnckiwi31 5:63063a9fa51c 395
cnckiwi31 5:63063a9fa51c 396 /*
cnckiwi31 5:63063a9fa51c 397 * button lower toggles printing of data
cnckiwi31 5:63063a9fa51c 398 */
cnckiwi31 5:63063a9fa51c 399 void Bench::ToggleLogging()
cnckiwi31 5:63063a9fa51c 400 {
cnckiwi31 5:63063a9fa51c 401 if (not is_logging) {
cnckiwi31 5:63063a9fa51c 402 StartLogging();
cnckiwi31 5:63063a9fa51c 403 } else {
cnckiwi31 5:63063a9fa51c 404 is_logging = false;
cnckiwi31 5:63063a9fa51c 405 StopLogging();
cnckiwi31 5:63063a9fa51c 406 }
cnckiwi31 5:63063a9fa51c 407 PrintMenu();
cnckiwi31 5:63063a9fa51c 408 }
cnckiwi31 5:63063a9fa51c 409
cnckiwi31 5:63063a9fa51c 410 /*
cnckiwi31 5:63063a9fa51c 411 * Indicates if we are now data logging
cnckiwi31 5:63063a9fa51c 412 */
cnckiwi31 5:63063a9fa51c 413 bool Bench::isLogging()
cnckiwi31 5:63063a9fa51c 414 {
cnckiwi31 5:63063a9fa51c 415 return is_logging;
cnckiwi31 5:63063a9fa51c 416 }
cnckiwi31 5:63063a9fa51c 417
megrootens 0:3855d4588f76 418 /**
megrootens 0:3855d4588f76 419 * Obtain the joint angle in degrees.
megrootens 0:3855d4588f76 420 * These are the angles at the time of two Update() calls back
megrootens 0:3855d4588f76 421 * @param joint: the joint for which the angle is requested
megrootens 0:3855d4588f76 422 * @return: joint angle
megrootens 0:3855d4588f76 423 */
megrootens 0:3855d4588f76 424 float Bench::getDegrees(Joint joint)
megrootens 0:3855d4588f76 425 {
megrootens 0:3855d4588f76 426 return getDegrees(joint);
megrootens 0:3855d4588f76 427 }
megrootens 0:3855d4588f76 428
megrootens 0:3855d4588f76 429 float Bench::getRadians(int i_joint)
megrootens 0:3855d4588f76 430 {
megrootens 0:3855d4588f76 431 float ang = as5048_.getAngleRadians(i_joint);
megrootens 0:3855d4588f76 432 if (ang>kCutOffRadians) {
megrootens 0:3855d4588f76 433 return ang-As5048::kRadPerRev;
megrootens 0:3855d4588f76 434 }
megrootens 0:3855d4588f76 435 return ang;
megrootens 0:3855d4588f76 436 }
megrootens 0:3855d4588f76 437
megrootens 0:3855d4588f76 438 /**
megrootens 0:3855d4588f76 439 * Obtain the joint angle in radians.
megrootens 0:3855d4588f76 440 * These are the angles at the time of two Update() calls back
megrootens 0:3855d4588f76 441 * @param joint: the joint for which the angle is requested
megrootens 0:3855d4588f76 442 * @return: joint angle
megrootens 0:3855d4588f76 443 */
megrootens 0:3855d4588f76 444 float Bench::getRadians(Joint joint)
megrootens 0:3855d4588f76 445 {
megrootens 0:3855d4588f76 446 return getRadians(joint);
megrootens 0:3855d4588f76 447 }
megrootens 0:3855d4588f76 448
megrootens 0:3855d4588f76 449
megrootens 0:3855d4588f76 450 const char* Bench::getJointName(int i_joint)
megrootens 0:3855d4588f76 451 {
megrootens 0:3855d4588f76 452 return sensors::kJointNames[i_joint];
megrootens 0:3855d4588f76 453 }
megrootens 0:3855d4588f76 454
megrootens 0:3855d4588f76 455 const char* Bench::getJointName(Joint joint)
megrootens 0:3855d4588f76 456 {
megrootens 0:3855d4588f76 457 return getJointName(joint);
megrootens 0:3855d4588f76 458 }
megrootens 0:3855d4588f76 459
megrootens 0:3855d4588f76 460 As5048* Bench::get_as5048()
megrootens 0:3855d4588f76 461 {
megrootens 0:3855d4588f76 462 return &as5048_;
megrootens 0:3855d4588f76 463 }
megrootens 0:3855d4588f76 464
megrootens 0:3855d4588f76 465 float Bench::getForce()
megrootens 0:3855d4588f76 466 {
cnckiwi31 5:63063a9fa51c 467 if (Bench::use5kN) {
cnckiwi31 5:63063a9fa51c 468 return loadCell5kN.getForce();
cnckiwi31 5:63063a9fa51c 469 } else {
cnckiwi31 5:63063a9fa51c 470 return loadCell1kN.getForce();
cnckiwi31 5:63063a9fa51c 471 }
cnckiwi31 5:63063a9fa51c 472
megrootens 0:3855d4588f76 473 }
megrootens 0:3855d4588f76 474
cnckiwi31 4:1cdce6c6c94e 475 void Bench::nullForce()
cnckiwi31 4:1cdce6c6c94e 476 {
cnckiwi31 5:63063a9fa51c 477 if (use5kN) {
cnckiwi31 5:63063a9fa51c 478 return loadCell5kN.nullForce();
cnckiwi31 5:63063a9fa51c 479 } else {
cnckiwi31 5:63063a9fa51c 480 return loadCell1kN.nullForce();
cnckiwi31 5:63063a9fa51c 481 }
cnckiwi31 4:1cdce6c6c94e 482 }
cnckiwi31 4:1cdce6c6c94e 483
cnckiwi31 4:1cdce6c6c94e 484 float Bench::getPressure0()
cnckiwi31 4:1cdce6c6c94e 485 {
cnckiwi31 4:1cdce6c6c94e 486 return spte0.getPressure();
cnckiwi31 4:1cdce6c6c94e 487 }
cnckiwi31 4:1cdce6c6c94e 488
cnckiwi31 4:1cdce6c6c94e 489 void Bench::nullPressure0()
cnckiwi31 4:1cdce6c6c94e 490 {
cnckiwi31 4:1cdce6c6c94e 491 return spte0.nullPressure();
cnckiwi31 4:1cdce6c6c94e 492 }
cnckiwi31 4:1cdce6c6c94e 493
cnckiwi31 4:1cdce6c6c94e 494 float Bench::getPressure1()
cnckiwi31 4:1cdce6c6c94e 495 {
cnckiwi31 4:1cdce6c6c94e 496 return spte1.getPressure();
cnckiwi31 4:1cdce6c6c94e 497 }
cnckiwi31 4:1cdce6c6c94e 498
cnckiwi31 4:1cdce6c6c94e 499 void Bench::nullPressure1()
cnckiwi31 4:1cdce6c6c94e 500 {
cnckiwi31 4:1cdce6c6c94e 501 return spte1.nullPressure();
cnckiwi31 4:1cdce6c6c94e 502 }
megrootens 0:3855d4588f76 503
cnckiwi31 5:63063a9fa51c 504 void Bench::setValve(bool set)
cnckiwi31 5:63063a9fa51c 505 {
cnckiwi31 5:63063a9fa51c 506 valveFesto.setValve(set);
cnckiwi31 5:63063a9fa51c 507 }
megrootens 0:3855d4588f76 508
cnckiwi31 5:63063a9fa51c 509 bool Bench::getValve() {
cnckiwi31 5:63063a9fa51c 510 return valveFesto.getValve();
cnckiwi31 5:63063a9fa51c 511 }