CatPot 2015-2016
ジャパンオープン用のLPC824専用プログラム
Dependencies: mbed-src Ping SDFileSystem
Diff: main/format.cpp
- Revision:
- 0:6c79a352010b
- Child:
- 1:ea7307724f1c
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main/format.cpp Sat Feb 27 06:35:13 2016 +0000
@@ -0,0 +1,386 @@
+#include "mbed.h"
+#include "extern.h"
+
+#ifdef SD_CARD
+void Sd_System(void){
+
+ char buf[BUFSIZE];
+ uint16_t FileData[FDATA_NUM]={0}, num, data;
+ uint8_t spi_num, h_byte, l_byte, val, packet;
+ int i;
+ FILE *fp;
+ char *fname="/sd/mydir/sdtest.txt";
+ char *dname="/sd/mydir";
+
+ mkdir(dname, 0777);
+
+ pc.printf("Hello World!\n");
+
+ fp = fopen(fname, "r");
+ if(fp == NULL) {
+ error("Could not open file for read\n");
+ }
+ while(fgets(buf, sizeof(buf), fp) != NULL){
+ if(buf[0] == '#') continue;
+
+ num = atol(strtok(buf, ","));
+ data = atol(strtok(NULL, "\r\n\0"));
+
+ if(num<FDATA_NUM){
+ FileData[num] = data;
+ pc.printf("%d, %ld\n", num, FileData[num]);
+ }
+ else{
+ continue;
+ }
+ }
+ fclose(fp);
+
+ while(1){
+
+ val = nucleo.receive();
+ if(!val) continue;
+
+ spi_num = nucleo.read();
+ h_byte = nucleo.read();
+ l_byte = nucleo.read();
+
+ if((spi_num&0x80)>>7 == 1){//writing to sd
+ spi_num = spi_num & 0x7F;
+ if(spi_num<FDATA_NUM){
+ FileData[spi_num] = (h_byte<<8) | l_byte;
+ //pc.printf("%d, %ld\n", num, FileData[num]);
+ fp = fopen(fname, "w");
+ for(i=0; i<FDATA_NUM; i++) fprintf(fp, "%d, %ld\n", i, FileData[i]);
+ fclose(fp);
+ }
+ }
+ else{//reading from sd
+ spi_num = spi_num & 0x7F;
+ if(spi_num<FDATA_NUM){
+ h_byte = (FileData[spi_num] & 0xFF00)>>8;
+ l_byte = (FileData[spi_num] & 0x00FF);
+
+ packet = h_byte;
+ nucleo.reply(packet);
+
+ packet = l_byte;
+ nucleo.reply(packet);
+ }
+ }
+ //pc.printf("Goodbye World!\n");
+ }
+}
+#endif /*SD_CARD*/
+
+#if defined(ULTRA_SONIC) || defined(ULTRA_SONIC_2)
+
+uint16_t packet, val, order;
+uint16_t front_dis=0, rear_dis=0;
+
+void emergency(void){
+ led[1]=1;
+ //front_dis = 0xAA;
+ //rear_dis = 0xBB;
+ val = nucleo.receive();
+ if(val==1){
+ order = nucleo.read();
+ if(order == 0xABCD){
+ packet = ((front_dis<<8)|(rear_dis))&0xFFFF;nucleo.reply(packet);
+ }
+ else{
+ packet = 0xFFFF;nucleo.reply(packet);
+ }
+ //pc.printf("SSSSSSSSSSSSSSSSS%d\r\n", order);
+ }
+ else{
+ //pc.printf("FFFF\r\n");
+ }
+ wait_us(100);
+ led[1]=0;
+}
+void Usw_System(void){
+
+ nucleo.format(16, 3);
+ nucleo.frequency(1000000);
+ nucleo.reply(0x0000);
+
+ call.fall(&emergency);
+
+ //led[0]=led[1]=1;
+ while(1){
+
+ led[0]=1;
+ front.Send();
+ wait_ms(30);
+ front_dis = front.Read_cm();
+ if(front_dis>0xFE) front_dis=0xFE;
+
+
+ rear.Send();
+ wait_ms(30);
+ rear_dis = rear.Read_cm();
+ if(rear_dis>0xFE) rear_dis=0xFE;
+ led[0]=0;
+ wait_us(1000);
+ //pc.printf("%d\t%d\t\r\n", front_dis, rear_dis);
+
+ //emergency();
+ }
+}
+
+#endif /*(ULTRA_SONIC) || (ULTRA_SONIC_2)*/
+
+#ifdef COLOR_SENSOR
+void Color_System(void){
+ uint8_t val, order, packet;
+ uint16_t data;
+
+ while(1){
+ while(1){
+ val = nucleo.receive();
+ if(val){
+ order = nucleo.read();
+ order %= SENSOR_X_COLOR;
+ data = ColorSensor[order].read_u16();
+ packet = (data & 0xFF00)>>8;
+ nucleo.reply(packet);
+ }
+ else{
+ break;
+ }
+ }
+ }
+}
+#endif /*COLOR_SENSOR*/
+
+#ifdef IR_SENSOR
+
+uint8_t ir_notice=0;//4bit
+static uint8_t const ir_key=IR_KEY;//4bit
+uint8_t ir_position[2]={0};//4bit*2
+
+void emergency(void){
+ uint16_t order,packet,val;
+ //ir_key=0xA;
+ //ir_notice=0xC;
+ //ir_position[1]=0xE;
+ //ir_position[0]=0xF;
+ val = nucleo.receive();
+ if(val==1){
+ order = nucleo.read();
+ if(order == 0xABCD){
+ //key/notice/long/short
+ packet = ((ir_key<<12)|(ir_notice<<8)|(ir_position[LONG_SPOT]<<4)|(ir_position[SHORT_SPOT]<<0))&0xFFFF;nucleo.reply(packet);
+ }
+ else{
+ packet = 0xFFFF;nucleo.reply(packet);
+ }
+ //pc.printf("SSSSSSSSSSSSSSSSS%d\r\n", order);
+ }
+ else{
+ //pc.f("FFFF\r\n");
+ }
+}
+void Ir_System(void){
+
+ uint8_t i;//ic
+ uint8_t j;//ch
+
+ uint8_t num_long;
+ uint8_t num_short;
+
+ uint16_t ir_val_long[8];
+ uint16_t ir_val_short[12];
+
+ uint16_t ir_val, ir_val_diff;
+
+ uint8_t ir_posi;
+ uint8_t ir_posi_long[8];
+ uint8_t ir_posi_short[12];
+ static uint8_t const ch_num[3]={8, 8, 4};
+
+ nucleo.format(16, 3);
+ nucleo.frequency(1000000);
+ nucleo.reply(0x0000);
+
+ ir.format(8, 3);
+ ir.frequency(1000000);
+
+ //遠距離用電源
+ supply.period(0.010);//T=10[ms]
+ supply.write(0.9);//L...10%,H...90%
+
+ //Nucleoとの通信用のピン変化割り込み
+ //Nucleoとの通信をする際はTeraTermへの出力は控えるべき
+ call.fall(&emergency);
+
+ for(i=0; i<IC_NUM; i++) cs[i]=1;
+ while(1) {
+ //全ての素子の値を検出
+ num_short=0;
+ num_long=0;
+ for(i=0; i<IC_NUM; i++){//IC
+ for(j=0; j<ch_num[i]; j++){//Ch
+ ir_val = read_input(i, j);
+ ir_posi = read_position(i, j);
+
+ if(i==0){
+ ir_val_long[num_long] = ir_val;
+ ir_posi_long[num_long] = ir_posi;
+ num_long++;
+
+ }
+ else{
+ ir_val_short[num_short] = ir_val;
+ ir_posi_short[num_short] = ir_posi;
+ num_short++;
+ }
+
+ }
+ }
+ //バブルソート
+ BubbleSort(ir_val_short, ir_posi_short, 12);
+ BubbleSort(ir_val_long, ir_posi_long, 8);
+
+ //検出した最大値と最小値の差
+ ir_val_diff = ir_val_short[0]-ir_val_short[11];//After bubble
+ //最もボールがあるとされる位置を代入
+ ir_position[SHORT_SPOT] = ir_posi_short[11];//After bubble
+ ir_position[LONG_SPOT] = ir_posi_long[7];//After bubble
+
+ /*
+ #define IR_NOTE_NONE 0x0
+ #define IR_NOTE_FAR 0x1
+ #define IR_NOTE_CLOSE 0x2
+ #define IR_NOTE_CLOSER 0x3
+ */
+ //ボールが遠くにあるか，近くか，フィールドにないかを判断
+ ir_notice = IR_NOTE_NONE;//none
+
+ 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
+ }
+ }
+ 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_notice = IR_NOTE_NONE;//none
+ if((DIS_MORE_CLOSE<=ir_val_diff)&&(1)){
+ ir_notice = IR_NOTE_CLOSER;//short_data
+ }
+ else if((DIS_CLOSE<=ir_val_diff)&&(ir_val_diff<DIS_MORE_CLOSE)){
+ ir_notice = IR_NOTE_CLOSE;//short_data
+ }
+ else if((DIS_FAR<=ir_val_diff)&&(ir_val_diff<DIS_CLOSE)){
+ ir_notice = IR_NOTE_FAR;//long_data
+ }
+ else if((1)&&(ir_val_diff<DIS_FAR)){
+ ir_notice = IR_NOTE_NONE;//none
+ if(((ir_val_long[0]-ir_val_long[7])>900)&&1){
+ ir_notice = IR_NOTE_FAR;//long_data
+ }
+ }*/
+
+ //デバッグ用のTeraTermへの出力
+
+ /*
+ pc.printf("val_short:%d\t", ir_val_short[11]);//最小検出値(近)
+ pc.printf("val_long:%d\t", ir_val_long[7]);//最小検出値(遠)
+ 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("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]);//検出した最大値と最小値の差(近)
+ pc.printf("Dirr_long is %d\t", ir_val_long[0]-ir_val_long[7]);//検出した最大値と最小値の差(遠)
+ pc.printf("\r\n");
+ */
+
+ //検出した値全てを出力(バブルソートすると位置が変わるので注意)
+ /*pc.printf("%Value is ");
+ for(i=0; i<8; i++){
+ pc.printf("%d\t", ir_val_long[i]);//遠
+ }
+ for(i=0; i<12; i++){
+ pc.printf("%d\t", ir_val_short[i]);//近
+ }
+
+ pc.printf("\r\n");
+ */
+ }
+}
+uint16_t read_input(uint8_t ic, uint8_t channel)
+{
+ uint16_t command_high = START_BIT | MODE_SINGLE | ((channel & 0x04) >> 2);
+ uint16_t command_low = (channel & 0x03) << 6;
+
+ cs[ic] = 0;
+
+ ir.write(command_high);
+ uint16_t high_byte = ir.write(command_low) & 0x0F;
+ uint16_t low_byte = ir.write(0);
+
+ wait_us(1);
+ cs[ic] = 1;
+
+ return (high_byte << 8) | low_byte;//0x0000...0xFFFF
+}
+uint8_t read_position(uint8_t ic, uint8_t channel){
+ //DigitalOut cs[IC_NUM] = {cs1, cs2, cs3};
+ static uint8_t const ir_location[3][8]=
+ {//0,1,2...7(ch)
+ {1, 2, 3, 4, 5, 6, 7, 8},//cs1,Long*8
+ {1, 2, 3, 4, 5, 6, 7, 8},//cs2,Short*8
+ {9, 10, 11, 12},//cs3,Short*4
+ };
+ return ir_location[ic][channel];
+}
+void BubbleSort(uint16_t *data, uint8_t *data2, uint8_t n)//昇順にする
+{
+ bool flag;
+ uint8_t i, j;//inclement
+ uint16_t temp;//temporary for value
+ uint8_t temp2;//temporary for position
+
+ j=0;
+ do{
+ j++;
+ flag=0;
+ for (i=0; i<n-j; i++) {
+ if (data[i]<data[i+1]) {
+ // 左右の並びがおかしければ入れ替える
+ flag=1;
+ temp=data[i];
+ data[i]=data[i+1];
+ data[i+1]=temp;
+
+ temp2=data2[i];
+ data2[i]=data2[i+1];
+ data2[i+1]=temp2;
+ }
+ }
+ } while (flag==1); //入れ替えがある間，繰り返す．
+}
+#endif /*IR_SENSOR*/