118 lines
2.6 KiB
C++
118 lines
2.6 KiB
C++
|
#include <Arduino.h>
|
||
|
|
#include <SimpleFOC.h>
|
||
|
|
|
||
|
|
MagneticSensorAnalog sensor = MagneticSensorAnalog(A0, 183, 512);
|
||
|
|
|
||
|
|
BLDCMotor motorX = BLDCMotor(4);
|
||
|
|
BLDCDriver3PWM driverX = BLDCDriver3PWM(3, 5, 6);
|
||
|
|
|
||
|
|
BLDCMotor motorY = BLDCMotor(4);
|
||
|
|
BLDCDriver3PWM driverY = BLDCDriver3PWM(7, 8, 9);
|
||
|
|
|
||
|
|
float targetX = 0.0;
|
||
|
|
float targetY = 0.0;
|
||
|
|
|
||
|
|
float target = 6.3;
|
||
|
|
|
||
|
|
void setup()
|
||
|
|
{
|
||
|
|
|
||
|
|
sensor.init();
|
||
|
|
// motorY.linkSensor(&sensor);
|
||
|
|
|
||
|
|
// X MOTR STUFF
|
||
|
|
driverX.voltage_power_supply = 5.0;
|
||
|
|
driverX.init();
|
||
|
|
motorX.linkDriver(&driverX);
|
||
|
|
motorX.foc_modulation = FOCModulationType::SpaceVectorPWM;
|
||
|
|
motorX.controller = MotionControlType::angle_openloop;
|
||
|
|
motorX.PID_velocity.P = 0.02f;
|
||
|
|
motorX.PID_velocity.I = 20;
|
||
|
|
motorX.PID_velocity.D = 0;
|
||
|
|
motorX.voltage_limit = 0.8;
|
||
|
|
motorX.LPF_velocity.Tf = 0.01f;
|
||
|
|
motorX.P_angle.P = 20;
|
||
|
|
motorX.velocity_limit = 20;
|
||
|
|
|
||
|
|
// *************** Y MOTOR STUFF ******************
|
||
|
|
driverY.voltage_power_supply = 5.0;
|
||
|
|
driverY.init();
|
||
|
|
motorY.linkDriver(&driverY);
|
||
|
|
motorY.foc_modulation = FOCModulationType::SpaceVectorPWM;
|
||
|
|
motorY.controller = MotionControlType::angle_openloop;
|
||
|
|
motorY.PID_velocity.P = 0.001f;
|
||
|
|
motorY.PID_velocity.I = 0;
|
||
|
|
motorY.PID_velocity.D = 0;
|
||
|
|
motorY.voltage_limit = 0.8;
|
||
|
|
motorY.LPF_velocity.Tf = 0.01f;
|
||
|
|
motorY.P_angle.P = 0.1;
|
||
|
|
motorY.velocity_limit = 3;
|
||
|
|
|
||
|
|
Serial.begin(115200);
|
||
|
|
_delay(100);
|
||
|
|
Serial.println("INIT");
|
||
|
|
|
||
|
|
// initialize motor
|
||
|
|
motorX.init();
|
||
|
|
motorX.initFOC();
|
||
|
|
motorY.init();
|
||
|
|
motorY.initFOC();
|
||
|
|
|
||
|
|
Serial.println(F("Motor ready."));
|
||
|
|
_delay(1000);
|
||
|
|
// Serial.println(sensor.getPreciseAngle());
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
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 loop()
|
||
|
|
{
|
||
|
|
sensor.update();
|
||
|
|
|
||
|
|
// Serial.println(sensor.getPreciseAngle());
|
||
|
|
// delay(5);
|
||
|
|
|
||
|
|
serialLoop();
|
||
|
|
|
||
|
|
int senseX = analogRead(A2);
|
||
|
|
int senseY = analogRead(A3);
|
||
|
|
|
||
|
|
targetX = mapfloat(senseX, 0.0, 1024.0, -0.6, 1.2);
|
||
|
|
targetY = mapfloat(senseY, 0.0, 1024.0, -0.5, 2.2);
|
||
|
|
|
||
|
|
Serial.print("senseX=");
|
||
|
|
Serial.print(senseX);
|
||
|
|
Serial.print("\t mappedX=");
|
||
|
|
Serial.print(targetX);
|
||
|
|
Serial.print("\t senseY=");
|
||
|
|
Serial.print(senseY);
|
||
|
|
Serial.print("\t mappedY=");
|
||
|
|
Serial.println(targetY);
|
||
|
|
|
||
|
|
motorX.move(targetX);
|
||
|
|
motorX.loopFOC();
|
||
|
|
|
||
|
|
motorY.move(targetY);
|
||
|
|
motorY.loopFOC();
|
||
|
|
}
|