Revision 1:a20656b5bfe1, committed 2017-02-02

Comitter:

ojityan

Date:

Thu Feb 02 09:00:20 2017 +0000

Parent:

0:86efb6256f31

Child:

2:49d3b3eebc2e

Commit message:

can't move

Changed in this revision

--- a/main.cpp	Mon Jan 30 01:03:32 2017 +0000
+++ b/main.cpp	Thu Feb 02 09:00:20 2017 +0000
@@ -8,6 +8,12 @@
 
 void savedata(void);
 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 makefile(void);
+void weight_init(void);
+void INA226config(void);
 
 DigitalOut dir(p25);
 PwmOut duty(p24);
@@ -41,118 +47,31 @@
 
 int main()
 {
-
-
-
-//    int i;
-
-    char filename[80];
-    char a[200][250];
+    char buffer[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;
-
+    double acce_device;
+    double acce_output;
     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");
+    myled1 = 1;
+    ADXL_config();
+    myled2 = 1;
+//    makefile();
     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");
-    }
-
+    INA226config();
     myled4 = 1;
     wait(3.0);
     myled1 = 0;
     myled2 = 0;
     myled3 = 0;
     myled4 = 0;
-    
-    
+
+
 //---ammeter setting end---
     duty_output = 0.0;
     duty.period(0.00005);
@@ -163,20 +82,12 @@
 //---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);
+        
+        weight_init();
 
 //---sensing start---
 
@@ -186,13 +97,11 @@
         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;
@@ -200,55 +109,22 @@
         }
         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;
-                }*/
+                controller( &v, &duty_output, &acce_device, &acce_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;
+                                }*/
 
             }
             if(loop_break) {
@@ -261,18 +137,15 @@
 
 
         duty = 0.0f;
-
+/*
         for( j = 0; j < i; j++) {
-            fprintf(fp,a[j]);
+            fprintf(fp,buffer[j]);
         }
-
+*/
 //        sprintf(rotate,"%f,%d\n",t.read(),wheel.getPulses());
-        fprintf(fp,rotate);
-
+//        fprintf(fp,rotate);
 
-        wait(3.0);
-
-        save.detach();
+//        save.detach();
         while(1) {
             myled2 = 1;
             wait(0.2);
@@ -299,3 +172,143 @@
     debug.printf("flip sensorB!");
     __disable_irq();
 }
+
+void ADXL_config(void)
+{
+    //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);
+
+}
+
+void makefile(void)
+{
+    char filename[80];
+    int num;
+    //---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");
+}
+
+void INA226config(void)
+{
+
+    char p_addr = 0x00;
+    char p_addr_i = 0x04;
+    short Calibration_set = 0x0A00;
+    char write_item[3];
+//---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);
+
+    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");
+    }
+
+
+
+    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");
+    }
+}
+
+void weight_init(void)
+{
+//---position reset---
+    dir = 0;
+    duty = 0.50f;
+    while(!sensorD);
+    duty = 0.0f;
+    wait(2.0);
+}
+
+void controller(double *v,float *duty_output,double *acce_device, double *acce_output)
+{
+    
+    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;
+
+
+//---reading acceleration and control---
+    accelerometer.getOutput(readings);
+    *acce_device = (int16_t)readings[0] * 0.0383;
+    *v = *v + *acce_device * 0.01;
+    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;
+            *duty_output = *acce_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);
+        }
+    }
+}
+
+void read_current(double *C)
+{
+    char I[2];
+    unsigned short read_item;
+    char p_addr = 0x00;
+    char p_addr_i = 0x04;
+//---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 */;
+    debug.printf("%f\n",*C);
+}
\ No newline at end of file