匡尚 青柳 / Mbed 2 deprecated absorber

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   ADXL345_I2C QEI SDFileSystem mbed

main.cpp@6:f725ca58583f, 2017-02-09 (annotated)

Committer:
ojityan
Date:
Thu Feb 09 08:59:18 2017 +0000
Revision:
6:f725ca58583f
Parent:
5:473edb6558fa 
????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ojityan 0:86efb6256f31 1 #include "mbed.h"
ojityan 0:86efb6256f31 2 #include "SDFileSystem.h"
ojityan 0:86efb6256f31 3 #include "ADXL345_I2C.h"
ojityan 2:49d3b3eebc2e 4 //#include "QEI.h"
ojityan 0:86efb6256f31 5 #include <stdio.h>
ojityan 0:86efb6256f31 6 #include <stdlib.h>
ojityan 0:86efb6256f31 7
ojityan 0:86efb6256f31 8 void savedata(void);
ojityan 0:86efb6256f31 9 void flipB(void);
ojityan 1:a20656b5bfe1 10 void ADXL_config(void);
ojityan 1:a20656b5bfe1 11 void read_current(double *C);
ojityan 2:49d3b3eebc2e 12 void controller(double *v,float *duty_output,double *acce_device, double *acce_output, double *F);
ojityan 1:a20656b5bfe1 13 void makefile(void);
ojityan 1:a20656b5bfe1 14 void weight_init(void);
ojityan 1:a20656b5bfe1 15 void INA226config(void);
ojityan 0:86efb6256f31 16
ojityan 0:86efb6256f31 17 DigitalOut dir(p25);
ojityan 0:86efb6256f31 18 PwmOut duty(p24);
ojityan 0:86efb6256f31 19 I2C i2c(p28, p27);
ojityan 0:86efb6256f31 20
ojityan 0:86efb6256f31 21 SDFileSystem sd(p11, p12, p13, p14, "sd");
ojityan 0:86efb6256f31 22 ADXL345_I2C accelerometer(p9, p10);
ojityan 0:86efb6256f31 23 Serial debug(USBTX,USBRX);
ojityan 0:86efb6256f31 24 //QEI wheel (p21, p22, p23, 1000);
ojityan 0:86efb6256f31 25
ojityan 0:86efb6256f31 26 DigitalOut myled1(LED1);
ojityan 0:86efb6256f31 27 DigitalOut myled2(LED2);
ojityan 0:86efb6256f31 28 DigitalOut myled3(LED3);
ojityan 0:86efb6256f31 29 DigitalOut myled4(LED4);
ojityan 0:86efb6256f31 30 DigitalIn sensorA(p17);
ojityan 0:86efb6256f31 31 DigitalIn sensorB(p18);
ojityan 0:86efb6256f31 32 DigitalIn sensorC(p19);
ojityan 0:86efb6256f31 33 DigitalIn sensorD(p20);
ojityan 0:86efb6256f31 34 InterruptIn sensorB_(p18);
ojityan 0:86efb6256f31 35
ojityan 0:86efb6256f31 36 int flag = 0;
ojityan 0:86efb6256f31 37 int flipB_ = 0;
ojityan 0:86efb6256f31 38 Ticker save;
ojityan 0:86efb6256f31 39 FILE *fp;
ojityan 0:86efb6256f31 40 Timer t;
ojityan 4:1ec5063fb20e 41 int intrrupt_cnt = 0;
ojityan 0:86efb6256f31 42
ojityan 0:86efb6256f31 43 static char i2c_addr = 0x80;
ojityan 0:86efb6256f31 44
ojityan 0:86efb6256f31 45
ojityan 0:86efb6256f31 46
ojityan 0:86efb6256f31 47 int main()
ojityan 0:86efb6256f31 48 {
ojityan 6:f725ca58583f 49 char buffer[150][150];
ojityan 0:86efb6256f31 50 int i,j;
ojityan 0:86efb6256f31 51 double C;
ojityan 0:86efb6256f31 52 float duty_output;
ojityan 0:86efb6256f31 53 int loop_break;
ojityan 1:a20656b5bfe1 54 double acce_device;
ojityan 1:a20656b5bfe1 55 double acce_output;
ojityan 0:86efb6256f31 56 int readings[3] = {0, 0, 0};
ojityan 0:86efb6256f31 57 double v;
ojityan 2:49d3b3eebc2e 58 double F_output;
ojityan 0:86efb6256f31 59
ojityan 1:a20656b5bfe1 60 myled1 = 1;
ojityan 1:a20656b5bfe1 61 ADXL_config();
ojityan 1:a20656b5bfe1 62 myled2 = 1;
ojityan 1:a20656b5bfe1 63 // makefile();
ojityan 0:86efb6256f31 64 myled3 = 1;
ojityan 1:a20656b5bfe1 65 INA226config();
ojityan 0:86efb6256f31 66 myled4 = 1;
ojityan 0:86efb6256f31 67 wait(3.0);
ojityan 0:86efb6256f31 68 myled1 = 0;
ojityan 0:86efb6256f31 69 myled2 = 0;
ojityan 0:86efb6256f31 70 myled3 = 0;
ojityan 0:86efb6256f31 71 myled4 = 0;
ojityan 1:a20656b5bfe1 72
ojityan 1:a20656b5bfe1 73
ojityan 0:86efb6256f31 74 //---ammeter setting end---
ojityan 6:f725ca58583f 75 duty_output = 0.0;
ojityan 0:86efb6256f31 76 duty.period(0.00005);
ojityan 0:86efb6256f31 77 // NVIC_SetPriority(0, 1);
ojityan 0:86efb6256f31 78
ojityan 0:86efb6256f31 79
ojityan 0:86efb6256f31 80 /////////////////////
ojityan 0:86efb6256f31 81 //---main routen---
ojityan 0:86efb6256f31 82 /////////////////////
ojityan 0:86efb6256f31 83 while(1) {
ojityan 0:86efb6256f31 84 t.reset();
ojityan 0:86efb6256f31 85 i = 0;
ojityan 0:86efb6256f31 86 loop_break = 0;
ojityan 0:86efb6256f31 87 debug.printf("writing OK!\n");
ojityan 2:49d3b3eebc2e 88
ojityan 1:a20656b5bfe1 89 weight_init();
ojityan 0:86efb6256f31 90
ojityan 0:86efb6256f31 91 //---sensing start---
ojityan 0:86efb6256f31 92 flipB_ = 0;
ojityan 0:86efb6256f31 93 dir = 1;
ojityan 1:a20656b5bfe1 94
ojityan 0:86efb6256f31 95 while (1) {
ojityan 0:86efb6256f31 96 accelerometer.getOutput(readings);
ojityan 6:f725ca58583f 97 if( abs((int16_t)readings[0]) > 20) {
ojityan 0:86efb6256f31 98 myled3 = 1;
ojityan 0:86efb6256f31 99 break;
ojityan 0:86efb6256f31 100 }
ojityan 6:f725ca58583f 101 wait(0.001);
ojityan 0:86efb6256f31 102 }
ojityan 0:86efb6256f31 103 save.attach(&savedata, 0.01 );
ojityan 0:86efb6256f31 104 t.start();
ojityan 1:a20656b5bfe1 105
ojityan 1:a20656b5bfe1 106
ojityan 6:f725ca58583f 107 while(t.read() <= 0.50) {
ojityan 0:86efb6256f31 108 if(flag) {
ojityan 0:86efb6256f31 109 flag = 0;
ojityan 2:49d3b3eebc2e 110 controller( &v, &duty_output, &acce_device, &acce_output, &F_output);
ojityan 1:a20656b5bfe1 111 read_current( &C);
ojityan 2:49d3b3eebc2e 112 if(1) {
ojityan 2:49d3b3eebc2e 113 sprintf(buffer[i],"%f10,%f10,%f10,%f10,%f10,%f10",t.read(),C,acce_device,C,duty_output,F_output);
ojityan 2:49d3b3eebc2e 114 if( i > 299) {
ojityan 2:49d3b3eebc2e 115 loop_break = 1;
ojityan 2:49d3b3eebc2e 116 break;
ojityan 2:49d3b3eebc2e 117 }
ojityan 2:49d3b3eebc2e 118 i = i + 1;
ojityan 2:49d3b3eebc2e 119 }
ojityan 0:86efb6256f31 120
ojityan 0:86efb6256f31 121 }
ojityan 0:86efb6256f31 122 if(loop_break) {
ojityan 0:86efb6256f31 123 break;
ojityan 0:86efb6256f31 124 }
ojityan 0:86efb6256f31 125
ojityan 0:86efb6256f31 126 }
ojityan 0:86efb6256f31 127 myled2 = 1;
ojityan 0:86efb6256f31 128 t.stop();
ojityan 2:49d3b3eebc2e 129 save.detach();
ojityan 0:86efb6256f31 130
ojityan 0:86efb6256f31 131
ojityan 0:86efb6256f31 132 duty = 0.0f;
ojityan 2:49d3b3eebc2e 133
ojityan 2:49d3b3eebc2e 134
ojityan 2:49d3b3eebc2e 135 // makefile();
ojityan 2:49d3b3eebc2e 136 myled1 = 1;
ojityan 0:86efb6256f31 137 for( j = 0; j < i; j++) {
ojityan 2:49d3b3eebc2e 138 // fprintf(fp,buffer[j]);
ojityan 2:49d3b3eebc2e 139 debug.printf("%s\n",buffer[j]);
ojityan 0:86efb6256f31 140 }
ojityan 2:49d3b3eebc2e 141
ojityan 2:49d3b3eebc2e 142 // fclose(fp);
ojityan 0:86efb6256f31 143
ojityan 0:86efb6256f31 144 while(1) {
ojityan 2:49d3b3eebc2e 145 myled1 = 1;
ojityan 0:86efb6256f31 146 wait(0.2);
ojityan 2:49d3b3eebc2e 147 myled1 = 0;
ojityan 0:86efb6256f31 148 wait(0.2);
ojityan 0:86efb6256f31 149 }
ojityan 0:86efb6256f31 150 }
ojityan 0:86efb6256f31 151 }
ojityan 0:86efb6256f31 152
ojityan 0:86efb6256f31 153
ojityan 0:86efb6256f31 154
ojityan 0:86efb6256f31 155
ojityan 0:86efb6256f31 156 void savedata(void)
ojityan 0:86efb6256f31 157 {
ojityan 0:86efb6256f31 158 flag = 1;
ojityan 4:1ec5063fb20e 159 intrrupt_cnt++;
ojityan 2:49d3b3eebc2e 160
ojityan 0:86efb6256f31 161 }
ojityan 0:86efb6256f31 162
ojityan 0:86efb6256f31 163 void flipB(void)
ojityan 0:86efb6256f31 164 {
ojityan 0:86efb6256f31 165 flipB_ = 1;
ojityan 0:86efb6256f31 166 duty = 0.0f;
ojityan 0:86efb6256f31 167 debug.printf("flip sensorB!");
ojityan 0:86efb6256f31 168 __disable_irq();
ojityan 0:86efb6256f31 169 }
ojityan 1:a20656b5bfe1 170
ojityan 1:a20656b5bfe1 171 void ADXL_config(void)
ojityan 1:a20656b5bfe1 172 {
ojityan 1:a20656b5bfe1 173 //Go into standby mode to configure the device.
ojityan 1:a20656b5bfe1 174 accelerometer.setPowerControl(0x00);
ojityan 1:a20656b5bfe1 175
ojityan 1:a20656b5bfe1 176 //Full resolution, +/-16g, 4mg/LSB.
ojityan 1:a20656b5bfe1 177 accelerometer.setDataFormatControl(0x0B);
ojityan 1:a20656b5bfe1 178
ojityan 1:a20656b5bfe1 179 //3.2kHz data rate.
ojityan 1:a20656b5bfe1 180 accelerometer.setDataRate(ADXL345_3200HZ);
ojityan 1:a20656b5bfe1 181
ojityan 1:a20656b5bfe1 182 //Measurement mode.
ojityan 1:a20656b5bfe1 183 accelerometer.setPowerControl(0x08);
ojityan 1:a20656b5bfe1 184
ojityan 1:a20656b5bfe1 185 accelerometer.setOffset (0, 0);
ojityan 1:a20656b5bfe1 186
ojityan 1:a20656b5bfe1 187 }
ojityan 1:a20656b5bfe1 188
ojityan 1:a20656b5bfe1 189 void makefile(void)
ojityan 1:a20656b5bfe1 190 {
ojityan 2:49d3b3eebc2e 191 char filename[100];
ojityan 1:a20656b5bfe1 192 int num;
ojityan 1:a20656b5bfe1 193 //---SDsetting--
ojityan 1:a20656b5bfe1 194 mkdir("/sd/mydir", 0777);
ojityan 1:a20656b5bfe1 195 for(num=1; num<=256; num=num+1) {
ojityan 1:a20656b5bfe1 196 sprintf(filename,"/sd/mydir/data%d.csv",num);
ojityan 1:a20656b5bfe1 197 fp = fopen(filename, "r");
ojityan 1:a20656b5bfe1 198 if(fp == NULL) {
ojityan 1:a20656b5bfe1 199 fp = fopen(filename,"w");
ojityan 2:49d3b3eebc2e 200 debug.printf("file number is %d.\n",num);
ojityan 1:a20656b5bfe1 201 break;
ojityan 1:a20656b5bfe1 202 }
ojityan 1:a20656b5bfe1 203 fclose(fp);
ojityan 2:49d3b3eebc2e 204 debug.printf("data%d exists\n",num);
ojityan 1:a20656b5bfe1 205 }
ojityan 2:49d3b3eebc2e 206
ojityan 2:49d3b3eebc2e 207 // fprintf(fp,"Time,Current,acce_device,v_device,duty_output\n");
ojityan 2:49d3b3eebc2e 208
ojityan 1:a20656b5bfe1 209 // fprintf(fp, "Hello fun SD Card World!\n");
ojityan 1:a20656b5bfe1 210 }
ojityan 1:a20656b5bfe1 211
ojityan 1:a20656b5bfe1 212 void INA226config(void)
ojityan 1:a20656b5bfe1 213 {
ojityan 1:a20656b5bfe1 214
ojityan 1:a20656b5bfe1 215 char p_addr = 0x00;
ojityan 1:a20656b5bfe1 216 char p_addr_i = 0x04;
ojityan 1:a20656b5bfe1 217 short Calibration_set = 0x0A00;
ojityan 1:a20656b5bfe1 218 char write_item[3];
ojityan 1:a20656b5bfe1 219 //---ammeter setting---
ojityan 1:a20656b5bfe1 220 i2c.start();
ojityan 1:a20656b5bfe1 221 if(i2c.write(i2c_addr | 0,&p_addr,sizeof(p_addr)) == 0) {
ojityan 1:a20656b5bfe1 222 debug.printf("DA-ME-DA-YO!\n");
ojityan 1:a20656b5bfe1 223 }
ojityan 1:a20656b5bfe1 224 debug.printf("OK!\n");
ojityan 1:a20656b5bfe1 225
ojityan 1:a20656b5bfe1 226 //rawWrite(0x05,0x0A00);
ojityan 1:a20656b5bfe1 227
ojityan 1:a20656b5bfe1 228 short Config_set = 0x444F;//0x4A4F;//0x4E97;//0x4C97;
ojityan 1:a20656b5bfe1 229 write_item[0] = 0x00;
ojityan 1:a20656b5bfe1 230 write_item[1] = static_cast<char>((Config_set >> 8) & 0x00ff);
ojityan 1:a20656b5bfe1 231 write_item[2] = static_cast<char>(Config_set & 0x00ff);
ojityan 1:a20656b5bfe1 232 if(i2c.write(i2c_addr | 0,write_item,3) == 0) {
ojityan 1:a20656b5bfe1 233 debug.printf("OK!!\n");
ojityan 1:a20656b5bfe1 234 } else {
ojityan 1:a20656b5bfe1 235 debug.printf("OH!!\n");
ojityan 1:a20656b5bfe1 236 }
ojityan 1:a20656b5bfe1 237
ojityan 1:a20656b5bfe1 238
ojityan 1:a20656b5bfe1 239
ojityan 1:a20656b5bfe1 240 write_item[0] = 0x05;
ojityan 1:a20656b5bfe1 241 write_item[1] = static_cast<char>((Calibration_set >> 8) & 0x00ff);
ojityan 1:a20656b5bfe1 242 write_item[2] = static_cast<char>(Calibration_set & 0x00ff);
ojityan 1:a20656b5bfe1 243 if(i2c.write(i2c_addr | 0,write_item,3) == 0) {
ojityan 1:a20656b5bfe1 244 debug.printf("OK!!\n");
ojityan 1:a20656b5bfe1 245 } else {
ojityan 1:a20656b5bfe1 246 debug.printf("OH!!\n");
ojityan 1:a20656b5bfe1 247 }
ojityan 1:a20656b5bfe1 248 }
ojityan 1:a20656b5bfe1 249
ojityan 1:a20656b5bfe1 250 void weight_init(void)
ojityan 1:a20656b5bfe1 251 {
ojityan 1:a20656b5bfe1 252 //---position reset---
ojityan 1:a20656b5bfe1 253 dir = 0;
ojityan 1:a20656b5bfe1 254 duty = 0.50f;
ojityan 1:a20656b5bfe1 255 while(!sensorD);
ojityan 1:a20656b5bfe1 256 duty = 0.0f;
ojityan 1:a20656b5bfe1 257 wait(2.0);
ojityan 6:f725ca58583f 258 myled4 = 1;
ojityan 1:a20656b5bfe1 259 }
ojityan 1:a20656b5bfe1 260
ojityan 2:49d3b3eebc2e 261 void controller(double *v,float *duty_output,double *acce_device, double *acce_output, double *F)
ojityan 1:a20656b5bfe1 262 {
ojityan 2:49d3b3eebc2e 263
ojityan 1:a20656b5bfe1 264 double ka = 100;
ojityan 1:a20656b5bfe1 265 double kv = 100;
ojityan 2:49d3b3eebc2e 266
ojityan 1:a20656b5bfe1 267 int readings[3] = {0,0,0};
ojityan 2:49d3b3eebc2e 268 // static double F;
ojityan 2:49d3b3eebc2e 269 static double F_1;
ojityan 2:49d3b3eebc2e 270 static double F_2;
ojityan 2:49d3b3eebc2e 271 static double F_3;
ojityan 4:1ec5063fb20e 272 static int intrrupt_checker = 5;
ojityan 1:a20656b5bfe1 273
ojityan 1:a20656b5bfe1 274
ojityan 1:a20656b5bfe1 275 //---reading acceleration and control---
ojityan 1:a20656b5bfe1 276 accelerometer.getOutput(readings);
ojityan 1:a20656b5bfe1 277 *acce_device = (int16_t)readings[0] * 0.0383;
ojityan 2:49d3b3eebc2e 278 // debug.printf("acce_device = %f\n", *acce_device);
ojityan 1:a20656b5bfe1 279 *v = *v + *acce_device * 0.01;
ojityan 2:49d3b3eebc2e 280 if( *acce_device < 0) {
ojityan 1:a20656b5bfe1 281 if(abs(*v) > 0) {
ojityan 1:a20656b5bfe1 282 F_1 = -1 * ( ka * *acce_device) + (kv * *v);
ojityan 2:49d3b3eebc2e 283 *F = 0.35 * F_1 + 0.4 * F_2 + 0.25 * F_3;
ojityan 6:f725ca58583f 284 *acce_output = *F / 5.0;
ojityan 1:a20656b5bfe1 285 *duty_output = *acce_output / 118 + *duty_output;
ojityan 6:f725ca58583f 286
ojityan 6:f725ca58583f 287 /*
ojityan 6:f725ca58583f 288 if(intrrupt_checker <= intrrupt_cnt){
ojityan 6:f725ca58583f 289 *duty_output = *duty_output + 0.2;
ojityan 6:f725ca58583f 290 intrrupt_checker = intrrupt_cnt + 8;
ojityan 6:f725ca58583f 291 }*/
ojityan 3:de6579180af2 292 if(*duty_output > 1.0) {
ojityan 3:de6579180af2 293 *duty_output = 1.0;
ojityan 1:a20656b5bfe1 294 myled4 = 1;
ojityan 1:a20656b5bfe1 295 }
ojityan 4:1ec5063fb20e 296
ojityan 6:f725ca58583f 297
ojityan 4:1ec5063fb20e 298
ojityan 4:1ec5063fb20e 299
ojityan 4:1ec5063fb20e 300 duty.write(*duty_output);
ojityan 4:1ec5063fb20e 301 // duty.write(1.00);
ojityan 1:a20656b5bfe1 302 F_3 = F_2;
ojityan 2:49d3b3eebc2e 303 F_2 = *F;
ojityan 2:49d3b3eebc2e 304 // debug.printf("%i,%f,%f\n", (int16_t)readings[0],*v,*duty_output);
ojityan 1:a20656b5bfe1 305 }
ojityan 1:a20656b5bfe1 306 }
ojityan 1:a20656b5bfe1 307 }
ojityan 1:a20656b5bfe1 308
ojityan 1:a20656b5bfe1 309 void read_current(double *C)
ojityan 1:a20656b5bfe1 310 {
ojityan 1:a20656b5bfe1 311 char I[2];
ojityan 1:a20656b5bfe1 312 unsigned short read_item;
ojityan 1:a20656b5bfe1 313 char p_addr = 0x00;
ojityan 1:a20656b5bfe1 314 char p_addr_i = 0x04;
ojityan 1:a20656b5bfe1 315 //---reading current---
ojityan 1:a20656b5bfe1 316 while(i2c.write(i2c_addr | 0,&p_addr_i,sizeof(p_addr_i)) != 0);
ojityan 1:a20656b5bfe1 317 while(i2c.read(i2c_addr | 0x01,I,sizeof(I)) != 0);
ojityan 1:a20656b5bfe1 318 read_item = static_cast<unsigned short>(I[0]);
ojityan 1:a20656b5bfe1 319 read_item = (read_item << 8) | static_cast<unsigned short>(I[1]);
ojityan 1:a20656b5bfe1 320
ojityan 1:a20656b5bfe1 321 char *s_p = reinterpret_cast<char *>(&read_item);
ojityan 1:a20656b5bfe1 322 short d_s;
ojityan 1:a20656b5bfe1 323 char *d_p = reinterpret_cast<char *>(&d_s);
ojityan 1:a20656b5bfe1 324 *(d_p + 0) = *(s_p + 0);
ojityan 1:a20656b5bfe1 325 *(d_p + 1) = *(s_p + 1);
ojityan 1:a20656b5bfe1 326 *C = static_cast<double>(d_s) /* * 1.25 */;
ojityan 2:49d3b3eebc2e 327 // debug.printf("%f\n",*C);
ojityan 1:a20656b5bfe1 328 }