Angles working?
EmaMaker
parent
dc251467bd
commit
e84292568e
|
|
@ -1,6 +1,9 @@
|
|||
#pragma once
|
||||
#define startp 105
|
||||
#define endp 115
|
||||
//Coords are mapped from 0 up to this value
|
||||
#define MAP_MAX 100
|
||||
#define HALF_MAP_MAX 50
|
||||
//#define unkn 0b01101001
|
||||
#include "data_source.h"
|
||||
|
||||
|
|
@ -8,9 +11,7 @@ class DataSourceCamera : public DataSource{
|
|||
|
||||
public:
|
||||
DataSourceCamera(HardwareSerial* ser, int baud);
|
||||
void postProcess() override;
|
||||
void test() override;
|
||||
int fixCamIMU(int);
|
||||
void readSensor() override;
|
||||
int getValueAtk(bool);
|
||||
int getValueDef(bool);
|
||||
|
|
@ -18,18 +19,9 @@ class DataSourceCamera : public DataSource{
|
|||
int yAngle, bAngle, yAngleFix, bAngleFix, yDist, bDist;
|
||||
|
||||
int count = 0, unkn_counter;
|
||||
byte xb, yb, xy, yy, true_xb, true_xy, true_yb, true_yy;
|
||||
byte xb, yb, xy, yy, true_xb, true_xy, true_yb, true_yy, calc_xb, calc_yb, calc_xy, calc_yy;
|
||||
bool data_received = false, start = false, end = false;
|
||||
|
||||
int goalOrientation, pAtk, pDef, imuOff, portx, valX, valY, valB, oldGoalX, oldGoalY, oldGoalB;
|
||||
int cameraReady;
|
||||
int goalOrientation, pAtk, pDef;
|
||||
int value;
|
||||
int startpY = 0;
|
||||
int startpB = 0;
|
||||
int endpY = 0;
|
||||
int endpB = 0;
|
||||
int datavalid = 0;
|
||||
String valStringY = "";
|
||||
String valStringB = "";
|
||||
bool negateB, negateY;
|
||||
};
|
||||
|
|
@ -20,20 +20,21 @@ void DataSourceCamera :: readSensor(){
|
|||
true_xy = xy;
|
||||
true_yy = yy;
|
||||
|
||||
//Remap to [-50, +49] to correctly compute angles and distances
|
||||
true_xb -= 50;
|
||||
true_yb -= 50;
|
||||
true_xy -= 50;
|
||||
true_yy -= 50;
|
||||
//Remap from [0,100] to [-50, +49] to correctly compute angles and distances and calculate them
|
||||
yAngle = atan2(50-true_yy, 50-true_xy) * 180 / 3.14;
|
||||
bAngle = atan2(50-true_yb, 50-true_xb) * 180 / 3.14;
|
||||
//Subtract 90 to bring the angles back to euler angles (0 in front)
|
||||
yAngle = -90 + yAngle;
|
||||
bAngle = -90 + bAngle;
|
||||
//Now cast angles to [0, 359] domain angle flip them
|
||||
yAngle = (yAngle + 360) % 360;
|
||||
bAngle = (bAngle + 360) % 360;
|
||||
|
||||
//Now calculate angles and distance
|
||||
yAngle = atan2(true_yy, true_xy) * 180 / 3.14;
|
||||
bAngle = atan2(true_yb, true_xb) * 180 / 3.14;
|
||||
yAngleFix = yAngle - compass->getValue() ;
|
||||
bAngleFix = bAngle - compass->getValue() ;
|
||||
|
||||
yDist = sqrt( true_yy*true_yy + true_xy*true_xy );
|
||||
bDist = sqrt( true_yb*true_yb + true_xb*true_xb );
|
||||
yDist = sqrt( (50-true_yy)*(50-true_yy) + (50-true_xy)*(50-true_xy) );
|
||||
bDist = sqrt( (50-true_yb)*(50-true_yb) + (50-true_xb)*(50-true_xb) );
|
||||
}
|
||||
end=true;
|
||||
start=false;
|
||||
|
|
@ -50,62 +51,36 @@ void DataSourceCamera :: readSensor(){
|
|||
}
|
||||
|
||||
|
||||
void DataSourceCamera :: postProcess(){
|
||||
}
|
||||
|
||||
int DataSourceCamera::getValueAtk(bool fixed){
|
||||
//attacco gialla
|
||||
if(digitalRead(SWITCH_DX) == HIGH){
|
||||
if(fixed) return fixCamIMU(valY);
|
||||
return valY;
|
||||
}
|
||||
//attacco blu
|
||||
if(digitalRead(SWITCH_DX) == LOW){
|
||||
if(fixed) return fixCamIMU(valB);
|
||||
return valB;
|
||||
}
|
||||
}
|
||||
|
||||
int DataSourceCamera::getValueDef(bool fixed){
|
||||
//difendo gialla
|
||||
if(digitalRead(SWITCH_DX) == HIGH){
|
||||
if(fixed) return fixCamIMU(valY);
|
||||
return valY;
|
||||
}
|
||||
//difendo blu
|
||||
if(digitalRead(SWITCH_DX) == LOW){
|
||||
if(fixed) return fixCamIMU(valB);
|
||||
return valB;
|
||||
int DataSourceCamera::getValueAtk(bool b){
|
||||
return 0;
|
||||
}
|
||||
int DataSourceCamera::getValueDef(bool b){
|
||||
return 0;
|
||||
}
|
||||
|
||||
void DataSourceCamera::test(){
|
||||
goalOrientation = digitalRead(SWITCH_SX); //se HIGH attacco gialla, difendo blu
|
||||
update();
|
||||
DEBUG.print(bAngle);
|
||||
DEBUG.print(" | ");
|
||||
DEBUG.print(bAngleFix);
|
||||
DEBUG.print(" | ");
|
||||
DEBUG.println(bDist);
|
||||
DEBUG.println(" --- ");
|
||||
|
||||
DEBUG.print(yAngle);
|
||||
DEBUG.print(" | ");
|
||||
DEBUG.print(yAngleFix);
|
||||
DEBUG.println(" --- ");
|
||||
|
||||
DEBUG.print(bAngle);
|
||||
DEBUG.print(" | ");
|
||||
DEBUG.println(bAngleFix);
|
||||
DEBUG.println(yDist);
|
||||
DEBUG.println("---------------");
|
||||
DEBUG.print(true_xb);
|
||||
DEBUG.print(xb);
|
||||
DEBUG.print("|");
|
||||
DEBUG.print(true_yb);
|
||||
DEBUG.print(yb);
|
||||
DEBUG.print("|");
|
||||
DEBUG.print(true_xy);
|
||||
DEBUG.print(xy);
|
||||
DEBUG.print("|");
|
||||
DEBUG.print(true_yy);
|
||||
DEBUG.println(yy);
|
||||
DEBUG.println("---------------");
|
||||
delay(75);
|
||||
}
|
||||
|
||||
int DataSourceCamera::fixCamIMU(int d){
|
||||
if(compass->getValue() > 0 && compass->getValue() < 180) imuOff = compass->getValue();
|
||||
else if (compass->getValue() <= 360 && compass->getValue() >= 180) imuOff = compass->getValue() - 360;
|
||||
imuOff = constrain(imuOff*0.8, -30, 30);
|
||||
|
||||
return d + imuOff;
|
||||
delay(150);
|
||||
}
|
||||
|
|
@ -4,9 +4,9 @@
|
|||
#include "games.h"
|
||||
|
||||
void setup() {
|
||||
delay(500);
|
||||
delay(1000);
|
||||
|
||||
DEBUG.begin(19600);
|
||||
DEBUG.begin(9600);
|
||||
initSensors();
|
||||
initGames();
|
||||
|
||||
|
|
@ -20,7 +20,6 @@ void loop() {
|
|||
goalie->play(role==1);
|
||||
keeper->play(role==0);
|
||||
|
||||
|
||||
camera->test();
|
||||
|
||||
// Last thing to do: movement
|
||||
|
|
|
|||
|
|
@ -39,8 +39,8 @@ blue_led.on()
|
|||
|
||||
|
||||
|
||||
thresholds = [ (70, 100, -18, 21, 21, 63), # thresholds yellow goal
|
||||
(31, 49, -10, 5, -39, -18)] # thresholds blue goal (6, 31, -15, 4, -35, 0)
|
||||
thresholds = [ (49, 81, -18, 29, 22, 83), # thresholds yellow goal
|
||||
(0, 61, -4, 55, -81, -12)] # thresholds blue goal (6, 31, -15, 4, -35, 0)
|
||||
|
||||
roi = (0, 6, 318, 152)
|
||||
|
||||
|
|
@ -60,11 +60,11 @@ clock = time.clock()'''
|
|||
sensor.reset()
|
||||
sensor.set_pixformat(sensor.RGB565)
|
||||
sensor.set_framesize(sensor.QQVGA)
|
||||
sensor.set_contrast(+0)
|
||||
sensor.set_saturation(+0)
|
||||
sensor.set_brightness(0)
|
||||
sensor.set_contrast(+3)
|
||||
sensor.set_saturation(+3)
|
||||
sensor.set_brightness(-1)
|
||||
sensor.set_quality(0)
|
||||
sensor.set_auto_exposure(False, 3500)
|
||||
sensor.set_auto_exposure(False, 5000)
|
||||
sensor.set_auto_gain(True)
|
||||
sensor.skip_frames(time = 300)
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,218 @@
|
|||
/**
|
||||
Sensor Mapping
|
||||
Sensor Angle Pin Port
|
||||
S0 0 A3 C3
|
||||
S1 A2 C2
|
||||
S2 A1 C1
|
||||
S3 A0 C0
|
||||
S4 90 13 B5
|
||||
S5 12 B4
|
||||
S6 11 B3
|
||||
S7 10 B2
|
||||
S8 180 9 B1
|
||||
S9 8 B0
|
||||
S10 7 D7
|
||||
S11 6 D6
|
||||
S12 270 5 D5
|
||||
S13 4 D4
|
||||
S14 3 D3
|
||||
S15 2 D2
|
||||
loop cycle duration: 3.2 millis
|
||||
**/
|
||||
|
||||
#define S9 ((PINB & 1))
|
||||
#define S8 ((PINB & 2) >> 1)
|
||||
#define S7 ((PINB & 4) >> 2)
|
||||
#define S6 ((PINB & 8) >> 3)
|
||||
#define S5 ((PINB & 16) >> 4)
|
||||
#define S4 ((PINB & 32) >> 5)
|
||||
|
||||
#define S3 ((PINC & 1))
|
||||
#define S2 ((PINC & 2) >> 1)
|
||||
#define S1 ((PINC & 4) >> 2)
|
||||
#define S0 ((PINC & 8) >> 3)
|
||||
|
||||
#define S15 ((PIND & 4) >> 2)
|
||||
#define S14 ((PIND & 8) >> 3)
|
||||
#define S13 ((PIND & 16) >> 4)
|
||||
#define S12 ((PIND & 32) >> 5)
|
||||
#define S11 ((PIND & 64) >> 6)
|
||||
#define S10 ((PIND & 128) >> 7)
|
||||
|
||||
#define NCYCLES 350
|
||||
#define BROKEN 300
|
||||
#define TOO_LOW 60
|
||||
|
||||
int counter[16];
|
||||
int pins[] = {A3, A2, A1, A0, 13, 12, 11, 10, 9, 8, 7, 6, 5, 4, 3, 2};
|
||||
int distance;
|
||||
int nmax = 0;
|
||||
int sensor = 0;
|
||||
|
||||
int oldIndex = 0;
|
||||
int oldDistance = 0;
|
||||
|
||||
byte ballInfo = 0;
|
||||
|
||||
float xs[16];
|
||||
float ys[16];
|
||||
|
||||
float angle = 0, dist = 0;
|
||||
boolean sending = false;
|
||||
byte sendAngle = 0, sendDistance = 0;
|
||||
byte sendByte = 0;
|
||||
|
||||
unsigned long t = 0;
|
||||
|
||||
void setup() {
|
||||
delay(1000);
|
||||
|
||||
Serial.begin(57600);
|
||||
|
||||
pinMode(2, INPUT);
|
||||
pinMode(3, INPUT);
|
||||
pinMode(4, INPUT);
|
||||
pinMode(5, INPUT);
|
||||
pinMode(6, INPUT);
|
||||
pinMode(7, INPUT);
|
||||
pinMode(8, INPUT);
|
||||
pinMode(9, INPUT);
|
||||
pinMode(10, INPUT);
|
||||
pinMode(11, INPUT);
|
||||
pinMode(12, INPUT);
|
||||
pinMode(13, INPUT);
|
||||
pinMode(A0, INPUT);
|
||||
pinMode(A1, INPUT);
|
||||
pinMode(A2, INPUT);
|
||||
pinMode(A3, INPUT);
|
||||
|
||||
pinMode(A4, OUTPUT);
|
||||
|
||||
for (int i = 0; i < 16; i++) {
|
||||
xs[i] = cos((22.5 * PI / 180) * i);
|
||||
ys[i] = sin((22.5 * PI / 180) * i);
|
||||
}
|
||||
}
|
||||
|
||||
void loop() {
|
||||
readBallInterpolation();
|
||||
sendDataInterpolation();
|
||||
}
|
||||
|
||||
/**--- READ BALL USING SENSORS ANGLE INTERPOLATION ---**/
|
||||
|
||||
void readBallInterpolation() {
|
||||
for (int i = 0; i < 16; i++) {
|
||||
counter[i] = 0;
|
||||
}
|
||||
|
||||
//reads from the register
|
||||
for (int i = 0; i < NCYCLES; i++) {
|
||||
counter[0] += !S0;
|
||||
counter[1] += !S1;
|
||||
counter[2] += !S2;
|
||||
counter[3] += !S3;
|
||||
counter[4] += !S4;
|
||||
counter[5] += !S5;
|
||||
counter[6] += !S6;
|
||||
counter[7] += !S7;
|
||||
counter[8] += !S8;
|
||||
counter[9] += !S9;
|
||||
counter[10] += !S10;
|
||||
counter[11] += !S11;
|
||||
counter[12] += !S12;
|
||||
counter[13] += !S13;
|
||||
counter[14] += !S14;
|
||||
counter[15] += !S15;
|
||||
}
|
||||
|
||||
float x = 0, y = 0;
|
||||
for (int i = 0; i < 16; i++) {
|
||||
if (counter[i] > BROKEN || counter[i] < TOO_LOW) counter[i] = 0;
|
||||
|
||||
x += xs[i] * counter[i];
|
||||
y += ys[i] * counter[i];
|
||||
}
|
||||
|
||||
angle = atan2(y, x) * 180 / PI;
|
||||
angle = ((int)(angle + 360)) % 360;
|
||||
|
||||
//distance is 0 when not seeing ball
|
||||
//dist = hypot(x, y);
|
||||
|
||||
nmax = 0;
|
||||
//saves max value and sensor
|
||||
for (int i = 0; i < 16; i++) {
|
||||
if (counter[i] > nmax) {
|
||||
nmax = counter[i];
|
||||
}
|
||||
}
|
||||
|
||||
dist = nmax;
|
||||
|
||||
//turn led on
|
||||
if (dist == 0) {
|
||||
digitalWrite(A4, LOW);
|
||||
} else {
|
||||
digitalWrite(A4, HIGH);
|
||||
}
|
||||
}
|
||||
|
||||
void sendDataInterpolation() {
|
||||
if(sending){
|
||||
sendAngle = ((byte) (angle / 2)) & 0b11111110;
|
||||
Serial.write(sendAngle);
|
||||
}else{
|
||||
sendDistance = dist;
|
||||
if(sendDistance > 254) sendDistance = 254;
|
||||
sendDistance = sendDistance |= 0b00000001;
|
||||
Serial.write(sendDistance);
|
||||
}
|
||||
sending = !sending;
|
||||
}
|
||||
|
||||
void test() {
|
||||
readBallInterpolation();
|
||||
|
||||
Serial.print(S0);
|
||||
Serial.print(" | ");
|
||||
Serial.print(S1);
|
||||
Serial.print(" | ");
|
||||
Serial.print(S2);
|
||||
Serial.print(" | ");
|
||||
Serial.print(S3);
|
||||
Serial.print(" | ");
|
||||
Serial.print(S4);
|
||||
Serial.print(" | ");
|
||||
Serial.print(S5);
|
||||
Serial.print(" | ");
|
||||
Serial.print(S6);
|
||||
Serial.print(" | ");
|
||||
Serial.print(S7);
|
||||
Serial.print(" | ");
|
||||
Serial.print(S8);
|
||||
Serial.print(" | ");
|
||||
Serial.print(S9);
|
||||
Serial.print(" | ");
|
||||
Serial.print(S10);
|
||||
Serial.print(" | ");
|
||||
Serial.print(S11);
|
||||
Serial.print(" | ");
|
||||
Serial.print(S12);
|
||||
Serial.print(" | ");
|
||||
Serial.print(S13);
|
||||
Serial.print(" | ");
|
||||
Serial.print(S14);
|
||||
Serial.print(" | ");
|
||||
Serial.print(S15);
|
||||
Serial.print(" () ");
|
||||
Serial.println(sensor);
|
||||
}
|
||||
|
||||
void printCounter() {
|
||||
for (int i = 0; i < 16; i++) {
|
||||
Serial.print(counter[i]);
|
||||
Serial.print(" | ");
|
||||
}
|
||||
Serial.println();
|
||||
}
|
||||
|
|
@ -0,0 +1,56 @@
|
|||
/*
|
||||
Example Bluetooth Serial Passthrough Sketch
|
||||
by: Jim Lindblom
|
||||
SparkFun Electronics
|
||||
date: February 26, 2013
|
||||
license: Public domain
|
||||
|
||||
This example sketch converts an RN-42 bluetooth module to
|
||||
communicate at 9600 bps (from 115200), and passes any serial
|
||||
data between Serial Monitor and bluetooth module.
|
||||
*/
|
||||
#include <SoftwareSerial.h>
|
||||
|
||||
int bluetoothTx = 2; // TX-O pin of bluetooth mate, Arduino D2
|
||||
int bluetoothRx = 3; // RX-I pin of bluetooth mate, Arduino D3
|
||||
|
||||
//SoftwareSerial bluetooth(bluetoothTx, bluetoothRx);
|
||||
<<<<<<< HEAD
|
||||
=======
|
||||
|
||||
#define bluetooth Serial3
|
||||
>>>>>>> 5c0171dd181357ae6f20ec98cb5c060f067c30fc
|
||||
|
||||
#define bluetooth Serial3
|
||||
|
||||
void setup() {
|
||||
Serial.begin(9600); // Begin the serial monitor at 9600bps
|
||||
<<<<<<< HEAD
|
||||
|
||||
bluetooth.begin(9600); // Start bluetooth serial at 9600
|
||||
=======
|
||||
bluetooth.begin(9600);
|
||||
>>>>>>> 5c0171dd181357ae6f20ec98cb5c060f067c30fc
|
||||
}
|
||||
|
||||
void read() {
|
||||
while (bluetooth.available()) {
|
||||
// Send any characters the bluetooth prints to the serial monitor
|
||||
Serial.print((char)bluetooth.read());
|
||||
delay(50);
|
||||
}
|
||||
}
|
||||
|
||||
void write() {
|
||||
while (Serial.available()) {
|
||||
// Send any characters the Serial monitor prints to the bluetooth
|
||||
bluetooth.print((char)Serial.read());
|
||||
delay(50);
|
||||
}
|
||||
// and loop forever and ever!
|
||||
}
|
||||
|
||||
void loop() {
|
||||
read();
|
||||
write();
|
||||
}
|
||||
|
|
@ -0,0 +1,68 @@
|
|||
/*
|
||||
Example Bluetooth Serial Passthrough Sketch
|
||||
by: Jim Lindblom
|
||||
SparkFun Electronics
|
||||
date: February 26, 2013
|
||||
license: Public domain
|
||||
|
||||
This example sketch converts an RN-42 bluetooth module to
|
||||
communicate at 9600 bps (from 115200), and passes any serial
|
||||
data between Serial Monitor and bluetooth module.
|
||||
*/
|
||||
#include <SoftwareSerial.h>
|
||||
|
||||
int bluetoothTx = 2; // TX-O pin of bluetooth mate, Arduino D2
|
||||
int bluetoothRx = 3; // RX-I pin of bluetooth mate, Arduino D3
|
||||
|
||||
SoftwareSerial bluetooth(bluetoothTx, bluetoothRx);
|
||||
|
||||
void setup()
|
||||
{
|
||||
Serial.begin(9600); // Begin the serial monitor at 9600bps
|
||||
|
||||
/*bluetooth.begin(115200); // The Bluetooth Mate defaults to 115200bps
|
||||
bluetooth.print("$"); // Print three times individually
|
||||
bluetooth.print("$");
|
||||
bluetooth.print("$"); // Enter command mode
|
||||
delay(100); // Short delay, wait for the Mate to send back CMD
|
||||
bluetooth.println("U,9600,N"); // Temporarily Change the baudrate to 9600, no parity
|
||||
//115200 can be too fast at times for NewSoftSerial to relay the data reliably*/
|
||||
|
||||
delay(1000);
|
||||
|
||||
bluetooth.begin(9600); // Start bluetooth serial at 9600
|
||||
bluetooth.print("$"); // Print three times individually
|
||||
bluetooth.print("$");
|
||||
bluetooth.print("$");
|
||||
read();
|
||||
delay(1000);
|
||||
bluetooth.println("SA,0");
|
||||
read();
|
||||
bluetooth.println("SM,6");
|
||||
read();
|
||||
delay(1000);
|
||||
bluetooth.println("C");
|
||||
read();
|
||||
}
|
||||
|
||||
void read() {
|
||||
while (bluetooth.available()) {
|
||||
// Send any characters the bluetooth prints to the serial monitor
|
||||
Serial.print((char)bluetooth.read());
|
||||
delay(50);
|
||||
}
|
||||
}
|
||||
|
||||
void write() {
|
||||
while (Serial.available()) {
|
||||
// Send any characters the Serial monitor prints to the bluetooth
|
||||
bluetooth.print((char)Serial.read());
|
||||
delay(50);
|
||||
}
|
||||
// and loop forever and ever!
|
||||
}
|
||||
|
||||
void loop() {
|
||||
bluetooth.write(42);
|
||||
delay(500);
|
||||
}
|
||||
Binary file not shown.
|
|
@ -0,0 +1,41 @@
|
|||
#include <SoftwareSerial.h>
|
||||
|
||||
int bluetoothTx = 2; // TX-O pin of bluetooth mate, Arduino D2
|
||||
int bluetoothRx = 3; // RX-I pin of bluetooth mate, Arduino D3
|
||||
|
||||
SoftwareSerial bluetooth(bluetoothTx, bluetoothRx);
|
||||
void setup() {
|
||||
Serial.begin(9600);
|
||||
Serial.print("$");
|
||||
Serial.print("$");
|
||||
Serial.print("$");
|
||||
delay(1000);
|
||||
Serial.println("SF,1");
|
||||
delay(500);
|
||||
Serial.println("R,1");
|
||||
delay(2500);
|
||||
Serial.println("---");
|
||||
//bluetooth.begin(9600);
|
||||
}
|
||||
|
||||
void read() {
|
||||
if (bluetooth.available()) {
|
||||
// Send any characters the bluetooth prints to the serial monitor
|
||||
Serial.print((char)bluetooth.read());
|
||||
delay(75);
|
||||
}
|
||||
}
|
||||
|
||||
void write() {
|
||||
if (Serial.available()) {
|
||||
// Send any characters the Serial monitor prints to the bluetooth
|
||||
bluetooth.print((char)Serial.read());
|
||||
delay(75);
|
||||
}
|
||||
// and loop forever and ever!
|
||||
}
|
||||
|
||||
void loop() {
|
||||
read();
|
||||
write();
|
||||
}
|
||||
|
|
@ -0,0 +1,69 @@
|
|||
#include <Wire.h>
|
||||
|
||||
void setup() {
|
||||
Wire.begin(); // join i2c bus (address optional for master)
|
||||
Serial.begin(9600); // start serial communication at 9600bps
|
||||
|
||||
changeAddress(114, 230);
|
||||
}
|
||||
|
||||
int reading = 0;
|
||||
|
||||
void loop() {
|
||||
// step 1: instruct sensor to read echoes
|
||||
Wire.beginTransmission(115); // transmit to device #115 (0x70)
|
||||
// the address specified in the datasheet is 224 (0xE0)
|
||||
// but i2c adressing uses the high 7 bits so it's 115
|
||||
Wire.write(byte(0x00)); // sets register pointer to the command register (0x00)
|
||||
Wire.write(byte(0x51)); // command sensor to measure in "inches" (0x50)
|
||||
// use 0x51 for centimeters
|
||||
// use 0x52 for ping microseconds
|
||||
Wire.endTransmission(); // stop transmitting
|
||||
|
||||
// step 2: wait for readings to happen
|
||||
delay(70); // datasheet suggests at least 65 milliseconds
|
||||
|
||||
// step 3: instruct sensor to return a particular echo reading
|
||||
Wire.beginTransmission(115); // transmit to device #115
|
||||
Wire.write(byte(0x02)); // sets register pointer to echo #1 register (0x02)
|
||||
Wire.endTransmission(); // stop transmitting
|
||||
|
||||
// step 4: request reading from sensor
|
||||
Wire.requestFrom(115, 2); // request 2 bytes from slave device #115
|
||||
|
||||
// step 5: receive reading from sensor
|
||||
if (2 <= Wire.available()) { // if two bytes were received
|
||||
reading = Wire.read(); // receive high byte (overwrites previous reading)
|
||||
reading = reading << 8; // shift high byte to be high 8 bits
|
||||
reading |= Wire.read(); // receive low byte as lower 8 bits
|
||||
Serial.println(reading); // print the reading
|
||||
}
|
||||
|
||||
delay(250); // wait a bit since people have to read the output :)
|
||||
}
|
||||
|
||||
// The following code changes the address of a Devantech Ultrasonic Range Finder (SRF10 or SRF08)
|
||||
// usage: changeAddress(0x70, 0xE6);
|
||||
|
||||
void changeAddress(byte oldAddress, byte newAddress)
|
||||
{
|
||||
Wire.beginTransmission(oldAddress);
|
||||
Wire.write(byte(0x00));
|
||||
Wire.write(byte(0xA0));
|
||||
Wire.endTransmission();
|
||||
|
||||
Wire.beginTransmission(oldAddress);
|
||||
Wire.write(byte(0x00));
|
||||
Wire.write(byte(0xAA));
|
||||
Wire.endTransmission();
|
||||
|
||||
Wire.beginTransmission(oldAddress);
|
||||
Wire.write(byte(0x00));
|
||||
Wire.write(byte(0xA5));
|
||||
Wire.endTransmission();
|
||||
|
||||
Wire.beginTransmission(oldAddress);
|
||||
Wire.write(byte(0x00));
|
||||
Wire.write(newAddress);
|
||||
Wire.endTransmission();
|
||||
}
|
||||
Loading…
Reference in New Issue