main.cpp

00001 #include "mbed.h"
00002 #include "HMC6352.h"
00003 /*PING DEFINE */
00004 #define ERROR_PING 0xFFF
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.9
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 Timer time_ping;
00026 Timer time_ir; 
00027 
00028 PinName num_ping[ALL_PING] = {p5,p6,p7,p8};
00029 PinName num_ir[ALL_IR] = {p21,p22,p23,p24,p25,p26,p27,p28,p29,p30};
00030 PinName num_line[ALL_LINE] = {p17,p18,p19,p20};
00031 
00032 enum sensors{Ping,Ir,Line};
00033 volatile unsigned int value_ping[ALL_PING] = {0};
00034 volatile unsigned int value_ir[ALL_IR] = {0};
00035 volatile unsigned int value_ir_min = 0;
00036 volatile unsigned int ir_min_num = 0;
00037 volatile unsigned int value_line[ALL_LINE] = {0};
00038 volatile unsigned int value_compass = 0;
00039 
00040 
00041 /*unsigned int*/void moving_ave(enum sensors kind,int num, unsigned int data){
00042     static unsigned int sum[ALL_KIND][MAX_NUM] = {0};
00043     static unsigned int temp[ALL_KIND][MAX_NUM][10] = {0};
00044     //static unsigned int ave[MAX_NUM] = {0};
00045     sum[kind][num] -= temp[kind][num][9];
00046     sum[kind][num] += data;
00047     temp[kind][num][9] = temp[kind][num][8];
00048     temp[kind][num][8] = temp[kind][num][7];
00049     temp[kind][num][7] = temp[kind][num][6];
00050     temp[kind][num][6] = temp[kind][num][5];
00051     temp[kind][num][5] = temp[kind][num][4];
00052     temp[kind][num][4] = temp[kind][num][3];
00053     temp[kind][num][3] = temp[kind][num][2];
00054     temp[kind][num][2] = temp[kind][num][1];
00055     temp[kind][num][1] = temp[kind][num][0];
00056     temp[kind][num][0] = data;
00057     //ave[kind][num] = sum[kind][num]/100;
00058     
00059     switch (kind) {
00060         case Ping: 
00061             value_ping[num] = sum[kind][num]/10;
00062             break;
00063         case Ir:
00064             value_ir[num] = sum[kind][num]/100;
00065             break;
00066         case Line:
00067             if((sum[kind][num]/10000.0) >=JUDGEVOL_LINE){
00068                 value_line[num] = 1;//white
00069             }else{
00070                 value_line[num] = 0;//green
00071             }
00072             break;
00073         default :
00074             break;
00075     }
00076 
00077     //return sum[kind][num];//return no config sum
00078        
00079 }
00080 unsigned int ping_function (int num){
00081     DigitalInOut sensor_ping(num_ping[num]);
00082     unsigned int memory = 0;//return value
00083     time_ping.reset();
00084     /*make a pulse */
00085     sensor_ping.output();
00086     sensor_ping = 1;
00087     wait_us(5);
00088     sensor_ping = 0;
00089     sensor_ping.input();
00090     /*finish,and start timer*/
00091     time_ping.start();
00092     while(!sensor_ping){
00093         if(time_ping.read_ms() > 1){
00094             time_ping.stop();
00095             return ERROR_PING;
00096         }
00097     }
00098     time_ping.reset();
00099     while(sensor_ping){
00100         if(time_ping.read_ms() >18.5){
00101             time_ping.stop();
00102             return ERROR_PING;
00103         }
00104     }
00105     memory = time_ping.read_us();
00106     time_ping.stop();
00107     
00108     return memory;
00109 }
00110 unsigned int ir_function (int num){
00111     DigitalIn sensor_ir(num_ir[num]);
00112     int flag = 0;
00113     unsigned int memory = 0;
00114     unsigned int temp = 0;   
00115     flag = 1;
00116     time_ir.start();
00117     if((sensor_ir)){
00118         while((sensor_ir)){
00119             if(time_ir.read_us() >= BREAKPT_IR){
00120                 flag = 0;
00121                 break;
00122             }
00123         }
00124     }
00125     time_ir.stop();
00126     time_ir.reset();
00127     if(flag){
00128         time_ir.start();
00129         while(!sensor_ir){//!
00130             if(time_ir.read_us() >= BREAKPT_IR){
00131                 break;
00132             }
00133         }
00134         memory = time_ir.read_us();
00135         while(1){
00136             if(!sensor_ir){
00137                 temp = (time_ir.read_us() - memory);
00138                 time_ir.stop();
00139                 time_ir.reset();
00140                 //wait(0.01);
00141                 return temp;
00142             }
00143         }
00144     }else{//not found
00145     }
00146     time_ir.stop();
00147     time_ir.reset();
00148     return ERROR_IR;
00149 }
00150 int line_function(int num){
00151     float memory = 0;
00152     AnalogIn sensor_line(num_line[num]);
00153     memory = sensor_line;
00154     return (int)(memory*1000);
00155 
00156 }
00157 void ir_min_fun(){
00158     static unsigned int min;
00159     static int i, num = 0;
00160     min = value_ir[0];
00161         for (i = 0;i <=ALL_IR -1;i++){
00162             if(min >= value_ir[i]){
00163                 min = value_ir[i];
00164                 num = i;
00165         }
00166     }
00167     value_ir_min = min;
00168     ir_min_num = num;
00169 }
00170 int main() {
00171     //enum sensors kinds;
00172     uint8_t compassdef = 0;
00173     int num[ALL_KIND] = {0};
00174     //unsigned int value1[ALL_PING] = {0};
00175     //unsigned int value2[ALL_IR] = {0};
00176     //float value3[ALL_LINE] = {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_compass = ((compass.sample() / 10) + 540 - compassdef) % 360;
00186         moving_ave(Ping,num[Ping],ping_function(num[Ping]));
00187         
00188         pc.printf("value_compass=%d\n",value_compass);
00189         //ir_min_fun();
00190         //num[Ping]++;
00191         //num[Ir]++;
00192         //num[Line]++;        
00193         if(num[Ping]>= ALL_PING){
00194             num[Ping] = 0; 
00195             putchar('\n');
00196         }
00197         if(num[Ir]>= ALL_IR){
00198             num[Ir] = 0;
00199             putchar('\n');
00200             pc.printf("min=%d ,num = %d\n",value_ir_min,ir_min_num);
00201         }
00202         if(num[Line]>= ALL_LINE){
00203             num[Line] = 0;
00204             putchar('\n');
00205         }
00206         
00207     }
00208 }