Satoshi Tohda / Mbed 2 deprecated BodySwap2

Prototype program for the Body Swap Machine

Dependencies:   mbed

main.cpp@0:19ea917e5234, 2017-03-25 (annotated)

Committer:
moshi
Date:
Sat Mar 25 10:33:36 2017 +0000
Revision:
0:19ea917e5234
BodySwap

Who changed what in which revision?

UserRevisionLine numberNew contents of line
moshi 0:19ea917e5234 1 #include "mbed.h"
moshi 0:19ea917e5234 2
moshi 0:19ea917e5234 3 /***
moshi 0:19ea917e5234 4 / Body Swap Machine Program
moshi 0:19ea917e5234 5 ***/
moshi 0:19ea917e5234 6
moshi 0:19ea917e5234 7 #define actuatorFrequency 250 //Frequency of actuator vibration[Hz]
moshi 0:19ea917e5234 8
moshi 0:19ea917e5234 9 #define sscTickerInterval 0.1 //Ticker Interval [s]
moshi 0:19ea917e5234 10 #define updateFrequency 20000 //Update frequency for sine wave[Hz]
moshi 0:19ea917e5234 11
moshi 0:19ea917e5234 12 #define MIN_TIME_DIFF 500 //Chatter elimination delay for tact switch [us]
moshi 0:19ea917e5234 13
moshi 0:19ea917e5234 14 DigitalOut led1(LED1);
moshi 0:19ea917e5234 15 DigitalOut led2(LED2);
moshi 0:19ea917e5234 16 DigitalOut led3(LED3);
moshi 0:19ea917e5234 17 DigitalOut led4(LED4);
moshi 0:19ea917e5234 18
moshi 0:19ea917e5234 19 AnalogIn ain1(p15);
moshi 0:19ea917e5234 20 AnalogIn ain2(p16);
moshi 0:19ea917e5234 21
moshi 0:19ea917e5234 22 InterruptIn buttonA(p12); //Blue Button
moshi 0:19ea917e5234 23 InterruptIn buttonB(p13); //Yellow Button
moshi 0:19ea917e5234 24 InterruptIn buttonC(p14); //Red Button
moshi 0:19ea917e5234 25
moshi 0:19ea917e5234 26 SPI spi(p5, NC, p7); //mosi (master output slave input), miso, sclk
moshi 0:19ea917e5234 27 DigitalOut cs(p8); //chip select pin
moshi 0:19ea917e5234 28 Serial pc(USBTX, USBRX); //tx, rx
moshi 0:19ea917e5234 29
moshi 0:19ea917e5234 30 int phase[2] = {0}; //current phase of sinusoid (int from 0 to 999)
moshi 0:19ea917e5234 31 char phasetick[2][1000] = {0}; //how much one ticker call moves the phase
moshi 0:19ea917e5234 32 int phasetickcounter = 0; //which phasetick to use
moshi 0:19ea917e5234 33
moshi 0:19ea917e5234 34 float ms_passed_per_t = 0; //how many milliseconds will pass per an increase in t
moshi 0:19ea917e5234 35
moshi 0:19ea917e5234 36 float amplitude[2] = {0, 0}; //Amplitude of vibration[%]
moshi 0:19ea917e5234 37 int frequency[2] = {0, 0}; //Frequency of vibration[Hz]
moshi 0:19ea917e5234 38
moshi 0:19ea917e5234 39 long int t = 0; //Time. Equivalent to (10^-4)s. Resets when going into state 3 or state 4
moshi 0:19ea917e5234 40
moshi 0:19ea917e5234 41 short waveTable[1100] = {0}; //table for output values in case of sine wave
moshi 0:19ea917e5234 42 //waveTable[k][i] = 1/2 * ( 1 + sin( 2*PI * (i/1000 ) ), waveTable[1][i] = actual strength at those points
moshi 0:19ea917e5234 43
moshi 0:19ea917e5234 44 short DA[2] = {0}; //table for actual binary passed to DA converter
moshi 0:19ea917e5234 45
moshi 0:19ea917e5234 46 short silence[4] = {4096,8192,12288,16384};
moshi 0:19ea917e5234 47 //pass to DAC when output is zero
moshi 0:19ea917e5234 48 short channel[4] = {4096,8192,12288,16384};
moshi 0:19ea917e5234 49 //binary for each channel
moshi 0:19ea917e5234 50
moshi 0:19ea917e5234 51 float adc1 = 0; //input from adc1
moshi 0:19ea917e5234 52 float adc2 = 0;
moshi 0:19ea917e5234 53 float volts1 = 0; //convert to voltage
moshi 0:19ea917e5234 54 float volts2 = 0;
moshi 0:19ea917e5234 55
moshi 0:19ea917e5234 56 float prev_button_press_A = -100; //timing of previous button press[us]
moshi 0:19ea917e5234 57 float prev_button_press_B = -100; //timing of previous button press[us]
moshi 0:19ea917e5234 58 float prev_button_press_C = -100; //timing of previous button press[us]
moshi 0:19ea917e5234 59
moshi 0:19ea917e5234 60 float reset_pressed_time = false;
moshi 0:19ea917e5234 61
moshi 0:19ea917e5234 62 Ticker time_ticker; //ticker
moshi 0:19ea917e5234 63
moshi 0:19ea917e5234 64 LocalFileSystem local("local"); // Create the local filesystem under the name "local"
moshi 0:19ea917e5234 65 FILE *fp;
moshi 0:19ea917e5234 66
moshi 0:19ea917e5234 67 //for measuring time
moshi 0:19ea917e5234 68 Timer time1;
moshi 0:19ea917e5234 69 int ttaken[100] = {0};
moshi 0:19ea917e5234 70 int cont = 0;
moshi 0:19ea917e5234 71 float ave = 0;
moshi 0:19ea917e5234 72 int max = 0;
moshi 0:19ea917e5234 73 //for measuring time
moshi 0:19ea917e5234 74
moshi 0:19ea917e5234 75 void vibration();
moshi 0:19ea917e5234 76 void waveDefine(int);
moshi 0:19ea917e5234 77
moshi 0:19ea917e5234 78
moshi 0:19ea917e5234 79 //time_ticker
moshi 0:19ea917e5234 80 void time(){
moshi 0:19ea917e5234 81
moshi 0:19ea917e5234 82 //time1.start();
moshi 0:19ea917e5234 83
moshi 0:19ea917e5234 84 t++; //every 50us
moshi 0:19ea917e5234 85 vibration();
moshi 0:19ea917e5234 86
moshi 0:19ea917e5234 87 //resets time if t is greater than 1800000000 (30 minutes)
moshi 0:19ea917e5234 88
moshi 0:19ea917e5234 89 if(t - 1800000000 > 0){
moshi 0:19ea917e5234 90 t = 0;
moshi 0:19ea917e5234 91 }
moshi 0:19ea917e5234 92
moshi 0:19ea917e5234 93 if(t%20 == 0){
moshi 0:19ea917e5234 94
moshi 0:19ea917e5234 95 amplitude[0] = ain1.read(); //reads input from sensor 1 (value 0~1) and inputs value into actuator vibration amplitude
moshi 0:19ea917e5234 96
moshi 0:19ea917e5234 97 }else if(t%20 - 1 == 0){
moshi 0:19ea917e5234 98
moshi 0:19ea917e5234 99 amplitude[1] = ain2.read();
moshi 0:19ea917e5234 100
moshi 0:19ea917e5234 101 }
moshi 0:19ea917e5234 102
moshi 0:19ea917e5234 103 /*
moshi 0:19ea917e5234 104 time1.stop();
moshi 0:19ea917e5234 105
moshi 0:19ea917e5234 106 if(cont < 99){
moshi 0:19ea917e5234 107 ttaken[cont] = time1.read_us();
moshi 0:19ea917e5234 108 cont++;
moshi 0:19ea917e5234 109 }else if(cont == 99){
moshi 0:19ea917e5234 110 ttaken[cont] = time1.read_us();
moshi 0:19ea917e5234 111 for(int i = 0; i<100;i++){
moshi 0:19ea917e5234 112 ave += ttaken[i];
moshi 0:19ea917e5234 113 if(ttaken[i] > max){
moshi 0:19ea917e5234 114 max = ttaken[i];
moshi 0:19ea917e5234 115 }
moshi 0:19ea917e5234 116 }
moshi 0:19ea917e5234 117 ave = ave/100;
moshi 0:19ea917e5234 118 cont = 1000;
moshi 0:19ea917e5234 119 pc.printf("%f\r\n",ave);
moshi 0:19ea917e5234 120 }
moshi 0:19ea917e5234 121
moshi 0:19ea917e5234 122 time1.reset();
moshi 0:19ea917e5234 123 */
moshi 0:19ea917e5234 124
moshi 0:19ea917e5234 125 }
moshi 0:19ea917e5234 126
moshi 0:19ea917e5234 127
moshi 0:19ea917e5234 128 void initVariables(){
moshi 0:19ea917e5234 129
moshi 0:19ea917e5234 130 ms_passed_per_t = (float)1000.000 / (float)updateFrequency;
moshi 0:19ea917e5234 131 t = 0;
moshi 0:19ea917e5234 132
moshi 0:19ea917e5234 133 for(int i = 0; i < 2; i++){
moshi 0:19ea917e5234 134 frequency[i] = actuatorFrequency;
moshi 0:19ea917e5234 135 }
moshi 0:19ea917e5234 136
moshi 0:19ea917e5234 137 amplitude[0] = 0.1;
moshi 0:19ea917e5234 138 amplitude[1] = 0.1;
moshi 0:19ea917e5234 139
moshi 0:19ea917e5234 140 buttonA.mode(PullUp);
moshi 0:19ea917e5234 141 buttonB.mode(PullUp);
moshi 0:19ea917e5234 142 buttonC.mode(PullUp);
moshi 0:19ea917e5234 143
moshi 0:19ea917e5234 144 }
moshi 0:19ea917e5234 145
moshi 0:19ea917e5234 146
moshi 0:19ea917e5234 147 void setWaveTable(){
moshi 0:19ea917e5234 148
moshi 0:19ea917e5234 149 /***
moshi 0:19ea917e5234 150 / Sets the Sine Wave Table
moshi 0:19ea917e5234 151 ***/
moshi 0:19ea917e5234 152
moshi 0:19ea917e5234 153 double temp = 0.0;
moshi 0:19ea917e5234 154 double ttactual = 0.0; //actual(optimal) value of phasetic in double
moshi 0:19ea917e5234 155 double ticksum = 0.0; //sum of all ticker calls
moshi 0:19ea917e5234 156
moshi 0:19ea917e5234 157 for(int i = 0; i < 2; i++){
moshi 0:19ea917e5234 158
moshi 0:19ea917e5234 159 phase[i] = 0;
moshi 0:19ea917e5234 160 ttactual = 1000.0 * frequency[i] / updateFrequency;
moshi 0:19ea917e5234 161
moshi 0:19ea917e5234 162 if(ttactual == (int)ttactual){
moshi 0:19ea917e5234 163 for(int j = 0; j < 1000; j++){
moshi 0:19ea917e5234 164 phasetick[i][j] = (int)(ttactual); //if the tick in phase is an integer, simply input the integer
moshi 0:19ea917e5234 165 }
moshi 0:19ea917e5234 166 ticksum = ttactual*100;
moshi 0:19ea917e5234 167
moshi 0:19ea917e5234 168 }else{
moshi 0:19ea917e5234 169 for(int j = 0; j < 1000; j++){ //if the tick in phase is NOT an integer
moshi 0:19ea917e5234 170 phasetick[i][j] = (int)(ttactual); //cast value to integer (integer < double) and input
moshi 0:19ea917e5234 171 ticksum += phasetick[i][j]; //find sum of phases
moshi 0:19ea917e5234 172
moshi 0:19ea917e5234 173 if(ttactual * (j + 1.0) - ticksum >= 0.99999){ //if difference between target and actual sum is equal to or greater than 1
moshi 0:19ea917e5234 174 phasetick[i][j]++; //add 1
moshi 0:19ea917e5234 175 ticksum++; //add 1
moshi 0:19ea917e5234 176
moshi 0:19ea917e5234 177 }
moshi 0:19ea917e5234 178 }
moshi 0:19ea917e5234 179 }
moshi 0:19ea917e5234 180 }
moshi 0:19ea917e5234 181
moshi 0:19ea917e5234 182 for(int k = 0; k < 1100; k++){
moshi 0:19ea917e5234 183 temp = (sin(2.0*3.14159*k/1000)*0.5+0.5)*0x3FF;
moshi 0:19ea917e5234 184 waveTable[k] = (short)temp;
moshi 0:19ea917e5234 185 }
moshi 0:19ea917e5234 186
moshi 0:19ea917e5234 187 }
moshi 0:19ea917e5234 188
moshi 0:19ea917e5234 189
moshi 0:19ea917e5234 190 void vibration(){
moshi 0:19ea917e5234 191 phasetickcounter++;
moshi 0:19ea917e5234 192
moshi 0:19ea917e5234 193 if(phasetickcounter >= 1000)
moshi 0:19ea917e5234 194 phasetickcounter = 0;
moshi 0:19ea917e5234 195
moshi 0:19ea917e5234 196 for(int i = 0; i < 2; i++){ // i=channel number
moshi 0:19ea917e5234 197
moshi 0:19ea917e5234 198 //Part 1 : tick the phase
moshi 0:19ea917e5234 199 phase[i] += phasetick[i][phasetickcounter];
moshi 0:19ea917e5234 200
moshi 0:19ea917e5234 201 if(phase[i] >= 1000)
moshi 0:19ea917e5234 202 phase[i] -= 1000;
moshi 0:19ea917e5234 203 //Part 1 end
moshi 0:19ea917e5234 204
moshi 0:19ea917e5234 205 //Part 2 : change input to relevant data
moshi 0:19ea917e5234 206 waveDefine(i);
moshi 0:19ea917e5234 207 //Part 2 End.
moshi 0:19ea917e5234 208
moshi 0:19ea917e5234 209 //Part 3: write input
moshi 0:19ea917e5234 210 cs = 0; //enable clock
moshi 0:19ea917e5234 211 spi.write(DA[i]);
moshi 0:19ea917e5234 212 cs = 1; //disable clock and load data
moshi 0:19ea917e5234 213 //part 3 end
moshi 0:19ea917e5234 214 }
moshi 0:19ea917e5234 215 }
moshi 0:19ea917e5234 216
moshi 0:19ea917e5234 217
moshi 0:19ea917e5234 218 void waveDefine(int i){
moshi 0:19ea917e5234 219
moshi 0:19ea917e5234 220 double inputvalue = 0;
moshi 0:19ea917e5234 221
moshi 0:19ea917e5234 222 //amplitude[i] = 1; //temporary measure - change this to sensor input
moshi 0:19ea917e5234 223 //notQuiet[i] = 1;
moshi 0:19ea917e5234 224
moshi 0:19ea917e5234 225 inputvalue = amplitude[i] * (double)waveTable[phase[i]];
moshi 0:19ea917e5234 226 DA[i] = (((i&0x07)+1)<<12)|((((int)inputvalue)&0x3FF)<<2);
moshi 0:19ea917e5234 227
moshi 0:19ea917e5234 228 return;
moshi 0:19ea917e5234 229 }
moshi 0:19ea917e5234 230
moshi 0:19ea917e5234 231 void writeVoltage(){
moshi 0:19ea917e5234 232
moshi 0:19ea917e5234 233 if(t - prev_button_press_A > MIN_TIME_DIFF){
moshi 0:19ea917e5234 234 led2 = 1;
moshi 0:19ea917e5234 235 prev_button_press_A = t;
moshi 0:19ea917e5234 236 adc1 = ain1.read();
moshi 0:19ea917e5234 237 adc2 = ain2.read();
moshi 0:19ea917e5234 238 volts1 = adc1 * 3.3;
moshi 0:19ea917e5234 239 volts2 = adc2 * 3.3;
moshi 0:19ea917e5234 240
moshi 0:19ea917e5234 241 fp = fopen("/local/out.csv", "a");
moshi 0:19ea917e5234 242 // prints sensor 1 voltage, sensor 2 voltage, time[seconds]
moshi 0:19ea917e5234 243 fprintf(fp, "%.5f, %.5f, %.3f\n",volts1,volts2,(float)t/20000);
moshi 0:19ea917e5234 244 fclose(fp);
moshi 0:19ea917e5234 245 led2 = 0;
moshi 0:19ea917e5234 246 }
moshi 0:19ea917e5234 247 }
moshi 0:19ea917e5234 248
moshi 0:19ea917e5234 249 void softReset_f(){
moshi 0:19ea917e5234 250
moshi 0:19ea917e5234 251 if(t - prev_button_press_C > MIN_TIME_DIFF){
moshi 0:19ea917e5234 252 prev_button_press_C = t;
moshi 0:19ea917e5234 253 reset_pressed_time = t;
moshi 0:19ea917e5234 254 led4 = 1;
moshi 0:19ea917e5234 255 }
moshi 0:19ea917e5234 256 }
moshi 0:19ea917e5234 257
moshi 0:19ea917e5234 258 void softReset_r(){
moshi 0:19ea917e5234 259
moshi 0:19ea917e5234 260 if(t - prev_button_press_C > MIN_TIME_DIFF){
moshi 0:19ea917e5234 261 prev_button_press_C = t;
moshi 0:19ea917e5234 262 led4 = 0;
moshi 0:19ea917e5234 263 if(t - reset_pressed_time > 20000){
moshi 0:19ea917e5234 264 NVIC_SystemReset();
moshi 0:19ea917e5234 265 }
moshi 0:19ea917e5234 266 }
moshi 0:19ea917e5234 267 }
moshi 0:19ea917e5234 268 /*
moshi 0:19ea917e5234 269 void blinkLED1(){
moshi 0:19ea917e5234 270
moshi 0:19ea917e5234 271 if(t - prev_button_press_A > MIN_TIME_DIFF){
moshi 0:19ea917e5234 272 prev_button_press_A = t;
moshi 0:19ea917e5234 273 led2 = !led2;
moshi 0:19ea917e5234 274 }
moshi 0:19ea917e5234 275 }
moshi 0:19ea917e5234 276 */
moshi 0:19ea917e5234 277 void blinkLED2(){
moshi 0:19ea917e5234 278
moshi 0:19ea917e5234 279 if(t - prev_button_press_B > MIN_TIME_DIFF){
moshi 0:19ea917e5234 280 prev_button_press_B = t;
moshi 0:19ea917e5234 281 led3 = !led3;
moshi 0:19ea917e5234 282 }
moshi 0:19ea917e5234 283 }
moshi 0:19ea917e5234 284
moshi 0:19ea917e5234 285 /*
moshi 0:19ea917e5234 286 void blinkLED3_f(){
moshi 0:19ea917e5234 287
moshi 0:19ea917e5234 288 if(t - prev_button_press_C > MIN_TIME_DIFF){
moshi 0:19ea917e5234 289 prev_button_press_C = t;
moshi 0:19ea917e5234 290 reset_pressed_time = t;
moshi 0:19ea917e5234 291 }
moshi 0:19ea917e5234 292 }
moshi 0:19ea917e5234 293
moshi 0:19ea917e5234 294 void blinkLED3_r(){
moshi 0:19ea917e5234 295
moshi 0:19ea917e5234 296 if(t - prev_button_press_C > MIN_TIME_DIFF){
moshi 0:19ea917e5234 297 prev_button_press_C = t;
moshi 0:19ea917e5234 298
moshi 0:19ea917e5234 299 if(t - reset_pressed_time > 20000){
moshi 0:19ea917e5234 300 led4 = !led4;
moshi 0:19ea917e5234 301 }
moshi 0:19ea917e5234 302 }
moshi 0:19ea917e5234 303 }
moshi 0:19ea917e5234 304 */
moshi 0:19ea917e5234 305
moshi 0:19ea917e5234 306 int main() {
moshi 0:19ea917e5234 307
moshi 0:19ea917e5234 308 initVariables();
moshi 0:19ea917e5234 309 setWaveTable();
moshi 0:19ea917e5234 310
moshi 0:19ea917e5234 311 // Setup SPI communications w/ DA converter
moshi 0:19ea917e5234 312 spi.format(16,0);
moshi 0:19ea917e5234 313 spi.frequency(10000000); //10MHz
moshi 0:19ea917e5234 314
moshi 0:19ea917e5234 315 //attack ticker
moshi 0:19ea917e5234 316 time_ticker.attach_us(&time,1000000 / updateFrequency); //Every (10^-4)s
moshi 0:19ea917e5234 317
moshi 0:19ea917e5234 318 //attach button
moshi 0:19ea917e5234 319 buttonA.fall(&writeVoltage);
moshi 0:19ea917e5234 320 buttonB.fall(&blinkLED2);
moshi 0:19ea917e5234 321
moshi 0:19ea917e5234 322 //attach reset button (Resets mbed if RED button is pressed for more than 1 second and released)
moshi 0:19ea917e5234 323 buttonC.fall(&softReset_f);
moshi 0:19ea917e5234 324 buttonC.rise(&softReset_r);
moshi 0:19ea917e5234 325
moshi 0:19ea917e5234 326 //test serial output
moshi 0:19ea917e5234 327 pc.printf("Hello, world!\n\r");
moshi 0:19ea917e5234 328
moshi 0:19ea917e5234 329 //LED1 Blink every 0.5s
moshi 0:19ea917e5234 330 while(1){
moshi 0:19ea917e5234 331 wait(0.5);
moshi 0:19ea917e5234 332 led1 = !led1;
moshi 0:19ea917e5234 333 pc.printf("S1: %f\r\n", amplitude[0]);
moshi 0:19ea917e5234 334 pc.printf("S2: %f\r\n", amplitude[1]);
moshi 0:19ea917e5234 335 }
moshi 0:19ea917e5234 336
moshi 0:19ea917e5234 337 }