CatPot 2015-2016
ジャパンオープン用のLPC824専用プログラム
Dependencies: mbed-src Ping SDFileSystem
Diff: main/format.cpp
- Revision:
- 5:d666b328c093
- Parent:
- 4:a0755903bd6e
- Child:
- 6:80c2f4fce2eb
--- a/main/format.cpp Wed Mar 02 09:56:06 2016 +0000
+++ b/main/format.cpp Thu Mar 03 04:17:06 2016 +0000
@@ -63,6 +63,9 @@
uint8_t ir_notice=0;//4bit
uint8_t ir_val_phase[2];
+uint8_t ir_dif_phase[2];
+uint16_t ir_val_diff[2];
+uint8_t ir_place;//0<=x<=20
static uint8_t const ir_key=IR_KEY;//4bit
uint8_t ir_position[2]={0};//4bit*2
@@ -77,6 +80,19 @@
if(val==1){
order = nucleo.read();
if(order == 0xABCD){
+ //key/phaseL/phaseS/long_diff/position/
+ // 2/ 3/ 3/ 3/ 5/
+ packet = (
+ ((0xC000)&(ir_key<<14)) |//1100000000000000
+ ((0x3800)&(ir_val_phase[IR_LONG]<<11)) |//0011100000000000
+ ((0x0700)&(ir_val_phase[IR_SHORT]<<8)) |//0000011100000000
+ ((0x00E0)&(ir_dif_phase[IR_LONG]<<5)) |//0000000011100000
+ ((0x001F)&(ir_place<<0)) //0000000000011111
+ )&0xFFFF;
+
+ nucleo.reply(packet);
+ }
+ else if(order == 0xBCDA){
//key/phaseL/phaseS/long/short/
// 2/ 3/ 3/ 4/ 4/
packet = (
@@ -109,16 +125,31 @@
uint16_t ir_val_long[8];
uint16_t ir_val_short[12];
- uint16_t ir_val, ir_val_diff;
+ uint16_t ir_val;
uint8_t ir_posi;
uint8_t ir_posi_long[8];
uint8_t ir_posi_short[12];
+
+ uint16_t ave_buff_long[LONG_AVE];
+ uint16_t ave_buff_long2[LONG_AVE];
+ uint32_t ave_sum_long;
+ uint32_t ave_sum_long2;
+ uint16_t ir_val_long_plus;
+ uint16_t ir_val_long_plus2;
+
+
static uint8_t const ch_num[3]={8, 8, 4};
static uint16_t const ir_dis_range[2][8]={
{0, 2000, 2250, 2500, 2600, 2700, 3000, 3900, },//IR_SHORT 0
- {0, 1000, 1100, 1200, 1300, 1600, 1800, 2000, },//IR_LONG 1
+ {0, 1000, 1100, 1200, 1300, 1500, 1800, 2000, },//IR_LONG 1
+ };
+
+
+ static uint16_t const ir_dif_range[2][8]={
+ {0, 500, 750, 1000, 1250, 1500, 1750, 2000, },//IR_SHORT 0
+ {0, 500, 600, 650, 700, 800, 900, 1000, },//IR_LONG 1
};
nucleo.format(16, 3);
@@ -136,6 +167,9 @@
//Nucleoとの通信をする際はTeraTermへの出力は控えるべき
call.fall(&emergency);
+ for(i=0,ave_sum_long =2100*LONG_AVE; i<LONG_AVE; i++) ave_buff_long[i]=2100;
+ for(i=0,ave_sum_long2=2100*LONG_AVE; i<LONG_AVE; i++) ave_buff_long2[i]=2100;
+
for(i=0; i<IC_NUM; i++) cs[i]=1;
ir_val_phase[IR_SHORT] = DIS_7;
@@ -167,8 +201,32 @@
BubbleSort(ir_val_short, ir_posi_short, 12);
BubbleSort(ir_val_long, ir_posi_long, 8);
+ //移動平均
+ ave_sum_long += ir_val_long[7];
+ ave_sum_long -= ave_buff_long[LONG_AVE-1];
+ for(i=LONG_AVE-1; i>0; i--){
+ ave_buff_long[i] = ave_buff_long[i-1];
+ }
+ ave_buff_long[0] = ir_val_long[7];
+
+ ir_val_long_plus = ir_val_long[7];
+ ir_val_long[7] = ave_sum_long/LONG_AVE;
+
+ //移動平均2
+ ave_sum_long2 += ir_val_long[0];
+ ave_sum_long2 -= ave_buff_long2[LONG_AVE-1];
+ for(i=LONG_AVE-1; i>0; i--){
+ ave_buff_long2[i] = ave_buff_long2[i-1];
+ }
+ ave_buff_long2[0] = ir_val_long[0];
+
+ ir_val_long_plus2 = ir_val_long[0];
+ ir_val_long[0] = ave_sum_long2/LONG_AVE;
+
+
//検出した最大値と最小値の差
- ir_val_diff = ir_val_short[0]-ir_val_short[11];//After bubble
+ ir_val_diff[IR_SHORT] = ir_val_short[0]-ir_val_short[11];//After bubble
+ ir_val_diff[IR_LONG] = abs(ir_val_long[0]-ir_val_long[7]);//After bubble
//最もボールがあるとされる位置を代入
ir_position[IR_SHORT] = ir_posi_short[11];//After bubble
ir_position[IR_LONG] = ir_posi_long[7];//After bubble
@@ -236,48 +294,60 @@
ir_val_phase[IR_LONG] = DIS_0;
}
- /*
- //old
- ir_notice = IR_NOTE_NONE;//none
+ //long_dif
- if((ir_val_short[11]>=3900)&&(1)){
- if((ir_val_long[7]<=1700)&&(1)){
- ir_notice = IR_NOTE_FAR;//long_data
- }
- else{
- ir_notice = IR_NOTE_NONE;//none
- }
+ if(ir_val_diff[IR_LONG]>=ir_dif_range[IR_LONG][DIS_7]){
+ ir_dif_phase[IR_LONG] = DIS_7;
+ }
+ else if(ir_val_diff[IR_LONG]>=ir_dif_range[IR_LONG][DIS_6]){
+ ir_dif_phase[IR_LONG] = DIS_6;
+ }
+ else if(ir_val_diff[IR_LONG]>=ir_dif_range[IR_LONG][DIS_5]){
+ ir_dif_phase[IR_LONG] = DIS_5;
+ }
+ else if(ir_val_diff[IR_LONG]>=ir_dif_range[IR_LONG][DIS_4]){
+ ir_dif_phase[IR_LONG] = DIS_4;
+ }
+ else if(ir_val_diff[IR_LONG]>=ir_dif_range[IR_LONG][DIS_3]){
+ ir_dif_phase[IR_LONG] = DIS_3;
+ }
+ else if(ir_val_diff[IR_LONG]>=ir_dif_range[IR_LONG][DIS_2]){
+ ir_dif_phase[IR_LONG] = DIS_2;
+ }
+ else if(ir_val_diff[IR_LONG]>=ir_dif_range[IR_LONG][DIS_1]){
+ ir_dif_phase[IR_LONG] = DIS_1;
+ }
+ else{//ir_val_diff[IR_LONG]>=0
+ ir_dif_phase[IR_LONG] = DIS_0;
+ }
+
+ //ir_place
+ if(ir_val_phase[IR_SHORT] >= DIS_7){
+ ir_place = ir_position[IR_SHORT]+7;
}
else{
- if((ir_val_short[11]<=2400)&&(1)){
- if(ir_val_long[7]<=1000){
- ir_notice = IR_NOTE_CLOSER;//short_data
- }
- else{
- ir_notice = IR_NOTE_FAR;//long_data
- }
-
- }
- else{
- ir_notice = IR_NOTE_CLOSE;//short_data
- }
- }*/
+ ir_place = ir_position[IR_LONG]-1;
+ }
//デバッグ用のTeraTermへの出力
+
pc.printf("val_short:%d\t", ir_val_short[11]);//最小検出値(近)
- pc.printf("val_long:%d\t", ir_val_long[7]);//最小検出値(遠)
+ pc.printf("val_long(AVE):%d\t", ir_val_long[7]);//最小検出値(遠)
+ //pc.printf("val_long:%d\t", ir_val_long_plus);//最小検出値(遠)...平均なし
+
pc.printf("state_short:%d\t", ir_position[0]);//ボール位置(近)
pc.printf("state_long:%d\t", ir_position[1]);//ボール位置(遠)
- //pc.printf("Diff_short:%d\t", ir_val_short[0]-ir_val_short[11]);//検出した最大値と最小値の差(近)
- //pc.printf("Diff_long:%d\t", ir_val_long[0]-ir_val_long[7]);//検出した最大値と最小値の差(遠)
-
+ //pc.printf("Diff_short:%d\t", ir_val_diff[IR_SHORT]);//検出した最大値と最小値の差(近)
+ pc.printf("Diff_long:%d\t", ir_val_diff[IR_LONG]);//検出した最大値と最小値の差(遠)
pc.printf("ir_val_phase[IR_SHORT]:%d\t", ir_val_phase[IR_SHORT]);//判定されたボールの距離の段階(遠)
pc.printf("ir_val_phase[IR_LONG]:%d\t", ir_val_phase[IR_LONG]);//判定されたボールの距離の段階(遠)
+ //pc.printf("notice:%d\t", ir_notice);//判定されたボールの状況
- //pc.printf("notice:%d\t", ir_notice);//判定されたボールの状況
+
pc.printf("\r\n");
+
/*
pc.printf("%Value is ");
pc.printf("Dirr_short is %d\t", ir_val_short[0]-ir_val_short[11]);//検出した最大値と最小値の差(近)