Revision 5:7d768f3acdd8, committed 2020-12-28

Comitter:

sashida_h

Date:

Mon Dec 28 02:43:23 2020 +0000

Parent:

4:56d1f3a81b30

Commit message:

version used in NSE2020

Changed in this revision

--- a/main.cpp	Thu Sep 17 02:18:16 2020 +0000
+++ b/main.cpp	Mon Dec 28 02:43:23 2020 +0000
@@ -14,16 +14,21 @@
 LC1025 eeprom = LC1025(PB_7, PB_6);
 DigitalOut led(PB_5);
 
+int rom_write(float);
+int most_brightness(float,float,float,float);
 
+Timer timer;
 CANMessage msg;
 
 #define T 0.03
 
-float cal_x=0,cal_y=0,cal_z=0;
-int i,j,cnt=0,page_frg=0;
-float world_time;
+float x=0.0,y=0.0,z=0.0;
+float x_cal=0.0,y_cal=0.0,z_cal=0.0;
+int i,j,m_bri,cnt=0,page_frg=0;
+float world_time,t;
 uint16_t eeprom_cnt = 0x0000;
 char serialData[4];
+int tf_mem =0;
 
 union Float2Byte{
     float _float;
@@ -65,7 +70,7 @@
     //ここに送りたい値を入れる．
     
     char serialData[6];
-    
+
     serialData[0] = sendInt._byte[0];
     serialData[1] = sendInt._byte[1];
         //pc.printf("send_char: %d\n\r", serialData[i]);
@@ -96,89 +101,22 @@
                 //pc.printf("get_char: %d\n\r", getFloat._byte[i]);
             }
             world_time = getFloat._float;
