sensor

Dependencies:   HMC6352 mbed

Fork of program_SensorBlock by Fumiya Fujisawa

main.cpp

Committer:
com3
Date:
2014-03-12
Revision:
8:f547b66f3c43
Parent:
6:97015325107b

File content as of revision 8:f547b66f3c43:

#include "mbed.h"
#include "HMC6352.h"
/*PING DEFINE */
#define ERROR_PING 0xFF
#define ALL_PING 4
/*IR DEFINE*/
#define BREAKPT_IR 833
#define ERROR_IR 0xFFF
#define ALL_IR 12
#define NOTBALL 3
#define NO_BALL 45
#define ERR 7
//#define OVERLINE_IR 46
/*LINE DEFINE*/
#define JUDGEVOL_LINE 0.1
#define ALL_LINE 4
//OTHERS
#define ALL_KIND 3 //kind of sensors
#define MAX_NUM 12 //sensors most number
extern void micon_tx();
/*
Ping , IrSensor , LineSensor ,compass
*/
//LINE sensor is float value

BusOut myled(LED1, LED2, LED3, LED4);
 
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] = {p29,p21,p22,p23,p24,p25,p26,p27,p30,p29,p12,p15};
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  int hosei_ir[ALL_IR] = {-1,1,0,0,0,0,0,1,-2,-1,0,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
            }
            //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);
            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){//18.5
            time_ping.stop();
            return ERROR_PING;
        }
    }
    memory = time_ping.read_us();
    memory >>= 6;
    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(sensor_ir){
            if((time_ir.read_us() - memory) >= BREAKPT_IR){
                break;
            }
        }
        temp = (time_ir.read_us() - memory);
        time_ir.stop();
        time_ir.reset();
        //wait(0.01);
        return temp;
        /*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, sum = 0;
    sum = 0;
    min = value_ir[0];
    for (i = 0;i <= ALL_IR - NOTBALL;i++){
            value_ir[i] += hosei_ir[i];
            sum += value_ir[i];
            if(min > value_ir[i]){
                min = value_ir[i];
                num = i;
            }
    }
    value_ir_min = min;
    if(/*(value_ir_min < NO_BALL) || */(value_ir[11] > 45)){
        if(sum < 4000){
            //myled = 1;
            if((num == 0) || (num == 8) || (num == 9)){
                //myled = 4;
                if(value_ir_min <= 25){
                    num = 0;
                } else if((value_ir[1] >= value_ir[7] - ERR) && (value_ir[1] <= value_ir[7] + ERR)){
                    num = 0;
                    myled = 0;
                } else if(value_ir[1] > value_ir[7]){
                    num = 9;
                    myled = 3;
                } else if(value_ir[1] < value_ir[7]){
                    num = 8;
                    myled = 12;
                }
                if((value_ir[0] + value_ir[8] + value_ir[9]) < 72){
                    myled = 15;
                }
                /*
                if(value_ir[0] == value_ir[8]){
                    num = 10;
                    myled = 6;
                } else if(value_ir[0] == value_ir[9]){
                    num = 11;
                    myled = 12;
                }
                */
            } else if(num == 8){
                //myled = 2;
            } else if(num == 9){
                //myled = 8;
            }
            
            ir_min_num = num;
        }else{
            ir_min_num = NO_BALL;
            myled = 0;
        }
    } else {
        ir_min_num = NO_BALL;
        myled = 0;
    }
}
int main() {
    //enum sensors kinds;
    int compassdef = 0;
    int num[ALL_KIND] = {0};
    int ping_flag = 0;
    compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
    //送信開始
    sensor.putc(1);
    //送信空き割り込み設定
    sensor.attach(&micon_tx,Serial::TxIrq);
    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;
        }
        //myled = 1;
        //pc.printf("%d\t%d\n", value_compass0, compassdef);
        
        for(num[Ir] = 0; num[Ir] < ALL_IR; num[Ir]++){
            moving_ave(Ir,num[Ir],ir_function(num[Ir]));
        }
        ir_min_fun();
        //pc.printf("%d %d\n", value_ir[9], value_compass0);
        //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);
        //pc.printf("%d\t%d\n",value_ir_min,ir_min_num);
        //pc.printf("%d\n", value_ir[11]);
        //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つ
        //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つ
        //myled = 2;
        
        if(ping_flag == 2){
            ping_flag = 0;
            for(num[Ping] = 0; num[Ping] < ALL_PING; num[Ping]++){
                 moving_ave(Ping,num[Ping],ping_function(num[Ping]));
             }
            //pc.printf("%d %d %d %d\n", value_ping[0], value_ping[1], value_ping[2], value_ping[3]);
        }
        ping_flag++;
        //myled = 4;
        
    }
}