#include <Arduino.h>
#include <Wire.h>
#include <smartmotor.h>
#include <SMC_gains.h>

SmartMotor left_motor(0x0A); 
SmartMotor right_motor(0x0B); 

void setup() {
    // INIT SERIAL
    Serial.begin(115200);
    while(!Serial);

    Wire.begin(); // INIT ARDUINO UNO AS I2C CONTROLLER
    
    // TUNE POSITION PID
    left_motor.tune_pos_pid(0.65,0.060,0.065); 
    right_motor.tune_pos_pid(0.65,0.060,0.065); 
    delay(1000);


    // SET MOTOR POSITION
    //int32_t angle=360;
    //uint8_t status= left_motor.write_angle(angle);
    //delay(1000);

    // READ MOTOR POSITION
    int32_t pos = left_motor.read_angle();
    Serial.print("Pos: ");
    Serial.print(pos);
    Serial.println(" deg");
}

int32_t angle = 0;

enum ROBOT_STATE {
  FORWARD,
  TURN,
  REVERSE
};

enum ROBOT_STATE robot_state = FORWARD;

void loop() {
    angle = (millis() / 2000) * 360;
    left_motor.write_angle(angle);
    right_motor.write_angle(angle);
    delay(20);

    // READ MOTOR POSITION
    int32_t pos = left_motor.read_angle();
    int32_t error = angle - pos;
    Serial.print("Pos: ");
    Serial.print(pos);
    Serial.print(" Setpoint: ");
    Serial.print(angle);
    Serial.print(" Err: ");
    Serial.print(error);
    Serial.println("");
}