Allan Veale / Mbed 2 deprecated heros_leg_readout_torque_addition

HeRoS: read out and log joint angles and force sensor data from the leg test bench. Now with additional features to read pressure sensors and set the null values of the pressure and force sensors

Dependencies:   SPTE_10Bar_5V mbed AS5048 SDFileSystem MODSERIAL PinDetect Valve LCM101

Fork of heros_leg_readout_torque_addition by K K

main.cpp@10:77fcbad99a31, 2020-02-17 (annotated)

Committer:
cnckiwi31
Date:
Mon Feb 17 15:15:24 2020 +0000
Revision:
10:77fcbad99a31
Parent:
5:63063a9fa51c
Child:
11:b0a6faaa5e9f
Latest commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cnckiwi31 5:63063a9fa51c 1 /**
cnckiwi31 5:63063a9fa51c 2 * Author: Allan Veale
cnckiwi31 5:63063a9fa51c 3 * Date: 27/11/19
cnckiwi31 5:63063a9fa51c 4 * Purpose: Datalog from the active wearable test rig fitted with the first
cnckiwi31 5:63063a9fa51c 5 * realistic (foam tissue) leg
cnckiwi31 5:63063a9fa51c 6 */
megrootens 0:3855d4588f76 7
cnckiwi31 5:63063a9fa51c 8 //Both the general mbed header and the test rig bench header are needed
cnckiwi31 5:63063a9fa51c 9 #include "mbed.h"
cnckiwi31 5:63063a9fa51c 10 #include "bench.h"
megrootens 0:3855d4588f76 11
cnckiwi31 10:77fcbad99a31 12
cnckiwi31 5:63063a9fa51c 13 //Example experiment method
cnckiwi31 5:63063a9fa51c 14 void runDemoExperiment0(int cycles, float targetkPa);
megrootens 0:3855d4588f76 15
cnckiwi31 10:77fcbad99a31 16 //Methods for testing DAC chip - leave for now
cnckiwi31 10:77fcbad99a31 17 void selectDACB();
cnckiwi31 10:77fcbad99a31 18 void deselectDACB();
cnckiwi31 10:77fcbad99a31 19
cnckiwi31 10:77fcbad99a31 20 void testDAC();
cnckiwi31 10:77fcbad99a31 21 void testToggleChannel();
cnckiwi31 10:77fcbad99a31 22
cnckiwi31 10:77fcbad99a31 23 DigitalOut myled(LED1);
cnckiwi31 10:77fcbad99a31 24 DigitalOut CSA(DAC_CSA);
cnckiwi31 10:77fcbad99a31 25 DigitalOut CSB(DAC_CSB);
cnckiwi31 10:77fcbad99a31 26 SPI spi(DAC_MOSI,DAC_MISO,DAC_SCLK);
cnckiwi31 10:77fcbad99a31 27
cnckiwi31 5:63063a9fa51c 28 // Create bench object - this is used to control the test rig
cnckiwi31 5:63063a9fa51c 29 Bench leg;
megrootens 0:3855d4588f76 30
megrootens 0:3855d4588f76 31 /**
cnckiwi31 5:63063a9fa51c 32 * Main loop
megrootens 0:3855d4588f76 33 */
megrootens 0:3855d4588f76 34 int main()
cnckiwi31 5:63063a9fa51c 35 {
cnckiwi31 5:63063a9fa51c 36 leg.setLoggingFrequency(100); //Set datalogging frequency
cnckiwi31 5:63063a9fa51c 37
cnckiwi31 5:63063a9fa51c 38 /* Two extra columns of data will be recorded in this experiment.
cnckiwi31 5:63063a9fa51c 39 One for the target pressure, and the other for the number of sit and
cnckiwi31 5:63063a9fa51c 40 stand cycles currently completed in the experiment */
cnckiwi31 5:63063a9fa51c 41 string colNames[] = {"Target pressure (kPa)","Cycle"}; //add data headings
cnckiwi31 5:63063a9fa51c 42 leg.setExtraColumns(colNames,2);
megrootens 0:3855d4588f76 43
cnckiwi31 5:63063a9fa51c 44 float targetP = 200; //Target pressure in kPa
cnckiwi31 5:63063a9fa51c 45 int expCycles = 2; //Number of sit to stand to sit cycles
cnckiwi31 5:63063a9fa51c 46 float vals[] = {targetP,0}; //set initial values of data that will be logged
cnckiwi31 5:63063a9fa51c 47 leg.setExtraData(vals);
cnckiwi31 4:1cdce6c6c94e 48
cnckiwi31 5:63063a9fa51c 49 /* Setup all peripherals on rig, display info about SD card and the
cnckiwi31 5:63063a9fa51c 50 user interface menu */
cnckiwi31 10:77fcbad99a31 51 leg.initialise(); //this gives the 10MHz, normal clock polarity, mode 1 spi we need, pins are same as for encoder
cnckiwi31 10:77fcbad99a31 52
megrootens 0:3855d4588f76 53
cnckiwi31 5:63063a9fa51c 54
cnckiwi31 5:63063a9fa51c 55 /*Run an experiment when the button is pressed to start datalogging and
cnckiwi31 5:63063a9fa51c 56 stop it if the button is pressed again to stop datalogging
cnckiwi31 5:63063a9fa51c 57 (or when experiment stops - then datalogging stops by itself) */
cnckiwi31 5:63063a9fa51c 58 while (true) {
cnckiwi31 5:63063a9fa51c 59 if (leg.isLogging()) {
cnckiwi31 5:63063a9fa51c 60 runDemoExperiment0(expCycles,targetP);
cnckiwi31 5:63063a9fa51c 61 }
megrootens 0:3855d4588f76 62 }
megrootens 0:3855d4588f76 63 }
megrootens 0:3855d4588f76 64
cnckiwi31 5:63063a9fa51c 65 /**
cnckiwi31 5:63063a9fa51c 66 * Shows how a demo experiment works. This experiment pressurises the leg to
cnckiwi31 5:63063a9fa51c 67 * pressure targetkPa, depressurises it, and then repeats the process cycles
cnckiwi31 5:63063a9fa51c 68 * number of times
cnckiwi31 5:63063a9fa51c 69 * @param cycles: the number of cycles the leg goes up and down
cnckiwi31 5:63063a9fa51c 70 * @param targetkPa: the pressure at which the valve is opened to let the leg go down
cnckiwi31 5:63063a9fa51c 71 */
cnckiwi31 5:63063a9fa51c 72 void runDemoExperiment0(int cycles, float targetkPa)
megrootens 0:3855d4588f76 73 {
cnckiwi31 5:63063a9fa51c 74 //The experiment starts when logging does
cnckiwi31 5:63063a9fa51c 75 bool experimentRunning = leg.isLogging();
cnckiwi31 5:63063a9fa51c 76
cnckiwi31 5:63063a9fa51c 77 //Stop the Bench class from printing, so this method can print
cnckiwi31 5:63063a9fa51c 78 leg.pausePrint();
cnckiwi31 5:63063a9fa51c 79
cnckiwi31 5:63063a9fa51c 80 if (experimentRunning) {
cnckiwi31 5:63063a9fa51c 81 // Pressurise and depressurise the leg cycles number of times
cnckiwi31 5:63063a9fa51c 82 for (int i=0; i<cycles; i++) {
cnckiwi31 5:63063a9fa51c 83 leg.pc.printf("\r\nCycle: \t%i out of \t%i",i+1,cycles);
cnckiwi31 5:63063a9fa51c 84
cnckiwi31 5:63063a9fa51c 85 //Update cycles logged
cnckiwi31 5:63063a9fa51c 86 float data[] = {targetkPa,i+1};
cnckiwi31 5:63063a9fa51c 87 leg.setExtraData(data);
cnckiwi31 5:63063a9fa51c 88
cnckiwi31 5:63063a9fa51c 89 //Pressurise
cnckiwi31 5:63063a9fa51c 90 leg.setValve(true);
megrootens 0:3855d4588f76 91
cnckiwi31 5:63063a9fa51c 92 //Wait until measured pressure reaches target pressure
cnckiwi31 5:63063a9fa51c 93 float measureP = 0;
cnckiwi31 5:63063a9fa51c 94 while(measureP < targetkPa) {
cnckiwi31 5:63063a9fa51c 95 //Keep checking logging is going
cnckiwi31 5:63063a9fa51c 96 experimentRunning = leg.isLogging();
cnckiwi31 5:63063a9fa51c 97
cnckiwi31 5:63063a9fa51c 98 if (experimentRunning) {
cnckiwi31 5:63063a9fa51c 99 measureP = leg.getPressure0()*100;//Conversion of bar to kPa
cnckiwi31 10:77fcbad99a31 100 wait(0.05);//Wait a bit
cnckiwi31 5:63063a9fa51c 101 } else { //Logging stopped
cnckiwi31 5:63063a9fa51c 102 leg.stopLogging(); //Stop logging data
cnckiwi31 5:63063a9fa51c 103 leg.setValve(false); //Depressurise
cnckiwi31 5:63063a9fa51c 104 leg.resumePrint(); //Let the Bench class print
cnckiwi31 5:63063a9fa51c 105 return;
cnckiwi31 5:63063a9fa51c 106 }
cnckiwi31 5:63063a9fa51c 107 }
megrootens 0:3855d4588f76 108
cnckiwi31 5:63063a9fa51c 109 //Depressurise
cnckiwi31 5:63063a9fa51c 110 leg.setValve(false);
megrootens 0:3855d4588f76 111
cnckiwi31 5:63063a9fa51c 112 /*Wait until depressurised (completely depressurised is
cnckiwi31 5:63063a9fa51c 113 around 10-12 kPa due to current sensor calibration)*/
cnckiwi31 5:63063a9fa51c 114 while(measureP > 15) {
cnckiwi31 5:63063a9fa51c 115 //Keep checking logging is going
cnckiwi31 5:63063a9fa51c 116 experimentRunning = leg.isLogging();
cnckiwi31 5:63063a9fa51c 117
cnckiwi31 5:63063a9fa51c 118 if (experimentRunning) {
cnckiwi31 5:63063a9fa51c 119 measureP = leg.getPressure0()*100;//Conversion of bar to kpa
cnckiwi31 10:77fcbad99a31 120 wait(0.05);//Wait a bit
cnckiwi31 5:63063a9fa51c 121 } else { //Logging stopped
cnckiwi31 5:63063a9fa51c 122 leg.stopLogging(); //Stop logging data
cnckiwi31 5:63063a9fa51c 123 leg.resumePrint(); //Let the Bench class print
cnckiwi31 5:63063a9fa51c 124 return;
cnckiwi31 5:63063a9fa51c 125 }
megrootens 0:3855d4588f76 126 }
megrootens 0:3855d4588f76 127 }
cnckiwi31 5:63063a9fa51c 128 // Logging stopped as experiment is fully completed
cnckiwi31 5:63063a9fa51c 129 leg.stopLogging(); //Stop logging data
cnckiwi31 5:63063a9fa51c 130 leg.resumePrint(); //Let the Bench class print
megrootens 0:3855d4588f76 131 }
megrootens 0:3855d4588f76 132 }
megrootens 0:3855d4588f76 133
cnckiwi31 10:77fcbad99a31 134 void testDAC() {
cnckiwi31 10:77fcbad99a31 135 //setup SPI to write 8 bit words, mode 1 and turn select lines high
cnckiwi31 10:77fcbad99a31 136 spi.format(8,1);
cnckiwi31 10:77fcbad99a31 137 spi.frequency(200000);
cnckiwi31 10:77fcbad99a31 138 deselectDACB();
cnckiwi31 10:77fcbad99a31 139 wait_ms(20);
cnckiwi31 10:77fcbad99a31 140
cnckiwi31 10:77fcbad99a31 141 //Power up DAC
cnckiwi31 10:77fcbad99a31 142 selectDACB();
cnckiwi31 10:77fcbad99a31 143 spi.write(0xE0);
cnckiwi31 10:77fcbad99a31 144 spi.write(0x00);
cnckiwi31 10:77fcbad99a31 145 deselectDACB();
cnckiwi31 10:77fcbad99a31 146
cnckiwi31 10:77fcbad99a31 147 //Write a value to a channel
cnckiwi31 10:77fcbad99a31 148 selectDACB();
cnckiwi31 10:77fcbad99a31 149 spi.write(0x47);
cnckiwi31 10:77fcbad99a31 150 spi.write(0xFF);
cnckiwi31 10:77fcbad99a31 151 deselectDACB();
cnckiwi31 10:77fcbad99a31 152
megrootens 0:3855d4588f76 153
cnckiwi31 10:77fcbad99a31 154
cnckiwi31 10:77fcbad99a31 155
cnckiwi31 10:77fcbad99a31 156 /* selectDACB();
cnckiwi31 10:77fcbad99a31 157 spi.write(0x2F);
cnckiwi31 10:77fcbad99a31 158 spi.write(0xFF);
cnckiwi31 10:77fcbad99a31 159 deselectDACB();
cnckiwi31 10:77fcbad99a31 160
cnckiwi31 10:77fcbad99a31 161 selectDACB();
cnckiwi31 10:77fcbad99a31 162 spi.write(0x60);
cnckiwi31 10:77fcbad99a31 163 spi.write(0x00);
cnckiwi31 10:77fcbad99a31 164 deselectDACB();*/
cnckiwi31 10:77fcbad99a31 165
cnckiwi31 10:77fcbad99a31 166 //wait(3);
cnckiwi31 10:77fcbad99a31 167
cnckiwi31 10:77fcbad99a31 168 //Write a value to a channel
cnckiwi31 10:77fcbad99a31 169 /* selectDACB();
cnckiwi31 10:77fcbad99a31 170 spi.write(0x40);
cnckiwi31 10:77fcbad99a31 171 spi.write(0x00);
cnckiwi31 10:77fcbad99a31 172 deselectDACB();*/
cnckiwi31 10:77fcbad99a31 173
cnckiwi31 10:77fcbad99a31 174 while (true) {
cnckiwi31 10:77fcbad99a31 175 myled = !myled;
cnckiwi31 10:77fcbad99a31 176
cnckiwi31 10:77fcbad99a31 177
cnckiwi31 10:77fcbad99a31 178 wait_ms(500);
cnckiwi31 10:77fcbad99a31 179 }
cnckiwi31 10:77fcbad99a31 180 }
cnckiwi31 10:77fcbad99a31 181
cnckiwi31 10:77fcbad99a31 182 /** Selects DAC B (enable line goes low)
cnckiwi31 10:77fcbad99a31 183 */
cnckiwi31 10:77fcbad99a31 184 void selectDACB()
cnckiwi31 10:77fcbad99a31 185 {
cnckiwi31 10:77fcbad99a31 186 CSB.write(0);
cnckiwi31 10:77fcbad99a31 187 wait_us(1);
cnckiwi31 10:77fcbad99a31 188 }
cnckiwi31 10:77fcbad99a31 189
cnckiwi31 10:77fcbad99a31 190 /** Deselects DAC B (enable line goes high)
cnckiwi31 10:77fcbad99a31 191 */
cnckiwi31 10:77fcbad99a31 192 void deselectDACB()
cnckiwi31 10:77fcbad99a31 193 {
cnckiwi31 10:77fcbad99a31 194 CSB.write(1);
cnckiwi31 10:77fcbad99a31 195 wait_us(1);
cnckiwi31 10:77fcbad99a31 196 }
cnckiwi31 10:77fcbad99a31 197
cnckiwi31 10:77fcbad99a31 198 void testToggleChannel()
cnckiwi31 10:77fcbad99a31 199 {
cnckiwi31 10:77fcbad99a31 200 // POWER up dac
cnckiwi31 10:77fcbad99a31 201 //select chip
cnckiwi31 10:77fcbad99a31 202 CSB.write(0);
cnckiwi31 10:77fcbad99a31 203 wait_us(1);
cnckiwi31 10:77fcbad99a31 204
cnckiwi31 10:77fcbad99a31 205 spi.write(0b11100000);
cnckiwi31 10:77fcbad99a31 206 spi.write(0b00000000);
cnckiwi31 10:77fcbad99a31 207
cnckiwi31 10:77fcbad99a31 208 //deselect chip
cnckiwi31 10:77fcbad99a31 209 CSB.write(1);
cnckiwi31 10:77fcbad99a31 210 wait_us(1);
cnckiwi31 10:77fcbad99a31 211
cnckiwi31 10:77fcbad99a31 212
cnckiwi31 10:77fcbad99a31 213 //write output on a
cnckiwi31 10:77fcbad99a31 214 //select chip
cnckiwi31 10:77fcbad99a31 215 CSB.write(0);
cnckiwi31 10:77fcbad99a31 216 wait_us(1);
cnckiwi31 10:77fcbad99a31 217
cnckiwi31 10:77fcbad99a31 218 spi.write(0b01000011);
cnckiwi31 10:77fcbad99a31 219 spi.write(0b11111111);
cnckiwi31 10:77fcbad99a31 220
cnckiwi31 10:77fcbad99a31 221 //deselect chip
cnckiwi31 10:77fcbad99a31 222 CSB.write(1);
cnckiwi31 10:77fcbad99a31 223 wait_us(1);
cnckiwi31 10:77fcbad99a31 224
cnckiwi31 10:77fcbad99a31 225
cnckiwi31 10:77fcbad99a31 226 //write output on b
cnckiwi31 10:77fcbad99a31 227 //select chip
cnckiwi31 10:77fcbad99a31 228 CSB.write(0);
cnckiwi31 10:77fcbad99a31 229 wait_us(1);
cnckiwi31 10:77fcbad99a31 230
cnckiwi31 10:77fcbad99a31 231 spi.write(0b01011011);
cnckiwi31 10:77fcbad99a31 232 spi.write(0b11111111);
cnckiwi31 10:77fcbad99a31 233
cnckiwi31 10:77fcbad99a31 234 //deselect chip
cnckiwi31 10:77fcbad99a31 235 CSB.write(1);
cnckiwi31 10:77fcbad99a31 236 wait_us(1);
cnckiwi31 10:77fcbad99a31 237
cnckiwi31 10:77fcbad99a31 238 while (true) {
cnckiwi31 10:77fcbad99a31 239 //leg.pc.printf("Hi");
cnckiwi31 10:77fcbad99a31 240 // POWER up dac
cnckiwi31 10:77fcbad99a31 241 //select chip
cnckiwi31 10:77fcbad99a31 242 CSB.write(0);
cnckiwi31 10:77fcbad99a31 243 wait_us(1);
cnckiwi31 10:77fcbad99a31 244
cnckiwi31 10:77fcbad99a31 245 spi.write(0b11100000);
cnckiwi31 10:77fcbad99a31 246 spi.write(0b00000000);
cnckiwi31 10:77fcbad99a31 247
cnckiwi31 10:77fcbad99a31 248 //deselect chip
cnckiwi31 10:77fcbad99a31 249 CSB.write(1);
cnckiwi31 10:77fcbad99a31 250 wait_us(1);
cnckiwi31 10:77fcbad99a31 251 //write output on a
cnckiwi31 10:77fcbad99a31 252 //select chip
cnckiwi31 10:77fcbad99a31 253 CSB.write(0);
cnckiwi31 10:77fcbad99a31 254 wait_us(1);
cnckiwi31 10:77fcbad99a31 255
cnckiwi31 10:77fcbad99a31 256 spi.write(0b01010011);
cnckiwi31 10:77fcbad99a31 257 spi.write(0b11111111);
cnckiwi31 10:77fcbad99a31 258
cnckiwi31 10:77fcbad99a31 259 spi.write(0b01001011);
cnckiwi31 10:77fcbad99a31 260 spi.write(0b11111111);
cnckiwi31 10:77fcbad99a31 261
cnckiwi31 10:77fcbad99a31 262 //deselect chip
cnckiwi31 10:77fcbad99a31 263 CSB.write(1);
cnckiwi31 10:77fcbad99a31 264 wait_us(1);
cnckiwi31 10:77fcbad99a31 265
cnckiwi31 10:77fcbad99a31 266 wait_ms(100);
cnckiwi31 10:77fcbad99a31 267
cnckiwi31 10:77fcbad99a31 268
cnckiwi31 10:77fcbad99a31 269 }
cnckiwi31 10:77fcbad99a31 270 bool ch = true;
cnckiwi31 10:77fcbad99a31 271 while (true) {
cnckiwi31 10:77fcbad99a31 272 //data value
cnckiwi31 10:77fcbad99a31 273 unsigned int data = 0xFFF;
cnckiwi31 10:77fcbad99a31 274
cnckiwi31 10:77fcbad99a31 275 //if more than 12 bits (0xfff) then set all bits true)
cnckiwi31 10:77fcbad99a31 276 if (data > 0xFFF) {
cnckiwi31 10:77fcbad99a31 277 data = 0xFFF;
cnckiwi31 10:77fcbad99a31 278 }
cnckiwi31 10:77fcbad99a31 279
cnckiwi31 10:77fcbad99a31 280 //select chip
cnckiwi31 10:77fcbad99a31 281 //bring cs low
cnckiwi31 10:77fcbad99a31 282 CSB.write(0);
cnckiwi31 10:77fcbad99a31 283 //wait a bit (more than 40ns)
cnckiwi31 10:77fcbad99a31 284 wait_us(1);
cnckiwi31 10:77fcbad99a31 285
cnckiwi31 10:77fcbad99a31 286 //transfer a command (for channel a 0x4<<12 + data masked to 12 bits, for channel b 0x5<<12 + data masked to 12 bits)
cnckiwi31 10:77fcbad99a31 287
cnckiwi31 10:77fcbad99a31 288 int command = (0x01<<12);//default to channel a
cnckiwi31 10:77fcbad99a31 289 /*if (!ch) {
cnckiwi31 10:77fcbad99a31 290 command = (0x05<<12);
cnckiwi31 10:77fcbad99a31 291 } */
cnckiwi31 10:77fcbad99a31 292 data = command + (data&0xFFF);
cnckiwi31 10:77fcbad99a31 293 //spi.write(data);
cnckiwi31 10:77fcbad99a31 294 spi.write(data>>8);
cnckiwi31 10:77fcbad99a31 295 spi.write(data & 0x00FF);
cnckiwi31 10:77fcbad99a31 296 //bring cs high
cnckiwi31 10:77fcbad99a31 297 CSB.write(1);
cnckiwi31 10:77fcbad99a31 298 //wait a bit (more than 10-15ns)
cnckiwi31 10:77fcbad99a31 299 wait_us(1);
cnckiwi31 10:77fcbad99a31 300 wait_ms(10);
cnckiwi31 10:77fcbad99a31 301 //leg.pc.printf("\r\nCommand: \t%i",command);
cnckiwi31 10:77fcbad99a31 302
cnckiwi31 10:77fcbad99a31 303 //leg.pc.printf("\r\nData: \t%i",data);
cnckiwi31 10:77fcbad99a31 304 ch = !ch;
cnckiwi31 10:77fcbad99a31 305 }
cnckiwi31 10:77fcbad99a31 306 }