Allan Veale / Mbed 2 deprecated DROPSAWTestRigCode_V4

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

bench.cpp@7:edb876c98565, 2019-12-09 (annotated)

Committer:
cnckiwi31
Date:
Mon Dec 09 12:51:14 2019 +0000
Revision:
7:edb876c98565
Parent:
6:02507d7a6f51
Child:
8:bc467f5fe7c3
Updated documentation grammar

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 /**
cnckiwi31 7:edb876c98565 4 * Create an object representing the test rig
cnckiwi31 7:edb876c98565 5 * @param mosi: mosi pin for sensor chain (SPI angle sensors)
cnckiwi31 7:edb876c98565 6 * @param miso: miso pin for sensor chain (SPI angle sensors)
cnckiwi31 7:edb876c98565 7 * @param sck: clock pin for sensor chain (SPI angle sensors)
cnckiwi31 7:edb876c98565 8 * @param cs: chip select pin for sensor chain (SPI angle sensors)
cnckiwi31 5:63063a9fa51c 9 * @param use5kNLoadCell: if 5kN load cell is used (if false, 1kN sensor is used)
cnckiwi31 4:1cdce6c6c94e 10 * @param p_lcm101: analog input pin for load cell
cnckiwi31 4:1cdce6c6c94e 11 * @param p_spte0: analog input pin for pressure sensor 0
cnckiwi31 4:1cdce6c6c94e 12 * @param p_spte1: analog input pin for pressure sensor 1
cnckiwi31 5:63063a9fa51c 13 * @param p_valve: digital output for valve (on/off) that inflates leg actuator
cnckiwi31 5:63063a9fa51c 14 * @param mosi: mosi pin for sd card
cnckiwi31 5:63063a9fa51c 15 * @param miso: miso pin for sd card
cnckiwi31 5:63063a9fa51c 16 * @param sck: clock pin for sd card
cnckiwi31 5:63063a9fa51c 17 * @param cs: chip select pin for sd card
cnckiwi31 5:63063a9fa51c 18 * @param tx: serial transmission line
cnckiwi31 5:63063a9fa51c 19 * @param rx: serial receive line
cnckiwi31 5:63063a9fa51c 20 * @param but0: first input button
cnckiwi31 5:63063a9fa51c 21 * @param but1: second input button
megrootens 0:3855d4588f76 22 */
megrootens 0:3855d4588f76 23 Bench::Bench(PinName mosi, PinName miso, PinName sck, PinName cs,
cnckiwi31 5:63063a9fa51c 24 bool use5kNLoadCell,PinName p_lcm101, PinName p_spte0, PinName p_spte1, PinName p_valve,
cnckiwi31 5:63063a9fa51c 25 PinName sd_mosi, PinName sd_miso, PinName sd_sck, PinName sd_cs,
cnckiwi31 5:63063a9fa51c 26 PinName tx, PinName rx,
cnckiwi31 5:63063a9fa51c 27 PinName but0, PinName but1) :
cnckiwi31 5:63063a9fa51c 28 pc(tx,rx),
cnckiwi31 5:63063a9fa51c 29 lowerBut(but0,PullUp),
cnckiwi31 5:63063a9fa51c 30 upperBut(but1,PullUp),
megrootens 0:3855d4588f76 31 as5048_(mosi, miso, sck, cs, sensors::kNumJoints),
cnckiwi31 5:63063a9fa51c 32 loadCell5kN(p_lcm101, sensors::kGen5kNOffset, sensors::kGen5kNFactor),
cnckiwi31 5:63063a9fa51c 33 loadCell1kN(p_lcm101, sensors::kLcm101Offset, sensors::kLcm101Factor),
cnckiwi31 5:63063a9fa51c 34 use5kN(use5kNLoadCell),
cnckiwi31 4:1cdce6c6c94e 35 spte0(p_spte0, sensors::kSPTE0Offset, sensors::kSPTE0Factor),
cnckiwi31 5:63063a9fa51c 36 spte1(p_spte1, sensors::kSPTE1Offset, sensors::kSPTE1Factor),
cnckiwi31 5:63063a9fa51c 37 valveFesto(p_valve),
cnckiwi31 5:63063a9fa51c 38 sd(sd_mosi, sd_miso, sd_sck, sd_cs, "sd")
cnckiwi31 5:63063a9fa51c 39 {
cnckiwi31 5:63063a9fa51c 40 //init serial things
cnckiwi31 5:63063a9fa51c 41 pc.baud(timing::kSerialBaudrate);//set the serial rate
cnckiwi31 5:63063a9fa51c 42
cnckiwi31 5:63063a9fa51c 43 sd_card_present = false;
cnckiwi31 5:63063a9fa51c 44 fname_prepend = 0;
cnckiwi31 5:63063a9fa51c 45 is_logging = false;
cnckiwi31 5:63063a9fa51c 46 is_printing = false;
cnckiwi31 5:63063a9fa51c 47 was_printing = false;
cnckiwi31 5:63063a9fa51c 48
cnckiwi31 5:63063a9fa51c 49 usedExtraCols = 0;
cnckiwi31 5:63063a9fa51c 50
cnckiwi31 5:63063a9fa51c 51 firstReadMS = 0; //first timer value read
cnckiwi31 5:63063a9fa51c 52 startedLogging = false; //in the middle of a logging cycle
cnckiwi31 5:63063a9fa51c 53
cnckiwi31 5:63063a9fa51c 54 //set data logging frequency
cnckiwi31 5:63063a9fa51c 55 loggingUS = timing::kTimeLogDataUs;
cnckiwi31 5:63063a9fa51c 56 loggingHz = (int)(1000000/timing::kTimeLogDataUs);
cnckiwi31 5:63063a9fa51c 57
megrootens 0:3855d4588f76 58 for (int i=0; i<sensors::kNumJoints; ++i) {
megrootens 0:3855d4588f76 59 as5048_.setOffsetDegrees(i,sensors::kOffsetsDegrees[i]);
megrootens 0:3855d4588f76 60 as5048_.setDirection(i,sensors::kDirections[i]);
cnckiwi31 5:63063a9fa51c 61 }
cnckiwi31 5:63063a9fa51c 62 }
cnckiwi31 5:63063a9fa51c 63
cnckiwi31 5:63063a9fa51c 64 /**
cnckiwi31 5:63063a9fa51c 65 * Initialises: timers, SD card
cnckiwi31 7:edb876c98565 66 * Prints a welcome message and information on the logging frequency and sensor
cnckiwi31 7:edb876c98565 67 * reading rate
cnckiwi31 5:63063a9fa51c 68 */
cnckiwi31 5:63063a9fa51c 69 void Bench::initialise()
cnckiwi31 5:63063a9fa51c 70 {
cnckiwi31 5:63063a9fa51c 71 //setup the timing
cnckiwi31 5:63063a9fa51c 72 tick_update.attach_us(this,&Bench::update,timing::kTimeControlUs);
cnckiwi31 7:edb876c98565 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 6:02507d7a6f51 150 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
cnckiwi31 6:02507d7a6f51 151 // IMPLEMENTATION HARDWARE INTERFACE
cnckiwi31 6:02507d7a6f51 152 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
cnckiwi31 6:02507d7a6f51 153
cnckiwi31 5:63063a9fa51c 154 /**
cnckiwi31 7:edb876c98565 155 * Update routine for the test rig
megrootens 0:3855d4588f76 156 * - Updates the angle buffer of the sensor array
megrootens 0:3855d4588f76 157 * - ... that's it for now (add filtering?)
cnckiwi31 7:edb876c98565 158 * Note that angles lag one update() behind, due to the way the SPI
megrootens 0:3855d4588f76 159 * protocol works.
megrootens 0:3855d4588f76 160 */
cnckiwi31 5:63063a9fa51c 161 void Bench::update()
megrootens 0:3855d4588f76 162 {
megrootens 0:3855d4588f76 163 as5048_.UpdateAngleBuffer();
megrootens 0:3855d4588f76 164 }
megrootens 0:3855d4588f76 165
cnckiwi31 6:02507d7a6f51 166 /**
cnckiwi31 7:edb876c98565 167 * Obtain the joint angle in degrees
cnckiwi31 6:02507d7a6f51 168 * These are the angles at the time of two Update() calls back
cnckiwi31 6:02507d7a6f51 169 * @param joint: the joint for which the angle is requested (as enumerated in
cnckiwi31 6:02507d7a6f51 170 * class header - TOES, KNEE, ANKLE, HIP
cnckiwi31 6:02507d7a6f51 171 * @return: joint angle
cnckiwi31 6:02507d7a6f51 172 */
cnckiwi31 6:02507d7a6f51 173 float Bench::getDegrees(Joint joint)
cnckiwi31 6:02507d7a6f51 174 {
cnckiwi31 6:02507d7a6f51 175 return getDegrees(joint);
cnckiwi31 6:02507d7a6f51 176 }
cnckiwi31 6:02507d7a6f51 177
cnckiwi31 6:02507d7a6f51 178 /**
cnckiwi31 7:edb876c98565 179 * Obtain the joint angle in degrees
cnckiwi31 6:02507d7a6f51 180 * These are the angles at the time of two Update() calls back
cnckiwi31 6:02507d7a6f51 181 * @param joint: the joint for which the angle is requested (a number starting
cnckiwi31 6:02507d7a6f51 182 * from 0 indicating the position in the sensor daisy chain)
cnckiwi31 6:02507d7a6f51 183 * @return: joint angle
cnckiwi31 6:02507d7a6f51 184 */
megrootens 0:3855d4588f76 185 float Bench::getDegrees(int i_joint)
megrootens 0:3855d4588f76 186 {
megrootens 0:3855d4588f76 187 float ang = as5048_.getAngleDegrees(i_joint);
megrootens 0:3855d4588f76 188 if (ang>kCutOffDegrees) {
megrootens 0:3855d4588f76 189 return ang-As5048::kDegPerRev;
megrootens 0:3855d4588f76 190 }
megrootens 0:3855d4588f76 191 return ang;
megrootens 0:3855d4588f76 192 }
megrootens 0:3855d4588f76 193
cnckiwi31 6:02507d7a6f51 194 /**
cnckiwi31 7:edb876c98565 195 * Obtain the joint angle in radians
cnckiwi31 6:02507d7a6f51 196 * These are the angles at the time of two Update() calls back
cnckiwi31 6:02507d7a6f51 197 * @param joint: the joint for which the angle is requested (as enumerated in
cnckiwi31 6:02507d7a6f51 198 * class header - TOES, KNEE, ANKLE, HIP
cnckiwi31 6:02507d7a6f51 199 * @return: joint angle
cnckiwi31 6:02507d7a6f51 200 */
cnckiwi31 6:02507d7a6f51 201 float Bench::getRadians(Joint joint)
cnckiwi31 6:02507d7a6f51 202 {
cnckiwi31 6:02507d7a6f51 203 return getRadians(joint);
cnckiwi31 6:02507d7a6f51 204 }
cnckiwi31 6:02507d7a6f51 205
cnckiwi31 6:02507d7a6f51 206 /**
cnckiwi31 7:edb876c98565 207 * Obtain the joint angle in radians
cnckiwi31 6:02507d7a6f51 208 * These are the angles at the time of two Update() calls back
cnckiwi31 6:02507d7a6f51 209 * @param joint: the joint for which the angle is requested (a number starting
cnckiwi31 6:02507d7a6f51 210 * from 0 indicating the position in the sensor daisy chain)
cnckiwi31 6:02507d7a6f51 211 * @return: joint angle
cnckiwi31 6:02507d7a6f51 212 */
cnckiwi31 6:02507d7a6f51 213 float Bench::getRadians(int i_joint)
cnckiwi31 6:02507d7a6f51 214 {
cnckiwi31 6:02507d7a6f51 215 float ang = as5048_.getAngleRadians(i_joint);
cnckiwi31 6:02507d7a6f51 216 if (ang>kCutOffRadians) {
cnckiwi31 6:02507d7a6f51 217 return ang-As5048::kRadPerRev;
cnckiwi31 6:02507d7a6f51 218 }
cnckiwi31 6:02507d7a6f51 219 return ang;
cnckiwi31 6:02507d7a6f51 220 }
cnckiwi31 6:02507d7a6f51 221
cnckiwi31 6:02507d7a6f51 222
cnckiwi31 6:02507d7a6f51 223 const char* Bench::getJointName(int i_joint)
cnckiwi31 6:02507d7a6f51 224 {
cnckiwi31 6:02507d7a6f51 225 return sensors::kJointNames[i_joint];
cnckiwi31 6:02507d7a6f51 226 }
cnckiwi31 6:02507d7a6f51 227
cnckiwi31 6:02507d7a6f51 228 const char* Bench::getJointName(Joint joint)
cnckiwi31 6:02507d7a6f51 229 {
cnckiwi31 6:02507d7a6f51 230 return getJointName(joint);
cnckiwi31 6:02507d7a6f51 231 }
cnckiwi31 6:02507d7a6f51 232
cnckiwi31 6:02507d7a6f51 233 As5048* Bench::get_as5048()
cnckiwi31 6:02507d7a6f51 234 {
cnckiwi31 6:02507d7a6f51 235 return &as5048_;
cnckiwi31 6:02507d7a6f51 236 }
cnckiwi31 6:02507d7a6f51 237
cnckiwi31 6:02507d7a6f51 238 /**
cnckiwi31 7:edb876c98565 239 * The force applied vertically at the hip joint in N
cnckiwi31 6:02507d7a6f51 240 * @return: force
cnckiwi31 6:02507d7a6f51 241 */
cnckiwi31 6:02507d7a6f51 242 float Bench::getForce()
cnckiwi31 6:02507d7a6f51 243 {
cnckiwi31 6:02507d7a6f51 244 if (Bench::use5kN) {
cnckiwi31 6:02507d7a6f51 245 return loadCell5kN.getForce();
cnckiwi31 6:02507d7a6f51 246 } else {
cnckiwi31 6:02507d7a6f51 247 return loadCell1kN.getForce();
cnckiwi31 6:02507d7a6f51 248 }
cnckiwi31 6:02507d7a6f51 249
cnckiwi31 6:02507d7a6f51 250 }
cnckiwi31 6:02507d7a6f51 251
cnckiwi31 6:02507d7a6f51 252 void Bench::nullForce()
cnckiwi31 6:02507d7a6f51 253 {
cnckiwi31 6:02507d7a6f51 254 if (use5kN) {
cnckiwi31 6:02507d7a6f51 255 return loadCell5kN.nullForce();
cnckiwi31 6:02507d7a6f51 256 } else {
cnckiwi31 6:02507d7a6f51 257 return loadCell1kN.nullForce();
cnckiwi31 6:02507d7a6f51 258 }
cnckiwi31 6:02507d7a6f51 259 }
cnckiwi31 6:02507d7a6f51 260
cnckiwi31 6:02507d7a6f51 261 /**
cnckiwi31 6:02507d7a6f51 262 * The pressure measured by pressure sensor 0 in bar
cnckiwi31 6:02507d7a6f51 263 * @return: pressure
cnckiwi31 6:02507d7a6f51 264 */
cnckiwi31 6:02507d7a6f51 265 float Bench::getPressure0()
cnckiwi31 6:02507d7a6f51 266 {
cnckiwi31 6:02507d7a6f51 267 return spte0.getPressure();
cnckiwi31 6:02507d7a6f51 268 }
cnckiwi31 6:02507d7a6f51 269
cnckiwi31 6:02507d7a6f51 270 void Bench::nullPressure0()
cnckiwi31 6:02507d7a6f51 271 {
cnckiwi31 6:02507d7a6f51 272 return spte0.nullPressure();
cnckiwi31 6:02507d7a6f51 273 }
cnckiwi31 6:02507d7a6f51 274
cnckiwi31 6:02507d7a6f51 275 float Bench::getPressure1()
cnckiwi31 6:02507d7a6f51 276 {
cnckiwi31 6:02507d7a6f51 277 return spte1.getPressure();
cnckiwi31 6:02507d7a6f51 278 }
cnckiwi31 6:02507d7a6f51 279
cnckiwi31 6:02507d7a6f51 280 void Bench::nullPressure1()
cnckiwi31 6:02507d7a6f51 281 {
cnckiwi31 6:02507d7a6f51 282 return spte1.nullPressure();
cnckiwi31 6:02507d7a6f51 283 }
cnckiwi31 6:02507d7a6f51 284
cnckiwi31 6:02507d7a6f51 285 /**
cnckiwi31 6:02507d7a6f51 286 * Control the valve (true turns the valve on to pressurise, false turns
cnckiwi31 6:02507d7a6f51 287 * the valve off to depressurise)
cnckiwi31 6:02507d7a6f51 288 * @param set: valve state
cnckiwi31 6:02507d7a6f51 289 */
cnckiwi31 6:02507d7a6f51 290 void Bench::setValve(bool set)
cnckiwi31 6:02507d7a6f51 291 {
cnckiwi31 6:02507d7a6f51 292 valveFesto.setValve(set);
cnckiwi31 6:02507d7a6f51 293 }
cnckiwi31 6:02507d7a6f51 294
cnckiwi31 6:02507d7a6f51 295 /**
cnckiwi31 7:edb876c98565 296 * The valve state (true is on/pressurising and false is off/depressurising).
cnckiwi31 6:02507d7a6f51 297 * @return: valve state
cnckiwi31 6:02507d7a6f51 298 */
cnckiwi31 6:02507d7a6f51 299 bool Bench::getValve() {
cnckiwi31 6:02507d7a6f51 300 return valveFesto.getValve();
cnckiwi31 6:02507d7a6f51 301 }
cnckiwi31 6:02507d7a6f51 302
cnckiwi31 5:63063a9fa51c 303 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
cnckiwi31 5:63063a9fa51c 304 // IMPLEMENTATION DATA LOGGING
cnckiwi31 5:63063a9fa51c 305 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
cnckiwi31 5:63063a9fa51c 306
cnckiwi31 5:63063a9fa51c 307 /**
cnckiwi31 5:63063a9fa51c 308 * Check contents of SD card and count files in order
cnckiwi31 7:edb876c98565 309 * to ensure unique file name for logging data.
cnckiwi31 5:63063a9fa51c 310 */
cnckiwi31 5:63063a9fa51c 311 void Bench::InitSdCard()
cnckiwi31 5:63063a9fa51c 312 {
cnckiwi31 5:63063a9fa51c 313 pc.printf("\r\nINITIALISING SD CARD\r\n");
cnckiwi31 5:63063a9fa51c 314
cnckiwi31 5:63063a9fa51c 315 int num_files = 0;
cnckiwi31 5:63063a9fa51c 316
cnckiwi31 5:63063a9fa51c 317 // scan dir
cnckiwi31 5:63063a9fa51c 318 DIR *d;
cnckiwi31 5:63063a9fa51c 319 struct dirent *p;
cnckiwi31 5:63063a9fa51c 320
cnckiwi31 5:63063a9fa51c 321 d = opendir("/sd");
cnckiwi31 5:63063a9fa51c 322 if (d != NULL) {
cnckiwi31 5:63063a9fa51c 323 sd_card_present = true;
cnckiwi31 5:63063a9fa51c 324
cnckiwi31 5:63063a9fa51c 325 pc.printf("\t> Contents of SD Card:\r\n");
cnckiwi31 5:63063a9fa51c 326 while ((p = readdir(d)) != NULL) {
cnckiwi31 5:63063a9fa51c 327 if (p->d_name[0] != '.') {
cnckiwi31 5:63063a9fa51c 328 // skip files starting with '.'
cnckiwi31 5:63063a9fa51c 329 pc.printf("\t %s",p->d_name);
cnckiwi31 5:63063a9fa51c 330 ++num_files;
cnckiwi31 5:63063a9fa51c 331 }
cnckiwi31 5:63063a9fa51c 332 }
cnckiwi31 5:63063a9fa51c 333 pc.printf("\r\n\t> Counted %d visible files.\r\n",num_files);
cnckiwi31 5:63063a9fa51c 334
cnckiwi31 5:63063a9fa51c 335 closedir(d);
cnckiwi31 5:63063a9fa51c 336 } else {
cnckiwi31 5:63063a9fa51c 337 sd_card_present = false;
cnckiwi31 5:63063a9fa51c 338
cnckiwi31 5:63063a9fa51c 339 pc.printf("\t> No SD Card present. Data cannot be logged.\r\n");
cnckiwi31 5:63063a9fa51c 340 }
cnckiwi31 5:63063a9fa51c 341
cnckiwi31 5:63063a9fa51c 342 // id to be appended to logged data files
cnckiwi31 5:63063a9fa51c 343 fname_prepend = num_files;
cnckiwi31 5:63063a9fa51c 344 }
cnckiwi31 5:63063a9fa51c 345
cnckiwi31 5:63063a9fa51c 346 /**
cnckiwi31 5:63063a9fa51c 347 * Start logging data
cnckiwi31 6:02507d7a6f51 348 * param fname_append: a string describing the name appended to each logged file
cnckiwi31 7:edb876c98565 349 * (along with its unique loggging number).
cnckiwi31 5:63063a9fa51c 350 */
cnckiwi31 5:63063a9fa51c 351 void Bench::StartLogging(const char * fname_append)
cnckiwi31 5:63063a9fa51c 352 {
cnckiwi31 5:63063a9fa51c 353 pc.printf("\r\nDATA LOGGING");
cnckiwi31 5:63063a9fa51c 354 if (sd_card_present) {
cnckiwi31 5:63063a9fa51c 355
cnckiwi31 5:63063a9fa51c 356 // create unique file name
cnckiwi31 5:63063a9fa51c 357 ++fname_prepend;
cnckiwi31 5:63063a9fa51c 358 char fname[50];
cnckiwi31 5:63063a9fa51c 359 sprintf(fname, "/sd/%d_%s.csv",fname_prepend,fname_append);
cnckiwi31 5:63063a9fa51c 360
cnckiwi31 5:63063a9fa51c 361 pc.printf("\t> Opening data log file '%s'...\r\n",fname);
cnckiwi31 5:63063a9fa51c 362
cnckiwi31 5:63063a9fa51c 363 // open file for writing and start logging after success
cnckiwi31 5:63063a9fa51c 364 fp_data = fopen(fname,"w");
cnckiwi31 5:63063a9fa51c 365 if (fp_data==NULL) {
cnckiwi31 5:63063a9fa51c 366 pc.printf("\t> ERROR: failed to open log file (t=%d ms)\r\n",
cnckiwi31 5:63063a9fa51c 367 timer.read_ms());
cnckiwi31 5:63063a9fa51c 368 } else {
cnckiwi31 5:63063a9fa51c 369 string fHeader = "time (s),theta_knee (deg),force (N),pressure (kPa)";
cnckiwi31 5:63063a9fa51c 370
cnckiwi31 5:63063a9fa51c 371 for (int i=0; i<usedExtraCols; i++) {
cnckiwi31 5:63063a9fa51c 372 if (extraColNames[i] != "") {
cnckiwi31 5:63063a9fa51c 373 fHeader = fHeader + "," + extraColNames[i];
cnckiwi31 5:63063a9fa51c 374 }
cnckiwi31 5:63063a9fa51c 375 }
cnckiwi31 5:63063a9fa51c 376 //pc.printf("%s", fHeader.c_str());
cnckiwi31 5:63063a9fa51c 377 fprintf(fp_data, "%s", fHeader.c_str());
cnckiwi31 5:63063a9fa51c 378 tick_logging.attach_us(this,&Bench::LogData,loggingUS);
cnckiwi31 5:63063a9fa51c 379 timer.start();
cnckiwi31 5:63063a9fa51c 380 startedLogging = true;
cnckiwi31 5:63063a9fa51c 381
cnckiwi31 5:63063a9fa51c 382 pc.printf("\t> Logging started.\r\n");
cnckiwi31 5:63063a9fa51c 383
cnckiwi31 5:63063a9fa51c 384 is_logging = true;
cnckiwi31 5:63063a9fa51c 385 }
cnckiwi31 5:63063a9fa51c 386 } else {
cnckiwi31 5:63063a9fa51c 387 pc.printf("\t> No SD Card; no data will be logged.\r\n");
cnckiwi31 5:63063a9fa51c 388 startedLogging = false;
cnckiwi31 5:63063a9fa51c 389 }
cnckiwi31 5:63063a9fa51c 390 }
cnckiwi31 5:63063a9fa51c 391
cnckiwi31 5:63063a9fa51c 392
cnckiwi31 5:63063a9fa51c 393 /**
cnckiwi31 7:edb876c98565 394 * Stop logging data.
cnckiwi31 5:63063a9fa51c 395 */
cnckiwi31 5:63063a9fa51c 396 void Bench::StopLogging()
cnckiwi31 5:63063a9fa51c 397 {
cnckiwi31 5:63063a9fa51c 398 Bench::pc.printf("\r\nDATA LOGGING:");
cnckiwi31 5:63063a9fa51c 399 if (sd_card_present) {
cnckiwi31 5:63063a9fa51c 400 // close data file, stop logging
cnckiwi31 5:63063a9fa51c 401 fclose(fp_data);
cnckiwi31 5:63063a9fa51c 402 tick_logging.detach();
cnckiwi31 5:63063a9fa51c 403 timer.stop();
cnckiwi31 5:63063a9fa51c 404 timer.reset();
cnckiwi31 5:63063a9fa51c 405 Bench::pc.printf("\r> Stopped.");
cnckiwi31 5:63063a9fa51c 406 } else {
cnckiwi31 5:63063a9fa51c 407 Bench::pc.printf("\t> No data was logged.");
cnckiwi31 5:63063a9fa51c 408 }
cnckiwi31 5:63063a9fa51c 409
cnckiwi31 5:63063a9fa51c 410 is_logging = false;
cnckiwi31 5:63063a9fa51c 411 }
cnckiwi31 5:63063a9fa51c 412
cnckiwi31 5:63063a9fa51c 413 /**
cnckiwi31 5:63063a9fa51c 414 * Stop logging data and print the menu
cnckiwi31 5:63063a9fa51c 415 */
cnckiwi31 5:63063a9fa51c 416 void Bench::stopLogging()
cnckiwi31 5:63063a9fa51c 417 {
cnckiwi31 5:63063a9fa51c 418 if(is_logging) {
cnckiwi31 5:63063a9fa51c 419 is_logging = false;
cnckiwi31 5:63063a9fa51c 420 StopLogging();
cnckiwi31 5:63063a9fa51c 421 PrintMenu();
cnckiwi31 5:63063a9fa51c 422 }
cnckiwi31 5:63063a9fa51c 423 }
cnckiwi31 5:63063a9fa51c 424
cnckiwi31 5:63063a9fa51c 425 /**
cnckiwi31 5:63063a9fa51c 426 * Log data
cnckiwi31 5:63063a9fa51c 427 */
cnckiwi31 5:63063a9fa51c 428 void Bench::LogData()
cnckiwi31 5:63063a9fa51c 429 {
cnckiwi31 5:63063a9fa51c 430 int currTime = timer.read_ms();
cnckiwi31 5:63063a9fa51c 431 if(startedLogging) {
cnckiwi31 5:63063a9fa51c 432 firstReadMS = currTime;
cnckiwi31 5:63063a9fa51c 433 startedLogging = false;
cnckiwi31 5:63063a9fa51c 434 }
cnckiwi31 5:63063a9fa51c 435 double currTimeS = ((double)currTime - firstReadMS)/1000;
cnckiwi31 5:63063a9fa51c 436
cnckiwi31 5:63063a9fa51c 437 // time
cnckiwi31 5:63063a9fa51c 438 fprintf(fp_data,"\n%f",currTimeS);
cnckiwi31 5:63063a9fa51c 439
cnckiwi31 5:63063a9fa51c 440 // bench: joint angles and force sensor and pressure sensor values
cnckiwi31 5:63063a9fa51c 441 fprintf(fp_data,",%f,%f,%f",
cnckiwi31 5:63063a9fa51c 442 getDegrees(0),
cnckiwi31 5:63063a9fa51c 443 getForce(),
cnckiwi31 5:63063a9fa51c 444 getPressure0()*100
cnckiwi31 5:63063a9fa51c 445 );
cnckiwi31 5:63063a9fa51c 446 for (int i=0; i<usedExtraCols; i++) {
cnckiwi31 5:63063a9fa51c 447 fprintf(fp_data,",%f",extraColValues[i]);
cnckiwi31 5:63063a9fa51c 448 }
cnckiwi31 5:63063a9fa51c 449 }
cnckiwi31 5:63063a9fa51c 450
cnckiwi31 5:63063a9fa51c 451 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
cnckiwi31 5:63063a9fa51c 452 // IMPLEMENTATION SERIAL COM
cnckiwi31 5:63063a9fa51c 453 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
cnckiwi31 5:63063a9fa51c 454 /**
cnckiwi31 5:63063a9fa51c 455 * Prints the values of the key measurements in the setup
cnckiwi31 5:63063a9fa51c 456 */
cnckiwi31 5:63063a9fa51c 457 void Bench::PrintStatus()
cnckiwi31 5:63063a9fa51c 458 {
cnckiwi31 5:63063a9fa51c 459 if (is_printing) {
cnckiwi31 5:63063a9fa51c 460 if(sd_card_present)
cnckiwi31 5:63063a9fa51c 461 {
cnckiwi31 5:63063a9fa51c 462 Bench::pc.printf("\tFile number %15i\r\n", fname_prepend);
cnckiwi31 5:63063a9fa51c 463 Bench::pc.printf("\tLogging? %18s\r\n",is_logging?"Yes":"No");
cnckiwi31 5:63063a9fa51c 464 }
cnckiwi31 5:63063a9fa51c 465
cnckiwi31 5:63063a9fa51c 466 for (int i=0; i<sensors::kNumJoints; ++i)
cnckiwi31 5:63063a9fa51c 467 {
cnckiwi31 5:63063a9fa51c 468 string jointName = sensors::kJointNames[i];
cnckiwi31 5:63063a9fa51c 469 jointName = jointName + " (deg)";
cnckiwi31 5:63063a9fa51c 470 Bench::pc.printf("\t%15s %7.2f\r\n",jointName, getDegrees(i));
cnckiwi31 5:63063a9fa51c 471 }
cnckiwi31 5:63063a9fa51c 472 Bench::pc.printf("\t%15s %7.2f\r\n","Force (N)", getForce());
cnckiwi31 5:63063a9fa51c 473
cnckiwi31 5:63063a9fa51c 474 Bench::pc.printf("\t%15s %7.2f\r\n","Pressure0 (kPa)", getPressure0()*100);
cnckiwi31 5:63063a9fa51c 475 }
cnckiwi31 5:63063a9fa51c 476 }
cnckiwi31 5:63063a9fa51c 477
cnckiwi31 5:63063a9fa51c 478 /**
cnckiwi31 5:63063a9fa51c 479 * Prints the user choices to be made
cnckiwi31 5:63063a9fa51c 480 */
cnckiwi31 5:63063a9fa51c 481 void Bench::PrintMenu()
cnckiwi31 5:63063a9fa51c 482 {
cnckiwi31 5:63063a9fa51c 483 Bench::pc.printf("\r\nMENU\r\n");
cnckiwi31 5:63063a9fa51c 484 Bench::pc.printf("\t> Press SW2 to toggle printing leg status\r\n");
cnckiwi31 5:63063a9fa51c 485 Bench::pc.printf("\t> Press SW3 to toggle data logging\r\n");
cnckiwi31 5:63063a9fa51c 486
cnckiwi31 5:63063a9fa51c 487 Bench::pc.printf("\tSD card detected? %9s\r\n",sd_card_present?"Yes":"No");
cnckiwi31 5:63063a9fa51c 488 if(sd_card_present)
cnckiwi31 5:63063a9fa51c 489 {
cnckiwi31 5:63063a9fa51c 490 Bench::pc.printf("\tFile number %15i\r\n", fname_prepend);
cnckiwi31 5:63063a9fa51c 491 Bench::pc.printf("\tLogging? %18s\r\n",is_logging?"Yes":"No");
cnckiwi31 5:63063a9fa51c 492 }
cnckiwi31 5:63063a9fa51c 493 }
cnckiwi31 5:63063a9fa51c 494
cnckiwi31 5:63063a9fa51c 495 /**
cnckiwi31 5:63063a9fa51c 496 * Stops the Bench class from printing data if it is
cnckiwi31 5:63063a9fa51c 497 */
cnckiwi31 5:63063a9fa51c 498 void Bench::pausePrint()
cnckiwi31 5:63063a9fa51c 499 {
cnckiwi31 5:63063a9fa51c 500 was_printing = is_printing;
cnckiwi31 5:63063a9fa51c 501 is_printing = false;
cnckiwi31 5:63063a9fa51c 502 }
cnckiwi31 5:63063a9fa51c 503
cnckiwi31 5:63063a9fa51c 504 /**
cnckiwi31 5:63063a9fa51c 505 * Resumes printing in the Bench class
cnckiwi31 5:63063a9fa51c 506 */
cnckiwi31 5:63063a9fa51c 507 void Bench::resumePrint()
cnckiwi31 5:63063a9fa51c 508 {
cnckiwi31 5:63063a9fa51c 509 is_printing = was_printing;
cnckiwi31 5:63063a9fa51c 510 }
cnckiwi31 5:63063a9fa51c 511
cnckiwi31 5:63063a9fa51c 512 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
cnckiwi31 5:63063a9fa51c 513 // IMPLEMENTATION USER IO
cnckiwi31 5:63063a9fa51c 514 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
cnckiwi31 5:63063a9fa51c 515
cnckiwi31 5:63063a9fa51c 516 /**
cnckiwi31 6:02507d7a6f51 517 * SW2 toggles printing of data
cnckiwi31 5:63063a9fa51c 518 */
cnckiwi31 5:63063a9fa51c 519 void Bench::TogglePrinting()
cnckiwi31 5:63063a9fa51c 520 {
cnckiwi31 5:63063a9fa51c 521 if (not is_printing) {
cnckiwi31 5:63063a9fa51c 522 is_printing = true;
cnckiwi31 5:63063a9fa51c 523 } else {
cnckiwi31 5:63063a9fa51c 524 is_printing = false;
cnckiwi31 5:63063a9fa51c 525 PrintMenu();
cnckiwi31 5:63063a9fa51c 526 }
cnckiwi31 5:63063a9fa51c 527 was_printing = is_printing;
cnckiwi31 5:63063a9fa51c 528 }
cnckiwi31 5:63063a9fa51c 529
cnckiwi31 5:63063a9fa51c 530 /*
cnckiwi31 6:02507d7a6f51 531 * SW3 toggles logging of data
cnckiwi31 5:63063a9fa51c 532 */
cnckiwi31 5:63063a9fa51c 533 void Bench::ToggleLogging()
cnckiwi31 5:63063a9fa51c 534 {
cnckiwi31 5:63063a9fa51c 535 if (not is_logging) {
cnckiwi31 5:63063a9fa51c 536 StartLogging();
cnckiwi31 5:63063a9fa51c 537 } else {
cnckiwi31 5:63063a9fa51c 538 is_logging = false;
cnckiwi31 5:63063a9fa51c 539 StopLogging();
cnckiwi31 5:63063a9fa51c 540 }
cnckiwi31 5:63063a9fa51c 541 PrintMenu();
cnckiwi31 5:63063a9fa51c 542 }
cnckiwi31 5:63063a9fa51c 543
cnckiwi31 5:63063a9fa51c 544 /*
cnckiwi31 5:63063a9fa51c 545 * Indicates if we are now data logging
cnckiwi31 6:02507d7a6f51 546 * return: true if logging
cnckiwi31 5:63063a9fa51c 547 */
cnckiwi31 5:63063a9fa51c 548 bool Bench::isLogging()
cnckiwi31 5:63063a9fa51c 549 {
cnckiwi31 5:63063a9fa51c 550 return is_logging;
cnckiwi31 5:63063a9fa51c 551 }
cnckiwi31 5:63063a9fa51c 552