98 lines
2.0 KiB
C++
98 lines
2.0 KiB
C++
#include <Arduino.h>
|
|
#include <SimpleFOC.h>
|
|
#include <linearHallSensor.h>
|
|
|
|
BLDCMotor motor = BLDCMotor(4);
|
|
BLDCDriver3PWM driver = BLDCDriver3PWM(7, 8, 9);
|
|
|
|
LinearHallSensor linearSensor = LinearHallSensor(A0, A1, -0.75, 2.35);
|
|
|
|
void initSensor()
|
|
{
|
|
linearSensor.init(motor);
|
|
}
|
|
|
|
float callback()
|
|
{
|
|
return linearSensor.readSensorCallback();
|
|
}
|
|
|
|
GenericSensor sensor = GenericSensor(callback, initSensor);
|
|
|
|
float targetX = 0.0;
|
|
float targetY = 0.0;
|
|
|
|
float target = 0.0;
|
|
|
|
void serialLoop()
|
|
{
|
|
static String received_chars;
|
|
while (Serial.available())
|
|
{
|
|
char inChar = (char)Serial.read();
|
|
received_chars += inChar;
|
|
if (inChar == '\n')
|
|
{
|
|
target = received_chars.toFloat();
|
|
Serial.print("Target = ");
|
|
Serial.print(target);
|
|
received_chars = "";
|
|
}
|
|
}
|
|
}
|
|
|
|
float mapfloat(float x, float in_min, float in_max, float out_min, float out_max)
|
|
{
|
|
return (float)(x - in_min) * (out_max - out_min) / (float)(in_max - in_min) + out_min;
|
|
}
|
|
|
|
void setup()
|
|
{
|
|
Serial.begin(115200);
|
|
_delay(100);
|
|
Serial.println("INIT");
|
|
pinMode(3, OUTPUT);
|
|
pinMode(5, OUTPUT);
|
|
pinMode(6, OUTPUT);
|
|
digitalWrite(3, LOW);
|
|
digitalWrite(5, LOW);
|
|
digitalWrite(6, LOW);
|
|
|
|
driver.voltage_power_supply = 6.0;
|
|
driver.init();
|
|
motor.linkDriver(&driver);
|
|
motor.useMonitoring(Serial);
|
|
motor.controller = MotionControlType::angle;
|
|
motor.foc_modulation = FOCModulationType::SinePWM;
|
|
motor.voltage_limit = 1.5;
|
|
motor.voltage_sensor_align = 0.8;
|
|
motor.PID_velocity.P = 0.075f;
|
|
motor.PID_velocity.I = 0.01;
|
|
motor.PID_velocity.D = 0.0;
|
|
motor.LPF_velocity.Tf = 0.01f;
|
|
motor.P_angle.P = 120.0;
|
|
motor.P_angle.I = 10.0;
|
|
motor.velocity_limit = 50;
|
|
|
|
motor.useMonitoring(Serial);
|
|
|
|
// Init sensor
|
|
motor.init();
|
|
Serial.println("calibrating sensor in open loop...");
|
|
sensor.init();
|
|
Serial.println("Done");
|
|
delay(1000);
|
|
|
|
motor.linkSensor(&sensor);
|
|
motor.init();
|
|
// motor.initFOC(5.48, CCW);
|
|
motor.initFOC();
|
|
}
|
|
|
|
void loop()
|
|
{
|
|
serialLoop();
|
|
motor.move(target);
|
|
motor.loopFOC();
|
|
motor.monitor();
|
|
} |