modified: firmware/src/main.cpp
This commit is contained in:
parent
1d998ea45b
commit
798b54aa91
1 changed files with 71 additions and 55 deletions
|
@ -12,55 +12,16 @@
|
||||||
#define AS5040_MISO PB14
|
#define AS5040_MISO PB14
|
||||||
#define AS5040_MOSI PC1
|
#define AS5040_MOSI PC1
|
||||||
#define AS5040_SCLK PB10
|
#define AS5040_SCLK PB10
|
||||||
|
#define CURRENT_SENSOR_1 PB1 // INA180A
|
||||||
#define EN_W_Pin LL_GPIO_PIN_6
|
#define CURRENT_SENSOR_2 PB0
|
||||||
#define EN_W_GPIO_Port GPIOC
|
#define CURRENT_SENSOR_3 PC5
|
||||||
#define DRV_FAULT_Pin LL_GPIO_PIN_7
|
|
||||||
#define DRV_FAULT_GPIO_Port GPIOC
|
|
||||||
#define DRV_RESET_Pin LL_GPIO_PIN_8
|
|
||||||
#define DRV_RESET_GPIO_Port GPIOC
|
|
||||||
#define DRV_SLEEP_Pin LL_GPIO_PIN_9
|
|
||||||
#define DRV_SLEEP_GPIO_Port GPIOC
|
|
||||||
#define EN_U_Pin LL_GPIO_PIN_11
|
|
||||||
#define EN_U_GPIO_Port GPIOA
|
|
||||||
#define EN_V_Pin LL_GPIO_PIN_12
|
|
||||||
#define EN_V_GPIO_Port GPIOA
|
|
||||||
#define SENSE3_Pin GPIO_PIN_5
|
|
||||||
#define SENSE3_GPIO_Port GPIOC
|
|
||||||
#define SENSE2_Pin GPIO_PIN_0
|
|
||||||
#define SENSE2_GPIO_Port GPIOB
|
|
||||||
#define SENSE1_Pin GPIO_PIN_1
|
|
||||||
#define SENSE1_GPIO_Port GPIOB
|
|
||||||
#define AS5045_CS_Pin GPIO_PIN_15
|
|
||||||
#define AS5045_CS_GPIO_Port GPIOB
|
|
||||||
#define EN_W_Pin GPIO_PIN_6
|
|
||||||
#define EN_W_GPIO_Port GPIOC
|
|
||||||
#define DRV_FAULT_Pin GPIO_PIN_7
|
|
||||||
#define DRV_FAULT_GPIO_Port GPIOC
|
|
||||||
#define DRV_RESET_Pin GPIO_PIN_8
|
|
||||||
#define DRV_RESET_GPIO_Port GPIOC
|
|
||||||
#define DRV_SLEEP_Pin GPIO_PIN_9
|
|
||||||
#define DRV_SLEEP_GPIO_Port GPIOC
|
|
||||||
#define EN_U_Pin GPIO_PIN_11
|
|
||||||
#define EN_U_GPIO_Port GPIOA
|
|
||||||
#define EN_V_Pin GPIO_PIN_12
|
|
||||||
#define EN_V_GPIO_Port GPIOA
|
|
||||||
#define LED1_Pin GPIO_PIN_10
|
|
||||||
#define LED1_GPIO_Port GPIOC
|
|
||||||
#define LED2_Pin GPIO_PIN_11
|
|
||||||
#define LED2_GPIO_Port GPIOC
|
|
||||||
#define LED3_Pin GPIO_PIN_12
|
|
||||||
#define LED3_GPIO_Port GPIOC
|
|
||||||
#define spi1_cs_Pin GPIO_PIN_2
|
|
||||||
#define spi1_cs_GPIO_Port GPIOD
|
|
||||||
|
|
||||||
#pragma endregion
|
#pragma endregion
|
||||||
|
|
||||||
AS5040Sensor sensor(AS5040_CS, AS5040_MOSI, AS5040_MISO, AS5040_SCLK);
|
AS5040Sensor sensor(AS5040_CS, AS5040_MOSI, AS5040_MISO, AS5040_SCLK);
|
||||||
// Init iPower 6208
|
// Init iPower 6208
|
||||||
BLDCMotor motor = BLDCMotor(12);
|
BLDCMotor motor = BLDCMotor(12);
|
||||||
BLDCDriver3PWM driver = BLDCDriver3PWM(PA10, PA11, PA12, PC6, PA11, PA12);
|
BLDCDriver3PWM driver = BLDCDriver3PWM(PA10, PA11, PA12, PC6, PA11, PA12);
|
||||||
|
InlineCurrentSense current_sense = InlineCurrentSense(0.51, 50, PB1, PB0, PC5);
|
||||||
Commander command = Commander(Serial);
|
Commander command = Commander(Serial);
|
||||||
void onMotor(char *cmd) { command.motor(&motor, cmd); }
|
void onMotor(char *cmd) { command.motor(&motor, cmd); }
|
||||||
|
|
||||||
|
@ -83,32 +44,87 @@ void setup() {
|
||||||
digitalWrite(PA11, HIGH);
|
digitalWrite(PA11, HIGH);
|
||||||
digitalWrite(PA12, HIGH);
|
digitalWrite(PA12, HIGH);
|
||||||
digitalWrite(PC6, HIGH);
|
digitalWrite(PC6, HIGH);
|
||||||
Serial.begin(115200);
|
|
||||||
|
// arduino pin initialization INA180A current sensor
|
||||||
|
pinMode(CURRENT_SENSOR_1, OUTPUT);
|
||||||
|
pinMode(CURRENT_SENSOR_2, OUTPUT);
|
||||||
|
pinMode(CURRENT_SENSOR_3, OUTPUT);
|
||||||
|
analogReadResolution(12);
|
||||||
sensor.init();
|
sensor.init();
|
||||||
motor.linkSensor(&sensor);
|
motor.linkSensor(&sensor);
|
||||||
|
|
||||||
|
driver.pwm_frequency = 20000;
|
||||||
driver.voltage_power_supply = 12;
|
driver.voltage_power_supply = 12;
|
||||||
|
driver.voltage_limit = 12;
|
||||||
driver.init();
|
driver.init();
|
||||||
motor.linkDriver(&driver);
|
motor.linkDriver(&driver);
|
||||||
|
|
||||||
|
current_sense.linkDriver(&driver);
|
||||||
|
// init current sense
|
||||||
|
current_sense.init();
|
||||||
|
// link the motor to current sense
|
||||||
|
motor.linkCurrentSense(¤t_sense);
|
||||||
|
// control loop type and torque mode
|
||||||
|
motor.torque_controller = TorqueControlType::foc_current;
|
||||||
motor.controller = MotionControlType::torque;
|
motor.controller = MotionControlType::torque;
|
||||||
motor.PID_velocity.P = 0.2;
|
motor.motion_downsample = 0;
|
||||||
motor.PID_velocity.I = 20;
|
|
||||||
|
// velocity loop PID
|
||||||
|
motor.PID_velocity.P = 0;
|
||||||
|
motor.PID_velocity.I = 0;
|
||||||
motor.PID_velocity.D = 0;
|
motor.PID_velocity.D = 0;
|
||||||
motor.voltage_limit = 12;
|
motor.PID_velocity.output_ramp = 0;
|
||||||
motor.LPF_velocity.Tf = 0.01;
|
motor.PID_velocity.limit = 0;
|
||||||
motor.P_angle.P = 20;
|
// Low pass filtering time constant
|
||||||
motor.velocity_limit = 50;
|
motor.LPF_velocity.Tf = 0;
|
||||||
Serial.begin(115200);
|
// angle loop PID
|
||||||
|
motor.P_angle.P = 0;
|
||||||
|
motor.P_angle.I = 0;
|
||||||
|
motor.P_angle.D = 0;
|
||||||
|
motor.P_angle.output_ramp = 0;
|
||||||
|
motor.P_angle.limit = 0;
|
||||||
|
// Low pass filtering time constant
|
||||||
|
motor.LPF_angle.Tf = 0;
|
||||||
|
// current q loop PID
|
||||||
|
motor.PID_current_q.P = 0;
|
||||||
|
motor.PID_current_q.I = 0;
|
||||||
|
motor.PID_current_q.D = 0;
|
||||||
|
motor.PID_current_q.output_ramp = 0;
|
||||||
|
motor.PID_current_q.limit = 0;
|
||||||
|
// Low pass filtering time constant
|
||||||
|
motor.LPF_current_q.Tf = 0;
|
||||||
|
// current d loop PID
|
||||||
|
motor.PID_current_d.P = 0;
|
||||||
|
motor.PID_current_d.I = 0;
|
||||||
|
motor.PID_current_d.D = 0;
|
||||||
|
motor.PID_current_d.output_ramp = 0;
|
||||||
|
motor.PID_current_d.limit = 0;
|
||||||
|
// Low pass filtering time constant
|
||||||
|
motor.LPF_current_d.Tf = 0;
|
||||||
|
// Limits
|
||||||
|
motor.velocity_limit = 0;
|
||||||
|
motor.voltage_limit = 0;
|
||||||
|
motor.current_limit = 0;
|
||||||
|
// sensor zero offset - home position
|
||||||
|
motor.sensor_offset = 0;
|
||||||
|
// general settings
|
||||||
|
// motor phase resistance
|
||||||
|
motor.phase_resistance = 0;
|
||||||
|
// pwm modulation settings
|
||||||
|
motor.foc_modulation = FOCModulationType::SinePWM;
|
||||||
|
motor.modulation_centered = 1;
|
||||||
|
command.add('A', onMotor, "motor");
|
||||||
|
// tell the motor to use the monitoring
|
||||||
motor.useMonitoring(Serial);
|
motor.useMonitoring(Serial);
|
||||||
|
motor.monitor_downsample = 0; // disable monitor at first - optional
|
||||||
|
|
||||||
motor.init();
|
motor.init();
|
||||||
motor.initFOC();
|
motor.initFOC();
|
||||||
motor.target = 2;
|
|
||||||
command.add('A', onMotor, "motor");
|
|
||||||
|
|
||||||
_delay(1000);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void loop() {
|
void loop() {
|
||||||
motor.loopFOC();
|
motor.loopFOC();
|
||||||
|
motor.monitor();
|
||||||
motor.move();
|
motor.move();
|
||||||
command.run();
|
command.run();
|
||||||
}
|
}
|
Loading…
Add table
Add a link
Reference in a new issue