#include <SPI.h>
#include <mcp2515.h>

#define MOSI PB5
#define MISO PB4
#define SCLK PB3


#define ledPin PC13
#define buttonPin PA0

byte data;
struct can_frame canMsg;
struct can_frame canMsg1;
struct can_frame canMsg2;

MCP2515 mcp2515(PA4);

void setup ()
{


while (!Serial);
  Serial.begin(115200);
   mcp2515.reset();
  mcp2515.setBitrate(CAN_1000KBPS, MCP_8MHZ);
 mcp2515.setNormalMode ();
//mcp2515.setLoopbackMode();
  //mcp2515.setListenOnlyMode();
  Serial.println("Example: Write to CAN");

  canMsg1.can_id  = 0x142;
  canMsg1.can_dlc = 8;
  canMsg1.data[0] = 0x88; //старт мотор
  canMsg1.data[1] = 0x00;
  canMsg1.data[2] = 0x00;
  canMsg1.data[3] = 0x00;
  canMsg1.data[4] = 0x00;
  canMsg1.data[5] = 0x00;
  canMsg1.data[6] = 0x00;
  canMsg1.data[7] = 0x00;

  canMsg2.can_id  = 0x142;
  canMsg2.can_dlc = 8;
  canMsg2.data[0] = 0xA1;
  canMsg2.data[1] = 0x00;
  canMsg2.data[2] = 0x00;
  canMsg2.data[3] = 0x00;
  canMsg2.data[4] = 0x00;
  canMsg2.data[5] = 0x50;
  canMsg2.data[6] = 0x00;
  canMsg2.data[7] = 0x00;

mcp2515.sendMessage(&canMsg1);
}

void loop(void)
{

mcp2515.sendMessage(&canMsg2);
 

  Serial.println("Messages sent");
   if (mcp2515.readMessage(&canMsg1) == MCP2515::ERROR_OK) {
    Serial.print(canMsg.can_id, HEX); // print ID
    Serial.print(" ");
    for (int i = 0; i<10; i++)  {  // print the data
      Serial.print(canMsg.data[i],HEX);
      Serial.print(" ");
    }
    Serial.println();
  }
  delay(1000);
}

#include <SPI.h>
//            MOSI  MISO  SCLK
SPIClass SPI3(PC12, PC11, PC10);

void setup() {
  SPI3.begin(2); //Enables the SPI3 instance with default settings and attaches the CS pin 
  SPI3.beginTransaction(1, settings); //Attaches another CS pin and configure the SPI3 instance with other settings 
  SPI3.transfer(2, 0x52); //Transfers data to the first device
  SPI3.transfer(1, 0xA4); //Transfers data to the second device. The SPI3 instance is configured with the right settings 
  SPI3.end() //SPI3 instance is disabled
}

