New PID coefficients; remove legacy
This commit is contained in:
sosiskovich
parent
8525854579
commit
631cadca26
2 changed files with 81 additions and 47 deletions
|
|
@ -48,40 +48,37 @@ void setup_foc(MagneticSensorAS5045 *encoder, BLDCMotor *motor,
|
|||
DRV8313Driver *driver, LowsideCurrentSense *current_sense,
|
||||
Commander *commander, CommandCallback callback) {
|
||||
encoder->init(&spi);
|
||||
motor->linkSensor(encoder);
|
||||
|
||||
driver->pwm_frequency = 20000;
|
||||
driver->voltage_power_supply = 20;
|
||||
driver->voltage_limit = 40;
|
||||
driver->init();
|
||||
motor->linkDriver(driver);
|
||||
driver->voltage_power_supply = 24;
|
||||
driver->voltage_limit = 24;
|
||||
driver->init();
|
||||
|
||||
current_sense->linkDriver(driver);
|
||||
current_sense->init();
|
||||
|
||||
motor->linkSensor(encoder);
|
||||
motor->linkDriver(driver);
|
||||
motor->linkCurrentSense(current_sense);
|
||||
// SimpleFOCDebug::enable(nullptr);
|
||||
commander->add('M', callback, "motor");
|
||||
motor->useMonitoring(Serial);
|
||||
// motor->monitor_start_char = 'M';
|
||||
// motor->monitor_end_char = 'M';
|
||||
// motor->monitor_variables = _MON_TARGET | _MON_VEL | _MON_ANGLE;
|
||||
motor->monitor_downsample = 10; // default 10
|
||||
// commander->verbose = VerboseMode::user_friendly;
|
||||
motor->monitor_downsample = 5000; // default 0
|
||||
motor->controller = MotionControlType::angle;
|
||||
motor->torque_controller = TorqueControlType::voltage;
|
||||
motor->foc_modulation = FOCModulationType::SinePWM;
|
||||
motor->PID_velocity.P = 0.3;
|
||||
motor->PID_velocity.I = 15;
|
||||
motor->PID_velocity.D = 0.001;
|
||||
motor->PID_velocity.output_ramp = 1000;
|
||||
motor->LPF_velocity.Tf = 0.005;
|
||||
motor->LPF_angle.Tf = 0.005;
|
||||
motor->P_angle.P = 15;
|
||||
// motor->P_angle.I = 15;
|
||||
// motor->P_angle.3 = 15;
|
||||
motor->velocity_limit = 40;
|
||||
motor->voltage_limit = 40;
|
||||
motor->foc_modulation = FOCModulationType::SpaceVectorPWM;
|
||||
|
||||
//PID start
|
||||
motor->PID_velocity.P = 0.75;
|
||||
motor->PID_velocity.I = 20;
|
||||
motor->LPF_velocity.Tf = 0.005;
|
||||
motor->P_angle.P = 0.5 ;
|
||||
motor->LPF_angle.Tf = 0.001;
|
||||
//PID end
|
||||
|
||||
motor->velocity_limit = 40; // Ограничение по скорости вращения rad/s (382 rpm)
|
||||
motor->voltage_limit = 24;
|
||||
motor->current_limit = 0.5;
|
||||
|
||||
motor->sensor_direction = Direction::CCW;
|
||||
current_sense->skip_align = false;
|
||||
motor->init();
|
||||
motor->initFOC();
|
||||
}
|
||||
|
|
@ -113,7 +110,6 @@ void send_motor_enabled() {
|
|||
}
|
||||
|
||||
void send_data() {
|
||||
// TODO(vanyabeat) add can functionability for ROS packets
|
||||
send_velocity();
|
||||
send_angle();
|
||||
send_motor_enabled();
|
||||
|
|
@ -124,9 +120,11 @@ void send_data() {
|
|||
void read_can_step() {
|
||||
char flag = CAN_inMsg.buf[0];
|
||||
if (flag == 'V') {
|
||||
motor.enable();
|
||||
memcpy(&motor_control_inputs.target_velocity, &CAN_inMsg.buf[1],
|
||||
sizeof(motor_control_inputs.target_velocity));
|
||||
} else if (flag == 'A') {
|
||||
motor.enable();
|
||||
memcpy(&motor_control_inputs.target_angle, &CAN_inMsg.buf[1],
|
||||
sizeof(motor_control_inputs.target_angle));
|
||||
} else if (flag == 'E') {
|
||||
|
|
@ -141,7 +139,6 @@ void read_can_step() {
|
|||
motor.disable();
|
||||
}
|
||||
}
|
||||
// motor.target = angle;
|
||||
digitalWrite(PC10, !digitalRead(PC10));
|
||||
}
|
||||
|
||||
|
|
@ -152,6 +149,7 @@ void foc_step(BLDCMotor *motor, Commander *commander) {
|
|||
motor->controller = MotionControlType::velocity;
|
||||
}
|
||||
motor->target = motor_control_inputs.target_velocity;
|
||||
|
||||
} else {
|
||||
if (motor->controller != MotionControlType::angle) {
|
||||
motor->controller = MotionControlType::angle;
|
||||
|
|
@ -168,7 +166,7 @@ void foc_step(BLDCMotor *motor, Commander *commander) {
|
|||
void setup() {
|
||||
Serial.setRx(HARDWARE_SERIAL_RX_PIN);
|
||||
Serial.setTx(HARDWARE_SERIAL_TX_PIN);
|
||||
Serial.begin(19200);
|
||||
Serial.begin(115200);
|
||||
// setup led pin
|
||||
pinMode(PC11, OUTPUT);
|
||||
pinMode(PC10, OUTPUT);
|
||||
|
|
@ -182,14 +180,10 @@ void setup() {
|
|||
SendTimer->attachInterrupt(send_data);
|
||||
SendTimer->resume();
|
||||
setup_foc(&encoder, &motor, &driver, ¤t_sense, &command, doMotor);
|
||||
// _delay(1000);
|
||||
}
|
||||
|
||||
// float current_angle = 0.0;
|
||||
void loop() {
|
||||
foc_step(&motor, &command);
|
||||
// current_angle = encoder.getAngle();
|
||||
// memcpy(&CAN_TX_msg.buf[1], ¤t_angle, sizeof(current_angle));
|
||||
while (Can.read(CAN_inMsg)) {
|
||||
read_can_step();
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,6 +1,7 @@
|
|||
import can
|
||||
import struct
|
||||
import argparse
|
||||
import time
|
||||
import sys
|
||||
|
||||
# Function to send the target speed
|
||||
def send_target_speed(bus, target_speed):
|
||||
|
|
@ -8,29 +9,68 @@ def send_target_speed(bus, target_speed):
|
|||
msg.arbitration_id = 1 # Message ID
|
||||
msg.is_extended_id = False
|
||||
msg.dlc = 5 # Message length
|
||||
msg.data = [ord('V')] + list(struct.pack('<f', target_speed)) # 'V' for the command identifier, followed by the speed in float format
|
||||
msg.data = bytearray([ord('V')] + list(struct.pack('<f', target_speed))) # 'V' for the command identifier, followed by the speed in float format
|
||||
|
||||
try:
|
||||
bus.send(msg)
|
||||
print(f"Sent message with target speed: {target_speed} m/s")
|
||||
print(f"Message data: {msg.data}")
|
||||
print(f"Sent message with target speed: {target_speed} rad/s")
|
||||
except can.CanError:
|
||||
print("Message failed to send")
|
||||
|
||||
# Main function
|
||||
# Function to send the motor enable/disable command
|
||||
def send_motor_enable(bus, enable):
|
||||
"""
|
||||
Sends a command to enable or disable the motor.
|
||||
|
||||
:param bus: The CAN bus
|
||||
:param enable: 1 to enable the motor, 0 to disable it
|
||||
"""
|
||||
msg = can.Message()
|
||||
msg.arbitration_id = 1 # Message ID
|
||||
msg.is_extended_id = False
|
||||
msg.dlc = 2 # Message length (flag + 1 byte of data)
|
||||
msg.data = bytearray([ord('E'), enable]) # 'E' for the command, followed by 0 or 1
|
||||
|
||||
try:
|
||||
bus.send(msg)
|
||||
state = "enabled" if enable else "disabled"
|
||||
print(f"Sent message to {state} motor")
|
||||
except can.CanError as e:
|
||||
print(f"Message failed to send: {e}")
|
||||
sys.exit(1) # Exit the program on failure
|
||||
|
||||
send_target_speed(bus,0.0)
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser(description="Send target speed over CAN bus.")
|
||||
parser.add_argument("--speed", type=float, required=True, help="Target speed to send over the CAN bus (in m/s)")
|
||||
args = parser.parse_args()
|
||||
|
||||
target_speed = args.speed
|
||||
|
||||
# CAN interface setup
|
||||
bus = can.interface.Bus(channel='can0', bustype='socketcan', bitrate=1000000) # Ensure the bitrate matches the microcontroller settings
|
||||
print("CAN bus initialized, sending target speed...")
|
||||
bus = None # Define outside the try block for proper shutdown
|
||||
try:
|
||||
bus = can.interface.Bus(channel='COM4', bustype='slcan', bitrate=1000000) # Ensure the bitrate matches the microcontroller settings
|
||||
print("CAN bus initialized.")
|
||||
|
||||
# Send the message once
|
||||
send_target_speed(bus, target_speed)
|
||||
while True:
|
||||
user_input = input("Enter target speed: ")
|
||||
if user_input.lower() == 'exit':
|
||||
print("Exiting...")
|
||||
break
|
||||
try:
|
||||
target_speed = float(user_input)
|
||||
send_target_speed(bus, target_speed)
|
||||
except ValueError:
|
||||
print("Invalid input. Please enter a valid number.")
|
||||
|
||||
# Disable motor before exiting
|
||||
send_motor_enable(bus, 0)
|
||||
print("Motor disabled.")
|
||||
|
||||
except Exception as e:
|
||||
print(f"Error initializing1 CAN bus: {e}")
|
||||
sys.exit(1)
|
||||
|
||||
finally:
|
||||
if bus is not None:
|
||||
bus.shutdown()
|
||||
print("CAN bus shut down.")
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue