program_SensorBlock //{Ping,Ir,Lines,compass} Sensor

Dependencies:   HMC6352 mbed

Fork of program_BlockPIL by Fumiya Fujisawa

main.cpp

Committer:
ryuna
Date:
2014-02-25
Revision:
5:3457e0e66104
Parent:
4:1d54610dba80
Child:
6:500e2c97a690

File content as of revision 5:3457e0e66104:

#include "mbed.h"
#include "HMC6352.h"
/*PING DEFINE */
#define ERROR_PING 0xFFF
#define ALL_PING 4
/*IR DEFINE*/
#define BREAKPT_IR 833
#define ERROR_IR 0xFFF
#define ALL_IR 10
//#define OVERLINE_IR 46
/*LINE DEFINE*/
#define JUDGEVOL_LINE 0.9
#define ALL_LINE 4
//OTHERS
#define ALL_KIND 3 //kind of sensors
#define MAX_NUM 10 //sensors most number
extern void micon_tx();
/*
Ping , IrSensor , LineSensor ,compass
*/
//LINE sensor is float value 
Serial sensor(p13,p14);
Serial pc(USBTX,USBRX);
HMC6352 compass(p9, p10);

Timer time_ping;
Timer time_ir; 

PinName num_ping[ALL_PING] = {p5,p6,p7,p8};
PinName num_ir[ALL_IR] = {p21,p22,p23,p24,p25,p26,p27,p28,p29,p30};
PinName num_line[ALL_LINE] = {p17,p18,p19,p20};

enum sensors{Ping,Ir,Line};
volatile unsigned int value_ping[ALL_PING] = {0};
volatile unsigned int value_ir[ALL_IR] = {0};
volatile unsigned int value_ir_min = 0;
volatile unsigned int ir_min_num = 0;
volatile unsigned int value_line[ALL_LINE] = {0};
volatile unsigned int value_compass0 = 0;
volatile unsigned int value_compass[2] = {0};


/*unsigned int*/void moving_ave(enum sensors kind,int num, unsigned int data){
    static unsigned int sum[ALL_KIND][MAX_NUM] = {0};
    static unsigned int temp[ALL_KIND][MAX_NUM][10] = {0};
    //static unsigned int ave[MAX_NUM] = {0};
    sum[kind][num] -= temp[kind][num][9];
    sum[kind][num] += data;
    temp[kind][num][9] = temp[kind][num][8];
    temp[kind][num][8] = temp[kind][num][7];
    temp[kind][num][7] = temp[kind][num][6];
    temp[kind][num][6] = temp[kind][num][5];
    temp[kind][num][5] = temp[kind][num][4];
    temp[kind][num][4] = temp[kind][num][3];
    temp[kind][num][3] = temp[kind][num][2];
    temp[kind][num][2] = temp[kind][num][1];
    temp[kind][num][1] = temp[kind][num][0];
    temp[kind][num][0] = data;
    //ave[kind][num] = sum[kind][num]/100;
    
    switch (kind) {
        case Ping: 
            value_ping[num] = sum[kind][num]/10;
            break;
        case Ir:
            value_ir[num] = sum[kind][num]/100;
            break;
        case Line:
            if((sum[kind][num]/10000.0) >=JUDGEVOL_LINE){
                value_line[num] = 1;//white
            }else{
                value_line[num] = 0;//green
            }
            break;
        default :
            break;
    }

    //return sum[kind][num];//return no config sum
       
}
unsigned int ping_function (int num){
    DigitalInOut sensor_ping(num_ping[num]);
    unsigned int memory = 0;//return value
    time_ping.reset();
    /*make a pulse */
    sensor_ping.output();
    sensor_ping = 1;
    wait_us(5);
    sensor_ping = 0;
    sensor_ping.input();
    /*finish,and start timer*/
    time_ping.start();
    while(!sensor_ping){
        if(time_ping.read_ms() > 1){
            time_ping.stop();
            return ERROR_PING;
        }
    }
    time_ping.reset();
    while(sensor_ping){
        if(time_ping.read_ms() >18.5){
            time_ping.stop();
            return ERROR_PING;
        }
    }
    memory = time_ping.read_us();
    time_ping.stop();
    
    return memory;
}
unsigned int ir_function (int num){
    DigitalIn sensor_ir(num_ir[num]);
    int flag = 0;
    unsigned int memory = 0;
    unsigned int temp = 0;   
    flag = 1;
    time_ir.start();
    if((sensor_ir)){
        while((sensor_ir)){
            if(time_ir.read_us() >= BREAKPT_IR){
                flag = 0;
                break;
            }
        }
    }
    time_ir.stop();
    time_ir.reset();
    if(flag){
        time_ir.start();
        while(!sensor_ir){//!
            if(time_ir.read_us() >= BREAKPT_IR){
                break;
            }
        }
        memory = time_ir.read_us();
        while(1){
            if(!sensor_ir){
                temp = (time_ir.read_us() - memory);
                time_ir.stop();
                time_ir.reset();
                //wait(0.01);
                return temp;
            }
        }
    }else{//not found
    }
    time_ir.stop();
    time_ir.reset();
    return ERROR_IR;
}
int line_function(int num){
    float memory = 0;
    AnalogIn sensor_line(num_line[num]);
    memory = sensor_line;
    return (int)(memory*1000);

}
void ir_min_fun(){
    static unsigned int min;
    static int i, num = 0;
    min = value_ir[0];
        for (i = 0;i <=ALL_IR -1;i++){
            if(min >= value_ir[i]){
                min = value_ir[i];
                num = i;
        }
    }
    value_ir_min = min;
    ir_min_num = num;
}
int main() {
    //enum sensors kinds;
    int compassdef = 0;
    int num[ALL_KIND] = {0};
    //送信開始
    sensor.putc(1);
    //送信空き割り込み設定
    sensor.attach(&micon_tx,Serial::TxIrq);
    compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
    compassdef = (compass.sample() / 10);
    
    while(1) {
        value_compass0 = ((compass.sample() / 10) + 540 - compassdef) % 360;
        if(value_compass0 >=255 ){
            value_compass[0] = 255;
            value_compass[1] = value_compass0 - 255 ;
        }else{
            value_compass[0] = value_compass0;
            value_compass[1] = 0;
        }
        moving_ave(Ping,num[Ping],ping_function(num[Ping]));
        pc.printf("compass0=%d compass[0]= %d  compass[1]= %d\n",value_compass0,value_compass[0],value_compass[1]);
        //ir_min_fun();
        //num[Ping]++;
        //num[Ir]++;
        //num[Line]++;        
        if(num[Ping]>= ALL_PING){
            num[Ping] = 0; 
            putchar('\n');
        }
        if(num[Ir]>= ALL_IR){
            num[Ir] = 0;
            putchar('\n');
            pc.printf("min=%d ,num = %d\n",value_ir_min,ir_min_num);
        }
        if(num[Line]>= ALL_LINE){
            num[Line] = 0;
            putchar('\n');
        }
        
    }
}