Blame | Last modification | View Log | RSS feed
#include "TinyWireS.h" // wrapper class for I2C slave routines
#include "Servo8Bit.h"
#define I2C_SLAVE_ADDR 0x24 // i2c slave address
#define CMD_PING_ANGLE 0x01 // Receive byte (0-180), ping that direction
#define CMD_PING_CURR 0x02
#define CMD_GET_RANGE 0x03
#define CMD_ROTATE_SERV 0x04
#define CMD_SURVEY 0x05
const uint8_t PING0 = 4;
const uint8_t LED_S1 = 3;
const uint8_t SERVO = 1;
uint16_t pingRange = 0;
uint8_t cmd = 0;
uint8_t options = 0;
Servo8Bit servo;
void setup(){
pinMode(LED_S1,OUTPUT);
digitalWrite(LED_S1,HIGH);
RBlink(LED_S1,2); // show it's alive
TinyWireS.begin(I2C_SLAVE_ADDR); // init I2C Slave mode
servo.attach(SERVO, 600, 2400);
servo.write(90);
delay(500);
//servo.detach();
}
void loop(){
if (TinyWireS.available()){ // got I2C input!
cmd = TinyWireS.receive(); // get the byte from master
if (cmd == CMD_PING_ANGLE) {
uint8_t pos = TinyWireS.receive();
servoPos(pos);
pingRange = ping();
} else if (cmd == CMD_PING_CURR) {
pingRange = ping();
} else if (cmd == CMD_GET_RANGE) {
//TinyWireS.send(pingRange >> 8);
TinyWireS.send(pingRange);
} else if (cmd == CMD_ROTATE_SERV) {
uint8_t pos = TinyWireS.receive();
servoPos(pos);
} else if (cmd == CMD_SURVEY) {
uint8_t survey[5];
for (uint8_t x = 0; x <5; x++) {
uint8_t angle = 45 * x;
servoPos(angle);
delay (100);
survey[x] = ping();
delay (100);
}
servoPos(90);
for (uint8_t x = 0; x <5; x++) {
//TinyWireS.send(survey[x] >> 8);
TinyWireS.send(survey[x]);
}
}
RBlink(LED_S1,1);
}
}
void servoPos(uint8_t pos) {
//if (!servo.attached())
servo.attach(SERVO);
if (pos > 180)
pos = 180;
if (pos <= 5)
pos = 5;
servo.write(pos);
delay(500);
//servo.detach();
}
uint8_t ping() {
uint16_t duration = 0;
// The PING))) is triggered by a HIGH pulse of 2 or more microseconds.
// Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
pinMode(PING0, OUTPUT);
digitalWrite(PING0, LOW);
delayMicroseconds(20);
digitalWrite(PING0, HIGH);
delayMicroseconds(50);
digitalWrite(PING0, LOW);
// The same pin is used to read the signal from the PING))): a HIGH
// pulse whose duration is the time (in microseconds) from the sending
// of the ping to the reception of its echo off of an object.
pinMode(PING0, INPUT);
duration = pulseIn(PING0, HIGH);
uint16_t range = (duration / 29 / 2);
if (range > 255)
range = 255;
return range;
}
void RBlink(byte led, byte times){ // poor man's display
for (byte i=0; i< times; i++){
digitalWrite(led,LOW);
delay (100);
digitalWrite(led,HIGH);
delay (75);
}
}