114 lines
2.9 KiB
C++
114 lines
2.9 KiB
C++
|
#include <AS5040.h>
|
||
|
|
#include <Arduino.h>
|
||
|
|
#include <SPI.h>
|
||
|
|
#include <SimpleFOC.h>
|
||
|
|
|
||
|
|
#pragma region "Motor and sensor setup"
|
||
|
|
#define LED1 PC10
|
||
|
|
#define LED2 PC11
|
||
|
|
#define HARDWARE_SERIAL_RX_PIN PB7
|
||
|
|
#define HARDWARE_SERIAL_TX_PIN PB6
|
||
|
|
#define AS5040_CS PB15
|
||
|
|
#define AS5040_MISO PB14
|
||
|
|
#define AS5040_MOSI PC1
|
||
|
|
#define AS5040_SCLK PB10
|
||
|
|
|
||
|
|
#define EN_W_Pin LL_GPIO_PIN_6
|
||
|
|
#define EN_W_GPIO_Port GPIOC
|
||
|
|
#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
|
||
|
|
|
||
|
|
AS5040Sensor sensor(AS5040_CS, AS5040_MOSI, AS5040_MISO, AS5040_SCLK);
|
||
|
|
// Init iPower 6208
|
||
|
|
BLDCMotor motor = BLDCMotor(12);
|
||
|
|
BLDCDriver3PWM driver = BLDCDriver3PWM(PA10, PA11, PA12, PC6, PA11, PA12);
|
||
|
|
|
||
|
|
Commander command = Commander(Serial);
|
||
|
|
void onMotor(char *cmd) { command.motor(&motor, cmd); }
|
||
|
|
|
||
|
|
void setup() {
|
||
|
|
Serial.setRx(HARDWARE_SERIAL_RX_PIN);
|
||
|
|
Serial.setTx(HARDWARE_SERIAL_TX_PIN);
|
||
|
|
Serial.begin(115200);
|
||
|
|
|
||
|
|
pinMode(PC8, OUTPUT);
|
||
|
|
pinMode(PC9, OUTPUT);
|
||
|
|
pinMode(PA11, OUTPUT);
|
||
|
|
pinMode(PA12, OUTPUT);
|
||
|
|
pinMode(PC6, OUTPUT);
|
||
|
|
pinMode(PA8, OUTPUT);
|
||
|
|
pinMode(PA9, OUTPUT);
|
||
|
|
pinMode(PA10, OUTPUT);
|
||
|
|
digitalWrite(PC8, HIGH);
|
||
|
|
digitalWrite(PC9, HIGH);
|
||
|
|
delay(1);
|
||
|
|
digitalWrite(PA11, HIGH);
|
||
|
|
digitalWrite(PA12, HIGH);
|
||
|
|
digitalWrite(PC6, HIGH);
|
||
|
|
Serial.begin(115200);
|
||
|
|
sensor.init();
|
||
|
|
motor.linkSensor(&sensor);
|
||
|
|
driver.voltage_power_supply = 12;
|
||
|
|
driver.init();
|
||
|
|
motor.linkDriver(&driver);
|
||
|
|
motor.controller = MotionControlType::torque;
|
||
|
|
motor.PID_velocity.P = 0.2;
|
||
|
|
motor.PID_velocity.I = 20;
|
||
|
|
motor.PID_velocity.D = 0;
|
||
|
|
motor.voltage_limit = 12;
|
||
|
|
motor.LPF_velocity.Tf = 0.01;
|
||
|
|
motor.P_angle.P = 20;
|
||
|
|
motor.velocity_limit = 50;
|
||
|
|
Serial.begin(115200);
|
||
|
|
motor.useMonitoring(Serial);
|
||
|
|
motor.init();
|
||
|
|
motor.initFOC();
|
||
|
|
motor.target = 2;
|
||
|
|
command.add('A', onMotor, "motor");
|
||
|
|
|
||
|
|
_delay(1000);
|
||
|
|
}
|
||
|
|
|
||
|
|
void loop() {
|
||
|
|
motor.loopFOC();
|
||
|
|
motor.move();
|
||
|
|
command.run();
|
||
|
|
}
|