あの装置用
Dependencies: ADXL345_I2C QEI SDFileSystem mbed
Revision 2:49d3b3eebc2e, committed 2017-02-07
- Comitter:
- ojityan
- Date:
- Tue Feb 07 18:55:27 2017 +0000
- Parent:
- 1:a20656b5bfe1
- Child:
- 3:de6579180af2
- Commit message:
- ???????????
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Thu Feb 02 09:00:20 2017 +0000 +++ b/main.cpp Tue Feb 07 18:55:27 2017 +0000 @@ -1,8 +1,7 @@ #include "mbed.h" #include "SDFileSystem.h" #include "ADXL345_I2C.h" -#include "QEI.h" - +//#include "QEI.h" #include <stdio.h> #include <stdlib.h> @@ -10,7 +9,7 @@ void flipB(void); void ADXL_config(void); void read_current(double *C); -void controller(double *v,float *duty_output,double *acce_device, double *acce_output); +void controller(double *v,float *duty_output,double *acce_device, double *acce_output, double *F); void makefile(void); void weight_init(void); void INA226config(void); @@ -47,8 +46,7 @@ int main() { - char buffer[200][250]; - char rotate[80]; + char buffer[100][150]; int i,j; double C; float duty_output; @@ -57,6 +55,7 @@ double acce_output; int readings[3] = {0, 0, 0}; double v; + double F_output; myled1 = 1; ADXL_config(); @@ -86,26 +85,20 @@ i = 0; loop_break = 0; debug.printf("writing OK!\n"); - + weight_init(); //---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); if( abs((int16_t)readings[0]) > 30) { myled3 = 1; break; } + wait(0.01); } save.attach(&savedata, 0.01 ); t.start(); @@ -115,16 +108,18 @@ // debug.printf("loop sensorB\n"); if(flag) { flag = 0; - controller( &v, &duty_output, &acce_device, &acce_output); + controller( &v, &duty_output, &acce_device, &acce_output, &F_output); +// duty.write(duty_output); read_current( &C); - /* if(1) { - 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); - if( i > 299) { - loop_break = 1; - break; - } - i = i + 1; - }*/ +// debug.printf("duty = %f\n",duty_output); + if(1) { + sprintf(buffer[i],"%f10,%f10,%f10,%f10,%f10,%f10",t.read(),C,acce_device,C,duty_output,F_output); + if( i > 299) { + loop_break = 1; + break; + } + i = i + 1; + } } if(loop_break) { @@ -134,26 +129,28 @@ } myled2 = 1; t.stop(); + save.detach(); duty = 0.0f; -/* + + +// makefile(); + myled1 = 1; for( j = 0; j < i; j++) { - fprintf(fp,buffer[j]); +// fprintf(fp,buffer[j]); + debug.printf("%s\n",buffer[j]); } -*/ -// sprintf(rotate,"%f,%d\n",t.read(),wheel.getPulses()); -// fprintf(fp,rotate); + +// fclose(fp); -// save.detach(); while(1) { - myled2 = 1; + myled1 = 1; wait(0.2); - myled2 = 0; + myled1 = 0; wait(0.2); } } - } @@ -162,7 +159,7 @@ void savedata(void) { flag = 1; -// debug.printf("interrupt!\n"); + } void flipB(void) @@ -193,7 +190,7 @@ void makefile(void) { - char filename[80]; + char filename[100]; int num; //---SDsetting-- mkdir("/sd/mydir", 0777); @@ -202,10 +199,15 @@ fp = fopen(filename, "r"); if(fp == NULL) { fp = fopen(filename,"w"); + debug.printf("file number is %d.\n",num); break; } fclose(fp); + debug.printf("data%d exists\n",num); } + +// fprintf(fp,"Time,Current,acce_device,v_device,duty_output\n"); + // fprintf(fp, "Hello fun SD Card World!\n"); } @@ -257,28 +259,29 @@ wait(2.0); } -void controller(double *v,float *duty_output,double *acce_device, double *acce_output) +void controller(double *v,float *duty_output,double *acce_device, double *acce_output, double *F) { - + double ka = 100; double kv = 100; - + int readings[3] = {0,0,0}; - static double F = 0; - static double F_1 = 0; - static double F_2 = 0; - static double F_3 = 0; +// static double F; + static double F_1; + static double F_2; + static double F_3; //---reading acceleration and control--- accelerometer.getOutput(readings); *acce_device = (int16_t)readings[0] * 0.0383; +// debug.printf("acce_device = %f\n", *acce_device); *v = *v + *acce_device * 0.01; - if( acce_device < 0) { + if( *acce_device < 0) { if(abs(*v) > 0) { F_1 = -1 * ( ka * *acce_device) + (kv * *v); - F = 0.35 * F_1 + 0.4 * F_2 + 0.25 * F_3; - *acce_output = F / 5.0; + *F = 0.35 * F_1 + 0.4 * F_2 + 0.25 * F_3; + *acce_output = *F / 8.5; *duty_output = *acce_output / 118 + *duty_output; if(*duty_output > 0.65) { *duty_output = 0.65; @@ -286,8 +289,8 @@ } duty.write(*duty_output); F_3 = F_2; - F_2 = F; - debug.printf("%i,%f,%f\n", (int16_t)readings[0],*v,*duty_output); + F_2 = *F; +// debug.printf("%i,%f,%f\n", (int16_t)readings[0],*v,*duty_output); } } } @@ -310,5 +313,5 @@ *(d_p + 0) = *(s_p + 0); *(d_p + 1) = *(s_p + 1); *C = static_cast<double>(d_s) /* * 1.25 */; - debug.printf("%f\n",*C); +// debug.printf("%f\n",*C); } \ No newline at end of file