#include <SPI.h>
#include <mcp2515.h>
#include <ros2arduino.h>
#include <user_config.h>
//#define VSPI_CLOCK_PIN      39  //4
//#define VSPI_MOSI_PIN       36 // 15
//#define VSPI_MISO_PIN         38//32
#define VSPI_CS_MCP2515_PIN   5//34//25
#define RMD_X6_READ_PID_DATA                (0x30)
#define RMD_X6_WRITE_PID_TO_RAM             (0x31)
#define RMD_X6_WRITE_PID_TO_ROM             (0x32)
#define RMD_X6_READ_ACCELERATION            (0x33)
#define RMD_X6_WRITE_ACCELERATION           (0x34)
#define RMD_X6_READ_ENCODE_DATA             (0x90)
#define RMD_X6_WRITE_ENCODER_OFFSET         (0x91)
#define RMD_X6_WRITE_CURRENT_POSITION       (0x19)
#define RMD_X6_READ_MULTI_TURNS_ANGLE       (0x92)
#define RMD_X6_READ_SINGLE_CIRCLE_ANGLE     (0x94)
#define RMD_X6_READ_MOTOR_STATUS            (0x9A)
#define RMD_X6_CLEAR_MOTOR_ERROR_FLAG       (0x9B)
#define RMD_X6_READ_MOTOR_STATUS_2          (0x9C)
#define RMD_X6_READ_MOTOR_STATUS_3          (0x9D)
#define RMD_X6_MOTOR_OFF                    (0x80)
#define RMD_X6_MOTOR_STOP                   (0x81)
#define RMD_X6_MOTOR_RUNNING                (0x88)
#define RMD_X6_TORQUE_CLOSED_LOOP           (0xA1)
#define RMD_X6_SPEED_CLOSED_LOOP            (0xA2)
#define RMD_X6_POSITION_CTRL_1              (0xA3)
#define RMD_X6_POSITION_CTRL_2              (0xA4)
#define RMD_X6_POSITION_CTRL_3              (0xA5)
#define RMD_X6_POSITION_CTRL_4              (0xA6)
#define RMD_X6_POSITION                     (0xA7)
#define RMD_X6_STATUS_NULL                  (0x00)
struct can_frame canMsg;
MCP2515 can(VSPI_CS_MCP2515_PIN); //10
//RMD_X6_CAN_MSG_ID
unsigned long leftFrontWheel = 0x142;  //  передний левый
unsigned long leftBackWheel = 0x141;   //  задний левый
unsigned long rightFrontWheel = 0x144; //  передний правый
unsigned long rightBackWheel = 0x143;  //  задний правый
long wheelSpeed = 500;
long wheelPos = 3600;
//long GenPos1=-2600; //Left
//long GenPos2= 2600; //Righ
int8_t    temperatureMotor;
int16_t   torqueCurrentMotor;
int16_t   speedMotor;
uint16_t  encoderMotor;
void motorRun(unsigned long id)
{
  canMsg.can_id  = id;
  canMsg.can_dlc = 8;
  canMsg.data[0] = RMD_X6_MOTOR_RUNNING;
  canMsg.data[1] = RMD_X6_STATUS_NULL;
  canMsg.data[2] = RMD_X6_STATUS_NULL;
  canMsg.data[3] = RMD_X6_STATUS_NULL;
  canMsg.data[4] = RMD_X6_STATUS_NULL;
  canMsg.data[5] = RMD_X6_STATUS_NULL;
  canMsg.data[6] = RMD_X6_STATUS_NULL;
  canMsg.data[7] = RMD_X6_STATUS_NULL;
  can.sendMessage(&canMsg);
}
void motorStop(unsigned long id)
{
  canMsg.can_id  = id;
  canMsg.can_dlc = 8;
  canMsg.data[0] = RMD_X6_MOTOR_STOP;
  canMsg.data[1] = RMD_X6_STATUS_NULL;
  canMsg.data[2] = RMD_X6_STATUS_NULL;
  canMsg.data[3] = RMD_X6_STATUS_NULL;
  canMsg.data[4] = RMD_X6_STATUS_NULL;
  canMsg.data[5] = RMD_X6_STATUS_NULL;
  canMsg.data[6] = RMD_X6_STATUS_NULL;
  canMsg.data[7] = RMD_X6_STATUS_NULL;
  can.sendMessage(&canMsg);
}
void motorOff(unsigned long id)
{
  canMsg.can_id  = id;
  canMsg.can_dlc = 8;
  canMsg.data[0] = RMD_X6_MOTOR_OFF;//RMD_X6_MOTOR_STOP;
  canMsg.data[1] = RMD_X6_STATUS_NULL;
  canMsg.data[2] = RMD_X6_STATUS_NULL;
  canMsg.data[3] = RMD_X6_STATUS_NULL;
  canMsg.data[4] = RMD_X6_STATUS_NULL;
  canMsg.data[5] = RMD_X6_STATUS_NULL;
  canMsg.data[6] = RMD_X6_STATUS_NULL;
  canMsg.data[7] = RMD_X6_STATUS_NULL;
  can.sendMessage(&canMsg);
}
void readReply()
{
  Serial.print(canMsg.can_id, HEX); // print ID
  Serial.print(" ");
  Serial.print(canMsg.can_dlc, HEX); // print DLC
  Serial.print(" ");
   for (int i = 0; i<canMsg.can_dlc; i++)  {  // print the data
     Serial.print(canMsg.data[i],HEX);
     Serial.print(" ");
   }
    Serial.println();
     // Get motor temperature
 temperatureMotor =  (int8_t)canMsg.data[1];
  // Get motor torque current
 torqueCurrentMotor = (int16_t(canMsg.data[2]) << 8) |
                                           canMsg.data[3];
  // Get motor speed
  speedMotor = (int16_t(canMsg.data[4]) << 8) |
                                  canMsg.data[5];
  // Get motor encoder
  encoderMotor = (uint16_t(canMsg.data[6]) << 8) |
                                    canMsg.data[7];
  // Cover dps to rpm
  speedMotor =  speedMotor*60;
  speedMotor = speedMotor/360;
   Serial.print(temperatureMotor);
   Serial.print(" ");
   Serial.print(torqueCurrentMotor);
   Serial.print(" ");
   Serial.print(speedMotor);
   Serial.print(" ");
   Serial.print(encoderMotor);
   Serial.print(" ");
}
void setPosition(unsigned long id, long wheelPos)
{
  // Convert 1degree/LSB to 0.01degree/LSB
  wheelPos = wheelPos * 100;
  wheelPos = wheelPos * 6;
  canMsg.can_id  = id;
  canMsg.can_dlc = 8;
  canMsg.data[0] = RMD_X6_POSITION_CTRL_1;
  canMsg.data[1] = RMD_X6_STATUS_NULL;
  canMsg.data[2] = RMD_X6_STATUS_NULL;
  canMsg.data[3] = RMD_X6_STATUS_NULL;
  canMsg.data[4] = wheelPos;
  canMsg.data[5] = wheelPos >> 8;
  canMsg.data[6] = wheelPos >> 16;
  canMsg.data[7] = wheelPos >> 24;
  can.sendMessage(&canMsg);
  delay(3);
}
void setVelocity(unsigned long id, long wheelSpeed)
{
  // Cover rpm to dps
  wheelSpeed = wheelSpeed*360;
  wheelSpeed = wheelSpeed/60;
  // Cover 1dsp/LSB to 0.01dsp/LSB
  wheelSpeed = wheelSpeed * 100;
  canMsg.can_id  = id;
  canMsg.can_dlc = 8;
  canMsg.data[0] = RMD_X6_SPEED_CLOSED_LOOP;
  canMsg.data[1] = RMD_X6_STATUS_NULL;
  canMsg.data[2] = RMD_X6_STATUS_NULL;
  canMsg.data[3] = RMD_X6_STATUS_NULL;
  canMsg.data[4] = wheelSpeed;
  canMsg.data[5] = wheelSpeed >> 8;
  canMsg.data[6] = wheelSpeed >> 16;
  canMsg.data[7] = wheelSpeed >> 24;
  can.sendMessage(&canMsg);
  delay(3);
}
void moveForward() {
setVelocity(leftFrontWheel, wheelSpeed);
setVelocity(leftBackWheel, wheelSpeed);
setVelocity(rightFrontWheel, wheelSpeed);
setVelocity(rightBackWheel, wheelSpeed);
}
void moveBackward() {
setVelocity(leftFrontWheel, -wheelSpeed);
setVelocity(leftBackWheel, -wheelSpeed);
setVelocity(rightFrontWheel, -wheelSpeed);
setVelocity(rightBackWheel, -wheelSpeed);
}
void moveSidewaysRight() {
setVelocity(leftFrontWheel, wheelSpeed);
setVelocity(leftBackWheel, -wheelSpeed);
setVelocity(rightFrontWheel, -wheelSpeed);
setVelocity(rightBackWheel, wheelSpeed);
}
void moveSidewaysLeft() {
setVelocity(leftFrontWheel, -wheelSpeed);
setVelocity(leftBackWheel, wheelSpeed);
setVelocity(rightFrontWheel, wheelSpeed);
setVelocity(rightBackWheel, -wheelSpeed);
}
void rotateLeft() {
setVelocity(leftFrontWheel, -wheelSpeed);
setVelocity(leftBackWheel, -wheelSpeed);
setVelocity(rightFrontWheel, wheelSpeed);
setVelocity(rightBackWheel, wheelSpeed);
}
void rotateRight() {
setVelocity(leftFrontWheel, wheelSpeed);
setVelocity(leftBackWheel, wheelSpeed);
setVelocity(rightFrontWheel, -wheelSpeed);
setVelocity(rightBackWheel, -wheelSpeed);
}
void moveRightForward() {
setVelocity(leftFrontWheel, wheelSpeed);
setVelocity(leftBackWheel, 0);
setVelocity(rightFrontWheel, 0);
setVelocity(rightBackWheel, wheelSpeed);
}
void moveRightBackward() {
setVelocity(leftFrontWheel, 0);
setVelocity(leftBackWheel, -wheelSpeed);
setVelocity(rightFrontWheel, -wheelSpeed);
setVelocity(rightBackWheel, 0);
}
void moveLeftForward() {
setVelocity(leftFrontWheel, 0);
setVelocity(leftBackWheel, wheelSpeed);
setVelocity(rightFrontWheel, wheelSpeed);
setVelocity(rightBackWheel, 0);
}
void moveLeftBackward() {
setVelocity(leftFrontWheel, -wheelSpeed);
setVelocity(leftBackWheel, 0);
setVelocity(rightFrontWheel, 0);
setVelocity(rightBackWheel, -wheelSpeed);
}
void stopMoving() {
motorStop(leftFrontWheel);
motorStop(leftBackWheel);
motorStop(rightFrontWheel);
motorStop(rightBackWheel);
}
void offMoving() {
motorOff(leftFrontWheel);
motorOff(leftBackWheel);
motorOff(rightFrontWheel);
motorOff(rightBackWheel);
}
void whellsRun() {
motorRun(leftFrontWheel);
motorRun(leftBackWheel);
motorRun(rightFrontWheel);
motorRun(rightBackWheel);
}
void setup() {

  //
  Serial.begin(115200);
  can.reset();
  can.setBitrate(CAN_1000KBPS, MCP_8MHZ);
  can.setNormalMode();
// SPIClass mffVSPI = SPIClass(VSPI);
 // mffVSPI.begin(VSPI_CLOCK_PIN, VSPI_MISO_PIN, VSPI_MOSI_PIN);
  //pinMode(VSPI_MOSI_PIN, INPUT_PULLUP);
  //pinMode(VSPI_MISO_PIN, INPUT_PULLUP);
 // MCP_CAN mffCAN(VSPI_CS_MCP2515_PIN);
  //mffCAN.begin(mffVSPI, MCP_ANY, CAN_250KBPS, MCP_8MHZ);
 // mffCAN.setMode(MCP_NORMAL);
  Serial.println("Initialized");
  whellsRun();
   Serial.println("Initialized Successfully!Write to CAN");
}
void loop() {
  Serial.println("Messages loop");
//moveLeftBackward();
setVelocity(rightFrontWheel, -wheelSpeed);
setVelocity(rightBackWheel, -wheelSpeed);
// stopMoving();
//offMoving();
Serial.println("Messages sent");
   if (can.readMessage(&canMsg) == MCP2515::ERROR_OK) {
     readReply();
   }
    Serial.println();
  delay(100);
}

#include <SPI.h>
//            MOSI  MISO  SCLK
SPIClass SPI3(PC12, PC11, PC10);