// AK T-motor 70-10 Control CAN Send
//

/*********************************************************************************************************
  DEFINITION SECTION
*********************************************************************************************************/

#include <mcp_can.h>
#include <SPI.h>

// DEFINE CONST VALUE
const int cs_pin = 10;
const unsigned long CAN_ID = 0x01;

// INIT CAN BUS SHEILD
MCP_CAN CAN(cs_pin);

//VALUE LIMIS (for AK70-10)
#define P_MIN -12.5f
#define P_MAX 12.5f
#define V_MIN -50.0f
#define V_MAX 50.0f
#define KP_MIN 0.0f
#define KP_MAX 500.0f
#define KD_MIN 0.0f
#define KD_MAX 5.0f
#define T_MIN -25.0f
#define T_MAX 25.0f

// SET VALUES
float p_in = 3.0f;
float v_in = 0.0f;
float kp_in = 0.5f;
float kd_in = 0.0f;
float t_in = 0.0f;

//measured values
float p_out = 0.0f;
float v_out = 0.0f;
float t_out = 0.0f;

/*********************************************************************************************************
  MAIN FUNCTION SECTION
*********************************************************************************************************/

void setup() {
  Serial.begin(115200);
  delay(1000);
  Serial.println("Arduino Initialized Successfully!!");
    while (CAN.begin(MCP_ANY, CAN_1000KBPS, MCP_16MHZ) != CAN_OK)
  {
    Serial.println("Error Initializing MCP2515...");
    delay(1000);
  }
  CAN.setMode(MCP_NORMAL);   // Change to normal mode to allow messages to be transmitted
  Serial.println("CAN BUS Shield Setting OK!!");
  Zero();
  delay(1000); 
}

void loop() {
  // check key inputs
  char rc;
  while (Serial.available() > 0) 
  {
    rc = Serial.read();
    Serial.println(rc);
    switch (rc) {

      case 'd':
        Serial.println("MOTOR DISABLE!!");
        ExitMotorMode();
        unpack_reply();
        break;

      case 'e':
        Serial.println("MOTOR ENABLE!!");
        EnterMotorMode();
        unpack_reply();
        break;

      case 'q':
      p_in = p_in + 1.0;
        Serial.println("targetPos [rad]:    ");
        pack_cmd();
        break;

      case 'w':
      p_in = p_in - 1.0;
        Serial.println("targetPos [rad]:    ");
        pack_cmd();
        break; 

      case 'r':
        unpack_reply();
        break;          
    }
  }
}

/*********************************************************************************************************
  MOTOR MODE SECTION
*********************************************************************************************************/

// ENABLE MOTOR
void EnterMotorMode() {
  byte buf[8];
  buf[0] = 0xFF;
  buf[1] = 0xFF;
  buf[2] = 0xFF;
  buf[3] = 0xFF;
  buf[4] = 0xFF;
  buf[5] = 0xFF;
  buf[6] = 0xFF;
  buf[7] = 0xFC;
 
  // send data:  ID = 0x01, Standard CAN Frame, Data length = 8 bytes, 'data' = array of data bytes to send
  CAN.sendMsgBuf(CAN_ID, 0, 8, buf);
  if (CAN_OK != CAN.sendMsgBuf(CAN_ID, 0, 8, buf))
    {
      Serial.println("Error Enabling Motor Mode");
      Serial.println(CAN.sendMsgBuf(CAN_ID, 0, 8, buf));
      delay(100);
    }
 

  delay(100);   // send data per 100ms
}

// DISABLE MOTOR
void ExitMotorMode() {
  byte buf[8];
  buf[0] = 0xFF;
  buf[1] = 0xFF;
  buf[2] = 0xFF;
  buf[3] = 0xFF;
  buf[4] = 0xFF;
  buf[5] = 0xFF;
  buf[6] = 0xFF;
  buf[7] = 0xFD;

  // send data:  ID = 0x01, Standard CAN Frame, Data length = 8 bytes, 'data' = array of data bytes to send
  byte sndStat = CAN.sendMsgBuf(CAN_ID, 0, 8, buf);
  if(sndStat == CAN_OK){
    Serial.println("Motor Exited Successfully!");
  } else {
    Serial.println("Error for Exiting...");
  }
  delay(100);   // send data per 100ms
}

// ZEROING MOTOR
void Zero() {
  byte buf[8];
  buf[0] = 0xFF;
  buf[1] = 0xFF;
  buf[2] = 0xFF;
  buf[3] = 0xFF;
  buf[4] = 0xFF;
  buf[5] = 0xFF;
  buf[6] = 0xFF;
  buf[7] = 0xFE;
  
 // send data:  ID = 0x01, Standard CAN Frame, Data length = 8 bytes, 'data' = array of data bytes to send
  byte sndStat = CAN.sendMsgBuf(CAN_ID, 0, 8, buf);
  if(sndStat == CAN_OK){
    Serial.println("Motor Zeroing Successfully!");
  } else {
    Serial.println("Error for Zeroing...");
  }
  delay(100);   // send data per 100ms
}

/*********************************************************************************************************
  CAN COMMUNICATION SECTION
*********************************************************************************************************/

