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

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:fc82dd22a527
Child:
12: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 }