60 lines
1.2 KiB
C++
60 lines
1.2 KiB
C++
#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("");
|
|
}
|