/
main.cpp
65 lines (48 loc) · 1.74 KB
/
main.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
#include <SimpleFOC.h>
BLDCMotor motor = BLDCMotor(7);
BLDCDriver6PWM driver = BLDCDriver6PWM(A_PHASE_UH, A_PHASE_UL, A_PHASE_VH, A_PHASE_VL, A_PHASE_WH, A_PHASE_WL);
LowsideCurrentSense currentSense = LowsideCurrentSense(0.003f, -64.0f / 7.0f, A_OP1_OUT, A_OP2_OUT, A_OP3_OUT);
Encoder encoder = Encoder(A_HALL1, A_HALL2, 2048, A_HALL3);
void doA() { encoder.handleA(); }
void doB() { encoder.handleB(); }
void doIndex() { encoder.handleIndex(); }
Commander command = Commander(Serial);
void doTarget(char* cmd) { command.motion(&motor, cmd); }
void setup() {
encoder.init();
encoder.enableInterrupts(doA, doB, doIndex);
motor.linkSensor(&encoder);
driver.voltage_power_supply = 12;
driver.init();
motor.linkDriver(&driver);
currentSense.linkDriver(&driver);
currentSense.init();
currentSense.skip_align = true;
motor.linkCurrentSense(¤tSense);
motor.voltage_sensor_align = 1;
motor.velocity_index_search = 3;
motor.voltage_limit = 6;
motor.velocity_limit = 1000;
motor.controller = MotionControlType::velocity;
motor.torque_controller = TorqueControlType::foc_current;
motor.PID_current_q.P = motor.PID_current_d.P = 0.1;
motor.PID_current_q.I = motor.PID_current_d.I = 10;
motor.PID_velocity.P = 0.5;
motor.PID_velocity.I = 1;
motor.PID_velocity.output_ramp = 1000;
motor.LPF_velocity.Tf = 0.01;
motor.P_angle.P = 20;
Serial.begin(115200);
motor.useMonitoring(Serial);
motor.init();
motor.initFOC();
command.add('T', doTarget, "target angle");
Serial.println(F("Motor ready."));
Serial.println(F("Set the target angle using serial terminal:"));
_delay(1000);
}
void loop() {
motor.move();
motor.loopFOC();
command.run();
}