-            pc.printf("world_time:%.2f\r\n", world_time);
+            //pc.printf("world_time:%.2f\r\n", world_time);
         }
         
         if(msg.id == 0x02){
-            //pc.printf("ID: 0x02\n\r");
-            getInt._byte[0] = msg.data[0];
-            getInt._byte[1] = msg.data[1];
-            cal_x=(float)getInt._int /10.0f;
-            pc.printf("Mag:%.2f", cal_x);
-            
-            getInt._byte[0] = msg.data[2];
-            getInt._byte[1] = msg.data[3];
-            cal_y=(float)getInt._int /10.0f;
-            pc.printf(",%.2f", cal_y);
-            
-            getInt._byte[0] = msg.data[4];
-            getInt._byte[1] = msg.data[5];
-            cal_z=(float)getInt._int /10.0f;
-            pc.printf(",%.2f\r\n", cal_z);
-            
-            if(page_frg == 0){
-                pc.printf("s:%d,0x%x\r\n",page_frg,eeprom_cnt);
-                *(float*)serialData=0.0f;
-                eeprom.PageWrite(ADDRESS_24LC1025_BLOCK0,eeprom_cnt,serialData,4);
-                eeprom_cnt = 0x04 + eeprom_cnt;
-                wait(0.01);
-                pc.printf("s:%d,0x%x\r\n",page_frg,eeprom_cnt);
-                *(float*)serialData=cal_x;
-                eeprom.PageWrite(ADDRESS_24LC1025_BLOCK0,eeprom_cnt,serialData,4);
-                wait(0.01);
-                eeprom_cnt = 0x04 + eeprom_cnt;
-                pc.printf("s:%d,0x%x\r\n",page_frg,eeprom_cnt);
-                *(float*)serialData=cal_y;
-                eeprom.PageWrite(ADDRESS_24LC1025_BLOCK0,eeprom_cnt,serialData,4);
-                wait(0.01);
-                eeprom_cnt = 0x04 + eeprom_cnt;
-                pc.printf("s:%d,0x%x\r\n",page_frg,eeprom_cnt);
-                *(float*)serialData=cal_z;
-                eeprom.PageWrite(ADDRESS_24LC1025_BLOCK0,eeprom_cnt,serialData,4);
-                wait(0.01);
-                eeprom_cnt = 0x04 + eeprom_cnt;
-                pc.printf("s:%d,0x%x\r\n",page_frg,eeprom_cnt);
-                *(float*)serialData=world_time;
-                eeprom.PageWrite(ADDRESS_24LC1025_BLOCK0,eeprom_cnt,serialData,4);
-                wait(0.01);
-                eeprom_cnt = 0x04 + eeprom_cnt;
-                pc.printf("s:%d,0x%x\r\n",page_frg,eeprom_cnt);
-                *(float*)serialData=0.0f;
-                eeprom.PageWrite(ADDRESS_24LC1025_BLOCK0,eeprom_cnt,serialData,4);
-                wait(0.01);
-                eeprom_cnt = 0x04 + eeprom_cnt;
-                pc.printf("e:%d,0x%x\r\n",page_frg,eeprom_cnt);
-            }else if(page_frg == 1){
-                *(float*)serialData=0.0f;
-                pc.printf("%d,0x%x\r\n",page_frg,eeprom_cnt);
-                eeprom.PageWrite(ADDRESS_24LC1025_BLOCK1,eeprom_cnt,serialData,4);
-                wait(0.01);
-                eeprom_cnt = 0x04 + eeprom_cnt;
-                *(float*)serialData=cal_x;
-                eeprom.PageWrite(ADDRESS_24LC1025_BLOCK1,eeprom_cnt,serialData,4);
-                wait(0.01);
-                eeprom_cnt = 0x04 + eeprom_cnt;
-                *(float*)serialData=cal_y;
-                eeprom.PageWrite(ADDRESS_24LC1025_BLOCK1,eeprom_cnt,serialData,4);
-                wait(0.01);
-                eeprom_cnt = 0x04 + eeprom_cnt;
-                *(float*)serialData=cal_z;
-                eeprom.PageWrite(ADDRESS_24LC1025_BLOCK1,eeprom_cnt,serialData,4);
-                wait(0.01);
-                eeprom_cnt = 0x04 + eeprom_cnt;
-                *(float*)serialData=world_time;
-                eeprom.PageWrite(ADDRESS_24LC1025_BLOCK1,eeprom_cnt,serialData,4);
-                wait(0.01);
-                eeprom_cnt = 0x04 + eeprom_cnt;
-                *(float*)serialData=0.0f;
-                eeprom.PageWrite(ADDRESS_24LC1025_BLOCK1,eeprom_cnt,serialData,4);
-                eeprom_cnt = 0x04 + eeprom_cnt;
-                wait(0.01);            
-            }else{
-                pc.printf("EEPROM_ERROR!! You should erase memory\r\n");
+            if(tf_mem == 0){
+                tf_mem = 2;
+                x=0.0f;
+                y=0.0f;
+                z=0.0f;
+                timer.start();
+                if(m_bri == 2) z_cal= 0.0;
+                if(m_bri == 3) z_cal = 0 - 270.0f;
+                if(m_bri == 4) z_cal = 0 - 180.0f;
+                if(m_bri == 5) z_cal = 0 - 90.0f;
+                pc.printf("ID: 0x02\n\r");
             }
-            
-            
         }
         
         if(msg.id == 0x03){
@@ -212,11 +150,11 @@
     led = 1;
     wait(1.0);
     *(float*)serialData=0.0f;
-    for(i=0;i<6;i++){
+    for(i=0;i<10;i++){
         eeprom.PageWrite(ADDRESS_24LC1025_BLOCK0,eeprom_cnt,serialData,4);
         eeprom_cnt = 0x04 + eeprom_cnt;
         wait(0.1);
-    }            
+    }
     
     while(1){
         adc1[0] = mcp1.sgl( CH0 );
@@ -315,119 +253,130 @@
         //pc.printf("%d,%d,%d,%d\r\n",adc1[0],adc1[1],adc1[2],adc1[3]);
         ave[5] = (adc6[1] + adc6[2])/2.0 - 100.0;
         
-        /*
-        pc.printf("1");
-        for(i=0; i<4; i++){
-            //pc.printf("CH%d:%d\r\n",i,adc1[i]);
-            pc.printf(",%d",adc1[i]);
-        }
-        pc.printf(",%.1f\r\n", ave1);
-        
-        pc.printf("2");
-        for(i=0; i<4; i++){
-            //pc.printf("CH%d:%d\r\n",i,adc1[i]);
-            pc.printf(",%d",adc2[i]);
-        }
-        pc.printf(",%.1f\r\n", ave2);
-        
-        pc.printf("3");
-        for(i=0; i<4; i++){
-            //pc.printf("CH%d:%d\r\n",i,adc1[i]);
-            pc.printf(",%d",adc3[i]);
-        }
-        pc.printf(",%.1f\r\n", ave3);
-        
-        pc.printf("4");
-        for(i=0; i<4; i++){
-            //pc.printf("CH%d:%d\r\n",i,adc1[i]);
-            pc.printf(",%d",adc4[i]);
-        }
-        pc.printf(",%.1f\r\n", ave4);
-        
-        pc.printf("5");
-        for(i=0; i<4; i++){
-            //pc.printf("CH%d:%d\r\n",i,adc1[i]);
-            pc.printf(",%d",adc5[i]);
-        }
-        pc.printf(",%.1f\r\n", ave5);
-        
-        pc.printf("6");
-        for(i=0; i<4; i++){
-            //pc.printf("CH%d:%d\r\n",i,adc1[i]);
-            pc.printf(",%d",adc6[i]);
-        }
-        pc.printf(",%.1f\r\n", ave6);
-        */
         
         cnt = 0;
         
-        if(ave[1] < ave[2] && ave[3] > ave[5] && ave[4] < ave[0]){
-            send2((unsigned short)(0.0f+cal_x),(unsigned short)(0.0f+cal_y),(unsigned short)(0.0f+cal_z),6);
-            //else if(ave[1] < ave[2] && ave[3] < ave[5] && ave[4] < ave[0])pc.printf("x:90,y:0,z:0\r\n");
-        }else if(ave[1] < ave[2] && ave[3] < ave[5] && ave[4] > ave[0]){
-            send2((unsigned short)(180.0f+cal_x),(unsigned short)(0.0f+cal_y),(unsigned short)(0.0f+cal_z),6);
-            //else if(ave[1] < ave[2] && ave[3] > ave[5] && ave[4] > ave[0])pc.printf("x:270,y:0,z:0\r\n");
-        }else if(ave[1] < ave[2] && ave[3] > ave[5] && ave[4] < ave[0]){
-            send2((unsigned short)(0.0f+cal_x),(unsigned short)(0.0f+cal_y),(unsigned short)(0.0f+cal_z),6);
-            //else if(ave[1] < ave[2] && ave[3] < ave[5] && ave[4] < ave[0])pc.printf("x:0,y:0,z:90\r\n");
-        }else if(ave[1] > ave[2] && ave[3] < ave[5] && ave[4] < ave[0]){
-            send2((unsigned short)(0.0f+cal_x),(unsigned short)(0.0f+cal_y),(unsigned short)(180.0f+cal_z),6);
-            //else if(ave[1] > ave[2] && ave[3] > ave[5] && ave[4] < ave[0])pc.printf("x:0,y:0,z:270\r\n");
-        }else if(ave[1] < ave[2] && ave[3] > ave[5] && ave[4] < ave[0]){
-            send2((unsigned short)(0.0f+cal_x),(unsigned short)(0.0f+cal_y),(unsigned short)(0.0f+cal_z),6);
-            //else if(ave[1] < ave[2] && ave[3] > ave[5] && ave[4] > ave[0])pc.printf("x:0,y:90,z:0\r\n");
-        }else if(ave[1] > ave[2] && ave[3] > ave[5] && ave[4] > ave[0]){
-            send2((unsigned short)(0.0f+cal_x),(unsigned short)(180.0f+cal_y),(unsigned short)(0.0f+cal_z),6);
-            //else if(ave[1] > ave[2] && ave[3] > ave[5] && ave[4] > ave[0])pc.printf("x:0,y:270,z:0\r\n");
+        t = timer.read();
+        if(t > 1800){
+            timer.reset();
+        }
+        //光姿勢判定
+        for(i=0;i<6-1;i++){
+            for(j=1;j<6-i;j++){
+                if(ave[j-1] > ave[j]){
+                    buff = adc6[j-1];
+                    adc6[j-1] = adc6[j];
+                    adc6[j] = buff;
+                }
+            }
+        }
+        //光姿勢判定
+        m_bri = most_brightness(ave[2],ave[3],ave[4],ave[5]);
+        
+        if(ave[0] < ave[1]){
+            x = 0.0f;
+            y = 0.0f;
         }else{
-             pc.printf("none\r\n");
+            x = 180.0f;
+            y = 0.0f;
         }
-        //pc.printf("%d,%x,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f\r\n",page_frg,eeprom_cnt,ave[1],ave[2],ave[3],ave[5],ave[4],ave[0]);
+        
+        if(m_bri == 2) z = z_cal;
+        if(m_bri == 3) z = z_cal + 270.0f;
+        if(m_bri == 4) z = z_cal + 180.0f;
+        if(m_bri == 5) z = z_cal + 90.0f;
+        
+        if(z < 0.0f)z= z + 360.0f;
+        
+        z = (float)((int)(z*10.0f) % 3600)/10.0f;
         
-        for(i=0; i<6; i++){
-            *(float*)serialData = ave[i];
-            //pc.printf("%f,",ave[i]);
-            if(eeprom_cnt > 0xFFF0){
-                page_frg++;
-                eeprom_cnt = 0;
-            }
+        pc.printf("%d,%x,%f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%d\r\n",page_frg,eeprom_cnt,t,ave[0],ave[1],ave[2],ave[3],ave[4],ave[5],x,y,z,m_bri);
+        if(tf_mem != 2){
+            send2((unsigned short)(0.0f*10.0f),(unsigned short)(0.0f*10.0f),(unsigned short)(0.0f*10.0f),6);
+        }else{
+            send2((unsigned short)(x*10.0f),(unsigned short)(y*10.0f),(unsigned short)(z*10.0f),6);
+            
+            for(i=0; i<6; i++){
+                *(float*)serialData = ave[i];
+                //pc.printf("%f,",ave[i]);
+                if(eeprom_cnt > 0xFFF0){
+                    page_frg++;
+                    eeprom_cnt = 0;
+                }
             
-            if(page_frg == 0){
-                eeprom.PageWrite(ADDRESS_24LC1025_BLOCK0,eeprom_cnt,serialData,4);
-                pc.printf("%d,0x%x,%.1f\r\n",page_frg,eeprom_cnt,ave[i]);
-            }else if(page_frg == 1){
-                eeprom.PageWrite(ADDRESS_24LC1025_BLOCK1,eeprom_cnt,serialData,4);
-                //pc.printf("%d,0x%x\r\n",page_frg,eeprom_cnt);
-            }else{
-                //pc.printf("EEPROM_ERROR!! You should erase memory\r\n");
+                if(page_frg == 0){
+                    eeprom.PageWrite(ADDRESS_24LC1025_BLOCK0,eeprom_cnt,serialData,4);
+                    //pc.printf("%d,0x%x,%.1f\r\n",page_frg,eeprom_cnt,ave[i]);
+                }else if(page_frg == 1){
+                    eeprom.PageWrite(ADDRESS_24LC1025_BLOCK1,eeprom_cnt,serialData,4);
+                    //pc.printf("%d,0x%x\r\n",page_frg,eeprom_cnt);
+                }else{
+                    //pc.printf("EEPROM_ERROR!! You should erase memory\r\n");
+                }
+                eeprom_cnt = 0x04 + eeprom_cnt;
+                wait(0.01);
+            
             }
-            eeprom_cnt = 0x04 + eeprom_cnt;
-            wait(0.01);
-            
+        
+            rom_write(t);
+            rom_write(x);
+            rom_write(y);
+            rom_write(z);
         }
-        wait(0.1);
+        
+        wait(0.03);
+       
     }
     
 }
 
-/*
-int main(){
-    int adc[4];
-    int i;
-    
-    pc.printf("Hello world\r\n");
-    wait(1.0);
-    
-    while(1){
-        adc[0] = mcp.sgl( CH0 );
-        adc[1] = mcp.sgl( CH1 );
-        adc[2] = mcp.sgl( CH2 );
-        adc[3] = mcp.sgl( CH3 );
-        for(i=0; i<4; i++){
-            pc.printf("CH%d:%d\r\n",i,adc[i]);
+int rom_write(float data){
+    *(float*)serialData = data;
+        if(eeprom_cnt > 0xFFF0){
+            page_frg++;
+            eeprom_cnt = 0;
+        }
+          
+        if(page_frg == 0){
+            eeprom.PageWrite(ADDRESS_24LC1025_BLOCK0,eeprom_cnt,serialData,4);
+            //pc.printf("%d,0x%x,%.1f\r\n",page_frg,eeprom_cnt,ave[i]);
+        }else if(page_frg == 1){
+            eeprom.PageWrite(ADDRESS_24LC1025_BLOCK1,eeprom_cnt,serialData,4);
+            //pc.printf("%d,0x%x\r\n",page_frg,eeprom_cnt);
+        }else{
+            //pc.printf("EEPROM_ERROR!! You should erase memory\r\n");
         }
-        wait(0.5);
-    }
+        eeprom_cnt = 0x04 + eeprom_cnt;
+        wait(0.01);
+        return 0;
+}
 
-}
-*/
+int most_brightness(float data_mae_2,float data_migi_3,float data_usiro_4,float data_hidari_5){
+    int re_num;
+    if(data_mae_2 < data_migi_3){
+        if(data_mae_2 < data_usiro_4){
+            if(data_mae_2 < data_hidari_5){
+                re_num = 2;
+            }else{
+                re_num = 5;
+            }
+        }else if(data_usiro_4 < data_hidari_5){
+            re_num = 4;
+        }else{
+            re_num = 5;
+        }
+    }else if(data_migi_3 < data_usiro_4){
+        if(data_migi_3 < data_hidari_5){
+            re_num = 3;
+        }else{
+            re_num = 5;
+        }
+    }else{
+        if(data_usiro_4 < data_hidari_5){
+            re_num = 4;
+        }else{
+            re_num = 5;
+        }
+    }
+    return re_num;
+}
\ No newline at end of file