// RoombaCtrl.ino
// Experimental sketch to control iRobot Roomba 880
// - runs on Arduino Pro Mini (5V) under the robot's handle
// - receives commands via attached XL4432-SMT transceiver
// - SCI communication according to iRobot Roomba 500 Open Interface Specification
//
// rev 1.0 - 2015.07.18
//    - initial version
//    - uses RF22 driver by RadioHead http://www.airspayce.com/mikem/arduino/RadioHead/
//
// boredman@boredomprojects.net

#include <RHDatagram.h>
#include <RH_RF22.h>
#include <SPI.h>

#define MY_ADDRESS 5
#define RMT_ADDRESS 7

#define RF22_SDN_PIN 3
#define RMB_WKUP_PIN A3
#define RMB_VBAT_PIN A5

// instance of the radio driver
RH_RF22 rf22_driver;

// Class to manage message delivery and receipt, using the driver declared above
RHDatagram radio(rf22_driver, MY_ADDRESS);

uint8_t buf[RH_RF22_MAX_MESSAGE_LEN];


void setup() 
{
  pinMode(RMB_WKUP_PIN, OUTPUT);
  digitalWrite(RMB_WKUP_PIN, HIGH);

  pinMode(RF22_SDN_PIN, OUTPUT);
  digitalWrite(RF22_SDN_PIN, HIGH);
  delay(500);
  digitalWrite(RF22_SDN_PIN, LOW);
  delay(500);

  Serial.begin(115200);
  Serial.setTimeout(2); // 2 ms is much longer than the duration of 1 byte (10/115200=86us)
                        // for some reason, 1 ms still caused splitting reads
 
  if (radio.init())
  {
  // Defaults after init are 434.0MHz, 0.05MHz AFC pull-in, modulation FSK_Rb2_4Fd36
    rf22_driver.setModemConfig(RH_RF22::GFSK_Rb57_6Fd28_8);
    rf22_driver.setTxPower(RH_RF22_TXPOW_14DBM);  // 1, 2, 5, *8*, 11, 14, 17, 20
  }

}


void loop()
{

  if (radio.available())
  {
    // Wait for a message addressed to us from the client
    uint8_t len = sizeof(buf);
    uint8_t from;
    if (radio.recvfrom(buf, &len, &from) && (from==RMT_ADDRESS))
    {
      switch(buf[0])
      {
        // measure battery voltage
        case 'v'  : MeasureBatteryVoltage((int*)buf);
                    radio.sendto(buf, 1, RMT_ADDRESS);
                    break;
                    
        // wakeup roomba
        case 'W'  : digitalWrite(RMB_WKUP_PIN, LOW);
                    delay(600);
                    digitalWrite(RMB_WKUP_PIN, HIGH);
                    break;
                    
        // send a command to Roomba
        case '$'  : Serial.write(&buf[1],len-1);
                    break;
                    
        default   : break;
      }
    }
  }

  if (Serial.available())
  {
    byte len = Serial.readBytes(buf, RH_RF22_MAX_MESSAGE_LEN-10);
    radio.sendto(buf, len, RMT_ADDRESS);
  }

}


bool MeasureBatteryVoltage(int* vbat)
{
  int raw = analogRead(RMB_VBAT_PIN);
  *vbat = raw * 11 / 305;
  return true;
}