Diff: main.cpp

Revision:

2:49d3b3eebc2e

Parent:

1:a20656b5bfe1

Child:

3:de6579180af2

diff -r a20656b5bfe1 -r 49d3b3eebc2e main.cpp
--- 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