main.cpp

00001 #include "mbed.h"
00002 #include "HMC6352.h"
00003 /*PING DEFINE */
00004 #define ERROR_PING 0xFF
00005 #define ALL_PING 4
00006 /*IR DEFINE*/
00007 #define BREAKPT_IR 833
00008 #define ERROR_IR 0xFFF
00009 #define ALL_IR 12
00010 #define NOTBALL 3
00011 #define NO_BALL 45
00012 #define ERR 7
00013 //#define OVERLINE_IR 46
00014 /*LINE DEFINE*/
00015 #define JUDGEVOL_LINE 0.1
00016 #define ALL_LINE 4
00017 //OTHERS
00018 #define ALL_KIND 3 //kind of sensors
00019 #define MAX_NUM 12 //sensors most number
00020 extern void micon_tx();
00021 /*
00022 Ping , IrSensor , LineSensor ,compass
00023 */
00024 //LINE sensor is float value
00025 
00026 BusOut myled(LED1, LED2, LED3, LED4);
00027  
00028 Serial sensor(p13,p14);
00029 Serial pc(USBTX,USBRX);
00030 HMC6352 compass(p9, p10);
00031  
00032 Timer time_ping;
00033 Timer time_ir; 
00034  
00035 PinName num_ping[ALL_PING] = {p5,p6,p7,p8};
00036 PinName num_ir[ALL_IR] = {p29,p21,p22,p23,p24,p25,p26,p27,p30,p29,p12,p15};
00037 PinName num_line[ALL_LINE] = {p17,p18,p19,p20};
00038  
00039 enum sensors{Ping,Ir,Line};
00040 volatile unsigned int value_ping[ALL_PING] = {0};
00041 volatile unsigned int value_ir[ALL_IR] = {0};
00042 volatile  int hosei_ir[ALL_IR] = {-1,1,0,0,0,0,0,1,-2,-1,0,0};
00043 volatile unsigned int value_ir_min = 0;
00044 volatile unsigned int ir_min_num = 0;
00045 volatile unsigned int value_line[ALL_LINE] = {0};
00046 volatile unsigned int value_compass0 = 0;
00047 volatile unsigned int value_compass[2] = {0};
00048  
00049  
00050 /*unsigned int*/void moving_ave(enum sensors kind,int num, unsigned int data){
00051     static unsigned int sum[ALL_KIND][MAX_NUM] = {0};
00052     static unsigned int temp[ALL_KIND][MAX_NUM][10] = {0};
00053     //static unsigned int ave[MAX_NUM] = {0};
00054     sum[kind][num] -= temp[kind][num][9];
00055     sum[kind][num] += data;
00056     temp[kind][num][9] = temp[kind][num][8];
00057     temp[kind][num][8] = temp[kind][num][7];
00058     temp[kind][num][7] = temp[kind][num][6];
00059     temp[kind][num][6] = temp[kind][num][5];
00060     temp[kind][num][5] = temp[kind][num][4];
00061     temp[kind][num][4] = temp[kind][num][3];
00062     temp[kind][num][3] = temp[kind][num][2];
00063     temp[kind][num][2] = temp[kind][num][1];
00064     temp[kind][num][1] = temp[kind][num][0];
00065     temp[kind][num][0] = data;
00066     //ave[kind][num] = sum[kind][num]/100;
00067     
00068     switch (kind) {
00069         case Ping: 
00070             value_ping[num] = sum[kind][num]/10;
00071             break;
00072         case Ir:
00073             value_ir[num] = sum[kind][num]/100;
00074             break;
00075         case Line:
00076             if((sum[kind][num]/10000.0) >=JUDGEVOL_LINE){
00077                 value_line[num] = 1;//white
00078             }else{
00079                 value_line[num] = 0;//green
00080             }
00081             //pc.printf("%f %f %f %f\n", sum[kind][0]/10000.0, sum[kind][1]/10000.0, sum[kind][2]/10000.0, sum[kind][3]/10000.0);
00082             break;
00083         default :
00084             break;
00085     }
00086  
00087     //return sum[kind][num];//return no config sum
00088        
00089 }
00090 unsigned int ping_function (int num){
00091     DigitalInOut sensor_ping(num_ping[num]);
00092     unsigned int memory = 0;//return value
00093     time_ping.reset();
00094     /*make a pulse */
00095     sensor_ping.output();
00096     sensor_ping = 1;
00097     wait_us(5);
00098     sensor_ping = 0;
00099     sensor_ping.input();
00100     /*finish,and start timer*/
00101     time_ping.start();
00102     while(!sensor_ping){
00103         if(time_ping.read_ms() > 1){
00104             time_ping.stop();
00105             return ERROR_PING;
00106         }
00107     }
00108     time_ping.reset();
00109     while(sensor_ping){
00110         if(time_ping.read_ms() >18.5){//18.5
00111             time_ping.stop();
00112             return ERROR_PING;
00113         }
00114     }
00115     memory = time_ping.read_us();
00116     memory >>= 6;
00117     time_ping.stop();
00118     
00119     return memory;
00120 }
00121 unsigned int ir_function (int num){
00122     DigitalIn sensor_ir(num_ir[num]);
00123     int flag = 0;
00124     unsigned int memory = 0;
00125     unsigned int temp = 0;   
00126     flag = 1;
00127     time_ir.start();
00128     if((sensor_ir)){
00129         while((sensor_ir)){
00130             if(time_ir.read_us() >= BREAKPT_IR){
00131                 flag = 0;
00132                 break;
00133             }
00134         }
00135     }
00136     time_ir.stop();
00137     time_ir.reset();
00138     if(flag){
00139         time_ir.start();
00140         while(!sensor_ir){//!
00141             if(time_ir.read_us() >= BREAKPT_IR){
00142                 break;
00143             }
00144         }
00145         memory = time_ir.read_us();
00146         while(sensor_ir){
00147             if((time_ir.read_us() - memory) >= BREAKPT_IR){
00148                 break;
00149             }
00150         }
00151         temp = (time_ir.read_us() - memory);
00152         time_ir.stop();
00153         time_ir.reset();
00154         //wait(0.01);
00155         return temp;
00156         /*while(1){
00157             if(!sensor_ir){
00158                 temp = (time_ir.read_us() - memory);
00159                 time_ir.stop();
00160                 time_ir.reset();
00161                 //wait(0.01);
00162                 return temp;
00163             }
00164         }*/
00165     }else{//not found
00166     }
00167     time_ir.stop();
00168     time_ir.reset();
00169     return ERROR_IR;
00170 }
00171 int line_function(int num){
00172     float memory = 0;
00173     AnalogIn sensor_line(num_line[num]);
00174     memory = sensor_line;
00175     return (int)(memory*1000);
00176  
00177 }
00178 void ir_min_fun(){
00179     static unsigned int min;
00180     static int i, num = 0, sum = 0;
00181     sum = 0;
00182     min = value_ir[0];
00183     for (i = 0;i <= ALL_IR - NOTBALL;i++){
00184             value_ir[i] += hosei_ir[i];
00185             sum += value_ir[i];
00186             if(min > value_ir[i]){
00187                 min = value_ir[i];
00188                 num = i;
00189             }
00190     }
00191     value_ir_min = min;
00192     if(/*(value_ir_min < NO_BALL) || */(value_ir[11] > 45)){
00193         if(sum < 4000){
00194             //myled = 1;
00195             if((num == 0) || (num == 8) || (num == 9)){
00196                 //myled = 4;
00197                 if(value_ir_min <= 25){
00198                     num = 0;
00199                 } else if((value_ir[1] >= value_ir[7] - ERR) && (value_ir[1] <= value_ir[7] + ERR)){
00200                     num = 0;
00201                     myled = 0;
00202                 } else if(value_ir[1] > value_ir[7]){
00203                     num = 9;
00204                     myled = 3;
00205                 } else if(value_ir[1] < value_ir[7]){
00206                     num = 8;
00207                     myled = 12;
00208                 }
00209                 if((value_ir[0] + value_ir[8] + value_ir[9]) < 72){
00210                     myled = 15;
00211                 }
00212                 /*
00213                 if(value_ir[0] == value_ir[8]){
00214                     num = 10;
00215                     myled = 6;
00216                 } else if(value_ir[0] == value_ir[9]){
00217                     num = 11;
00218                     myled = 12;
00219                 }
00220                 */
00221             } else if(num == 8){
00222                 //myled = 2;
00223             } else if(num == 9){
00224                 //myled = 8;
00225             }
00226             
00227             ir_min_num = num;
00228         }else{
00229             ir_min_num = NO_BALL;
00230             myled = 0;
00231         }
00232     } else {
00233         ir_min_num = NO_BALL;
00234         myled = 0;
00235     }
00236 }
00237 int main() {
00238     //enum sensors kinds;
00239     int compassdef = 0;
00240     int num[ALL_KIND] = {0};
00241     int ping_flag = 0;
00242     compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
00243     //送信開始
00244     sensor.putc(1);
00245     //送信空き割り込み設定
00246     sensor.attach(&micon_tx,Serial::TxIrq);
00247     compassdef = (compass.sample() / 10);
00248     
00249     while(1) {
00250         value_compass0 = ((compass.sample() / 10) + 540 - compassdef) % 360;
00251         if(value_compass0 >=255 ){
00252             value_compass[0] = 255;
00253             value_compass[1] = value_compass0 - 255 ;
00254         }else{
00255             value_compass[0] = value_compass0;
00256             value_compass[1] = 0;
00257         }
00258         //myled = 1;
00259         //pc.printf("%d\t%d\n", value_compass0, compassdef);
00260         
00261         for(num[Ir] = 0; num[Ir] < ALL_IR; num[Ir]++){
00262             moving_ave(Ir,num[Ir],ir_function(num[Ir]));
00263         }
00264         ir_min_fun();
00265         //pc.printf("%d %d\n", value_ir[9], value_compass0);
00266         //pc.printf("%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t   %d   %d\n", value_ir[0], value_ir[1], value_ir[2], value_ir[3], value_ir[4], value_ir[5], value_ir[6], value_ir[7], value_ir[8], value_ir[9], value_ir[10], value_ir[11], value_ir_min, ir_min_num);
00267         //pc.printf("%d\t%d\n",value_ir_min,ir_min_num);
00268         //pc.printf("%d\n", value_ir[11]);
00269         //pc.printf("%d\t%d\t%d\t%d\t%d\t%d\n", value_ir[1], value_ir[8], value_ir[0], value_ir[9], value_ir[7], ir_min_num);       //前5つ
00270         //pc.printf("%d\t%d\t%d\t%d\t%d\n", value_ir[3], value_ir[2], value_ir[1], value_ir[8], ir_min_num);                        //左4つ
00271         //myled = 2;
00272         
00273         if(ping_flag == 2){
00274             ping_flag = 0;
00275             for(num[Ping] = 0; num[Ping] < ALL_PING; num[Ping]++){
00276                  moving_ave(Ping,num[Ping],ping_function(num[Ping]));
00277              }
00278             //pc.printf("%d %d %d %d\n", value_ping[0], value_ping[1], value_ping[2], value_ping[3]);
00279         }
00280         ping_flag++;
00281         //myled = 4;
00282         
00283     }
00284 }