void unpack_reply() {
  byte ext = 0;
  byte len = 8;
  byte dat[8];

  CAN.readMsgBuf(&CAN_ID, &ext, &len, dat); 
  
  /// unpack ints from CAN buffer ///
  unsigned int id = dat[0];
  unsigned int p_int = (dat[1] << 8) | dat[2];
  unsigned int v_int = (dat[3] << 4) | (dat[4] >> 4);
  unsigned int i_int = ((dat[4] & 0xF) << 8) | dat[5];
  /// convert uints to floats ///
  p_out = uint_to_float(p_int, P_MIN, P_MAX, 16);
  v_out = uint_to_float(v_int, V_MIN, V_MAX, 12);
  t_out = uint_to_float(i_int, T_MIN, T_MAX, 12);
  Serial.print("ID: ");
  Serial.print(id);
  Serial.print("      P[rad]: " );
  Serial.print(p_out);
  Serial.print("      V[rad/s]: ");
  Serial.print(v_out);
  Serial.print("      T[Nm]: ");
  Serial.println(t_out);
}

void pack_cmd() {
  byte buf[8];
  ///CAN Command Packet Structure///
  ///16 bit position command, between -12.5*pi and 12.5*pi
  ///12 bit velocity command, between -50.0 and +50.0rad/s
  //12 bit kp, between 0 and 500 N-m/rad
  ///12 bit kd, between 0 and 5 N-m*s/rad
  /// 12 bit feed forward toque, between -25 and 25 N-m
  ///CAN packet is 8 8-bit words
  ///Formatted as follows. For each quantity, bit i0 is LSB
  ///0: [position[15-8]]
  ///1: [position [7-0]]
  ///2: [velocity[11-4]]
  ///3: [velocity[3-0], kp[11-8]]
  ///4: [kp[7-0]]
  ///5: [kd[11-4]]
  ///6:[kd[3-0], torque [11-8]]
  ///7: [torque [7-0]]

  ///limit data to be withing bounds///
  float p_des = constrain(p_in, P_MIN, P_MAX); ///fminf(fmaxf(P_MIN, p_in(, P_MAX);
  float v_des = constrain(v_in, V_MIN, V_MAX); ///fminf(fmaxf(V_MIN, v_in(, V_MAX);
  float kp = constrain(kp_in, KP_MIN, KP_MAX); ///fminf(fmaxf(KP_MIN, kp_in(, KP_MAX);
  float kd = constrain(kd_in, KD_MIN, KD_MAX); ///fminf(fmaxf(KD_MIN, kd_in(, KD_MAX);
  float t_ff = constrain(t_in, T_MIN, T_MAX); ///fminf(fmaxf(T_MIN, t_in(, V_MAX);

 ///convert floats to unsigned ints///
  unsigned int p_int = float_to_uint(p_des, P_MIN, P_MAX, 16);
  unsigned int v_int = float_to_uint(v_des, V_MIN, V_MAX, 12);
  unsigned int kp_int = float_to_uint(kp, KP_MIN, KP_MAX, 12);
  unsigned int kd_int = float_to_uint(kd, KD_MIN, KD_MAX, 12);
  unsigned int t_int = float_to_uint(t_ff, T_MIN, T_MAX, 12);

 /// pack ints into the can buffer///
  buf[0] = p_int >> 8;
  buf[1] = p_int & 0xFF;;
  buf[2] = v_int >> 4;
  buf[3] = ((v_int & 0xF) <<4) | (kp_int >>8);
  buf[4] = kp_int & 0xFF;
  buf[5] = kd_int >>4;
  buf[6] = ((kd_int & 0xF) <<4) | (t_int >>8);
  buf[7] = t_int & 0xFF;
 
  // send data:  ID = 0x100, Standard CAN Frame, Data length = 8 bytes, 'data' = array of data bytes to send
  byte sndStat = CAN.sendMsgBuf(CAN_ID, 0, 8, buf);
  if(sndStat == CAN_OK){
    Serial.println("Command Sent Successfully!");
  } else {
    Serial.println("Error Sending Command...");
  }
 if ( CAN_OK != CAN.sendMsgBuf(CAN_ID, 0, 8, buf))
    {
      Serial.println("Error Sending Command...");
      delay(100);
    }
    else
  delay(100);   // send data per 100ms
}

/*********************************************************************************************************
  CONVERSION SECTION
*********************************************************************************************************/

unsigned int float_to_uint(float x, float x_min, float x_max, int bits) {
  ///Converts a  float to an unsigned int, given range and number of bits///
  float span = x_max-x_min;
  float offset = x_min;
  unsigned int pgg = 0;
  if(bits==12){
    pgg = (unsigned int) ((x-offset)*4095.0/span);
  }
  if(bits==16){
    pgg = (unsigned int) ((x-offset)*65535.0/span);
  }
  return pgg;
}

float uint_to_float(unsigned int x_int, float x_min, float x_max, int bits) {
  ///converts unsigned int to float, given range and number of bits///
  float span = x_max-x_min;
  float offset = x_min;
  float pgg = 0;
  if (bits==12){
    pgg = ((float) x_int)*span/4095.0 + offset;
  }
  if (bits==16){
    pgg = ((float) x_int)*span/65535.0 + offset;
  }
  return pgg;
}

/*********************************************************************************************************
  END FILE
*********************************************************************************************************/