/*
JrkSerial.cpp -- library for Pololu Jrk Motor Controller
                   operating in UART Serial Mode, with the
                   Arduino using hardware serial communication
                   via digital pins 0 and 1.
Created by Robin Sharp, June 2014.
Released into the public domain.
*/

#include "Arduino.h"
#include "JrkSerial.h"

JrkSerial::JrkSerial()
{ // Constructor
  _posMin = 0;
  _posMax = 4095;
  _currentpos = 2048;
  Serial.begin(19200);
  delay(1000);         // Allow controller to wake up!
  //  Serial.write(0xAA);  // To allow controller to auto-detect
                       //    the selected baud rate.
  Serial.write(0xFF);  // Make sure motor is stopped.
  getPosition();       // Sets _currentpos to true position.
}

JrkSerial::~JrkSerial()
{ // Destructor
  Serial.flush();      // Clear any pending output from buffer.
  Serial.write(0xFF);  // Make sure motor is stopped.
}

// Auxiliary function to read 2-byte responses from the controller
int      JrkSerial::get2byteResponse()
{ char    response[2];
  byte    charsread;
  int     result = -1; // Default if no valid response.
  charsread = Serial.readBytes(response,2);
  if (charsread == 2)
    { result = 256*response[1] + response[0];
      // result = int((response[1] << 8) | response[0]);
    };
  return result;
};

// Commands to controller.
int    JrkSerial::motorOff()
{ // Stops the motor.  
  // Depending on the (pre-set) controller configuration, 
  //   this may result in the motor braking or coasting.
  // Returns the Halt Error flags.
  Serial.write(0xFF);
  return getHaltErrors();
}

int     JrkSerial::getPosition()
{ // Returns the current position of the actuator
  _currentpos = getVariable(sfeedback);
  // This is just for testing.  Suitable scaling must be added!
  return _currentpos;
}

void JrkSerial::setPosition(const unsigned setpoint)
{ // Sets the target position of the actuator.
  // This version just for testing.  Scaling must be added!
  if (setpoint >= 2048)
    { Serial.write(0xE1);
      Serial.write(uint8_t((setpoint - 2048)/16));
    }
  else 
    { Serial.write(0xE0);
      Serial.write(uint8_t((2048 - setpoint)/16));
    }
}

void JrkSerial::setLimits(const int posMin, const int posMax)
{ // Sets software limits for the position.
  _posMin = posMin;
  _posMax = posMax;
}

int     JrkSerial::getVariable(const variable command)
{ // Returns the value of the Pololu controller variable 
  //   specified by the command argument.
  Serial.write(uint8_t((command << 1) | 0xA1));
  return get2byteResponse();
}

int     JrkSerial::getErrors()
{ // Returns all the unmasked flags for errors which have
  //   occurred since the last call of this method.
  Serial.write(0xB5);
  return get2byteResponse();
}

int     JrkSerial::getHaltErrors()
{ // Returns all the unmasked flags for errors which have caused
  //   the motor to stop.
  Serial.write(0xB3);
  return get2byteResponse();
}