Close-loop position OK :D
This commit is contained in:
parent
f28a03ff05
commit
1bcd56e5f5
@ -1,223 +1,128 @@
|
|||||||
#include "linearHallSensor.h"
|
#include "linearHallSensor.h"
|
||||||
|
|
||||||
float norm(float x, float in_min, float in_max)
|
float norm(float x, float in_min, float in_max, float out_min = -1.0, float out_max = 1.0)
|
||||||
{
|
{
|
||||||
return (float)(x + 1.0) * (2.0) / (float)(in_max - in_min) -1.0;
|
return (float)(x - in_min) * (out_max - out_min) / (float)(in_max - in_min) + out_min;
|
||||||
}
|
}
|
||||||
|
|
||||||
LinearHallSensor::LinearHallSensor(uint8_t ch1, uint8_t ch2)
|
LinearHallSensor::LinearHallSensor(uint8_t ch1, uint8_t ch2, float minPos, float maxPos)
|
||||||
{
|
{
|
||||||
_analogPin1 = ch1;
|
_analogPin1 = ch1;
|
||||||
_analogPin2 = ch2;
|
_analogPin2 = ch2;
|
||||||
|
_minPositionEndValue = minPos;
|
||||||
|
_maxPositionEndValue = maxPos;
|
||||||
}
|
}
|
||||||
|
|
||||||
void LinearHallSensor::init(BLDCMotor motor)
|
void LinearHallSensor::init(BLDCMotor motor)
|
||||||
{
|
{
|
||||||
int senseA = analogRead(A0);
|
MotionControlType prevController = motor.controller;
|
||||||
int senseB = analogRead(A1);
|
motor.controller = MotionControlType::angle_openloop;
|
||||||
|
float prevVoltageLimit = motor.voltage_limit;
|
||||||
|
motor.voltage_limit = 0.8;
|
||||||
|
|
||||||
int minA = senseA, maxA = senseA, minB = senseB, maxB = senseB;
|
uint32_t senseA = analogRead(_analogPin1);
|
||||||
|
uint32_t senseB = analogRead(_analogPin2);
|
||||||
|
|
||||||
// Swipe motor 2 times
|
_minCh1 = senseA, _maxCh1 = senseA;
|
||||||
for (float i = -0.55; i <= 2.25; i = i + 0.005)
|
_minCh2 = senseB, _maxCh2 = senseB;
|
||||||
|
|
||||||
|
// Swipe motor
|
||||||
|
for (float i = _minPositionEndValue; i <= _maxPositionEndValue; i = i + 0.005)
|
||||||
{
|
{
|
||||||
motor.move(i);
|
motor.move(i);
|
||||||
senseA = analogRead(A0);
|
|
||||||
senseB = analogRead(A1);
|
|
||||||
if (senseA > maxA)
|
|
||||||
maxA = senseA;
|
|
||||||
else if (senseA < minA)
|
|
||||||
minA = senseA;
|
|
||||||
if (senseB > maxB)
|
|
||||||
maxB = senseB;
|
|
||||||
else if (senseB < minB)
|
|
||||||
minB = senseB;
|
|
||||||
delay(1);
|
delay(1);
|
||||||
|
senseA = analogRead(_analogPin1);
|
||||||
|
senseB = analogRead(_analogPin2);
|
||||||
|
if (senseA > _maxCh1)
|
||||||
|
_maxCh1 = senseA;
|
||||||
|
else if (senseA < _minCh1)
|
||||||
|
_minCh1 = senseA;
|
||||||
|
if (senseB > _maxCh1)
|
||||||
|
_maxCh1 = senseB;
|
||||||
|
else if (senseB < _minCh2)
|
||||||
|
_minCh2 = senseB;
|
||||||
}
|
}
|
||||||
|
|
||||||
delay(100);
|
delay(100);
|
||||||
|
for (float i = _maxPositionEndValue; i >= _minPositionEndValue; i = i - 0.005)
|
||||||
for (float i = 2.25; i >= -0.55; i = i - 0.005)
|
|
||||||
{
|
{
|
||||||
motor.move(i);
|
motor.move(i);
|
||||||
senseA = analogRead(A0);
|
|
||||||
senseB = analogRead(A1);
|
|
||||||
if (senseA > maxA)
|
|
||||||
maxA = senseA;
|
|
||||||
else if (senseA < minA)
|
|
||||||
minA = senseA;
|
|
||||||
if (senseB > maxB)
|
|
||||||
maxB = senseB;
|
|
||||||
else if (senseB < minB)
|
|
||||||
minB = senseB;
|
|
||||||
delay(1);
|
delay(1);
|
||||||
|
senseA = analogRead(_analogPin1);
|
||||||
|
senseB = analogRead(_analogPin2);
|
||||||
|
if (senseA > _maxCh1)
|
||||||
|
_maxCh1 = senseA;
|
||||||
|
else if (senseA < _minCh1)
|
||||||
|
_minCh1 = senseA;
|
||||||
|
if (senseB > _maxCh1)
|
||||||
|
_maxCh1 = senseB;
|
||||||
|
else if (senseB < _minCh2)
|
||||||
|
_minCh2 = senseB;
|
||||||
}
|
}
|
||||||
|
|
||||||
_maxCh1 = maxA;
|
// Min offset with 50 values average
|
||||||
_maxCh2 = maxB;
|
float sum = 0.0;
|
||||||
_minCh1 = minA;
|
for (uint8_t i = 0; i <= 49; i++)
|
||||||
_minCh2 = minB;
|
{
|
||||||
|
_offset = 0.0;
|
||||||
|
sum += readSensorCallback();
|
||||||
|
}
|
||||||
|
_offset = -sum / 50.0;
|
||||||
|
|
||||||
// Intialise the state machine variables
|
Serial.println("\n\rmaxA: " + String(_maxCh1) + "\tminA: " + String(_minCh1) + "\tmaxB: " + String(_maxCh2) + "\tminB: " + String(_minCh2));
|
||||||
_state = 0; // Set the state machine to 0
|
Serial.println("Offset: " + String(_offset));
|
||||||
_prevCh1 = analogRead(_analogPin1);
|
|
||||||
_prevCh2 = analogRead(_analogPin2);
|
|
||||||
_directionCh1 = Direction::UNKNOWN;
|
|
||||||
_directionCh2 = Direction::UNKNOWN;
|
|
||||||
|
|
||||||
//measure offset angle
|
// Detect max angle
|
||||||
float normCh1 = norm(_prevCh1, _minCh1, _maxCh1);
|
for (float i = _minPositionEndValue; i <= _maxPositionEndValue; i = i + 0.01)
|
||||||
float normCh2 = norm(_prevCh2, _minCh2, _maxCh2);
|
{
|
||||||
float pos = atan2(normCh1, normCh2);
|
motor.move(i);
|
||||||
|
delay(1);
|
||||||
|
_maxSensor = readSensorCallback();
|
||||||
|
}
|
||||||
|
Serial.println("current motor angle: " + String(motor.electricalAngle()));
|
||||||
|
delay(250);
|
||||||
|
|
||||||
_offset = M_PI_2 - pos;
|
// take average value of 50 values
|
||||||
|
sum = 0.0;
|
||||||
|
for (uint8_t i = 0; i <= 49; i++)
|
||||||
|
sum += readSensorCallback();
|
||||||
|
_maxSensor = sum / 50.0;
|
||||||
|
|
||||||
|
Serial.println("Max sensor: " + String(_maxSensor));
|
||||||
|
|
||||||
Serial.println("maxA: " + String(maxA) + "\tminA: " + String(minA) + "\tmaxB: " + String(maxB) + "\tminB: " + String(minB));
|
// Recovering previous controller type and go in the middle
|
||||||
Serial.println("Current Ch1:" + String(_prevCh1) + "\tCh2:" + String(_prevCh2));
|
for (float i = _maxPositionEndValue; i >= _minPositionEndValue/2.0; i = i - 0.01)
|
||||||
|
{
|
||||||
|
motor.move(i);
|
||||||
|
delay(1);
|
||||||
|
readSensorCallback();
|
||||||
|
}
|
||||||
|
motor.controller = prevController;
|
||||||
|
motor.voltage_limit = prevVoltageLimit;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
float dist_angle(float newAngle, float prevAngle)
|
||||||
|
{
|
||||||
|
float diff = newAngle - prevAngle;
|
||||||
|
while (diff < (-M_PI))
|
||||||
|
diff += 2 * M_PI;
|
||||||
|
while (diff > M_PI)
|
||||||
|
diff -= 2 * M_PI;
|
||||||
|
return diff;
|
||||||
|
}
|
||||||
|
|
||||||
float LinearHallSensor::readSensorCallback()
|
float LinearHallSensor::readSensorCallback()
|
||||||
{
|
{
|
||||||
// Update variables
|
// Update variables
|
||||||
float currentCh1 = analogRead(_analogPin1);
|
uint32_t currentCh1 = analogRead(_analogPin1);
|
||||||
float currentCh2 = analogRead(_analogPin2);
|
uint32_t currentCh2 = analogRead(_analogPin2);
|
||||||
|
|
||||||
// Normalise variable between -1 and 1
|
// Normalise variable between -1 and 1
|
||||||
float normCh1 = norm(currentCh1, _minCh1, _maxCh1);
|
float normCh1 = norm((float)currentCh1, _minCh1, _maxCh1);
|
||||||
float normCh2 = norm(currentCh2, _minCh2, _maxCh2);
|
float normCh2 = norm((float)currentCh2, _minCh2, _maxCh2);
|
||||||
|
|
||||||
_currentPosition = atan2f(normCh2, normCh1);
|
_currentPosition = atan2f(normCh2, normCh1);
|
||||||
Serial.print("Current angle: " + String(_currentPosition));
|
_estimatePosition += dist_angle(_currentPosition, _prevPosition);
|
||||||
|
|
||||||
float temp1 = normCh1 + sin(_offset);
|
_prevPosition = _currentPosition;
|
||||||
normCh1 = temp1 * floorf(temp1);
|
return (_estimatePosition + _offset)/4.0;
|
||||||
|
|
||||||
float temp2 = normCh2 + cos(_offset);
|
|
||||||
normCh2 = temp2 * floorf(temp2);
|
|
||||||
|
|
||||||
_currentPosition = atan2f(normCh2, normCh1);
|
|
||||||
Serial.print("\tNew angle: " + String(_currentPosition)+ '\r');
|
|
||||||
|
|
||||||
return _currentPosition;
|
|
||||||
}
|
|
||||||
|
|
||||||
float LinearHallSensor::readSensorCallbackStateMachine()
|
|
||||||
{
|
|
||||||
// Update variables
|
|
||||||
float currentCh1 = analogRead(_analogPin1);
|
|
||||||
float currentCh2 = analogRead(_analogPin2);
|
|
||||||
|
|
||||||
if (currentCh1 > _prevCh1)
|
|
||||||
_directionCh1 = Slope::INCREASING;
|
|
||||||
else if (currentCh1 < _prevCh1)
|
|
||||||
_directionCh1 = Slope::DECREASING;
|
|
||||||
else
|
|
||||||
_directionCh1 = Slope::EQUAL;
|
|
||||||
|
|
||||||
if (currentCh2 > _prevCh2)
|
|
||||||
_directionCh2 = Slope::INCREASING;
|
|
||||||
else if (currentCh2 < _prevCh2)
|
|
||||||
_directionCh2 = Slope::DECREASING;
|
|
||||||
else
|
|
||||||
_directionCh2 = Slope::EQUAL;
|
|
||||||
|
|
||||||
// Normalise variable between -1 and 1
|
|
||||||
float normCh1 = norm(currentCh1, _minCh1, _maxCh1);
|
|
||||||
float normCh2 = norm(currentCh2, _minCh2, _maxCh2);
|
|
||||||
|
|
||||||
// Update previous var
|
|
||||||
_prevCh1 = currentCh1;
|
|
||||||
_prevCh2 = currentCh2;
|
|
||||||
|
|
||||||
// State machine
|
|
||||||
switch (_state)
|
|
||||||
{
|
|
||||||
case 0:
|
|
||||||
_currentPosition = 0.0;
|
|
||||||
if (_directionCh1 == Slope::INCREASING)
|
|
||||||
_state = 1;
|
|
||||||
break;
|
|
||||||
case 1:
|
|
||||||
if(_directionCh1 != Slope::EQUAL)
|
|
||||||
_currentPosition++;
|
|
||||||
|
|
||||||
if (_directionCh1 == Slope::DECREASING)
|
|
||||||
_state = 2;
|
|
||||||
else if (normCh1 >= 0.8 && _directionCh1 == Slope::INCREASING)
|
|
||||||
_state = 3;
|
|
||||||
break;
|
|
||||||
case 2:
|
|
||||||
if(_directionCh1 != Slope::EQUAL)
|
|
||||||
_currentPosition--;
|
|
||||||
|
|
||||||
if (_directionCh1 == Slope::INCREASING)
|
|
||||||
_state = 1;
|
|
||||||
else if (_directionCh1 == Slope::DECREASING && normCh2 <= 0.2)
|
|
||||||
_state = 8;
|
|
||||||
break;
|
|
||||||
case 3:
|
|
||||||
if(_directionCh2 != Slope::EQUAL)
|
|
||||||
_currentPosition++;
|
|
||||||
|
|
||||||
if (_directionCh2 == Slope::INCREASING)
|
|
||||||
_state = 4;
|
|
||||||
else if (_directionCh2 == Slope::DECREASING && normCh2 <= 0.2)
|
|
||||||
_state = 5;
|
|
||||||
break;
|
|
||||||
case 4:
|
|
||||||
if(_directionCh2 != Slope::EQUAL)
|
|
||||||
_currentPosition--;
|
|
||||||
|
|
||||||
if (_directionCh2 == Slope::DECREASING)
|
|
||||||
_state = 3;
|
|
||||||
else if (_directionCh2 == Slope::INCREASING && normCh1 <= 0.8)
|
|
||||||
_state = 2;
|
|
||||||
break;
|
|
||||||
case 5:
|
|
||||||
if(_directionCh1 != Slope::EQUAL)
|
|
||||||
_currentPosition++;
|
|
||||||
if (_directionCh1 == Slope::INCREASING)
|
|
||||||
_state = 6;
|
|
||||||
else if (_directionCh1 == Slope::DECREASING && normCh1 <= 0.2)
|
|
||||||
_state = 7;
|
|
||||||
break;
|
|
||||||
case 6:
|
|
||||||
if(_directionCh1 != Slope::EQUAL)
|
|
||||||
_currentPosition--;
|
|
||||||
|
|
||||||
if (_directionCh1 == Slope::DECREASING)
|
|
||||||
_state = 5;
|
|
||||||
else if (_directionCh1 == Slope::INCREASING && normCh2 >= 0.2)
|
|
||||||
_state = 4;
|
|
||||||
break;
|
|
||||||
case 7:
|
|
||||||
if(_directionCh2 != Slope::EQUAL)
|
|
||||||
_currentPosition++;
|
|
||||||
|
|
||||||
if (_directionCh2 == Slope::DECREASING)
|
|
||||||
_state = 8;
|
|
||||||
else if (_directionCh2 == Slope::INCREASING && normCh2 >= 0.8)
|
|
||||||
_state = 1;
|
|
||||||
break;
|
|
||||||
case 8:
|
|
||||||
if(_directionCh2 != Slope::EQUAL)
|
|
||||||
_currentPosition--;
|
|
||||||
|
|
||||||
if (_directionCh2 == Slope::INCREASING)
|
|
||||||
_state = 7;
|
|
||||||
else if (_directionCh2 == Slope::DECREASING && normCh1 >= 0.2)
|
|
||||||
_state = 6;
|
|
||||||
break;
|
|
||||||
|
|
||||||
default:
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
|
|
||||||
Serial.print("\rState: " + String(_state) +
|
|
||||||
"\tSlope Ch1: " + String(_directionCh1) + "\tnormCh1: " + String(normCh1)+
|
|
||||||
"\tSlope Ch2: " + String(_directionCh2) + "\tnormCh2: " + String(normCh2)+
|
|
||||||
"\tPosition: " + String(_currentPosition));
|
|
||||||
return _currentPosition;
|
|
||||||
}
|
}
|
||||||
|
@ -4,12 +4,6 @@
|
|||||||
#include "Arduino.h"
|
#include "Arduino.h"
|
||||||
#include "BLDCMotor.h"
|
#include "BLDCMotor.h"
|
||||||
|
|
||||||
enum Slope : int8_t{
|
|
||||||
INCREASING = 1,
|
|
||||||
DECREASING = -1,
|
|
||||||
EQUAL = 0
|
|
||||||
};
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief Linear Hall sensor class
|
* @brief Linear Hall sensor class
|
||||||
*
|
*
|
||||||
@ -21,8 +15,10 @@ class LinearHallSensor
|
|||||||
Linear Hall Sensor class constructor
|
Linear Hall Sensor class constructor
|
||||||
@param ch1 Analog pin for channel 1
|
@param ch1 Analog pin for channel 1
|
||||||
@param ch2 Analog pin for channel 2
|
@param ch2 Analog pin for channel 2
|
||||||
|
@param minPos Minimum position in Openloop
|
||||||
|
@param maxPos Maximum position in Openloop
|
||||||
*/
|
*/
|
||||||
LinearHallSensor(uint8_t ch1, uint8_t ch2);
|
LinearHallSensor(uint8_t ch1, uint8_t ch2, float minPos, float maxPos);
|
||||||
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
@ -42,27 +38,23 @@ class LinearHallSensor
|
|||||||
float readSensorCallback();
|
float readSensorCallback();
|
||||||
|
|
||||||
private:
|
private:
|
||||||
uint8_t _analogPin1;
|
uint32_t _analogPin1;
|
||||||
uint8_t _analogPin2;
|
uint32_t _analogPin2;
|
||||||
|
|
||||||
uint16_t _maxCh1;
|
// max values read from the ADC on channels during init
|
||||||
uint16_t _maxCh2;
|
uint32_t _maxCh1;
|
||||||
uint16_t _minCh1;
|
uint32_t _maxCh2;
|
||||||
uint16_t _minCh2;
|
uint32_t _minCh1;
|
||||||
|
uint32_t _minCh2;
|
||||||
|
|
||||||
|
float _prevPosition;
|
||||||
float _currentPosition;
|
float _currentPosition;
|
||||||
float _offset;
|
float _estimatePosition; // current position + offset
|
||||||
|
float _offset; // offset angle measured at init
|
||||||
|
float _maxSensor; // Max angle value measured (so command from 0 to that value)
|
||||||
|
|
||||||
float _minPositionEndValue;
|
float _minPositionEndValue; // min pos in open loop mode
|
||||||
float _maxPositionEndValue;
|
float _maxPositionEndValue;
|
||||||
|
|
||||||
uint16_t _prevCh1;
|
|
||||||
uint16_t _prevCh2;
|
|
||||||
|
|
||||||
int8_t _directionCh1;
|
|
||||||
int8_t _directionCh2;
|
|
||||||
|
|
||||||
uint8_t _state;
|
|
||||||
};
|
};
|
||||||
|
|
||||||
#endif
|
#endif
|
138
src/main.cpp
138
src/main.cpp
@ -2,19 +2,14 @@
|
|||||||
#include <SimpleFOC.h>
|
#include <SimpleFOC.h>
|
||||||
#include <linearHallSensor.h>
|
#include <linearHallSensor.h>
|
||||||
|
|
||||||
|
BLDCMotor motor = BLDCMotor(4);
|
||||||
|
BLDCDriver3PWM driver = BLDCDriver3PWM(7, 8, 9);
|
||||||
|
|
||||||
|
LinearHallSensor linearSensor = LinearHallSensor(A0, A1, -0.75, 2.35);
|
||||||
BLDCMotor motorX = BLDCMotor(4);
|
|
||||||
BLDCDriver3PWM driverX = BLDCDriver3PWM(3, 5, 6);
|
|
||||||
|
|
||||||
BLDCMotor motorY = BLDCMotor(4);
|
|
||||||
BLDCDriver3PWM driverY = BLDCDriver3PWM(7, 8, 9);
|
|
||||||
|
|
||||||
LinearHallSensor linearSensor = LinearHallSensor(A0, A1);
|
|
||||||
|
|
||||||
void initSensor()
|
void initSensor()
|
||||||
{
|
{
|
||||||
linearSensor.init(motorY);
|
linearSensor.init(motor);
|
||||||
}
|
}
|
||||||
|
|
||||||
float callback()
|
float callback()
|
||||||
@ -24,63 +19,10 @@ float callback()
|
|||||||
|
|
||||||
GenericSensor sensor = GenericSensor(callback, initSensor);
|
GenericSensor sensor = GenericSensor(callback, initSensor);
|
||||||
|
|
||||||
|
|
||||||
float targetX = 0.0;
|
float targetX = 0.0;
|
||||||
float targetY = 0.0;
|
float targetY = 0.0;
|
||||||
|
|
||||||
float target = 6.3;
|
float target = 0.0;
|
||||||
|
|
||||||
void setup()
|
|
||||||
{
|
|
||||||
// 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.voltage_limit = 0.6;
|
|
||||||
// motorX.PID_velocity.P = 0.001f;
|
|
||||||
// motorX.PID_velocity.I = 20;
|
|
||||||
// motorX.PID_velocity.D = 0;
|
|
||||||
// motorX.LPF_velocity.Tf = 0.01f;
|
|
||||||
// motorX.P_angle.P = 20;
|
|
||||||
motorX.velocity_limit = 10;
|
|
||||||
|
|
||||||
// *************** Y MOTOR STUFF ******************
|
|
||||||
driverY.voltage_power_supply = 5.0;
|
|
||||||
driverY.init();
|
|
||||||
motorY.linkDriver(&driverY);
|
|
||||||
motorY.foc_modulation = FOCModulationType::SinePWM;
|
|
||||||
motorY.controller = MotionControlType::angle_openloop;
|
|
||||||
motorY.voltage_limit = 0.8;
|
|
||||||
// motorY.PID_velocity.P = 0.001f;
|
|
||||||
// motorY.PID_velocity.I = 0;
|
|
||||||
// motorY.PID_velocity.D = 0;
|
|
||||||
// motorY.LPF_velocity.Tf = 0.01f;
|
|
||||||
// motorY.P_angle.P = 0.1;
|
|
||||||
motorY.velocity_limit = 10;
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
Serial.begin(115200);
|
|
||||||
_delay(100);
|
|
||||||
Serial.println("INIT");
|
|
||||||
|
|
||||||
// initialize motor
|
|
||||||
motorX.init();
|
|
||||||
motorX.initFOC();
|
|
||||||
motorY.init();
|
|
||||||
motorY.initFOC();
|
|
||||||
|
|
||||||
motorX.disable();
|
|
||||||
|
|
||||||
Serial.println(F("Motor ready."));
|
|
||||||
_delay(100);
|
|
||||||
Serial.println("calibrating...");
|
|
||||||
sensor.init();
|
|
||||||
Serial.println("Done\r\n");
|
|
||||||
motorY.disable();
|
|
||||||
}
|
|
||||||
|
|
||||||
void serialLoop()
|
void serialLoop()
|
||||||
{
|
{
|
||||||
@ -104,29 +46,53 @@ 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;
|
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 = 200.0;
|
||||||
|
motor.P_angle.I = 50.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();
|
||||||
|
}
|
||||||
|
|
||||||
void loop()
|
void loop()
|
||||||
{
|
{
|
||||||
sensor.update();
|
serialLoop();
|
||||||
// serialLoop();
|
motor.move(target);
|
||||||
|
motor.loopFOC();
|
||||||
// int senseX = analogRead(A2);
|
motor.monitor();
|
||||||
// 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();
|
|
||||||
}
|
}
|
Loading…
Reference in New Issue
Block a user