diff --git a/docs/src/examples/robot.md b/docs/src/examples/robot.md index 81a758be..1ea716d6 100644 --- a/docs/src/examples/robot.md +++ b/docs/src/examples/robot.md @@ -68,7 +68,19 @@ plot!(sol, idxs = [ ], sp=7:12, lab="Position error", link=:x) plot!(xlabel=[fill("", 1, 9) fill("Time [s]", 1, 3)]) ``` -We see that after an initial transient, the robot controller converges to tracking the reference trajectory well. +We see that after an initial transient, the robot controller converges to tracking the reference trajectory well. However, since the first three axes of the robot are modeled as slightly flexible, and we are ultimately interested in the tracking performance _after_ the gear box, we plot also this tracking error +```@example robot +plot(sol, idxs = [ + robot.axis1.controller.feedback1.output.u + robot.axis2.controller.feedback1.output.u + robot.axis3.controller.feedback1.output.u +], layout=(1,3), lab="Position error, motor side", link=:x) +plot!(sol, idxs = [ + robot.pathPlanning.controlBus.axisControlBus1.angle_ref - robot.axis1.motor.Jmotor.phi / ( -105) + robot.pathPlanning.controlBus.axisControlBus2.angle_ref - robot.axis2.motor.Jmotor.phi / (210) + robot.pathPlanning.controlBus.axisControlBus3.angle_ref - robot.axis3.motor.Jmotor.phi / (60) +], lab="Position error, arm side") +``` diff --git a/src/robot/robot_components.jl b/src/robot/robot_components.jl index 8b2a8a17..13917548 100644 --- a/src/robot/robot_components.jl +++ b/src/robot/robot_components.jl @@ -125,18 +125,14 @@ function AxisType2(; name, kp = 10, ks = 1, Ts = 0.01, k = 1.1616, w = 4590, D = end eqs = [ - connect(flange, gear.flange_b, angleSensor.flange, speedSensor.flange, accSensor.flange) - connect(motor.flange_motor, gear.flange_a) + connect(flange, gear.flange_b) + connect(motor.flange_motor, gear.flange_a, angleSensor.flange, speedSensor.flange, accSensor.flange) connect(motor.axisControlBus, axisControlBus) - (angleSensor.phi.u ~ axisControlBus.angle) - (speedSensor.w.u ~ axisControlBus.speed) - (accSensor.a.u ~ axisControlBus.acceleration) - # connect(axisControlBus.angle_ref, initializeFlange.phi_start) - # connect(axisControlBus.speed_ref, initializeFlange.w_start) - # connect(initializeFlange.flange, flange) - # connect(Const.y, initializeFlange.a_start) + (angleSensor.phi.u*ratio ~ axisControlBus.angle) + (speedSensor.w.u*ratio ~ axisControlBus.speed) + (accSensor.a.u*ratio ~ axisControlBus.acceleration) connect(controller.axisControlBus, axisControlBus)] ODESystem(eqs, t; name, systems) @@ -163,13 +159,13 @@ function AxisType1(; name, c = 43, cd = 0.005, kp = 10, ks = 1, Ts = 0.01, k = 1 end eqs = [ - connect(flange, gear.flange_b, angleSensor.flange, speedSensor.flange, accSensor.flange) + connect(flange, gear.flange_b) connect(gear.flange_a, spring.flange_b) - connect(motor.flange_motor, spring.flange_a) + connect(motor.flange_motor, spring.flange_a, angleSensor.flange, speedSensor.flange, accSensor.flange) connect(motor.axisControlBus, axisControlBus) - (angleSensor.phi.u ~ axisControlBus.angle) - (speedSensor.w.u ~ axisControlBus.speed) - (accSensor.a.u ~ axisControlBus.acceleration) + (angleSensor.phi.u*ratio ~ axisControlBus.angle) + (speedSensor.w.u*ratio ~ axisControlBus.speed) + (accSensor.a.u*ratio ~ axisControlBus.acceleration) connect(controller.axisControlBus, axisControlBus) ]