//com37 

HardwareSerial MySerial(0);   //Create a new HardwareSerial class.

#define BITMASK_MARKER 0b10000000 // 0b - binary (1,2,4...)
#define BITMASK_DATA 0b01111111
#define X_AXIS 500 // mm
#define Y_AXIS 550 // mm
#define Z_AXIS 460 // mm
#define XY_BUFFER 20 // mm
#define Z_BUFFER 80 // mm
#define SPINNER_DIAMETER 27 // mm
#define NUM_STEPS 800 

void setup() {
  // initialize both serial ports:
  Serial.begin(115200);
  MySerial.begin(115200, SERIAL_8N1, RX, TX); // at CPU Freq is 40MHz, work half speed of defined.

}

void send_message(int addr, int16_t pos, uint16_t delta) {
  MySerial.write(addr | BITMASK_MARKER);
  MySerial.write(((pos >> 7) & BITMASK_DATA) | BITMASK_MARKER);
  MySerial.write((pos & BITMASK_DATA) | BITMASK_MARKER);
  MySerial.write(((delta >> 7) & BITMASK_DATA) | BITMASK_MARKER);
  MySerial.write((delta & BITMASK_DATA) | BITMASK_MARKER);
  MySerial.write('\x00');
}

void loop() {
  // read from port 1, send to port 0:
  Serial.println("hello world");

  uint8_t value = 57;
  uint8_t b = BITMASK_MARKER | value;

  // address, position, delta
  send_message(0, 0, 2000); // 100 x 4000 should be microseconds of time to the complete task 
  send_message(1, 0, 2000); // 100 x 4000 should be microseconds of time to the complete task 
  send_message(2, 0, 2000); // 100 x 4000 should be microseconds of time to the complete task 
  send_message(3, 0, 2000); // 100 x 10000 should be microseconds of time to the complete task 
  delay(2000);

  send_message(0, 400, 2000); // 100 x 4000 should be microseconds of time to the complete task 
  send_message(1, 400, 2000); // 100 x 4000 should be microseconds of time to the complete task 
  send_message(2, 400, 2000); // 100 x 4000 should be microseconds of time to the complete task 
  send_message(3, 400, 2000); // 100 x 10000 should be microseconds of time to the complete task 
  delay(2000);
  
}