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@11:fc82dd22a527, 2020-08-12 (annotated)

Committer:
surajgiri
Date:
Wed Aug 12 12:05:58 2020 +0000
Revision:
11:fc82dd22a527
Parent:
9:60760807834f
Child:
15:a84d54e25775
with benchmark v1

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
surajgiri 11:fc82dd22a527 14 * @param p_laDir: digital output for linear actuator direction (retract/extend)
surajgiri 11:fc82dd22a527 15 * @param p_laPwm: pwm output for linear actuator speed
cnckiwi31 5:63063a9fa51c 16 * @param mosi: mosi pin for sd card
cnckiwi31 5:63063a9fa51c 17 * @param miso: miso pin for sd card
cnckiwi31 5:63063a9fa51c 18 * @param sck: clock pin for sd card
cnckiwi31 5:63063a9fa51c 19 * @param cs: chip select pin for sd card
cnckiwi31 5:63063a9fa51c 20 * @param tx: serial transmission line
cnckiwi31 5:63063a9fa51c 21 * @param rx: serial receive line
megrootens 0:3855d4588f76 22 */
megrootens 0:3855d4588f76 23 Bench::Bench(PinName mosi, PinName miso, PinName sck, PinName cs,
surajgiri 11:fc82dd22a527 24 bool use5kNLoadCell,PinName p_lcm101, PinName p_spte0, PinName p_spte1, PinName p_valve,PinName p_laDir, PinName p_laPwm,
cnckiwi31 5:63063a9fa51c 25 PinName sd_mosi, PinName sd_miso, PinName sd_sck, PinName sd_cs,
surajgiri 11:fc82dd22a527 26 PinName tx, PinName rx) :
cnckiwi31 5:63063a9fa51c 27 pc(tx,rx),
megrootens 0:3855d4588f76 28 as5048_(mosi, miso, sck, cs, sensors::kNumJoints),
cnckiwi31 5:63063a9fa51c 29 loadCell5kN(p_lcm101, sensors::kGen5kNOffset, sensors::kGen5kNFactor),
cnckiwi31 5:63063a9fa51c 30 loadCell1kN(p_lcm101, sensors::kLcm101Offset, sensors::kLcm101Factor),
cnckiwi31 5:63063a9fa51c 31 use5kN(use5kNLoadCell),
cnckiwi31 4:1cdce6c6c94e 32 spte0(p_spte0, sensors::kSPTE0Offset, sensors::kSPTE0Factor),
cnckiwi31 5:63063a9fa51c 33 spte1(p_spte1, sensors::kSPTE1Offset, sensors::kSPTE1Factor),
cnckiwi31 5:63063a9fa51c 34 valveFesto(p_valve),
surajgiri 11:fc82dd22a527 35 LinAct(p_laDir,p_laPwm),
cnckiwi31 5:63063a9fa51c 36 sd(sd_mosi, sd_miso, sd_sck, sd_cs, "sd")
cnckiwi31 5:63063a9fa51c 37 {
cnckiwi31 5:63063a9fa51c 38 //init serial things
cnckiwi31 5:63063a9fa51c 39 pc.baud(timing::kSerialBaudrate);//set the serial rate
cnckiwi31 5:63063a9fa51c 40
cnckiwi31 5:63063a9fa51c 41 sd_card_present = false;
cnckiwi31 5:63063a9fa51c 42 fname_prepend = 0;
cnckiwi31 5:63063a9fa51c 43 is_logging = false;
cnckiwi31 5:63063a9fa51c 44 is_printing = false;
cnckiwi31 5:63063a9fa51c 45 was_printing = false;
cnckiwi31 5:63063a9fa51c 46
cnckiwi31 5:63063a9fa51c 47 usedExtraCols = 0;
cnckiwi31 5:63063a9fa51c 48
cnckiwi31 5:63063a9fa51c 49 firstReadMS = 0; //first timer value read
cnckiwi31 5:63063a9fa51c 50 startedLogging = false; //in the middle of a logging cycle
cnckiwi31 5:63063a9fa51c 51
surajgiri 11:fc82dd22a527 52 keyIntOffset = timing::minKeyInt;//any right keypress is ok at the start
surajgiri 11:fc82dd22a527 53 k = '\0'; //keyboard value starts off as a null value
surajgiri 11:fc82dd22a527 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 7:edb876c98565 67 * Prints a welcome message and information on the logging frequency and sensor
cnckiwi31 7:edb876c98565 68 * reading rate
cnckiwi31 5:63063a9fa51c 69 */
cnckiwi31 5:63063a9fa51c 70 void Bench::initialise()
cnckiwi31 5:63063a9fa51c 71 {
cnckiwi31 5:63063a9fa51c 72 //setup the timing
cnckiwi31 5:63063a9fa51c 73 tick_update.attach_us(this,&Bench::update,timing::kTimeControlUs);
cnckiwi31 7:edb876c98565 74
cnckiwi31 5:63063a9fa51c 75 // set rate at which data is printed
cnckiwi31 5:63063a9fa51c 76 tick_serial.attach_us(this,&Bench::PrintStatus,timing::kTimeSerialPrintUs);
cnckiwi31 5:63063a9fa51c 77
surajgiri 11:fc82dd22a527 78 //setup the timer to read key presses
surajgiri 11:fc82dd22a527 79 Bench::pc.attach(this,&Bench::ToggleState,MODSERIAL::RxIrq);
cnckiwi31 5:63063a9fa51c 80
cnckiwi31 5:63063a9fa51c 81 //display welcome message
cnckiwi31 5:63063a9fa51c 82 pc.printf("\r\n**Hello!**\r\n");
cnckiwi31 5:63063a9fa51c 83 pc.printf("Version: 3/12/2019 - 00:00\r\n\n");
cnckiwi31 5:63063a9fa51c 84
cnckiwi31 5:63063a9fa51c 85 Bench::pc.printf("5kN load cell? %s\r\n\n",sensors::use5kN?"Yes":"No");
cnckiwi31 5:63063a9fa51c 86
cnckiwi31 5:63063a9fa51c 87 pc.printf("Bench update rate (Hz): %i\r\n",timing::TimeControlHertz);
cnckiwi31 5:63063a9fa51c 88 pc.printf("Logging rate (Hz): %i\r\n\n",(int)loggingHz);
cnckiwi31 5:63063a9fa51c 89
cnckiwi31 5:63063a9fa51c 90 //startup the SD card
cnckiwi31 5:63063a9fa51c 91 InitSdCard();
cnckiwi31 5:63063a9fa51c 92
cnckiwi31 5:63063a9fa51c 93 //Display the menu
cnckiwi31 5:63063a9fa51c 94 PrintMenu();
cnckiwi31 5:63063a9fa51c 95 }
cnckiwi31 5:63063a9fa51c 96
cnckiwi31 5:63063a9fa51c 97 /**
cnckiwi31 5:63063a9fa51c 98 * Sets the rate at which data is logged in an experiment
cnckiwi31 5:63063a9fa51c 99 * @param hertz: logging frequency in Hz
cnckiwi31 5:63063a9fa51c 100 */
cnckiwi31 5:63063a9fa51c 101 void Bench::setLoggingFrequency(float hertz)
cnckiwi31 5:63063a9fa51c 102 {
cnckiwi31 5:63063a9fa51c 103 //set data logging frequency
cnckiwi31 5:63063a9fa51c 104 if (hertz > 0) {
cnckiwi31 5:63063a9fa51c 105 loggingUS = 1000000/hertz;
cnckiwi31 5:63063a9fa51c 106 loggingHz = hertz;
cnckiwi31 5:63063a9fa51c 107 } else {
cnckiwi31 5:63063a9fa51c 108 loggingUS = timing::kTimeLogDataUs;
cnckiwi31 5:63063a9fa51c 109 loggingHz = (int)(1000000/timing::kTimeLogDataUs);
megrootens 0:3855d4588f76 110 }
megrootens 0:3855d4588f76 111 }
megrootens 0:3855d4588f76 112
megrootens 0:3855d4588f76 113 /**
cnckiwi31 5:63063a9fa51c 114 * Sets names of extra columns of data to be recorded
cnckiwi31 5:63063a9fa51c 115 * @param extraColumnNames: string array of the column names
cnckiwi31 5:63063a9fa51c 116 * @param numCols: number of elements in the string array
cnckiwi31 5:63063a9fa51c 117 */
cnckiwi31 5:63063a9fa51c 118 void Bench::setExtraColumns(string extraColumnNames[], int numCols)
cnckiwi31 5:63063a9fa51c 119 {
cnckiwi31 5:63063a9fa51c 120 usedExtraCols = numCols;
cnckiwi31 5:63063a9fa51c 121 //pc.printf("Length: %i\r\n",usedExtraCols);
cnckiwi31 5:63063a9fa51c 122 // save the names
cnckiwi31 5:63063a9fa51c 123 for(int i=0; i<maxCols; i++) {
cnckiwi31 5:63063a9fa51c 124 if (i<numCols) {
cnckiwi31 5:63063a9fa51c 125 extraColNames[i] = extraColumnNames[i];
cnckiwi31 5:63063a9fa51c 126 //pc.printf("\r\nS%d: %s\r\n",i,extraColNames[i]);
cnckiwi31 5:63063a9fa51c 127 } else {
cnckiwi31 5:63063a9fa51c 128 extraColNames[i] = ""; //less columns than the max possible are filled with a null space
cnckiwi31 5:63063a9fa51c 129 }
cnckiwi31 5:63063a9fa51c 130 }
cnckiwi31 5:63063a9fa51c 131
cnckiwi31 5:63063a9fa51c 132 }
cnckiwi31 5:63063a9fa51c 133
cnckiwi31 5:63063a9fa51c 134 /**
cnckiwi31 5:63063a9fa51c 135 * Sets values of extra columns of data to be recorded
cnckiwi31 5:63063a9fa51c 136 * @param data: data to be logged with the extra columns
cnckiwi31 5:63063a9fa51c 137 */
cnckiwi31 5:63063a9fa51c 138 void Bench::setExtraData(float data[])
cnckiwi31 5:63063a9fa51c 139 {
cnckiwi31 5:63063a9fa51c 140 for (int i=0; i<usedExtraCols; i++) {
cnckiwi31 5:63063a9fa51c 141 extraColValues[i] = data[i];
cnckiwi31 5:63063a9fa51c 142 //pc.printf("\r\nD%d: %f\r\n",i,extraColValues[i]);
cnckiwi31 5:63063a9fa51c 143 }
cnckiwi31 5:63063a9fa51c 144
cnckiwi31 5:63063a9fa51c 145 }
cnckiwi31 5:63063a9fa51c 146
cnckiwi31 6:02507d7a6f51 147 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
cnckiwi31 6:02507d7a6f51 148 // IMPLEMENTATION HARDWARE INTERFACE
cnckiwi31 6:02507d7a6f51 149 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
cnckiwi31 6:02507d7a6f51 150
cnckiwi31 5:63063a9fa51c 151 /**
cnckiwi31 7:edb876c98565 152 * Update routine for the test rig
megrootens 0:3855d4588f76 153 * - Updates the angle buffer of the sensor array
megrootens 0:3855d4588f76 154 * - ... that's it for now (add filtering?)
cnckiwi31 7:edb876c98565 155 * Note that angles lag one update() behind, due to the way the SPI
megrootens 0:3855d4588f76 156 * protocol works.
megrootens 0:3855d4588f76 157 */
cnckiwi31 5:63063a9fa51c 158 void Bench::update()
megrootens 0:3855d4588f76 159 {
megrootens 0:3855d4588f76 160 as5048_.UpdateAngleBuffer();
megrootens 0:3855d4588f76 161 }
megrootens 0:3855d4588f76 162
cnckiwi31 6:02507d7a6f51 163 /**
cnckiwi31 7:edb876c98565 164 * Obtain the joint angle in degrees
cnckiwi31 6:02507d7a6f51 165 * These are the angles at the time of two Update() calls back
cnckiwi31 6:02507d7a6f51 166 * @param joint: the joint for which the angle is requested (as enumerated in
cnckiwi31 6:02507d7a6f51 167 * class header - TOES, KNEE, ANKLE, HIP
cnckiwi31 6:02507d7a6f51 168 * @return: joint angle
cnckiwi31 6:02507d7a6f51 169 */
cnckiwi31 6:02507d7a6f51 170 float Bench::getDegrees(Joint joint)
cnckiwi31 6:02507d7a6f51 171 {
cnckiwi31 6:02507d7a6f51 172 return getDegrees(joint);
cnckiwi31 6:02507d7a6f51 173 }
cnckiwi31 6:02507d7a6f51 174
cnckiwi31 6:02507d7a6f51 175 /**
cnckiwi31 7:edb876c98565 176 * Obtain the joint angle in degrees
cnckiwi31 6:02507d7a6f51 177 * These are the angles at the time of two Update() calls back
cnckiwi31 6:02507d7a6f51 178 * @param joint: the joint for which the angle is requested (a number starting
cnckiwi31 6:02507d7a6f51 179 * from 0 indicating the position in the sensor daisy chain)
cnckiwi31 6:02507d7a6f51 180 * @return: joint angle
cnckiwi31 6:02507d7a6f51 181 */
megrootens 0:3855d4588f76 182 float Bench::getDegrees(int i_joint)
megrootens 0:3855d4588f76 183 {
megrootens 0:3855d4588f76 184 float ang = as5048_.getAngleDegrees(i_joint);
megrootens 0:3855d4588f76 185 if (ang>kCutOffDegrees) {
megrootens 0:3855d4588f76 186 return ang-As5048::kDegPerRev;
megrootens 0:3855d4588f76 187 }
megrootens 0:3855d4588f76 188 return ang;
megrootens 0:3855d4588f76 189 }
megrootens 0:3855d4588f76 190
cnckiwi31 6:02507d7a6f51 191 /**
cnckiwi31 7:edb876c98565 192 * Obtain the joint angle in radians
cnckiwi31 6:02507d7a6f51 193 * These are the angles at the time of two Update() calls back
cnckiwi31 6:02507d7a6f51 194 * @param joint: the joint for which the angle is requested (as enumerated in
cnckiwi31 6:02507d7a6f51 195 * class header - TOES, KNEE, ANKLE, HIP
cnckiwi31 6:02507d7a6f51 196 * @return: joint angle
cnckiwi31 6:02507d7a6f51 197 */
cnckiwi31 6:02507d7a6f51 198 float Bench::getRadians(Joint joint)
cnckiwi31 6:02507d7a6f51 199 {
cnckiwi31 6:02507d7a6f51 200 return getRadians(joint);
cnckiwi31 6:02507d7a6f51 201 }
cnckiwi31 6:02507d7a6f51 202
cnckiwi31 6:02507d7a6f51 203 /**
cnckiwi31 7:edb876c98565 204 * Obtain the joint angle in radians
cnckiwi31 6:02507d7a6f51 205 * These are the angles at the time of two Update() calls back
cnckiwi31 6:02507d7a6f51 206 * @param joint: the joint for which the angle is requested (a number starting
cnckiwi31 6:02507d7a6f51 207 * from 0 indicating the position in the sensor daisy chain)
cnckiwi31 6:02507d7a6f51 208 * @return: joint angle
cnckiwi31 6:02507d7a6f51 209 */
cnckiwi31 6:02507d7a6f51 210 float Bench::getRadians(int i_joint)
cnckiwi31 6:02507d7a6f51 211 {
cnckiwi31 6:02507d7a6f51 212 float ang = as5048_.getAngleRadians(i_joint);
cnckiwi31 6:02507d7a6f51 213 if (ang>kCutOffRadians) {
cnckiwi31 6:02507d7a6f51 214 return ang-As5048::kRadPerRev;
cnckiwi31 6:02507d7a6f51 215 }
cnckiwi31 6:02507d7a6f51 216 return ang;
cnckiwi31 6:02507d7a6f51 217 }
cnckiwi31 6:02507d7a6f51 218
cnckiwi31 6:02507d7a6f51 219
cnckiwi31 6:02507d7a6f51 220 const char* Bench::getJointName(int i_joint)
cnckiwi31 6:02507d7a6f51 221 {
cnckiwi31 6:02507d7a6f51 222 return sensors::kJointNames[i_joint];
cnckiwi31 6:02507d7a6f51 223 }
cnckiwi31 6:02507d7a6f51 224
cnckiwi31 6:02507d7a6f51 225 const char* Bench::getJointName(Joint joint)
cnckiwi31 6:02507d7a6f51 226 {
cnckiwi31 6:02507d7a6f51 227 return getJointName(joint);
cnckiwi31 6:02507d7a6f51 228 }
cnckiwi31 6:02507d7a6f51 229
cnckiwi31 6:02507d7a6f51 230 As5048* Bench::get_as5048()
cnckiwi31 6:02507d7a6f51 231 {
cnckiwi31 6:02507d7a6f51 232 return &as5048_;
cnckiwi31 6:02507d7a6f51 233 }
cnckiwi31 6:02507d7a6f51 234
cnckiwi31 6:02507d7a6f51 235 /**
cnckiwi31 7:edb876c98565 236 * The force applied vertically at the hip joint in N
cnckiwi31 6:02507d7a6f51 237 * @return: force
cnckiwi31 6:02507d7a6f51 238 */
cnckiwi31 6:02507d7a6f51 239 float Bench::getForce()
cnckiwi31 6:02507d7a6f51 240 {
cnckiwi31 6:02507d7a6f51 241 if (Bench::use5kN) {
cnckiwi31 6:02507d7a6f51 242 return loadCell5kN.getForce();
cnckiwi31 6:02507d7a6f51 243 } else {
cnckiwi31 6:02507d7a6f51 244 return loadCell1kN.getForce();
cnckiwi31 6:02507d7a6f51 245 }
cnckiwi31 6:02507d7a6f51 246
cnckiwi31 6:02507d7a6f51 247 }
cnckiwi31 6:02507d7a6f51 248
cnckiwi31 6:02507d7a6f51 249 void Bench::nullForce()
cnckiwi31 6:02507d7a6f51 250 {
cnckiwi31 6:02507d7a6f51 251 if (use5kN) {
cnckiwi31 6:02507d7a6f51 252 return loadCell5kN.nullForce();
cnckiwi31 6:02507d7a6f51 253 } else {
cnckiwi31 6:02507d7a6f51 254 return loadCell1kN.nullForce();
cnckiwi31 6:02507d7a6f51 255 }
cnckiwi31 6:02507d7a6f51 256 }
cnckiwi31 6:02507d7a6f51 257
cnckiwi31 6:02507d7a6f51 258 /**
cnckiwi31 6:02507d7a6f51 259 * The pressure measured by pressure sensor 0 in bar
cnckiwi31 6:02507d7a6f51 260 * @return: pressure
cnckiwi31 6:02507d7a6f51 261 */
cnckiwi31 6:02507d7a6f51 262 float Bench::getPressure0()
cnckiwi31 6:02507d7a6f51 263 {
cnckiwi31 6:02507d7a6f51 264 return spte0.getPressure();
cnckiwi31 6:02507d7a6f51 265 }
cnckiwi31 6:02507d7a6f51 266
cnckiwi31 6:02507d7a6f51 267 void Bench::nullPressure0()
cnckiwi31 6:02507d7a6f51 268 {
cnckiwi31 6:02507d7a6f51 269 return spte0.nullPressure();
cnckiwi31 6:02507d7a6f51 270 }
cnckiwi31 6:02507d7a6f51 271
cnckiwi31 6:02507d7a6f51 272 float Bench::getPressure1()
cnckiwi31 6:02507d7a6f51 273 {
cnckiwi31 6:02507d7a6f51 274 return spte1.getPressure();
cnckiwi31 6:02507d7a6f51 275 }
cnckiwi31 6:02507d7a6f51 276
cnckiwi31 6:02507d7a6f51 277 void Bench::nullPressure1()
cnckiwi31 6:02507d7a6f51 278 {
cnckiwi31 6:02507d7a6f51 279 return spte1.nullPressure();
cnckiwi31 6:02507d7a6f51 280 }
cnckiwi31 6:02507d7a6f51 281
cnckiwi31 6:02507d7a6f51 282 /**
cnckiwi31 6:02507d7a6f51 283 * Control the valve (true turns the valve on to pressurise, false turns
cnckiwi31 6:02507d7a6f51 284 * the valve off to depressurise)
cnckiwi31 6:02507d7a6f51 285 * @param set: valve state
cnckiwi31 6:02507d7a6f51 286 */
cnckiwi31 6:02507d7a6f51 287 void Bench::setValve(bool set)
cnckiwi31 6:02507d7a6f51 288 {
cnckiwi31 6:02507d7a6f51 289 valveFesto.setValve(set);
cnckiwi31 6:02507d7a6f51 290 }
cnckiwi31 6:02507d7a6f51 291
cnckiwi31 6:02507d7a6f51 292 /**
cnckiwi31 7:edb876c98565 293 * The valve state (true is on/pressurising and false is off/depressurising).
cnckiwi31 6:02507d7a6f51 294 * @return: valve state
cnckiwi31 6:02507d7a6f51 295 */
cnckiwi31 6:02507d7a6f51 296 bool Bench::getValve() {
cnckiwi31 6:02507d7a6f51 297 return valveFesto.getValve();
cnckiwi31 6:02507d7a6f51 298 }
cnckiwi31 6:02507d7a6f51 299
surajgiri 11:fc82dd22a527 300 /**
surajgiri 11:fc82dd22a527 301 * The linear actuator state (true is retracting and false is extending)
surajgiri 11:fc82dd22a527 302 * @return: linear actuator state
surajgiri 11:fc82dd22a527 303 */
surajgiri 11:fc82dd22a527 304 bool Bench::getDir() {
surajgiri 11:fc82dd22a527 305 return LinAct.getDirection();
surajgiri 11:fc82dd22a527 306 }
surajgiri 11:fc82dd22a527 307
surajgiri 11:fc82dd22a527 308 /**
surajgiri 11:fc82dd22a527 309 * Control the linear actuator direction (true retracts and false extends)
surajgiri 11:fc82dd22a527 310 * @param set: direction
surajgiri 11:fc82dd22a527 311 */
surajgiri 11:fc82dd22a527 312 void Bench::setDir(bool dir)
surajgiri 11:fc82dd22a527 313 {
surajgiri 11:fc82dd22a527 314 LinAct.setDirection(dir);
surajgiri 11:fc82dd22a527 315 }
surajgiri 11:fc82dd22a527 316
surajgiri 11:fc82dd22a527 317 /**
surajgiri 11:fc82dd22a527 318 * Control the linear actuator speed (0-100% integer of pwm. generally need
surajgiri 11:fc82dd22a527 319 * above 50% to get it moving)
surajgiri 11:fc82dd22a527 320 * @param pwm: pwm duty cycle percentage
surajgiri 11:fc82dd22a527 321 */
surajgiri 11:fc82dd22a527 322 void Bench::setPWM(int pwm)
surajgiri 11:fc82dd22a527 323 {
surajgiri 11:fc82dd22a527 324 LinAct.setPWM(pwm);
surajgiri 11:fc82dd22a527 325 }
cnckiwi31 5:63063a9fa51c 326 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
cnckiwi31 5:63063a9fa51c 327 // IMPLEMENTATION DATA LOGGING
cnckiwi31 5:63063a9fa51c 328 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
cnckiwi31 5:63063a9fa51c 329
cnckiwi31 5:63063a9fa51c 330 /**
cnckiwi31 5:63063a9fa51c 331 * Check contents of SD card and count files in order
cnckiwi31 7:edb876c98565 332 * to ensure unique file name for logging data.
cnckiwi31 5:63063a9fa51c 333 */
cnckiwi31 5:63063a9fa51c 334 void Bench::InitSdCard()
cnckiwi31 5:63063a9fa51c 335 {
cnckiwi31 5:63063a9fa51c 336 pc.printf("\r\nINITIALISING SD CARD\r\n");
cnckiwi31 5:63063a9fa51c 337
cnckiwi31 5:63063a9fa51c 338 int num_files = 0;
cnckiwi31 5:63063a9fa51c 339
cnckiwi31 5:63063a9fa51c 340 // scan dir
cnckiwi31 5:63063a9fa51c 341 DIR *d;
cnckiwi31 5:63063a9fa51c 342 struct dirent *p;
cnckiwi31 5:63063a9fa51c 343
cnckiwi31 5:63063a9fa51c 344 d = opendir("/sd");
cnckiwi31 5:63063a9fa51c 345 if (d != NULL) {
cnckiwi31 5:63063a9fa51c 346 sd_card_present = true;
cnckiwi31 5:63063a9fa51c 347
cnckiwi31 5:63063a9fa51c 348 pc.printf("\t> Contents of SD Card:\r\n");
cnckiwi31 5:63063a9fa51c 349 while ((p = readdir(d)) != NULL) {
cnckiwi31 5:63063a9fa51c 350 if (p->d_name[0] != '.') {
cnckiwi31 5:63063a9fa51c 351 // skip files starting with '.'
cnckiwi31 5:63063a9fa51c 352 pc.printf("\t %s",p->d_name);
cnckiwi31 5:63063a9fa51c 353 ++num_files;
cnckiwi31 5:63063a9fa51c 354 }
cnckiwi31 5:63063a9fa51c 355 }
cnckiwi31 5:63063a9fa51c 356 pc.printf("\r\n\t> Counted %d visible files.\r\n",num_files);
cnckiwi31 5:63063a9fa51c 357
cnckiwi31 5:63063a9fa51c 358 closedir(d);
cnckiwi31 5:63063a9fa51c 359 } else {
cnckiwi31 5:63063a9fa51c 360 sd_card_present = false;
cnckiwi31 5:63063a9fa51c 361
cnckiwi31 5:63063a9fa51c 362 pc.printf("\t> No SD Card present. Data cannot be logged.\r\n");
cnckiwi31 5:63063a9fa51c 363 }
cnckiwi31 5:63063a9fa51c 364
cnckiwi31 5:63063a9fa51c 365 // id to be appended to logged data files
cnckiwi31 5:63063a9fa51c 366 fname_prepend = num_files;
cnckiwi31 5:63063a9fa51c 367 }
cnckiwi31 5:63063a9fa51c 368
cnckiwi31 5:63063a9fa51c 369 /**
cnckiwi31 5:63063a9fa51c 370 * Start logging data
cnckiwi31 6:02507d7a6f51 371 * param fname_append: a string describing the name appended to each logged file
cnckiwi31 7:edb876c98565 372 * (along with its unique loggging number).
cnckiwi31 5:63063a9fa51c 373 */
cnckiwi31 5:63063a9fa51c 374 void Bench::StartLogging(const char * fname_append)
cnckiwi31 5:63063a9fa51c 375 {
cnckiwi31 5:63063a9fa51c 376 pc.printf("\r\nDATA LOGGING");
cnckiwi31 5:63063a9fa51c 377 if (sd_card_present) {
cnckiwi31 5:63063a9fa51c 378
cnckiwi31 5:63063a9fa51c 379 // create unique file name
cnckiwi31 5:63063a9fa51c 380 ++fname_prepend;
cnckiwi31 5:63063a9fa51c 381 char fname[50];
cnckiwi31 5:63063a9fa51c 382 sprintf(fname, "/sd/%d_%s.csv",fname_prepend,fname_append);
cnckiwi31 5:63063a9fa51c 383
cnckiwi31 5:63063a9fa51c 384 pc.printf("\t> Opening data log file '%s'...\r\n",fname);
cnckiwi31 5:63063a9fa51c 385
cnckiwi31 5:63063a9fa51c 386 // open file for writing and start logging after success
cnckiwi31 5:63063a9fa51c 387 fp_data = fopen(fname,"w");
cnckiwi31 5:63063a9fa51c 388 if (fp_data==NULL) {
cnckiwi31 5:63063a9fa51c 389 pc.printf("\t> ERROR: failed to open log file (t=%d ms)\r\n",
cnckiwi31 5:63063a9fa51c 390 timer.read_ms());
cnckiwi31 5:63063a9fa51c 391 } else {
cnckiwi31 5:63063a9fa51c 392 string fHeader = "time (s),theta_knee (deg),force (N),pressure (kPa)";
cnckiwi31 5:63063a9fa51c 393
cnckiwi31 5:63063a9fa51c 394 for (int i=0; i<usedExtraCols; i++) {
cnckiwi31 5:63063a9fa51c 395 if (extraColNames[i] != "") {
cnckiwi31 5:63063a9fa51c 396 fHeader = fHeader + "," + extraColNames[i];
cnckiwi31 5:63063a9fa51c 397 }
cnckiwi31 5:63063a9fa51c 398 }
cnckiwi31 5:63063a9fa51c 399 //pc.printf("%s", fHeader.c_str());
cnckiwi31 5:63063a9fa51c 400 fprintf(fp_data, "%s", fHeader.c_str());
surajgiri 11:fc82dd22a527 401 //tick_logging.attach_us(this,&Bench::LogData,loggingUS);
cnckiwi31 5:63063a9fa51c 402 timer.start();
cnckiwi31 5:63063a9fa51c 403 startedLogging = true;
cnckiwi31 5:63063a9fa51c 404
surajgiri 11:fc82dd22a527 405 //pc.printf("\t> Logging started.\r\n");
cnckiwi31 5:63063a9fa51c 406
cnckiwi31 5:63063a9fa51c 407 is_logging = true;
cnckiwi31 5:63063a9fa51c 408 }
cnckiwi31 5:63063a9fa51c 409 } else {
cnckiwi31 5:63063a9fa51c 410 pc.printf("\t> No SD Card; no data will be logged.\r\n");
cnckiwi31 5:63063a9fa51c 411 startedLogging = false;
cnckiwi31 5:63063a9fa51c 412 }
cnckiwi31 5:63063a9fa51c 413 }
cnckiwi31 5:63063a9fa51c 414
surajgiri 11:fc82dd22a527 415 /**
surajgiri 11:fc82dd22a527 416 * Restart logging data (opens data logging file)
surajgiri 11:fc82dd22a527 417 * @param fname_append: a string describing the name appended to each logged file
surajgiri 11:fc82dd22a527 418 * (along with its unique loggging number).
surajgiri 11:fc82dd22a527 419 */
surajgiri 11:fc82dd22a527 420 void Bench::RestartLogging(const char * fname_append)
surajgiri 11:fc82dd22a527 421 {
surajgiri 11:fc82dd22a527 422 if (sd_card_present) {
surajgiri 11:fc82dd22a527 423
surajgiri 11:fc82dd22a527 424 // file name carries on from what we last had
surajgiri 11:fc82dd22a527 425 char fname[50];
surajgiri 11:fc82dd22a527 426 sprintf(fname, "/sd/%d_%s.csv",fname_prepend,fname_append);
surajgiri 11:fc82dd22a527 427
surajgiri 11:fc82dd22a527 428 pc.printf("\t> Opening data log file '%s'...\r\n",fname);
surajgiri 11:fc82dd22a527 429
surajgiri 11:fc82dd22a527 430 // open file for writing and start logging after success
surajgiri 11:fc82dd22a527 431 fp_data = fopen(fname,"w");
surajgiri 11:fc82dd22a527 432 if (fp_data==NULL) {
surajgiri 11:fc82dd22a527 433 pc.printf("\t> ERROR: failed to open log file (t=%d ms)\r\n",
surajgiri 11:fc82dd22a527 434 timer.read_ms());
surajgiri 11:fc82dd22a527 435 } else {
surajgiri 11:fc82dd22a527 436 timer.start();
surajgiri 11:fc82dd22a527 437 startedLogging = true;
surajgiri 11:fc82dd22a527 438
surajgiri 11:fc82dd22a527 439 pc.printf("\t> Logging file opened.\r\n");
surajgiri 11:fc82dd22a527 440
surajgiri 11:fc82dd22a527 441 is_logging = true;
surajgiri 11:fc82dd22a527 442 }
surajgiri 11:fc82dd22a527 443 } else {
surajgiri 11:fc82dd22a527 444 pc.printf("\t> No SD Card; no data will be logged.\r\n");
surajgiri 11:fc82dd22a527 445 startedLogging = false;
surajgiri 11:fc82dd22a527 446 }
surajgiri 11:fc82dd22a527 447 }
cnckiwi31 5:63063a9fa51c 448
cnckiwi31 5:63063a9fa51c 449 /**
cnckiwi31 7:edb876c98565 450 * Stop logging data.
cnckiwi31 5:63063a9fa51c 451 */
cnckiwi31 5:63063a9fa51c 452 void Bench::StopLogging()
cnckiwi31 5:63063a9fa51c 453 {
cnckiwi31 5:63063a9fa51c 454 Bench::pc.printf("\r\nDATA LOGGING:");
cnckiwi31 5:63063a9fa51c 455 if (sd_card_present) {
cnckiwi31 5:63063a9fa51c 456 // close data file, stop logging
cnckiwi31 5:63063a9fa51c 457 fclose(fp_data);
cnckiwi31 5:63063a9fa51c 458 tick_logging.detach();
cnckiwi31 5:63063a9fa51c 459 timer.stop();
cnckiwi31 5:63063a9fa51c 460 timer.reset();
cnckiwi31 5:63063a9fa51c 461 Bench::pc.printf("\r> Stopped.");
cnckiwi31 5:63063a9fa51c 462 } else {
cnckiwi31 5:63063a9fa51c 463 Bench::pc.printf("\t> No data was logged.");
cnckiwi31 5:63063a9fa51c 464 }
cnckiwi31 5:63063a9fa51c 465
cnckiwi31 5:63063a9fa51c 466 is_logging = false;
cnckiwi31 5:63063a9fa51c 467 }
cnckiwi31 5:63063a9fa51c 468
cnckiwi31 5:63063a9fa51c 469 /**
cnckiwi31 5:63063a9fa51c 470 * Stop logging data and print the menu
cnckiwi31 5:63063a9fa51c 471 */
cnckiwi31 5:63063a9fa51c 472 void Bench::stopLogging()
cnckiwi31 5:63063a9fa51c 473 {
cnckiwi31 5:63063a9fa51c 474 if(is_logging) {
cnckiwi31 5:63063a9fa51c 475 is_logging = false;
cnckiwi31 5:63063a9fa51c 476 StopLogging();
cnckiwi31 5:63063a9fa51c 477 PrintMenu();
cnckiwi31 5:63063a9fa51c 478 }
cnckiwi31 5:63063a9fa51c 479 }
cnckiwi31 5:63063a9fa51c 480
cnckiwi31 5:63063a9fa51c 481 /**
cnckiwi31 5:63063a9fa51c 482 * Log data
cnckiwi31 5:63063a9fa51c 483 */
cnckiwi31 5:63063a9fa51c 484 void Bench::LogData()
cnckiwi31 5:63063a9fa51c 485 {
cnckiwi31 5:63063a9fa51c 486 int currTime = timer.read_ms();
cnckiwi31 5:63063a9fa51c 487 if(startedLogging) {
cnckiwi31 5:63063a9fa51c 488 firstReadMS = currTime;
cnckiwi31 5:63063a9fa51c 489 startedLogging = false;
cnckiwi31 5:63063a9fa51c 490 }
cnckiwi31 5:63063a9fa51c 491 double currTimeS = ((double)currTime - firstReadMS)/1000;
cnckiwi31 5:63063a9fa51c 492
cnckiwi31 5:63063a9fa51c 493 // time
cnckiwi31 5:63063a9fa51c 494 fprintf(fp_data,"\n%f",currTimeS);
cnckiwi31 5:63063a9fa51c 495
cnckiwi31 5:63063a9fa51c 496 // bench: joint angles and force sensor and pressure sensor values
cnckiwi31 5:63063a9fa51c 497 fprintf(fp_data,",%f,%f,%f",
cnckiwi31 5:63063a9fa51c 498 getDegrees(0),
cnckiwi31 5:63063a9fa51c 499 getForce(),
cnckiwi31 5:63063a9fa51c 500 getPressure0()*100
cnckiwi31 5:63063a9fa51c 501 );
cnckiwi31 5:63063a9fa51c 502 for (int i=0; i<usedExtraCols; i++) {
cnckiwi31 5:63063a9fa51c 503 fprintf(fp_data,",%f",extraColValues[i]);
cnckiwi31 5:63063a9fa51c 504 }
cnckiwi31 5:63063a9fa51c 505 }
cnckiwi31 5:63063a9fa51c 506
cnckiwi31 5:63063a9fa51c 507 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
cnckiwi31 5:63063a9fa51c 508 // IMPLEMENTATION SERIAL COM
cnckiwi31 5:63063a9fa51c 509 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
cnckiwi31 5:63063a9fa51c 510 /**
cnckiwi31 5:63063a9fa51c 511 * Prints the values of the key measurements in the setup
cnckiwi31 5:63063a9fa51c 512 */
cnckiwi31 5:63063a9fa51c 513 void Bench::PrintStatus()
cnckiwi31 5:63063a9fa51c 514 {
cnckiwi31 5:63063a9fa51c 515 if (is_printing) {
cnckiwi31 5:63063a9fa51c 516 if(sd_card_present)
cnckiwi31 5:63063a9fa51c 517 {
cnckiwi31 5:63063a9fa51c 518 Bench::pc.printf("\tFile number %15i\r\n", fname_prepend);
cnckiwi31 5:63063a9fa51c 519 Bench::pc.printf("\tLogging? %18s\r\n",is_logging?"Yes":"No");
cnckiwi31 5:63063a9fa51c 520 }
cnckiwi31 5:63063a9fa51c 521
cnckiwi31 5:63063a9fa51c 522 for (int i=0; i<sensors::kNumJoints; ++i)
cnckiwi31 5:63063a9fa51c 523 {
cnckiwi31 5:63063a9fa51c 524 string jointName = sensors::kJointNames[i];
cnckiwi31 5:63063a9fa51c 525 jointName = jointName + " (deg)";
cnckiwi31 5:63063a9fa51c 526 Bench::pc.printf("\t%15s %7.2f\r\n",jointName, getDegrees(i));
cnckiwi31 5:63063a9fa51c 527 }
cnckiwi31 5:63063a9fa51c 528 Bench::pc.printf("\t%15s %7.2f\r\n","Force (N)", getForce());
cnckiwi31 5:63063a9fa51c 529
cnckiwi31 5:63063a9fa51c 530 Bench::pc.printf("\t%15s %7.2f\r\n","Pressure0 (kPa)", getPressure0()*100);
cnckiwi31 5:63063a9fa51c 531 }
cnckiwi31 5:63063a9fa51c 532 }
cnckiwi31 5:63063a9fa51c 533
cnckiwi31 5:63063a9fa51c 534 /**
cnckiwi31 5:63063a9fa51c 535 * Prints the user choices to be made
cnckiwi31 5:63063a9fa51c 536 */
cnckiwi31 5:63063a9fa51c 537 void Bench::PrintMenu()
cnckiwi31 5:63063a9fa51c 538 {
cnckiwi31 5:63063a9fa51c 539 Bench::pc.printf("\r\nMENU\r\n");
surajgiri 11:fc82dd22a527 540 Bench::pc.printf("\t> Press s to toggle printing leg status\r\n");
surajgiri 11:fc82dd22a527 541 Bench::pc.printf("\t> Press l to toggle data logging\r\n");
cnckiwi31 5:63063a9fa51c 542
cnckiwi31 5:63063a9fa51c 543 Bench::pc.printf("\tSD card detected? %9s\r\n",sd_card_present?"Yes":"No");
cnckiwi31 5:63063a9fa51c 544 if(sd_card_present)
cnckiwi31 5:63063a9fa51c 545 {
cnckiwi31 5:63063a9fa51c 546 Bench::pc.printf("\tFile number %15i\r\n", fname_prepend);
cnckiwi31 5:63063a9fa51c 547 Bench::pc.printf("\tLogging? %18s\r\n",is_logging?"Yes":"No");
cnckiwi31 5:63063a9fa51c 548 }
cnckiwi31 5:63063a9fa51c 549 }
cnckiwi31 5:63063a9fa51c 550
cnckiwi31 5:63063a9fa51c 551 /**
cnckiwi31 5:63063a9fa51c 552 * Stops the Bench class from printing data if it is
cnckiwi31 5:63063a9fa51c 553 */
cnckiwi31 5:63063a9fa51c 554 void Bench::pausePrint()
cnckiwi31 5:63063a9fa51c 555 {
cnckiwi31 5:63063a9fa51c 556 was_printing = is_printing;
cnckiwi31 5:63063a9fa51c 557 is_printing = false;
cnckiwi31 5:63063a9fa51c 558 }
cnckiwi31 5:63063a9fa51c 559
cnckiwi31 5:63063a9fa51c 560 /**
cnckiwi31 5:63063a9fa51c 561 * Resumes printing in the Bench class
cnckiwi31 5:63063a9fa51c 562 */
cnckiwi31 5:63063a9fa51c 563 void Bench::resumePrint()
cnckiwi31 5:63063a9fa51c 564 {
cnckiwi31 5:63063a9fa51c 565 is_printing = was_printing;
cnckiwi31 5:63063a9fa51c 566 }
cnckiwi31 5:63063a9fa51c 567
cnckiwi31 5:63063a9fa51c 568 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
cnckiwi31 5:63063a9fa51c 569 // IMPLEMENTATION USER IO
cnckiwi31 5:63063a9fa51c 570 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
cnckiwi31 5:63063a9fa51c 571
cnckiwi31 5:63063a9fa51c 572 /**
surajgiri 11:fc82dd22a527 573 * d toggles printing state of sensors and l toggles logging of data
surajgiri 11:fc82dd22a527 574 * use the keyboard on the pc
surajgiri 11:fc82dd22a527 575 * do not press longer than a second or it will toggle back
surajgiri 11:fc82dd22a527 576 */
surajgiri 11:fc82dd22a527 577 void Bench::ToggleState(MODSERIAL_IRQ_INFO *q)
surajgiri 11:fc82dd22a527 578 {
surajgiri 11:fc82dd22a527 579 //if we have data and it was not too soon since the last data
surajgiri 11:fc82dd22a527 580 if(Bench::pc.readable() && (((Bench::keyIntT.read() + keyIntOffset) >= timing::minKeyInt))) {
surajgiri 11:fc82dd22a527 581 k = Bench::pc.getc();//read the keystroke
surajgiri 11:fc82dd22a527 582 Bench::pc.rxBufferFlush();
surajgiri 11:fc82dd22a527 583 //toggle status of logging
surajgiri 11:fc82dd22a527 584 if(k == 's') {//toggle printing of data
surajgiri 11:fc82dd22a527 585 Bench::pc.printf("\tInput: %c\r\n",k);
surajgiri 11:fc82dd22a527 586 TogglePrinting();
surajgiri 11:fc82dd22a527 587 keyIntT.reset();
surajgiri 11:fc82dd22a527 588 keyIntT.start();
surajgiri 11:fc82dd22a527 589 keyIntOffset = 0;
surajgiri 11:fc82dd22a527 590
surajgiri 11:fc82dd22a527 591 } else if (k == 'l') {
surajgiri 11:fc82dd22a527 592 Bench::pc.printf("\tInput: %c\r\n",k);
surajgiri 11:fc82dd22a527 593 ToggleLogging();
surajgiri 11:fc82dd22a527 594 keyIntT.reset();
surajgiri 11:fc82dd22a527 595 keyIntT.start();
surajgiri 11:fc82dd22a527 596 keyIntOffset = 0;
surajgiri 11:fc82dd22a527 597 }
surajgiri 11:fc82dd22a527 598 k = '\0';
surajgiri 11:fc82dd22a527 599 }
surajgiri 11:fc82dd22a527 600 }
surajgiri 11:fc82dd22a527 601
surajgiri 11:fc82dd22a527 602 /**
surajgiri 11:fc82dd22a527 603 * toggles printing of data
cnckiwi31 5:63063a9fa51c 604 */
cnckiwi31 5:63063a9fa51c 605 void Bench::TogglePrinting()
cnckiwi31 5:63063a9fa51c 606 {
cnckiwi31 5:63063a9fa51c 607 if (not is_printing) {
cnckiwi31 5:63063a9fa51c 608 is_printing = true;
cnckiwi31 5:63063a9fa51c 609 } else {
cnckiwi31 5:63063a9fa51c 610 is_printing = false;
cnckiwi31 5:63063a9fa51c 611 PrintMenu();
cnckiwi31 5:63063a9fa51c 612 }
cnckiwi31 5:63063a9fa51c 613 was_printing = is_printing;
cnckiwi31 5:63063a9fa51c 614 }
cnckiwi31 5:63063a9fa51c 615
cnckiwi31 5:63063a9fa51c 616 /*
surajgiri 11:fc82dd22a527 617 * toggles logging of data
cnckiwi31 5:63063a9fa51c 618 */
cnckiwi31 5:63063a9fa51c 619 void Bench::ToggleLogging()
cnckiwi31 5:63063a9fa51c 620 {
cnckiwi31 5:63063a9fa51c 621 if (not is_logging) {
surajgiri 11:fc82dd22a527 622 is_logging = true;
cnckiwi31 5:63063a9fa51c 623 } else {
cnckiwi31 5:63063a9fa51c 624 is_logging = false;
cnckiwi31 5:63063a9fa51c 625 }
surajgiri 11:fc82dd22a527 626 //PrintMenu();
cnckiwi31 5:63063a9fa51c 627 }
cnckiwi31 5:63063a9fa51c 628
cnckiwi31 8:bc467f5fe7c3 629 /**
cnckiwi31 5:63063a9fa51c 630 * Indicates if we are now data logging
cnckiwi31 9:60760807834f 631 * @return: true if logging
cnckiwi31 5:63063a9fa51c 632 */
cnckiwi31 5:63063a9fa51c 633 bool Bench::isLogging()
cnckiwi31 5:63063a9fa51c 634 {
cnckiwi31 5:63063a9fa51c 635 return is_logging;
cnckiwi31 5:63063a9fa51c 636 }
cnckiwi31 5:63063a9fa51c 637