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 10
00010 //#define OVERLINE_IR 46
00011 /*LINE DEFINE*/
00012 #define JUDGEVOL_LINE 0.8
00013 #define ALL_LINE 4
00014 //OTHERS
00015 #define ALL_KIND 3 //kind of sensors
00016 #define MAX_NUM 10 //sensors most number
00017 extern void micon_tx();
00018 /*
00019 Ping , IrSensor , LineSensor ,compass
00020 */
00021 //LINE sensor is float value 
00022 Serial sensor(p13,p14);
00023 Serial pc(USBTX,USBRX);
00024 HMC6352 compass(p9, p10);
00025 
00026 Timer time_ping;
00027 Timer time_ir; 
00028 
00029 PinName num_ping[ALL_PING] = {p5,p6,p7,p8};
00030 PinName num_ir[ALL_IR] = {p21,p22,p23,p24,p25,p26,p27,p28,p29,p30};
00031 PinName num_line[ALL_LINE] = {p17,p18,p19,p20};
00032 
00033 enum sensors{Ping,Ir,Line};
00034 volatile unsigned int value_ping[ALL_PING] = {0};
00035 volatile unsigned int value_ir[ALL_IR] = {0};
00036 volatile unsigned int value_ir_min = 0;
00037 volatile unsigned int ir_min_num = 0;
00038 volatile unsigned int value_line[ALL_LINE] = {0};
00039 volatile unsigned int value_compass0 = 0;
00040 volatile unsigned int value_compass[2] = {0};
00041 
00042 
00043 /*unsigned int*/void moving_ave(enum sensors kind,int num, unsigned int data){
00044     static unsigned int sum[ALL_KIND][MAX_NUM] = {0};
00045     static unsigned int temp[ALL_KIND][MAX_NUM][10] = {0};
00046     //static unsigned int ave[MAX_NUM] = {0};
00047     sum[kind][num] -= temp[kind][num][9];
00048     sum[kind][num] += data;
00049     temp[kind][num][9] = temp[kind][num][8];
00050     temp[kind][num][8] = temp[kind][num][7];
00051     temp[kind][num][7] = temp[kind][num][6];
00052     temp[kind][num][6] = temp[kind][num][5];
00053     temp[kind][num][5] = temp[kind][num][4];
00054     temp[kind][num][4] = temp[kind][num][3];
00055     temp[kind][num][3] = temp[kind][num][2];
00056     temp[kind][num][2] = temp[kind][num][1];
00057     temp[kind][num][1] = temp[kind][num][0];
00058     temp[kind][num][0] = data;
00059     //ave[kind][num] = sum[kind][num]/100;
00060     
00061     switch (kind) {
00062         case Ping: 
00063             value_ping[num] = sum[kind][num]/10;
00064             break;
00065         case Ir:
00066             value_ir[num] = sum[kind][num]/100;
00067             break;
00068         case Line:
00069             if((sum[kind][num]/10000.0) >=JUDGEVOL_LINE){
00070                 value_line[num] = 1;//white
00071             }else{
00072                 value_line[num] = 0;//green
00073             }
00074             break;
00075         default :
00076             break;
00077     }
00078 
00079     //return sum[kind][num];//return no config sum
00080        
00081 }
00082 unsigned int ping_function (int num){
00083     DigitalInOut sensor_ping(num_ping[num]);
00084     unsigned int memory = 0;//return value
00085     time_ping.reset();
00086     /*make a pulse */
00087     sensor_ping.output();
00088     sensor_ping = 1;
00089     wait_us(5);
00090     sensor_ping = 0;
00091     sensor_ping.input();
00092     /*finish,and start timer*/
00093     time_ping.start();
00094     while(!sensor_ping){
00095         if(time_ping.read_ms() > 1){
00096             time_ping.stop();
00097             return ERROR_PING;
00098         }
00099     }
00100     time_ping.reset();
00101     while(sensor_ping){
00102         if(time_ping.read_ms() >18.5){//18.5
00103             time_ping.stop();
00104             return ERROR_PING;
00105         }
00106     }
00107     memory = time_ping.read_us();
00108     memory >>= 6;
00109     time_ping.stop();
00110     
00111     return memory;
00112 }
00113 unsigned int ir_function (int num){
00114     DigitalIn sensor_ir(num_ir[num]);
00115     int flag = 0;
00116     unsigned int memory = 0;
00117     unsigned int temp = 0;   
00118     flag = 1;
00119     time_ir.start();
00120     if((sensor_ir)){
00121         while((sensor_ir)){
00122             if(time_ir.read_us() >= BREAKPT_IR){
00123                 flag = 0;
00124                 break;
00125             }
00126         }
00127     }
00128     time_ir.stop();
00129     time_ir.reset();
00130     if(flag){
00131         time_ir.start();
00132         while(!sensor_ir){//!
00133             if(time_ir.read_us() >= BREAKPT_IR){
00134                 break;
00135             }
00136         }
00137         memory = time_ir.read_us();
00138         while(1){
00139             if(!sensor_ir){
00140                 temp = (time_ir.read_us() - memory);
00141                 time_ir.stop();
00142                 time_ir.reset();
00143                 //wait(0.01);
00144                 return temp;
00145             }
00146         }
00147     }else{//not found
00148     }
00149     time_ir.stop();
00150     time_ir.reset();
00151     return ERROR_IR;
00152 }
00153 int line_function(int num){
00154     float memory = 0;
00155     AnalogIn sensor_line(num_line[num]);
00156     memory = sensor_line;
00157     return (int)(memory*1000);
00158 
00159 }
00160 void ir_min_fun(){
00161     static unsigned int min;
00162     static int i, num = 0;
00163     min = value_ir[0];
00164         for (i = 0;i <=ALL_IR -1;i++){
00165             if(min >= value_ir[i]){
00166                 min = value_ir[i];
00167                 num = i;
00168         }
00169     }
00170     value_ir_min = min;
00171     ir_min_num = num;
00172 }
00173 int main() {
00174     //enum sensors kinds;
00175     int compassdef = 0;
00176     int num[ALL_KIND] = {0};
00177     //送信開始
00178     sensor.putc(1);
00179     //送信空き割り込み設定
00180     sensor.attach(&micon_tx,Serial::TxIrq);
00181     compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
00182     compassdef = (compass.sample() / 10);
00183     
00184     while(1) {
00185         value_compass0 = ((compass.sample() / 10) + 540 - compassdef) % 360;
00186         if(value_compass0 >=255 ){
00187             value_compass[0] = 255;
00188             value_compass[1] = value_compass0 - 255 ;
00189         }else{
00190             value_compass[0] = value_compass0;
00191             value_compass[1] = 0;
00192         }
00193         moving_ave(Ping,num[Ping],ping_function(num[Ping]));
00194         moving_ave(Line,num[Line],line_function(num[Line]));
00195         moving_ave(Ir,num[Ir],ir_function(num[Ir]));
00196         ir_min_fun();
00197         //pc.printf("%d,%d\n",value_ir_min,ir_min_num);
00198         num[Ping]++;
00199         num[Ir]++;
00200         num[Line]++;        
00201         if(num[Ping]>= ALL_PING){
00202             num[Ping] = 0; 
00203             //putchar('\n');
00204         }
00205         if(num[Ir]>= ALL_IR){
00206             num[Ir] = 0;
00207             //putchar('\n');
00208         }
00209         if(num[Line]>= ALL_LINE){
00210             num[Line] = 0;
00211             //putchar('\n');
00212         }
00213         
00214     }
00215 }