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

Committer:
ojityan
Date:
Thu Feb 02 09:00:20 2017 +0000
Revision:
1:a20656b5bfe1
Parent:
0:86efb6256f31
Child:
2:49d3b3eebc2e
can't move

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 0:86efb6256f31 4 #include "QEI.h"
ojityan 0:86efb6256f31 5
ojityan 0:86efb6256f31 6 #include <stdio.h>
ojityan 0:86efb6256f31 7 #include <stdlib.h>
ojityan 0:86efb6256f31 8
ojityan 0:86efb6256f31 9 void savedata(void);
ojityan 0:86efb6256f31 10 void flipB(void);
ojityan 1:a20656b5bfe1 11 void ADXL_config(void);
ojityan 1:a20656b5bfe1 12 void read_current(double *C);
ojityan 1:a20656b5bfe1 13 void controller(double *v,float *duty_output,double *acce_device, double *acce_output);
ojityan 1:a20656b5bfe1 14 void makefile(void);
ojityan 1:a20656b5bfe1 15 void weight_init(void);
ojityan 1:a20656b5bfe1 16 void INA226config(void);
ojityan 0:86efb6256f31 17
ojityan 0:86efb6256f31 18 DigitalOut dir(p25);
ojityan 0:86efb6256f31 19 PwmOut duty(p24);
ojityan 0:86efb6256f31 20 I2C i2c(p28, p27);
ojityan 0:86efb6256f31 21
ojityan 0:86efb6256f31 22 SDFileSystem sd(p11, p12, p13, p14, "sd");
ojityan 0:86efb6256f31 23 ADXL345_I2C accelerometer(p9, p10);
ojityan 0:86efb6256f31 24 Serial debug(USBTX,USBRX);
ojityan 0:86efb6256f31 25 //QEI wheel (p21, p22, p23, 1000);
ojityan 0:86efb6256f31 26
ojityan 0:86efb6256f31 27 DigitalOut myled1(LED1);
ojityan 0:86efb6256f31 28 DigitalOut myled2(LED2);
ojityan 0:86efb6256f31 29 DigitalOut myled3(LED3);
ojityan 0:86efb6256f31 30 DigitalOut myled4(LED4);
ojityan 0:86efb6256f31 31 DigitalIn sensorA(p17);
ojityan 0:86efb6256f31 32 DigitalIn sensorB(p18);
ojityan 0:86efb6256f31 33 DigitalIn sensorC(p19);
ojityan 0:86efb6256f31 34 DigitalIn sensorD(p20);
ojityan 0:86efb6256f31 35 InterruptIn sensorB_(p18);
ojityan 0:86efb6256f31 36
ojityan 0:86efb6256f31 37 int flag = 0;
ojityan 0:86efb6256f31 38 int flipB_ = 0;
ojityan 0:86efb6256f31 39 Ticker save;
ojityan 0:86efb6256f31 40 FILE *fp;
ojityan 0:86efb6256f31 41 Timer t;
ojityan 0:86efb6256f31 42
ojityan 0:86efb6256f31 43
ojityan 0:86efb6256f31 44 static char i2c_addr = 0x80;
ojityan 0:86efb6256f31 45
ojityan 0:86efb6256f31 46
ojityan 0:86efb6256f31 47
ojityan 0:86efb6256f31 48 int main()
ojityan 0:86efb6256f31 49 {
ojityan 1:a20656b5bfe1 50 char buffer[200][250];
ojityan 0:86efb6256f31 51 char rotate[80];
ojityan 0:86efb6256f31 52 int i,j;
ojityan 0:86efb6256f31 53 double C;
ojityan 0:86efb6256f31 54 float duty_output;
ojityan 0:86efb6256f31 55 int loop_break;
ojityan 1:a20656b5bfe1 56 double acce_device;
ojityan 1:a20656b5bfe1 57 double acce_output;
ojityan 0:86efb6256f31 58 int readings[3] = {0, 0, 0};
ojityan 0:86efb6256f31 59 double v;
ojityan 0:86efb6256f31 60
ojityan 1:a20656b5bfe1 61 myled1 = 1;
ojityan 1:a20656b5bfe1 62 ADXL_config();
ojityan 1:a20656b5bfe1 63 myled2 = 1;
ojityan 1:a20656b5bfe1 64 // makefile();
ojityan 0:86efb6256f31 65 myled3 = 1;
ojityan 1:a20656b5bfe1 66 INA226config();
ojityan 0:86efb6256f31 67 myled4 = 1;
ojityan 0:86efb6256f31 68 wait(3.0);
ojityan 0:86efb6256f31 69 myled1 = 0;
ojityan 0:86efb6256f31 70 myled2 = 0;
ojityan 0:86efb6256f31 71 myled3 = 0;
ojityan 0:86efb6256f31 72 myled4 = 0;
ojityan 1:a20656b5bfe1 73
ojityan 1:a20656b5bfe1 74
ojityan 0:86efb6256f31 75 //---ammeter setting end---
ojityan 0:86efb6256f31 76 duty_output = 0.0;
ojityan 0:86efb6256f31 77 duty.period(0.00005);
ojityan 0:86efb6256f31 78 // NVIC_SetPriority(0, 1);
ojityan 0:86efb6256f31 79
ojityan 0:86efb6256f31 80
ojityan 0:86efb6256f31 81 /////////////////////
ojityan 0:86efb6256f31 82 //---main routen---
ojityan 0:86efb6256f31 83 /////////////////////
ojityan 0:86efb6256f31 84 while(1) {
ojityan 0:86efb6256f31 85 t.reset();
ojityan 0:86efb6256f31 86 i = 0;
ojityan 0:86efb6256f31 87 loop_break = 0;
ojityan 0:86efb6256f31 88 debug.printf("writing OK!\n");
ojityan 1:a20656b5bfe1 89
ojityan 1:a20656b5bfe1 90 weight_init();
ojityan 0:86efb6256f31 91
ojityan 0:86efb6256f31 92 //---sensing start---
ojityan 0:86efb6256f31 93
ojityan 0:86efb6256f31 94 // wheel.reset();
ojityan 0:86efb6256f31 95 flipB_ = 0;
ojityan 0:86efb6256f31 96 // __enable_irq();
ojityan 0:86efb6256f31 97 dir = 1;
ojityan 0:86efb6256f31 98 // duty.write(duty_);
ojityan 0:86efb6256f31 99 // debug.printf("motor start\n");
ojityan 1:a20656b5bfe1 100
ojityan 0:86efb6256f31 101 // debug.printf("interrupt start!\n");
ojityan 0:86efb6256f31 102 // sensorB_.rise(&flipB);
ojityan 0:86efb6256f31 103 while (1) {
ojityan 0:86efb6256f31 104 accelerometer.getOutput(readings);
ojityan 0:86efb6256f31 105 if( abs((int16_t)readings[0]) > 30) {
ojityan 0:86efb6256f31 106 myled3 = 1;
ojityan 0:86efb6256f31 107 break;
ojityan 0:86efb6256f31 108 }
ojityan 0:86efb6256f31 109 }
ojityan 0:86efb6256f31 110 save.attach(&savedata, 0.01 );
ojityan 0:86efb6256f31 111 t.start();
ojityan 1:a20656b5bfe1 112
ojityan 1:a20656b5bfe1 113
ojityan 0:86efb6256f31 114 while(t.read() <= 0.50) {
ojityan 0:86efb6256f31 115 // debug.printf("loop sensorB\n");
ojityan 0:86efb6256f31 116 if(flag) {
ojityan 0:86efb6256f31 117 flag = 0;
ojityan 1:a20656b5bfe1 118 controller( &v, &duty_output, &acce_device, &acce_output);
ojityan 1:a20656b5bfe1 119 read_current( &C);
ojityan 1:a20656b5bfe1 120 /* if(1) {
ojityan 1:a20656b5bfe1 121 sprintf(buffer[i],"%f10.3,%f10.2,%d,%f10.3,%f10.3,%f10.3,%f10.3\n",t.read(),C,wheel.getPulses(),acce_device,v,F,duty_output);
ojityan 1:a20656b5bfe1 122 if( i > 299) {
ojityan 1:a20656b5bfe1 123 loop_break = 1;
ojityan 1:a20656b5bfe1 124 break;
ojityan 1:a20656b5bfe1 125 }
ojityan 1:a20656b5bfe1 126 i = i + 1;
ojityan 1:a20656b5bfe1 127 }*/
ojityan 0:86efb6256f31 128
ojityan 0:86efb6256f31 129 }
ojityan 0:86efb6256f31 130 if(loop_break) {
ojityan 0:86efb6256f31 131 break;
ojityan 0:86efb6256f31 132 }
ojityan 0:86efb6256f31 133
ojityan 0:86efb6256f31 134 }
ojityan 0:86efb6256f31 135 myled2 = 1;
ojityan 0:86efb6256f31 136 t.stop();
ojityan 0:86efb6256f31 137
ojityan 0:86efb6256f31 138
ojityan 0:86efb6256f31 139 duty = 0.0f;
ojityan 1:a20656b5bfe1 140 /*
ojityan 0:86efb6256f31 141 for( j = 0; j < i; j++) {
ojityan 1:a20656b5bfe1 142 fprintf(fp,buffer[j]);
ojityan 0:86efb6256f31 143 }
ojityan 1:a20656b5bfe1 144 */
ojityan 0:86efb6256f31 145 // sprintf(rotate,"%f,%d\n",t.read(),wheel.getPulses());
ojityan 1:a20656b5bfe1 146 // fprintf(fp,rotate);
ojityan 0:86efb6256f31 147
ojityan 1:a20656b5bfe1 148 // save.detach();
ojityan 0:86efb6256f31 149 while(1) {
ojityan 0:86efb6256f31 150 myled2 = 1;
ojityan 0:86efb6256f31 151 wait(0.2);
ojityan 0:86efb6256f31 152 myled2 = 0;
ojityan 0:86efb6256f31 153 wait(0.2);
ojityan 0:86efb6256f31 154 }
ojityan 0:86efb6256f31 155 }
ojityan 0:86efb6256f31 156
ojityan 0:86efb6256f31 157 }
ojityan 0:86efb6256f31 158
ojityan 0:86efb6256f31 159
ojityan 0:86efb6256f31 160
ojityan 0:86efb6256f31 161
ojityan 0:86efb6256f31 162 void savedata(void)
ojityan 0:86efb6256f31 163 {
ojityan 0:86efb6256f31 164 flag = 1;
ojityan 0:86efb6256f31 165 // debug.printf("interrupt!\n");
ojityan 0:86efb6256f31 166 }
ojityan 0:86efb6256f31 167
ojityan 0:86efb6256f31 168 void flipB(void)
ojityan 0:86efb6256f31 169 {
ojityan 0:86efb6256f31 170 flipB_ = 1;
ojityan 0:86efb6256f31 171 duty = 0.0f;
ojityan 0:86efb6256f31 172 debug.printf("flip sensorB!");
ojityan 0:86efb6256f31 173 __disable_irq();
ojityan 0:86efb6256f31 174 }
ojityan 1:a20656b5bfe1 175
ojityan 1:a20656b5bfe1 176 void ADXL_config(void)
ojityan 1:a20656b5bfe1 177 {
ojityan 1:a20656b5bfe1 178 //Go into standby mode to configure the device.
ojityan 1:a20656b5bfe1 179 accelerometer.setPowerControl(0x00);
ojityan 1:a20656b5bfe1 180
ojityan 1:a20656b5bfe1 181 //Full resolution, +/-16g, 4mg/LSB.
ojityan 1:a20656b5bfe1 182 accelerometer.setDataFormatControl(0x0B);
ojityan 1:a20656b5bfe1 183
ojityan 1:a20656b5bfe1 184 //3.2kHz data rate.
ojityan 1:a20656b5bfe1 185 accelerometer.setDataRate(ADXL345_3200HZ);
ojityan 1:a20656b5bfe1 186
ojityan 1:a20656b5bfe1 187 //Measurement mode.
ojityan 1:a20656b5bfe1 188 accelerometer.setPowerControl(0x08);
ojityan 1:a20656b5bfe1 189
ojityan 1:a20656b5bfe1 190 accelerometer.setOffset (0, 0);
ojityan 1:a20656b5bfe1 191
ojityan 1:a20656b5bfe1 192 }
ojityan 1:a20656b5bfe1 193
ojityan 1:a20656b5bfe1 194 void makefile(void)
ojityan 1:a20656b5bfe1 195 {
ojityan 1:a20656b5bfe1 196 char filename[80];
ojityan 1:a20656b5bfe1 197 int num;
ojityan 1:a20656b5bfe1 198 //---SDsetting--
ojityan 1:a20656b5bfe1 199 mkdir("/sd/mydir", 0777);
ojityan 1:a20656b5bfe1 200 for(num=1; num<=256; num=num+1) {
ojityan 1:a20656b5bfe1 201 sprintf(filename,"/sd/mydir/data%d.csv",num);
ojityan 1:a20656b5bfe1 202 fp = fopen(filename, "r");
ojityan 1:a20656b5bfe1 203 if(fp == NULL) {
ojityan 1:a20656b5bfe1 204 fp = fopen(filename,"w");
ojityan 1:a20656b5bfe1 205 break;
ojityan 1:a20656b5bfe1 206 }
ojityan 1:a20656b5bfe1 207 fclose(fp);
ojityan 1:a20656b5bfe1 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 1:a20656b5bfe1 258 }
ojityan 1:a20656b5bfe1 259
ojityan 1:a20656b5bfe1 260 void controller(double *v,float *duty_output,double *acce_device, double *acce_output)
ojityan 1:a20656b5bfe1 261 {
ojityan 1:a20656b5bfe1 262
ojityan 1:a20656b5bfe1 263 double ka = 100;
ojityan 1:a20656b5bfe1 264 double kv = 100;
ojityan 1:a20656b5bfe1 265
ojityan 1:a20656b5bfe1 266 int readings[3] = {0,0,0};
ojityan 1:a20656b5bfe1 267 static double F = 0;
ojityan 1:a20656b5bfe1 268 static double F_1 = 0;
ojityan 1:a20656b5bfe1 269 static double F_2 = 0;
ojityan 1:a20656b5bfe1 270 static double F_3 = 0;
ojityan 1:a20656b5bfe1 271
ojityan 1:a20656b5bfe1 272
ojityan 1:a20656b5bfe1 273 //---reading acceleration and control---
ojityan 1:a20656b5bfe1 274 accelerometer.getOutput(readings);
ojityan 1:a20656b5bfe1 275 *acce_device = (int16_t)readings[0] * 0.0383;
ojityan 1:a20656b5bfe1 276 *v = *v + *acce_device * 0.01;
ojityan 1:a20656b5bfe1 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 1:a20656b5bfe1 280 F = 0.35 * F_1 + 0.4 * F_2 + 0.25 * F_3;
ojityan 1:a20656b5bfe1 281 *acce_output = F / 5.0;
ojityan 1:a20656b5bfe1 282 *duty_output = *acce_output / 118 + *duty_output;
ojityan 1:a20656b5bfe1 283 if(*duty_output > 0.65) {
ojityan 1:a20656b5bfe1 284 *duty_output = 0.65;
ojityan 1:a20656b5bfe1 285 myled4 = 1;
ojityan 1:a20656b5bfe1 286 }
ojityan 1:a20656b5bfe1 287 duty.write(*duty_output);
ojityan 1:a20656b5bfe1 288 F_3 = F_2;
ojityan 1:a20656b5bfe1 289 F_2 = F;
ojityan 1:a20656b5bfe1 290 debug.printf("%i,%f,%f\n", (int16_t)readings[0],*v,*duty_output);
ojityan 1:a20656b5bfe1 291 }
ojityan 1:a20656b5bfe1 292 }
ojityan 1:a20656b5bfe1 293 }
ojityan 1:a20656b5bfe1 294
ojityan 1:a20656b5bfe1 295 void read_current(double *C)
ojityan 1:a20656b5bfe1 296 {
ojityan 1:a20656b5bfe1 297 char I[2];
ojityan 1:a20656b5bfe1 298 unsigned short read_item;
ojityan 1:a20656b5bfe1 299 char p_addr = 0x00;
ojityan 1:a20656b5bfe1 300 char p_addr_i = 0x04;
ojityan 1:a20656b5bfe1 301 //---reading current---
ojityan 1:a20656b5bfe1 302 while(i2c.write(i2c_addr | 0,&p_addr_i,sizeof(p_addr_i)) != 0);
ojityan 1:a20656b5bfe1 303 while(i2c.read(i2c_addr | 0x01,I,sizeof(I)) != 0);
ojityan 1:a20656b5bfe1 304 read_item = static_cast<unsigned short>(I[0]);
ojityan 1:a20656b5bfe1 305 read_item = (read_item << 8) | static_cast<unsigned short>(I[1]);
ojityan 1:a20656b5bfe1 306
ojityan 1:a20656b5bfe1 307 char *s_p = reinterpret_cast<char *>(&read_item);
ojityan 1:a20656b5bfe1 308 short d_s;
ojityan 1:a20656b5bfe1 309 char *d_p = reinterpret_cast<char *>(&d_s);
ojityan 1:a20656b5bfe1 310 *(d_p + 0) = *(s_p + 0);
ojityan 1:a20656b5bfe1 311 *(d_p + 1) = *(s_p + 1);
ojityan 1:a20656b5bfe1 312 *C = static_cast<double>(d_s) /* * 1.25 */;
ojityan 1:a20656b5bfe1 313 debug.printf("%f\n",*C);
ojityan 1:a20656b5bfe1 314 }