ライントレーサー制御用のプログラムです.

Dependencies:   mbed

sensor.cpp

Committer:
Hirotomo777
Date:
2017-11-16
Revision:
0:e9af471e2a2b

File content as of revision 0:e9af471e2a2b:

#include "sensor.h"
#define threshold1 0.005
#define threshold2 0.005
/*
AnalogIn sensorfl(PTB0);
AnalogIn sensorfr(PTB1);
AnalogIn sensorbl(PTB2);
AnalogIn sensorbr(PTB3);
*/
AnalogIn sensor1(PTB0);  //左から数えて1,2,3,4
AnalogIn sensor2(PTB1);
AnalogIn sensor3(PTB2);
AnalogIn sensor4(PTB3);

DigitalIn sensorl(PTD6);
DigitalIn sensorr(PTD7);

Timer t;

static float th1 = 0.0;
static float th2 = 0.0;
static float th3 = 0.0;
static float th4 = 0.0;

void sensorInit(){
    float th1max = sensor1;
    float th1min = sensor1;
    float th2max = sensor2;
    float th2min = sensor2;
    float th3max = sensor3;
    float th3min = sensor3;
    float th4max = sensor4;
    float th4min = sensor4;
    handle.pulsewidth(0.001);
    t.start();
    while(1){
        if(t.read() > 1.0){
            break;
        }
        if(th1max < sensor1){
            th1max = sensor1;
        }
        if(th1min > sensor1){
            th1min = sensor1;
        }
        if(th2max < sensor2){
            th2max = sensor2;
        }
        if(th2min > sensor2){
            th2min = sensor2;
        }
        if(th3max < sensor3){
            th3max = sensor3;
        }
        if(th3min > sensor3){
            th3min = sensor3;
        }
        if(th4max < sensor4){
            th4max = sensor4;
        }
        if(th4min > sensor4){
            th4min = sensor4;
        }
    }
    //wait(1.0);
    t.stop();
    t.reset();
    handle.pulsewidth(0.0021);
    t.start();
    while(1){
        if(t.read() > 1.0){
            break;
        }
        if(th1max < sensor1){
            th1max = sensor1;
        }
        if(th1min > sensor1){
            th1min = sensor1;
        }
        if(th2max < sensor2){
            th2max = sensor2;
        }
        if(th2min > sensor2){
            th2min = sensor2;
        }
        if(th3max < sensor3){
            th3max = sensor3;
        }
        if(th3min > sensor3){
            th3min = sensor3;
        }
        if(th4max < sensor4){
            th4max = sensor4;
        }
        if(th4min > sensor4){
            th4min = sensor4;
        }
    }
    th1 = (th1max + th1min) / 2.0;
    th2 = (th2max + th2min) / 2.0;
    th3 = (th3max + th3min) / 2.0;
    th4 = (th4max + th4min) / 2.0;
    t.stop();
    //wait(1.0);
    handle.pulsewidth(0.00155);
    wait(0.5);
}

int getReaction(){
    int tmp = 0;
    if(sensor1 < th1){
        tmp += 0b100000;
    }
    
    if(sensor2 < th2){
        //tmp += 0b010000;
        tmp += 0b000000;
    }
    if(sensor3 < th3){
        tmp += 0b000000;
        //死んでる
    }
    
    if(sensor4 < th4){
        tmp += 0b000100;
    }
    
    tmp += 0b000010 * sensorl;
    tmp += 0b000001 * sensorr;
    return tmp;
}