bri/LineArray.cpp at master · RoboCup-workshop/bri · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
#include "LineArray.h"
LineArray::LineArray(){
	//Serial.begin(9600);
	//delay(100);
	//Serial.println("Start.....");
}

void LineArray::setPort(int pin) {
	DataPin = pin;
}

String LineArray::getBINValue() {
	int val = getValue();
	String b = String(val, BIN);
	return fillZero(b);
}

boolean LineArray::checkBIN(String val) {
	return (val.substring(1) == getBINValue());
}

int LineArray::getValue(){
	long time_out_flag = 0;
	pinMode(DataPin, OUTPUT);
	digitalWrite(DataPin, LOW);
	delayMicroseconds(980);
	digitalWrite(DataPin, HIGH);
	delayMicroseconds(40);
	pinMode(DataPin, INPUT_PULLUP);
	delayMicroseconds(50);
	time_out_flag = millis();
	while((digitalRead(DataPin) == 0)&&((millis() - time_out_flag) < 6));
	time_out_flag = millis();
	while((digitalRead(DataPin) == 1)&&((millis() - time_out_flag) < 6));
	for(uint8_t k=0; k<3; k++)
	{
			Sensor_Data[k] = 0x00;
			for(uint8_t i=0;i<8;i++)
			{
					time_out_flag = millis();
					while(digitalRead(DataPin) == 0&&((millis() - time_out_flag) < 6));
					uint32_t HIGH_level_read_time = micros();
					time_out_flag = millis();
					while(digitalRead(DataPin) == 1&&((millis() - time_out_flag) < 6));
					HIGH_level_read_time = micros() - HIGH_level_read_time;
				 if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100)
					{
							Sensor_Data[k] |= (0x80 >> i);
					}
			}
	}
	if (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0]))
	{
		 return Sensor_Data[0];
	} else {
		return 0;
	}
}

String LineArray::fillZero(String bin) {
  while(bin.length() < 6) {
    bin = "0" + bin;
  }
  return bin;
}