// RoombaRemote.ino
// Experimental sketch to control iRobot Roomba 880
// - runs on Arduino Pro Mini (5V) connected to computer's serial port
// - interprets keystrokes and sends commands to Roomba via XL4432-SMT transceiver
// - uses 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 7
#define CTRL_ADDRESS 5

#define RF22_SDN_PIN 3

// 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);

// Dont put this on the stack:
uint8_t buf[RH_RF22_MAX_MESSAGE_LEN];


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

  Serial.begin(115200);
  Serial.setTimeout(10);
  
  if (!radio.init())
    Serial.println("init failed");
  else
  {
    Serial.println(RH_RF22_MAX_MESSAGE_LEN);
  // 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()
{
  static int velocity = 0;
  static int angle = 0;
  byte str[3], cmd;

  if (Serial.available())
  {
    Serial.readBytes(str,3);
    if (str[0]==27 && str[1]==91) //direction keys
      cmd = str[2] - 53;
    else
      cmd = str[0];
      
    switch(cmd)
    {
      // measure battery voltage
      case 'v'  : buf[0] = 'v';
                  if(!radio.sendto(buf, 1, CTRL_ADDRESS))
                    Serial.print("sendto failed");
                  else
                    Serial.print("measured voltage [V]:");
                  break;
                  
      // Wakeup Roomba
      case 'W'  : buf[0] = 'W';
                  if(!radio.sendto(buf, 1, CTRL_ADDRESS))
                    Serial.print("sendto failed");
                  break;
                  
      // START command -> PASSIVE mode
      case 'S'  : buf[0] = '$';
                  buf[1] = 128;
                  if(!radio.sendto(buf, 2, CTRL_ADDRESS))
                    Serial.print("sendto failed");
                  break;

      // SAFE mode
      case 'A'  : velocity = 0;
                  angle = 0;
                  buf[0] = '$';
                  buf[1] = 131;
                  if(!radio.sendto(buf, 2, CTRL_ADDRESS))
                    Serial.print("sendto failed");
                  break;

      // FULL mode
      case 'F'  : velocity = 0;
                  angle = 0;
                  buf[0] = '$';
                  buf[1] = 132;
                  if(!radio.sendto(buf, 2, CTRL_ADDRESS))
                    Serial.print("sendto failed");
                  break;

      // CLEAN command
      case 'C'  : buf[0] = '$';
                  buf[1] = 135;
                  if(!radio.sendto(buf, 2, CTRL_ADDRESS))
                    Serial.print("sendto failed");
                  break;

      // SPOT command
      case 'O'  : buf[0] = '$';
                  buf[1] = 134;
                  if(!radio.sendto(buf, 2, CTRL_ADDRESS))
                    Serial.print("sendto failed");
                  break;

      // DOCK command
      case 'D'  : buf[0] = '$';
                  buf[1] = 143;
                  if(!radio.sendto(buf, 2, CTRL_ADDRESS))
                    Serial.print("sendto failed");
                  break;

      // POWER DOWN command
      case 'P'  : buf[0] = '$';
                  buf[1] = 133;
                  if(!radio.sendto(buf, 2, CTRL_ADDRESS))
                    Serial.print("sendto failed");
                  break;

      // Sensor - Battery voltage
      case 'b'  : buf[0] = '$';
                  buf[1] = 142;
                  buf[2] = 22;
                  if(!radio.sendto(buf, 3, CTRL_ADDRESS))
                    Serial.print("sendto failed");
                  else
                    Serial.print("sensor battery voltage [mV]:");
                  break;
                  
      // Sensor - Chargind state
      case 'x'  : buf[0] = '$';
                  buf[1] = 142;
                  buf[2] = 21;
                  if(!radio.sendto(buf, 3, CTRL_ADDRESS))
                    Serial.print("sendto failed");
                  else
                    Serial.print("sensor charging state:");
                  break;
                  
      // sensor battery charge
      case 'c'  : buf[0] = '$';
                  buf[1] = 142;
                  buf[2] = 25;
                  if(!radio.sendto(buf, 3, CTRL_ADDRESS))
                    Serial.print("sendto failed");
                  else
                    Serial.print("sensor battery charge [mAh]:");
                  break;

      // Drive command -> Increase Speed  (Arrow up)
      case 12   : velocity += 20;
                  buf[0] = '$';
                  buf[1] = 137;
                  buf[2] = highByte(velocity);
                  buf[3] = lowByte(velocity);
                  buf[4] = highByte(angle);
                  buf[5] = lowByte(angle);
                  if(!radio.sendto(buf, 6, CTRL_ADDRESS))
                    Serial.print("sendto failed");
                  break;
      
      // Drive command -> Stop  (Arrow down)
      case 13   : velocity -= 20;
                  buf[0] = '$';
                  buf[1] = 137;
                  buf[2] = highByte(velocity);
                  buf[3] = lowByte(velocity);
                  buf[4] = highByte(angle);
                  buf[5] = lowByte(angle);
                  if(!radio.sendto(buf, 6, CTRL_ADDRESS))
                    Serial.print("sendto failed");
                  break;
      
      // Drive command -> Turn Left   (Arrow left)
      case 14  : if (angle > 0)
                    angle = 0;
                  else if (angle == 0)
                    angle = -2000;
                  else
                    angle /= 2;
                  buf[0] = '$';
                  buf[1] = 137;
                  buf[2] = highByte(velocity);
                  buf[3] = lowByte(velocity);
                  buf[4] = highByte(angle);
                  buf[5] = lowByte(angle);
                  if(!radio.sendto(buf, 6, CTRL_ADDRESS))
                    Serial.print("sendto failed");
                  break;
      
      // Drive command -> Turn Right  (Arrow right)
      case 15   : if (angle < 0)
                    angle = 0;
                  else if (angle == 0)
                    angle = 2000;
                  else
                    angle /= 2;
                  buf[0] = '$';
                  buf[1] = 137;
                  buf[2] = highByte(velocity);
                  buf[3] = lowByte(velocity);
                  buf[4] = highByte(angle);
                  buf[5] = lowByte(angle);
                  if(!radio.sendto(buf, 6, CTRL_ADDRESS))
                    Serial.print("sendto failed");
                  break;

      // Drive command -> ???   (Page Up)
      case 0    : break;
      
      // Drive command -> Stop   (Page Dn)
      case 1    : velocity = 0;
                  angle = 0;
                  buf[0] = '$';
                  buf[1] = 137;
                  buf[2] = highByte(velocity);
                  buf[3] = lowByte(velocity);
                  buf[4] = highByte(angle);
                  buf[5] = lowByte(angle);
                  if(!radio.sendto(buf, 6, CTRL_ADDRESS))
                    Serial.print("sendto failed");
                  break;
      
      default   : break;
    }
  }

  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))
    {
      //Serial.print("received:");
      for(int i=0; i<len; i++)
      {
        Serial.print(' ');
        Serial.print(buf[i]);
      }
      Serial.println();
    }
  }

}