匡尚 青柳 / 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@0:86efb6256f31, 2017-01-30 (annotated)

Committer:
ojityan
Date:
Mon Jan 30 01:03:32 2017 +0000
Revision:
0:86efb6256f31
Child:
1:a20656b5bfe1
can't save

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