Subversion Repositories group.electronics

#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);
  }
}