匡尚 青柳 / 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@7:0e3fac3c6a6a, 2017-03-05 (annotated)

Committer:
ojityan
Date:
Sun Mar 05 18:48:04 2017 +0000
Revision:
7:0e3fac3c6a6a
Parent:
5:473edb6558fa
Child:
8:9059f95fc498
??????;

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 2:49d3b3eebc2e 49 char buffer[100][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 7:0e3fac3c6a6a 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 7:0e3fac3c6a6a 74 //---PWNの周期を設定---
ojityan 5:473edb6558fa 75 duty_output = 0.6;
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 7:0e3fac3c6a6a 81 //---制御開始---
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 7:0e3fac3c6a6a 89 weight_init(); //慣性力発生用おもりを初期位置に移動させる
ojityan 0:86efb6256f31 90
ojityan 7:0e3fac3c6a6a 91 //---加速度を感知したら移動開始---
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 7:0e3fac3c6a6a 97 if( abs((int16_t)readings[0]) > 25) {
ojityan 0:86efb6256f31 98 myled3 = 1;
ojityan 0:86efb6256f31 99 break;
ojityan 0:86efb6256f31 100 }
ojityan 0:86efb6256f31 101 }
ojityan 7:0e3fac3c6a6a 102 save.attach(&savedata, 0.01 );　//割り込み開始
ojityan 0:86efb6256f31 103 t.start();
ojityan 7:0e3fac3c6a6a 104
ojityan 7:0e3fac3c6a6a 105 duty.write(duty_output);
ojityan 1:a20656b5bfe1 106
ojityan 1:a20656b5bfe1 107
ojityan 7:0e3fac3c6a6a 108 while(t.read() <= 0.80) {　//内部タイマーが0.80秒に達するまで制御
ojityan 0:86efb6256f31 109 if(flag) {
ojityan 0:86efb6256f31 110 flag = 0;
ojityan 2:49d3b3eebc2e 111 controller( &v, &duty_output, &acce_device, &acce_output, &F_output);
ojityan 1:a20656b5bfe1 112 read_current( &C);
ojityan 2:49d3b3eebc2e 113 if(1) {
ojityan 7:0e3fac3c6a6a 114 sprintf(buffer[i],"%f10,%f10,%f10,%f10,%f10,%f10",t.read(),C,acce_device,C,duty_output,F_output);　//バッファにデータを溜めこみ終了時に吐き出す．
ojityan 2:49d3b3eebc2e 115 if( i > 299) {
ojityan 2:49d3b3eebc2e 116 loop_break = 1;
ojityan 2:49d3b3eebc2e 117 break;
ojityan 2:49d3b3eebc2e 118 }
ojityan 2:49d3b3eebc2e 119 i = i + 1;
ojityan 2:49d3b3eebc2e 120 }
ojityan 0:86efb6256f31 121
ojityan 0:86efb6256f31 122 }
ojityan 0:86efb6256f31 123 if(loop_break) {
ojityan 0:86efb6256f31 124 break;
ojityan 0:86efb6256f31 125 }
ojityan 0:86efb6256f31 126
ojityan 0:86efb6256f31 127 }
ojityan 7:0e3fac3c6a6a 128
ojityan 7:0e3fac3c6a6a 129 myled2 = 1;　//0.80秒経過したらLED2を光らせる
ojityan 0:86efb6256f31 130 t.stop();
ojityan 2:49d3b3eebc2e 131 save.detach();
ojityan 0:86efb6256f31 132
ojityan 0:86efb6256f31 133
ojityan 0:86efb6256f31 134 duty = 0.0f;
ojityan 2:49d3b3eebc2e 135
ojityan 2:49d3b3eebc2e 136 myled1 = 1;
ojityan 0:86efb6256f31 137 for( j = 0; j < i; j++) {
ojityan 7:0e3fac3c6a6a 138 debug.printf("%s\n",buffer[j]);//シリアル通信でバッファにあるデータを相手に送信する
ojityan 0:86efb6256f31 139 }
ojityan 7:0e3fac3c6a6a 140
ojityan 0:86efb6256f31 141 while(1) {
ojityan 2:49d3b3eebc2e 142 myled1 = 1;
ojityan 0:86efb6256f31 143 wait(0.2);
ojityan 2:49d3b3eebc2e 144 myled1 = 0;
ojityan 0:86efb6256f31 145 wait(0.2);
ojityan 0:86efb6256f31 146 }
ojityan 0:86efb6256f31 147 }
ojityan 0:86efb6256f31 148 }
ojityan 0:86efb6256f31 149
ojityan 0:86efb6256f31 150
ojityan 0:86efb6256f31 151
ojityan 0:86efb6256f31 152
ojityan 7:0e3fac3c6a6a 153 void savedata(void)//割り込み時の処理
ojityan 0:86efb6256f31 154 {
ojityan 0:86efb6256f31 155 flag = 1;
ojityan 4:1ec5063fb20e 156 intrrupt_cnt++;
ojityan 2:49d3b3eebc2e 157
ojityan 0:86efb6256f31 158 }
ojityan 0:86efb6256f31 159
ojityan 7:0e3fac3c6a6a 160 void flipB(void)//フォトリフレクタの反応によって止める関数
ojityan 0:86efb6256f31 161 {
ojityan 0:86efb6256f31 162 flipB_ = 1;
ojityan 0:86efb6256f31 163 duty = 0.0f;
ojityan 0:86efb6256f31 164 debug.printf("flip sensorB!");
ojityan 0:86efb6256f31 165 __disable_irq();
ojityan 0:86efb6256f31 166 }
ojityan 1:a20656b5bfe1 167
ojityan 1:a20656b5bfe1 168 void ADXL_config(void)
ojityan 1:a20656b5bfe1 169 {
ojityan 1:a20656b5bfe1 170 //Go into standby mode to configure the device.
ojityan 1:a20656b5bfe1 171 accelerometer.setPowerControl(0x00);
ojityan 1:a20656b5bfe1 172
ojityan 1:a20656b5bfe1 173 //Full resolution, +/-16g, 4mg/LSB.
ojityan 1:a20656b5bfe1 174 accelerometer.setDataFormatControl(0x0B);
ojityan 1:a20656b5bfe1 175
ojityan 1:a20656b5bfe1 176 //3.2kHz data rate.
ojityan 1:a20656b5bfe1 177 accelerometer.setDataRate(ADXL345_3200HZ);
ojityan 1:a20656b5bfe1 178
ojityan 1:a20656b5bfe1 179 //Measurement mode.
ojityan 1:a20656b5bfe1 180 accelerometer.setPowerControl(0x08);
ojityan 1:a20656b5bfe1 181
ojityan 1:a20656b5bfe1 182 accelerometer.setOffset (0, 0);
ojityan 1:a20656b5bfe1 183
ojityan 1:a20656b5bfe1 184 }
ojityan 1:a20656b5bfe1 185
ojityan 1:a20656b5bfe1 186 void makefile(void)
ojityan 1:a20656b5bfe1 187 {
ojityan 2:49d3b3eebc2e 188 char filename[100];
ojityan 1:a20656b5bfe1 189 int num;
ojityan 1:a20656b5bfe1 190 //---SDsetting--
ojityan 1:a20656b5bfe1 191 mkdir("/sd/mydir", 0777);
ojityan 1:a20656b5bfe1 192 for(num=1; num<=256; num=num+1) {
ojityan 1:a20656b5bfe1 193 sprintf(filename,"/sd/mydir/data%d.csv",num);
ojityan 1:a20656b5bfe1 194 fp = fopen(filename, "r");
ojityan 1:a20656b5bfe1 195 if(fp == NULL) {
ojityan 1:a20656b5bfe1 196 fp = fopen(filename,"w");
ojityan 2:49d3b3eebc2e 197 debug.printf("file number is %d.\n",num);
ojityan 1:a20656b5bfe1 198 break;
ojityan 1:a20656b5bfe1 199 }
ojityan 1:a20656b5bfe1 200 fclose(fp);
ojityan 2:49d3b3eebc2e 201 debug.printf("data%d exists\n",num);
ojityan 1:a20656b5bfe1 202 }
ojityan 2:49d3b3eebc2e 203
ojityan 2:49d3b3eebc2e 204 // fprintf(fp,"Time,Current,acce_device,v_device,duty_output\n");
ojityan 2:49d3b3eebc2e 205
ojityan 1:a20656b5bfe1 206 // fprintf(fp, "Hello fun SD Card World!\n");
ojityan 1:a20656b5bfe1 207 }
ojityan 1:a20656b5bfe1 208
ojityan 1:a20656b5bfe1 209 void INA226config(void)
ojityan 1:a20656b5bfe1 210 {
ojityan 1:a20656b5bfe1 211
ojityan 1:a20656b5bfe1 212 char p_addr = 0x00;
ojityan 1:a20656b5bfe1 213 char p_addr_i = 0x04;
ojityan 1:a20656b5bfe1 214 short Calibration_set = 0x0A00;
ojityan 1:a20656b5bfe1 215 char write_item[3];
ojityan 1:a20656b5bfe1 216 //---ammeter setting---
ojityan 1:a20656b5bfe1 217 i2c.start();
ojityan 1:a20656b5bfe1 218 if(i2c.write(i2c_addr | 0,&p_addr,sizeof(p_addr)) == 0) {
ojityan 1:a20656b5bfe1 219 debug.printf("DA-ME-DA-YO!\n");
ojityan 1:a20656b5bfe1 220 }
ojityan 1:a20656b5bfe1 221 debug.printf("OK!\n");
ojityan 1:a20656b5bfe1 222
ojityan 1:a20656b5bfe1 223 //rawWrite(0x05,0x0A00);
ojityan 1:a20656b5bfe1 224
ojityan 1:a20656b5bfe1 225 short Config_set = 0x444F;//0x4A4F;//0x4E97;//0x4C97;
ojityan 1:a20656b5bfe1 226 write_item[0] = 0x00;
ojityan 1:a20656b5bfe1 227 write_item[1] = static_cast<char>((Config_set >> 8) & 0x00ff);
ojityan 1:a20656b5bfe1 228 write_item[2] = static_cast<char>(Config_set & 0x00ff);
ojityan 1:a20656b5bfe1 229 if(i2c.write(i2c_addr | 0,write_item,3) == 0) {
ojityan 1:a20656b5bfe1 230 debug.printf("OK!!\n");
ojityan 1:a20656b5bfe1 231 } else {
ojityan 1:a20656b5bfe1 232 debug.printf("OH!!\n");
ojityan 1:a20656b5bfe1 233 }
ojityan 1:a20656b5bfe1 234
ojityan 1:a20656b5bfe1 235
ojityan 1:a20656b5bfe1 236
ojityan 1:a20656b5bfe1 237 write_item[0] = 0x05;
ojityan 1:a20656b5bfe1 238 write_item[1] = static_cast<char>((Calibration_set >> 8) & 0x00ff);
ojityan 1:a20656b5bfe1 239 write_item[2] = static_cast<char>(Calibration_set & 0x00ff);
ojityan 1:a20656b5bfe1 240 if(i2c.write(i2c_addr | 0,write_item,3) == 0) {
ojityan 1:a20656b5bfe1 241 debug.printf("OK!!\n");
ojityan 1:a20656b5bfe1 242 } else {
ojityan 1:a20656b5bfe1 243 debug.printf("OH!!\n");
ojityan 1:a20656b5bfe1 244 }
ojityan 1:a20656b5bfe1 245 }
ojityan 1:a20656b5bfe1 246
ojityan 1:a20656b5bfe1 247 void weight_init(void)
ojityan 1:a20656b5bfe1 248 {
ojityan 1:a20656b5bfe1 249 //---position reset---
ojityan 1:a20656b5bfe1 250 dir = 0;
ojityan 1:a20656b5bfe1 251 duty = 0.50f;
ojityan 7:0e3fac3c6a6a 252 while(!sensorB);
ojityan 1:a20656b5bfe1 253 duty = 0.0f;
ojityan 1:a20656b5bfe1 254 wait(2.0);
ojityan 7:0e3fac3c6a6a 255 myled1 = 1;
ojityan 1:a20656b5bfe1 256 }
ojityan 1:a20656b5bfe1 257
ojityan 2:49d3b3eebc2e 258 void controller(double *v,float *duty_output,double *acce_device, double *acce_output, double *F)
ojityan 1:a20656b5bfe1 259 {
ojityan 2:49d3b3eebc2e 260
ojityan 1:a20656b5bfe1 261 double ka = 100;
ojityan 1:a20656b5bfe1 262 double kv = 100;
ojityan 2:49d3b3eebc2e 263
ojityan 1:a20656b5bfe1 264 int readings[3] = {0,0,0};
ojityan 2:49d3b3eebc2e 265 // static double F;
ojityan 2:49d3b3eebc2e 266 static double F_1;
ojityan 2:49d3b3eebc2e 267 static double F_2;
ojityan 2:49d3b3eebc2e 268 static double F_3;
ojityan 7:0e3fac3c6a6a 269 static int intrrupt_checker = 1;
ojityan 1:a20656b5bfe1 270
ojityan 1:a20656b5bfe1 271
ojityan 1:a20656b5bfe1 272 //---reading acceleration and control---
ojityan 1:a20656b5bfe1 273 accelerometer.getOutput(readings);
ojityan 1:a20656b5bfe1 274 *acce_device = (int16_t)readings[0] * 0.0383;
ojityan 2:49d3b3eebc2e 275 // debug.printf("acce_device = %f\n", *acce_device);
ojityan 1:a20656b5bfe1 276 *v = *v + *acce_device * 0.01;
ojityan 2:49d3b3eebc2e 277 if( *acce_device < 0) {
ojityan 1:a20656b5bfe1 278 if(abs(*v) > 0) {
ojityan 1:a20656b5bfe1 279 F_1 = -1 * ( ka * *acce_device) + (kv * *v);
ojityan 2:49d3b3eebc2e 280 *F = 0.35 * F_1 + 0.4 * F_2 + 0.25 * F_3;
ojityan 2:49d3b3eebc2e 281 *acce_output = *F / 8.5;
ojityan 7:0e3fac3c6a6a 282 //*duty_output = *acce_output / 118 + *duty_output;
ojityan 3:de6579180af2 283 if(*duty_output > 1.0) {
ojityan 3:de6579180af2 284 *duty_output = 1.0;
ojityan 1:a20656b5bfe1 285 myled4 = 1;
ojityan 1:a20656b5bfe1 286 }
ojityan 4:1ec5063fb20e 287
ojityan 7:0e3fac3c6a6a 288 if(intrrupt_checker <= intrrupt_cnt){
ojityan 7:0e3fac3c6a6a 289 *duty_output = *duty_output + 0.04;
ojityan 7:0e3fac3c6a6a 290 intrrupt_checker = intrrupt_cnt + 1;
ojityan 4:1ec5063fb20e 291 }
ojityan 4:1ec5063fb20e 292
ojityan 4:1ec5063fb20e 293
ojityan 4:1ec5063fb20e 294 duty.write(*duty_output);
ojityan 4:1ec5063fb20e 295 // duty.write(1.00);
ojityan 1:a20656b5bfe1 296 F_3 = F_2;
ojityan 2:49d3b3eebc2e 297 F_2 = *F;
ojityan 2:49d3b3eebc2e 298 // debug.printf("%i,%f,%f\n", (int16_t)readings[0],*v,*duty_output);
ojityan 1:a20656b5bfe1 299 }
ojityan 1:a20656b5bfe1 300 }
ojityan 1:a20656b5bfe1 301 }
ojityan 1:a20656b5bfe1 302
ojityan 1:a20656b5bfe1 303 void read_current(double *C)
ojityan 1:a20656b5bfe1 304 {
ojityan 1:a20656b5bfe1 305 char I[2];
ojityan 1:a20656b5bfe1 306 unsigned short read_item;
ojityan 1:a20656b5bfe1 307 char p_addr = 0x00;
ojityan 1:a20656b5bfe1 308 char p_addr_i = 0x04;
ojityan 1:a20656b5bfe1 309 //---reading current---
ojityan 1:a20656b5bfe1 310 while(i2c.write(i2c_addr | 0,&p_addr_i,sizeof(p_addr_i)) != 0);
ojityan 1:a20656b5bfe1 311 while(i2c.read(i2c_addr | 0x01,I,sizeof(I)) != 0);
ojityan 1:a20656b5bfe1 312 read_item = static_cast<unsigned short>(I[0]);
ojityan 1:a20656b5bfe1 313 read_item = (read_item << 8) | static_cast<unsigned short>(I[1]);
ojityan 1:a20656b5bfe1 314
ojityan 1:a20656b5bfe1 315 char *s_p = reinterpret_cast<char *>(&read_item);
ojityan 1:a20656b5bfe1 316 short d_s;
ojityan 1:a20656b5bfe1 317 char *d_p = reinterpret_cast<char *>(&d_s);
ojityan 1:a20656b5bfe1 318 *(d_p + 0) = *(s_p + 0);
ojityan 1:a20656b5bfe1 319 *(d_p + 1) = *(s_p + 1);
ojityan 1:a20656b5bfe1 320 *C = static_cast<double>(d_s) /* * 1.25 */;
ojityan 2:49d3b3eebc2e 321 // debug.printf("%f\n",*C);
ojityan 1:a20656b5bfe1 322 }