[arduino code examples for MT4v2]-01 Control 4 channel DC motor

Code:
/*

 * KinCony MT4V2 DC Motor Test

 * Power ON -> M1-M4 run at 100%

 *

 * Made by KinCony IoT

 * https://www.kincony.com

 */



#include <Arduino.h>



#define RXD 15

#define TXD 16



HardwareSerial MotorSerial(1);



uint16_t crc16(const uint8_t *data, uint8_t len)

{

  uint16_t crc = 0xFFFF;



  while (len--)

  {

    crc ^= *data++;



    for (uint8_t i = 0; i < 8; i++)

      crc = (crc & 1) ? (crc >> 1) ^ 0xA001 : crc >> 1;

  }



  return crc;

}



void sendFrame(uint8_t *data)

{

  uint16_t crc = crc16(data, 6);



  MotorSerial.write(data, 6);

  MotorSerial.write(crc & 0xFF);

  MotorSerial.write(crc >> 8);

  MotorSerial.flush();



  delay(50);



  // Clear ARM response

  while (MotorSerial.available())

    MotorSerial.read();

}



void enableMotor()

{

  uint8_t cmd[] = {

    1, 0x05,

    0x01, 0xF7,     // Coil 503

    0xFF, 0x00      // ON

  };



  sendFrame(cmd);

}



void setMotor(uint8_t motor, int16_t speed)

{

  uint16_t reg = 2000 + motor - 1;



  uint8_t cmd[] = {

    1, 0x06,

    uint8_t(reg >> 8),

    uint8_t(reg),

    uint8_t(speed >> 8),

    uint8_t(speed)

  };



  sendFrame(cmd);

}



void startMotors()

{

  enableMotor();



  setMotor(1, 100);  //100 is speed

  setMotor(2, 100);  //100 is speed

  setMotor(3, 100);  //100 is speed

  setMotor(4, 100);  //100 is speed

}



void setup()

{

  MotorSerial.begin(115200, SERIAL_8N1, RXD, TXD);



  // Wait for ARM CPU startup

  delay(3000);



  startMotors();

}



void loop()

{

  // Refresh commands every 2 seconds

  delay(2000);



  startMotors();

}