匡尚 青柳 / 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@3:de6579180af2, 2017-02-07 (annotated)

Committer:
ojityan
Date:
Tue Feb 07 19:06:35 2017 +0000
Revision:
3:de6579180af2
Parent:
2:49d3b3eebc2e
Child:
4:1ec5063fb20e
?????????????????????

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 0:86efb6256f31 41
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 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 0:86efb6256f31 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 0:86efb6256f31 97 if( abs((int16_t)readings[0]) > 30) {
ojityan 0:86efb6256f31 98 myled3 = 1;
ojityan 0:86efb6256f31 99 break;
ojityan 0:86efb6256f31 100 }
ojityan 2:49d3b3eebc2e 101 wait(0.01);
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 3:de6579180af2 107 while(t.read() <= 0.51) {
ojityan 0:86efb6256f31 108 // debug.printf("loop sensorB\n");
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 2:49d3b3eebc2e 112 // duty.write(duty_output);
ojityan 1:a20656b5bfe1 113 read_current( &C);
ojityan 2:49d3b3eebc2e 114 // debug.printf("duty = %f\n",duty_output);
ojityan 2:49d3b3eebc2e 115 if(1) {
ojityan 2:49d3b3eebc2e 116 sprintf(buffer[i],"%f10,%f10,%f10,%f10,%f10,%f10",t.read(),C,acce_device,C,duty_output,F_output);
ojityan 2:49d3b3eebc2e 117 if( i > 299) {
ojityan 2:49d3b3eebc2e 118 loop_break = 1;
ojityan 2:49d3b3eebc2e 119 break;
ojityan 2:49d3b3eebc2e 120 }
ojityan 2:49d3b3eebc2e 121 i = i + 1;
ojityan 2:49d3b3eebc2e 122 }
ojityan 0:86efb6256f31 123
ojityan 0:86efb6256f31 124 }
ojityan 0:86efb6256f31 125 if(loop_break) {
ojityan 0:86efb6256f31 126 break;
ojityan 0:86efb6256f31 127 }
ojityan 0:86efb6256f31 128
ojityan 0:86efb6256f31 129 }
ojityan 0:86efb6256f31 130 myled2 = 1;
ojityan 0:86efb6256f31 131 t.stop();
ojityan 2:49d3b3eebc2e 132 save.detach();
ojityan 0:86efb6256f31 133
ojityan 0:86efb6256f31 134
ojityan 0:86efb6256f31 135 duty = 0.0f;
ojityan 2:49d3b3eebc2e 136
ojityan 2:49d3b3eebc2e 137
ojityan 2:49d3b3eebc2e 138 // makefile();
ojityan 2:49d3b3eebc2e 139 myled1 = 1;
ojityan 0:86efb6256f31 140 for( j = 0; j < i; j++) {
ojityan 2:49d3b3eebc2e 141 // fprintf(fp,buffer[j]);
ojityan 2:49d3b3eebc2e 142 debug.printf("%s\n",buffer[j]);
ojityan 0:86efb6256f31 143 }
ojityan 2:49d3b3eebc2e 144
ojityan 2:49d3b3eebc2e 145 // fclose(fp);
ojityan 0:86efb6256f31 146
ojityan 0:86efb6256f31 147 while(1) {
ojityan 2:49d3b3eebc2e 148 myled1 = 1;
ojityan 0:86efb6256f31 149 wait(0.2);
ojityan 2:49d3b3eebc2e 150 myled1 = 0;
ojityan 0:86efb6256f31 151 wait(0.2);
ojityan 0:86efb6256f31 152 }
ojityan 0:86efb6256f31 153 }
ojityan 0:86efb6256f31 154 }
ojityan 0:86efb6256f31 155
ojityan 0:86efb6256f31 156
ojityan 0:86efb6256f31 157
ojityan 0:86efb6256f31 158
ojityan 0:86efb6256f31 159 void savedata(void)
ojityan 0:86efb6256f31 160 {
ojityan 0:86efb6256f31 161 flag = 1;
ojityan 2:49d3b3eebc2e 162
ojityan 0:86efb6256f31 163 }
ojityan 0:86efb6256f31 164
ojityan 0:86efb6256f31 165 void flipB(void)
ojityan 0:86efb6256f31 166 {
ojityan 0:86efb6256f31 167 flipB_ = 1;
ojityan 0:86efb6256f31 168 duty = 0.0f;
ojityan 0:86efb6256f31 169 debug.printf("flip sensorB!");
ojityan 0:86efb6256f31 170 __disable_irq();
ojityan 0:86efb6256f31 171 }
ojityan 1:a20656b5bfe1 172
ojityan 1:a20656b5bfe1 173 void ADXL_config(void)
ojityan 1:a20656b5bfe1 174 {
ojityan 1:a20656b5bfe1 175 //Go into standby mode to configure the device.
ojityan 1:a20656b5bfe1 176 accelerometer.setPowerControl(0x00);
ojityan 1:a20656b5bfe1 177
ojityan 1:a20656b5bfe1 178 //Full resolution, +/-16g, 4mg/LSB.
ojityan 1:a20656b5bfe1 179 accelerometer.setDataFormatControl(0x0B);
ojityan 1:a20656b5bfe1 180
ojityan 1:a20656b5bfe1 181 //3.2kHz data rate.
ojityan 1:a20656b5bfe1 182 accelerometer.setDataRate(ADXL345_3200HZ);
ojityan 1:a20656b5bfe1 183
ojityan 1:a20656b5bfe1 184 //Measurement mode.
ojityan 1:a20656b5bfe1 185 accelerometer.setPowerControl(0x08);
ojityan 1:a20656b5bfe1 186
ojityan 1:a20656b5bfe1 187 accelerometer.setOffset (0, 0);
ojityan 1:a20656b5bfe1 188
ojityan 1:a20656b5bfe1 189 }
ojityan 1:a20656b5bfe1 190
ojityan 1:a20656b5bfe1 191 void makefile(void)
ojityan 1:a20656b5bfe1 192 {
ojityan 2:49d3b3eebc2e 193 char filename[100];
ojityan 1:a20656b5bfe1 194 int num;
ojityan 1:a20656b5bfe1 195 //---SDsetting--
ojityan 1:a20656b5bfe1 196 mkdir("/sd/mydir", 0777);
ojityan 1:a20656b5bfe1 197 for(num=1; num<=256; num=num+1) {
ojityan 1:a20656b5bfe1 198 sprintf(filename,"/sd/mydir/data%d.csv",num);
ojityan 1:a20656b5bfe1 199 fp = fopen(filename, "r");
ojityan 1:a20656b5bfe1 200 if(fp == NULL) {
ojityan 1:a20656b5bfe1 201 fp = fopen(filename,"w");
ojityan 2:49d3b3eebc2e 202 debug.printf("file number is %d.\n",num);
ojityan 1:a20656b5bfe1 203 break;
ojityan 1:a20656b5bfe1 204 }
ojityan 1:a20656b5bfe1 205 fclose(fp);
ojityan 2:49d3b3eebc2e 206 debug.printf("data%d exists\n",num);
ojityan 1:a20656b5bfe1 207 }
ojityan 2:49d3b3eebc2e 208
ojityan 2:49d3b3eebc2e 209 // fprintf(fp,"Time,Current,acce_device,v_device,duty_output\n");
ojityan 2:49d3b3eebc2e 210
ojityan 1:a20656b5bfe1 211 // fprintf(fp, "Hello fun SD Card World!\n");
ojityan 1:a20656b5bfe1 212 }
ojityan 1:a20656b5bfe1 213
ojityan 1:a20656b5bfe1 214 void INA226config(void)
ojityan 1:a20656b5bfe1 215 {
ojityan 1:a20656b5bfe1 216
ojityan 1:a20656b5bfe1 217 char p_addr = 0x00;
ojityan 1:a20656b5bfe1 218 char p_addr_i = 0x04;
ojityan 1:a20656b5bfe1 219 short Calibration_set = 0x0A00;
ojityan 1:a20656b5bfe1 220 char write_item[3];
ojityan 1:a20656b5bfe1 221 //---ammeter setting---
ojityan 1:a20656b5bfe1 222 i2c.start();
ojityan 1:a20656b5bfe1 223 if(i2c.write(i2c_addr | 0,&p_addr,sizeof(p_addr)) == 0) {
ojityan 1:a20656b5bfe1 224 debug.printf("DA-ME-DA-YO!\n");
ojityan 1:a20656b5bfe1 225 }
ojityan 1:a20656b5bfe1 226 debug.printf("OK!\n");
ojityan 1:a20656b5bfe1 227
ojityan 1:a20656b5bfe1 228 //rawWrite(0x05,0x0A00);
ojityan 1:a20656b5bfe1 229
ojityan 1:a20656b5bfe1 230 short Config_set = 0x444F;//0x4A4F;//0x4E97;//0x4C97;
ojityan 1:a20656b5bfe1 231 write_item[0] = 0x00;
ojityan 1:a20656b5bfe1 232 write_item[1] = static_cast<char>((Config_set >> 8) & 0x00ff);
ojityan 1:a20656b5bfe1 233 write_item[2] = static_cast<char>(Config_set & 0x00ff);
ojityan 1:a20656b5bfe1 234 if(i2c.write(i2c_addr | 0,write_item,3) == 0) {
ojityan 1:a20656b5bfe1 235 debug.printf("OK!!\n");
ojityan 1:a20656b5bfe1 236 } else {
ojityan 1:a20656b5bfe1 237 debug.printf("OH!!\n");
ojityan 1:a20656b5bfe1 238 }
ojityan 1:a20656b5bfe1 239
ojityan 1:a20656b5bfe1 240
ojityan 1:a20656b5bfe1 241
ojityan 1:a20656b5bfe1 242 write_item[0] = 0x05;
ojityan 1:a20656b5bfe1 243 write_item[1] = static_cast<char>((Calibration_set >> 8) & 0x00ff);
ojityan 1:a20656b5bfe1 244 write_item[2] = static_cast<char>(Calibration_set & 0x00ff);
ojityan 1:a20656b5bfe1 245 if(i2c.write(i2c_addr | 0,write_item,3) == 0) {
ojityan 1:a20656b5bfe1 246 debug.printf("OK!!\n");
ojityan 1:a20656b5bfe1 247 } else {
ojityan 1:a20656b5bfe1 248 debug.printf("OH!!\n");
ojityan 1:a20656b5bfe1 249 }
ojityan 1:a20656b5bfe1 250 }
ojityan 1:a20656b5bfe1 251
ojityan 1:a20656b5bfe1 252 void weight_init(void)
ojityan 1:a20656b5bfe1 253 {
ojityan 1:a20656b5bfe1 254 //---position reset---
ojityan 1:a20656b5bfe1 255 dir = 0;
ojityan 1:a20656b5bfe1 256 duty = 0.50f;
ojityan 1:a20656b5bfe1 257 while(!sensorD);
ojityan 1:a20656b5bfe1 258 duty = 0.0f;
ojityan 1:a20656b5bfe1 259 wait(2.0);
ojityan 1:a20656b5bfe1 260 }
ojityan 1:a20656b5bfe1 261
ojityan 2:49d3b3eebc2e 262 void controller(double *v,float *duty_output,double *acce_device, double *acce_output, double *F)
ojityan 1:a20656b5bfe1 263 {
ojityan 2:49d3b3eebc2e 264
ojityan 1:a20656b5bfe1 265 double ka = 100;
ojityan 1:a20656b5bfe1 266 double kv = 100;
ojityan 2:49d3b3eebc2e 267
ojityan 1:a20656b5bfe1 268 int readings[3] = {0,0,0};
ojityan 2:49d3b3eebc2e 269 // static double F;
ojityan 2:49d3b3eebc2e 270 static double F_1;
ojityan 2:49d3b3eebc2e 271 static double F_2;
ojityan 2:49d3b3eebc2e 272 static double F_3;
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 2:49d3b3eebc2e 284 *acce_output = *F / 8.5;
ojityan 1:a20656b5bfe1 285 *duty_output = *acce_output / 118 + *duty_output;
ojityan 3:de6579180af2 286 if(*duty_output > 1.0) {
ojityan 3:de6579180af2 287 *duty_output = 1.0;
ojityan 1:a20656b5bfe1 288 myled4 = 1;
ojityan 1:a20656b5bfe1 289 }
ojityan 3:de6579180af2 290 // duty.write(*duty_output);
ojityan 3:de6579180af2 291 duty.write(1.00);
ojityan 1:a20656b5bfe1 292 F_3 = F_2;
ojityan 2:49d3b3eebc2e 293 F_2 = *F;
ojityan 2:49d3b3eebc2e 294 // debug.printf("%i,%f,%f\n", (int16_t)readings[0],*v,*duty_output);
ojityan 1:a20656b5bfe1 295 }
ojityan 1:a20656b5bfe1 296 }
ojityan 1:a20656b5bfe1 297 }
ojityan 1:a20656b5bfe1 298
ojityan 1:a20656b5bfe1 299 void read_current(double *C)
ojityan 1:a20656b5bfe1 300 {
ojityan 1:a20656b5bfe1 301 char I[2];
ojityan 1:a20656b5bfe1 302 unsigned short read_item;
ojityan 1:a20656b5bfe1 303 char p_addr = 0x00;
ojityan 1:a20656b5bfe1 304 char p_addr_i = 0x04;
ojityan 1:a20656b5bfe1 305 //---reading current---
ojityan 1:a20656b5bfe1 306 while(i2c.write(i2c_addr | 0,&p_addr_i,sizeof(p_addr_i)) != 0);
ojityan 1:a20656b5bfe1 307 while(i2c.read(i2c_addr | 0x01,I,sizeof(I)) != 0);
ojityan 1:a20656b5bfe1 308 read_item = static_cast<unsigned short>(I[0]);
ojityan 1:a20656b5bfe1 309 read_item = (read_item << 8) | static_cast<unsigned short>(I[1]);
ojityan 1:a20656b5bfe1 310
ojityan 1:a20656b5bfe1 311 char *s_p = reinterpret_cast<char *>(&read_item);
ojityan 1:a20656b5bfe1 312 short d_s;
ojityan 1:a20656b5bfe1 313 char *d_p = reinterpret_cast<char *>(&d_s);
ojityan 1:a20656b5bfe1 314 *(d_p + 0) = *(s_p + 0);
ojityan 1:a20656b5bfe1 315 *(d_p + 1) = *(s_p + 1);
ojityan 1:a20656b5bfe1 316 *C = static_cast<double>(d_s) /* * 1.25 */;
ojityan 2:49d3b3eebc2e 317 // debug.printf("%f\n",*C);
ojityan 1:a20656b5bfe1 318 }