あの装置用
Dependencies: ADXL345_I2C QEI SDFileSystem mbed
Revision 0:86efb6256f31, committed 2017-01-30
- Comitter:
- ojityan
- Date:
- Mon Jan 30 01:03:32 2017 +0000
- Child:
- 1:a20656b5bfe1
- Commit message:
- can't save
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ADXL345_I2C.lib Mon Jan 30 01:03:32 2017 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/nimbusgb/code/ADXL345_I2C/#92fa975dab32
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/QEI.lib Mon Jan 30 01:03:32 2017 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/aberk/code/QEI/#5c2ad81551aa
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/SDFileSystem.lib Mon Jan 30 01:03:32 2017 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/neilt6/code/SDFileSystem/#e4d2567200db
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Mon Jan 30 01:03:32 2017 +0000 @@ -0,0 +1,301 @@ +#include "mbed.h" +#include "SDFileSystem.h" +#include "ADXL345_I2C.h" +#include "QEI.h" + +#include <stdio.h> +#include <stdlib.h> + +void savedata(void); +void flipB(void); + +DigitalOut dir(p25); +PwmOut duty(p24); +I2C i2c(p28, p27); + +SDFileSystem sd(p11, p12, p13, p14, "sd"); +ADXL345_I2C accelerometer(p9, p10); +Serial debug(USBTX,USBRX); +//QEI wheel (p21, p22, p23, 1000); + +DigitalOut myled1(LED1); +DigitalOut myled2(LED2); +DigitalOut myled3(LED3); +DigitalOut myled4(LED4); +DigitalIn sensorA(p17); +DigitalIn sensorB(p18); +DigitalIn sensorC(p19); +DigitalIn sensorD(p20); +InterruptIn sensorB_(p18); + +int flag = 0; +int flipB_ = 0; +Ticker save; +FILE *fp; +Timer t; + + +static char i2c_addr = 0x80; + + + +int main() +{ + + + +// int i; + + char filename[80]; + char a[200][250]; + char rotate[80]; + int i,j; + int num; + double C; + unsigned short read_item; + char p_addr = 0x00; + char p_addr_i = 0x04; + char I[2]; + float duty_output; + int loop_break; + + double F = 0; + double F_1 = 0; + double F_2 = 0; + double F_3 = 0; + + double a_device; + double a_output; + + + double ka = 100; + double kv = 100; + + int readings[3] = {0, 0, 0}; +// int16_t a[3] = {0,0,0}; + double a_; + double v; + + myled1 = 1; + +//---ADXL345setting--- +// debug.printf("Starting ADXL345 test...\n"); +// debug.printf("Device ID is: 0x%02x\n", accelerometer.getDevId()); + + //Go into standby mode to configure the device. + accelerometer.setPowerControl(0x00); + + //Full resolution, +/-16g, 4mg/LSB. + accelerometer.setDataFormatControl(0x0B); + + //3.2kHz data rate. + accelerometer.setDataRate(ADXL345_3200HZ); + + //Measurement mode. + accelerometer.setPowerControl(0x08); + + accelerometer.setOffset (0, 0); + + myled2 = 1; + +// printf("Hello World!\n"); + +//---SDsetting-- + mkdir("/sd/mydir", 0777); + for(num=1; num<=256; num=num+1) { + sprintf(filename,"/sd/mydir/data%d.csv",num); + fp = fopen(filename, "r"); + if(fp == NULL) { + fp = fopen(filename,"w"); + break; + } + fclose(fp); + } +// fprintf(fp, "Hello fun SD Card World!\n"); + myled3 = 1; +//---ammeter setting--- + i2c.start(); + if(i2c.write(i2c_addr | 0,&p_addr,sizeof(p_addr)) == 0) { + debug.printf("DA-ME-DA-YO!\n"); + } + debug.printf("OK!\n"); + + //rawWrite(0x05,0x0A00); + + + char write_item[3]; + short Config_set = 0x444F;//0x4A4F;//0x4E97;//0x4C97; + write_item[0] = 0x00; + write_item[1] = static_cast<char>((Config_set >> 8) & 0x00ff); + write_item[2] = static_cast<char>(Config_set & 0x00ff); + if(i2c.write(i2c_addr | 0,write_item,3) == 0) { + debug.printf("OK!!\n"); + } else { + debug.printf("OH!!\n"); + } + + + short Calibration_set = 0x0A00; + write_item[0] = 0x05; + write_item[1] = static_cast<char>((Calibration_set >> 8) & 0x00ff); + write_item[2] = static_cast<char>(Calibration_set & 0x00ff); + if(i2c.write(i2c_addr | 0,write_item,3) == 0) { + debug.printf("OK!!\n"); + } else { + debug.printf("OH!!\n"); + } + + myled4 = 1; + wait(3.0); + myled1 = 0; + myled2 = 0; + myled3 = 0; + myled4 = 0; + + +//---ammeter setting end--- + duty_output = 0.0; + duty.period(0.00005); +// NVIC_SetPriority(0, 1); + + +///////////////////// +//---main routen--- +///////////////////// + while(1) { + //debug.printf("enter the loop\n"); + t.reset(); + i = 0; + loop_break = 0; + debug.printf("writing OK!\n"); +//---position reset--- + dir = 0; + duty = 0.50f; + while(!sensorD) { +// debug.printf("loop sensorD\n"); + } +// debug.printf("loop end\n"); + duty = 0.0f; + wait(2.0); + +//---sensing start--- + +// wheel.reset(); + flipB_ = 0; +// __enable_irq(); + dir = 1; +// duty.write(duty_); +// debug.printf("motor start\n"); + +// debug.printf("interrupt start!\n"); +// sensorB_.rise(&flipB); + while (1) { + accelerometer.getOutput(readings); +// debug.printf("%i\n", abs((int16_t)readings[0])); + + if( abs((int16_t)readings[0]) > 30) { + myled3 = 1; + break; + } + } + save.attach(&savedata, 0.01 ); + t.start(); + + + while(t.read() <= 0.50) { +// debug.printf("loop sensorB\n"); + if(flag) { + flag = 0; + +//---reading acceleration and control--- + accelerometer.getOutput(readings); + //a[0] = (int16_t)readings[0]; + a_device = (int16_t)readings[0] * 0.0383; + v = v + a_device * 0.01; + if( a_device < 0) { + if(abs(v) > 0) { + F_1 = -1 * ( ka * a_device) + (kv * v); + F = 0.35 * F_1 + 0.4 * F_2 + 0.25 * F_3; + a_output = F / 5.0; + duty_output = a_output / 118 + duty_output; + if(duty_output > 0.65) { + duty_output = 0.65; + myled4 = 1; + } + duty.write(duty_output); + F_3 = F_2; + F_2 = F; + debug.printf("%i,%f,%f\n", (int16_t)readings[0],v,duty_output); + } + } + +//---reading current--- + while(i2c.write(i2c_addr | 0,&p_addr_i,sizeof(p_addr_i)) != 0); + while(i2c.read(i2c_addr | 0x01,I,sizeof(I)) != 0); + read_item = static_cast<unsigned short>(I[0]); + read_item = (read_item << 8) | static_cast<unsigned short>(I[1]); + + char *s_p = reinterpret_cast<char *>(&read_item); + short d_s; + char *d_p = reinterpret_cast<char *>(&d_s); + *(d_p + 0) = *(s_p + 0); + *(d_p + 1) = *(s_p + 1); + C = static_cast<double>(d_s) /* * 1.25 */; +/* if(1) { + 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); + if( i > 299) { + loop_break = 1; + break; + } + i = i + 1; + }*/ + + } + if(loop_break) { + break; + } + + } + myled2 = 1; + t.stop(); + + + duty = 0.0f; + + for( j = 0; j < i; j++) { + fprintf(fp,a[j]); + } + +// sprintf(rotate,"%f,%d\n",t.read(),wheel.getPulses()); + fprintf(fp,rotate); + + + wait(3.0); + + save.detach(); + while(1) { + myled2 = 1; + wait(0.2); + myled2 = 0; + wait(0.2); + } + } + +} + + + + +void savedata(void) +{ + flag = 1; +// debug.printf("interrupt!\n"); +} + +void flipB(void) +{ + flipB_ = 1; + duty = 0.0f; + debug.printf("flip sensorB!"); + __disable_irq(); +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Mon Jan 30 01:03:32 2017 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/99b5ccf27215 \ No newline at end of file