| 121 |
pfowler |
1 |
#include "TinyWireS.h" // wrapper class for I2C slave routines
|
|
|
2 |
#include "Servo8Bit.h"
|
|
|
3 |
|
|
|
4 |
#define I2C_SLAVE_ADDR 0x24 // i2c slave address
|
|
|
5 |
|
|
|
6 |
#define CMD_PING_ANGLE 0x01 // Receive byte (0-180), ping that direction
|
|
|
7 |
#define CMD_PING_CURR 0x02
|
|
|
8 |
#define CMD_GET_RANGE 0x03
|
|
|
9 |
#define CMD_ROTATE_SERV 0x04
|
|
|
10 |
#define CMD_SURVEY 0x05
|
|
|
11 |
|
|
|
12 |
const uint8_t PING0 = 4;
|
|
|
13 |
const uint8_t LED_S1 = 3;
|
|
|
14 |
const uint8_t SERVO = 1;
|
|
|
15 |
|
|
|
16 |
|
|
|
17 |
uint16_t pingRange = 0;
|
|
|
18 |
uint8_t cmd = 0;
|
|
|
19 |
uint8_t options = 0;
|
|
|
20 |
|
|
|
21 |
Servo8Bit servo;
|
|
|
22 |
|
|
|
23 |
void setup(){
|
|
|
24 |
pinMode(LED_S1,OUTPUT);
|
|
|
25 |
digitalWrite(LED_S1,HIGH);
|
|
|
26 |
|
|
|
27 |
RBlink(LED_S1,2); // show it's alive
|
|
|
28 |
TinyWireS.begin(I2C_SLAVE_ADDR); // init I2C Slave mode
|
|
|
29 |
|
|
|
30 |
servo.attach(SERVO, 600, 2400);
|
|
|
31 |
servo.write(90);
|
|
|
32 |
delay(500);
|
|
|
33 |
//servo.detach();
|
|
|
34 |
}
|
|
|
35 |
|
|
|
36 |
void loop(){
|
|
|
37 |
|
|
|
38 |
if (TinyWireS.available()){ // got I2C input!
|
|
|
39 |
cmd = TinyWireS.receive(); // get the byte from master
|
|
|
40 |
if (cmd == CMD_PING_ANGLE) {
|
|
|
41 |
uint8_t pos = TinyWireS.receive();
|
|
|
42 |
servoPos(pos);
|
|
|
43 |
pingRange = ping();
|
|
|
44 |
} else if (cmd == CMD_PING_CURR) {
|
|
|
45 |
pingRange = ping();
|
|
|
46 |
} else if (cmd == CMD_GET_RANGE) {
|
|
|
47 |
//TinyWireS.send(pingRange >> 8);
|
|
|
48 |
TinyWireS.send(pingRange);
|
|
|
49 |
} else if (cmd == CMD_ROTATE_SERV) {
|
|
|
50 |
uint8_t pos = TinyWireS.receive();
|
|
|
51 |
servoPos(pos);
|
|
|
52 |
} else if (cmd == CMD_SURVEY) {
|
|
|
53 |
uint8_t survey[5];
|
|
|
54 |
for (uint8_t x = 0; x <5; x++) {
|
|
|
55 |
uint8_t angle = 45 * x;
|
|
|
56 |
servoPos(angle);
|
|
|
57 |
delay (100);
|
|
|
58 |
survey[x] = ping();
|
|
|
59 |
delay (100);
|
|
|
60 |
}
|
|
|
61 |
servoPos(90);
|
|
|
62 |
for (uint8_t x = 0; x <5; x++) {
|
|
|
63 |
//TinyWireS.send(survey[x] >> 8);
|
|
|
64 |
TinyWireS.send(survey[x]);
|
|
|
65 |
}
|
|
|
66 |
}
|
|
|
67 |
RBlink(LED_S1,1);
|
|
|
68 |
|
|
|
69 |
}
|
|
|
70 |
|
|
|
71 |
}
|
|
|
72 |
|
|
|
73 |
void servoPos(uint8_t pos) {
|
|
|
74 |
//if (!servo.attached())
|
|
|
75 |
servo.attach(SERVO);
|
|
|
76 |
if (pos > 180)
|
|
|
77 |
pos = 180;
|
|
|
78 |
if (pos <= 5)
|
|
|
79 |
pos = 5;
|
|
|
80 |
servo.write(pos);
|
|
|
81 |
delay(500);
|
|
|
82 |
//servo.detach();
|
|
|
83 |
}
|
|
|
84 |
|
|
|
85 |
uint8_t ping() {
|
|
|
86 |
|
|
|
87 |
uint16_t duration = 0;
|
|
|
88 |
// The PING))) is triggered by a HIGH pulse of 2 or more microseconds.
|
|
|
89 |
// Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
|
|
|
90 |
pinMode(PING0, OUTPUT);
|
|
|
91 |
digitalWrite(PING0, LOW);
|
|
|
92 |
delayMicroseconds(20);
|
|
|
93 |
digitalWrite(PING0, HIGH);
|
|
|
94 |
delayMicroseconds(50);
|
|
|
95 |
digitalWrite(PING0, LOW);
|
|
|
96 |
|
|
|
97 |
// The same pin is used to read the signal from the PING))): a HIGH
|
|
|
98 |
// pulse whose duration is the time (in microseconds) from the sending
|
|
|
99 |
// of the ping to the reception of its echo off of an object.
|
|
|
100 |
pinMode(PING0, INPUT);
|
|
|
101 |
duration = pulseIn(PING0, HIGH);
|
|
|
102 |
uint16_t range = (duration / 29 / 2);
|
|
|
103 |
if (range > 255)
|
|
|
104 |
range = 255;
|
|
|
105 |
|
|
|
106 |
return range;
|
|
|
107 |
}
|
|
|
108 |
|
|
|
109 |
void RBlink(byte led, byte times){ // poor man's display
|
|
|
110 |
for (byte i=0; i< times; i++){
|
|
|
111 |
digitalWrite(led,LOW);
|
|
|
112 |
delay (100);
|
|
|
113 |
digitalWrite(led,HIGH);
|
|
|
114 |
delay (75);
|
|
|
115 |
}
|
|
|
116 |
}
|
|
|
117 |
|