13753907916

Committed 10 Mar 2025 12:08AM UTC coverage: 90.771% (+0.2%) from 90.599%

Build # 13753907916

Build Type

Pull #35

github

Committed by

web-flow

Commit Message

Merge 76633dfa7 into cc61929b1

Pull Request Pull Request #35: Features/s_plot recovery

Run Details

442 of 472 new or added lines in 7 files covered. (93.64%)

4 existing lines in 3 files now uncovered.

14832 of 16340 relevant lines covered (90.77%)

734462.57 hits per line

Source File
Press 'n' to go to next uncovered line, 'b' for previous

93.13

/printhor/src/bin/s_plot.rs

//! Auxiliary commandline program to investigate, evaluate and verify and visualize motion planing
extern crate alloc;
extern crate core;

pub mod control;
pub mod helpers;
pub mod hwa;

mod instrumentation;

use hwa::HwiContract;

#[allow(unused)]
use hwa::RawHwiResource;

use control::motion;
use control::task_stepper::{
    STEPPER_PLANNER_CLOCK_PERIOD_US, STEPPER_PLANNER_MICROSEGMENT_PERIOD_US,
};
use controllers::{LinearMicrosegmentStepInterpolator, SegmentIterator};
use hwa::controllers;
use motion::SCurveMotionProfile;
use printhor_hwa_common::CommChannel;

#[embassy_executor::main]
async fn main(spawner: embassy_executor::Spawner) {
    printhor_main(spawner, false).await;
}

async fn printhor_main(spawner: embassy_executor::Spawner, _keep_feeding: bool) {
    hwa::Contract::init_logger();
    hwa::Contract::init_heap();

    let c = instrumentation::machinery::init_splot(spawner).await;

    let event_bus = c.event_bus;
    let motion_planner = c.motion_planner;

    motion_planner.start(&event_bus).await;

    cfg_if::cfg_if! {
        if #[cfg(feature = "with-motion")] {
            // Max speed
            motion_planner.motion_config().set_max_speed(hwa::Contract::DEFAULT_MAX_SPEED_PS);

            motion_planner.motion_config().set_max_accel(hwa::Contract::DEFAULT_MAX_ACCEL_PS);

            motion_planner.motion_config().set_max_jerk(hwa::Contract::DEFAULT_MAX_JERK_PS);

            motion_planner.motion_config().set_default_travel_speed(hwa::Contract::DEFAULT_TRAVEL_SPEED_PS);

            motion_planner.motion_config().set_space_units_per_world_unit(hwa::Contract::DEFAULT_UNITS_PER_WU);

            motion_planner.motion_config().set_micro_steps_per_axis(
                hwa::make_vector!(x=16, y=16, z=16)
                //hwa::Contract::DEFAULT_MICRO_STEPS_PER_AXIS
            );

            motion_planner.motion_config().set_world_center(
                hwa::Contract::DEFAULT_WORLD_CENTER_WU
            );

            motion_planner.motion_config().set_world_size(
                hwa::Contract::DEFAULT_WORLD_SIZE_WU
            );

            motion_planner.motion_config().set_nozzle_offset(
                hwa::make_vector_real!(x=0.0, y=0.0,z=0.0)
            );

            motion_planner.motion_config().set_probe_offset(
                hwa::make_vector_real!(x=0.0, y=0.0,z=0.0)
            );

            motion_planner.motion_config().set_flow_rate(100);
            motion_planner.motion_config().set_speed_rate(100);

            // Compute min speed. Really useful because of discretion effects
            motion_planner.motion_config().compute_min_speed();

            // Make homing unneeded
            hwa::info!("Virtually homing");
            {
                let position = hwa::controllers::Position::new_with_world_projection(
                    &hwa::Contract::DEFAULT_WORLD_HOMING_POINT_WU,
                );
                motion_planner
                    .motion_status()
                    .update_last_planned_position(0, &position);
                motion_planner
                    .motion_status()
                    .update_current_position(0, &position);
            }
            #[cfg(all(feature = "native", feature = "with-ps-on"))]
            {
                hwa::info!("Virtually powering on");
                event_bus.publish_event(hwa::EventStatus::containing(hwa::EventFlags::ATX_ON)).await;
            }
        }
    }

    // Starting motion logic

    {
        let mut gcode_buff = instrumentation::gcode::GCodeBuffer::new();
        //gcode_buff.append("G0 Z2.2000 F3000; pen up\n");
        //gcode_buff.append("G0 X100.18164 Y105\n");
        //gcode_buff.append("G0 Z1.2000 F3000; pen down\n");
        gcode_buff.append("G92 X100.18164 Y105\n");
        gcode_buff.append("G1 X99.63526 Y104.99686 F7000 S0\n");
        gcode_buff.append("G1 X99.09117 Y104.98748 F7000 S0\n");
        gcode_buff.append("G1 X98.54937 Y104.97192 F7000 S0\n");
        gcode_buff.append("G1 X98.00986 Y104.95024 F7000 S0\n");
        gcode_buff.append("G1 X97.47261 Y104.92249 F7000 S0\n");
        gcode_buff.append("G1 X96.93764 Y104.88873 F7000 S0\n");
        gcode_buff.append("G1 X96.40494 Y104.84902 F7000 S0\n");
        gcode_buff.append("G1 X95.87449 Y104.80339 F7000 S0\n");
        gcode_buff.append("G1 X95.34630 Y104.75190 F7000 S0\n");
        gcode_buff.append("G1 X94.82036 Y104.69461 F7000 S0\n");
        gcode_buff.append("G1 X94.29131 Y104.63086 F7000 S0\n");
        gcode_buff.append("G1 X93.76473 Y104.56131 F7000 S0\n");
        gcode_buff.append("G1 X93.24062 Y104.48598 F7000 S0\n");
        gcode_buff.append("G1 X92.71898 Y104.40494 F7000 S0\n");
        gcode_buff.append("G1 X92.19980 Y104.31823 F7000 S0\n");
        gcode_buff.append("G1 X91.68308 Y104.22590 F7000 S0\n");
        gcode_buff.append("G1 X91.16881 Y104.12798 F7000 S0\n");
        gcode_buff.append("G1 X90.65699 Y104.02452 F7000 S0\n");
        gcode_buff.append("G1 X90.14763 Y103.91555 F7000 S0\n");
        gcode_buff.append("G1 X89.64071 Y103.80113 F7000 S0\n");
        gcode_buff.append("G1 X89.13232 Y103.68032 F7000 S0\n");
        gcode_buff.append("G1 X88.62661 Y103.55409 F7000 S0\n");
        gcode_buff.append("G1 X88.12357 Y103.42248 F7000 S0\n");
        gcode_buff.append("G1 X87.62321 Y103.28552 F7000 S0\n");

        let mut parser =
            control::GCodeLineParser::new(instrumentation::gcode::BufferStream::new(gcode_buff));

        loop {
            match parser.next_gcode(CommChannel::Internal).await {
                Ok(gcode) => {
                    if motion_planner
                        .plan(CommChannel::Internal, &gcode, false, &event_bus)
                        .await
                        .is_err()
                    {
                        break;
                    }
                }
                Err(_error) => {
                    break;
                }
            }
        }
    }

    let micro_segment_period_secs: hwa::math::Real =
        hwa::math::Real::from_lit(STEPPER_PLANNER_MICROSEGMENT_PERIOD_US as i64, 6).rdp(6);

    let _sampling_time: hwa::math::Real =
        hwa::math::Real::from_lit(STEPPER_PLANNER_CLOCK_PERIOD_US as i64, 6).rdp(6);

    let mut data_points = instrumentation::datapoints::DataPoints::new();
    loop {
        match motion_planner.next_plan(&event_bus).await {
            controllers::motion::ExecPlan::Segment(mut segment, _channel, _order_num) => {
                let current_real_pos = motion_planner.motion_status().get_current_position();
                let position_offset = segment.src_pos - current_real_pos.space_pos;
                hwa::info!(
                    "[task_stepper] order_num:{:?} Correcting offset [{:?}] {}",
                    _order_num,
                    position_offset,
                    hwa::Contract::SPACE_UNIT_MAGNITUDE,
                );
                segment.fix_deviation(
                    &position_offset,
                    motion_planner.motion_config().get_flow_rate_as_real(),
                );
                match SCurveMotionProfile::compute(
                    segment.displacement_su,
                    segment.speed_enter_su_s,
                    segment.speed_exit_su_s,
                    &segment.constraints,
                    false,
                ) {
                    Ok(trajectory) => {
                        // The relevant coords (those that will move)
                        let mut relevant_coords = hwa::math::CoordSel::empty();
                        // The relevant coords that will move forward
                        let mut relevant_coords_dir_fwd = hwa::math::CoordSel::empty();
                        segment.unit_vector_dir.foreach_values(|coord, val| {
                            relevant_coords.set(coord, !val.is_negligible());
                            relevant_coords_dir_fwd.set(coord, val.is_defined_positive());
                        });
                        hwa::debug!("Relevant coords: [{:?}]", relevant_coords);
                        hwa::debug!("Relevant coords_dir_fwd: [{:?}]", relevant_coords_dir_fwd);

                        let steps_per_su = motion_planner
                            .motion_config()
                            .get_units_per_space_magnitude()
                            * motion_planner.motion_config().get_micro_steps_as_vector();

                        let mut segment_iterator =
                            SegmentIterator::new(&trajectory, micro_segment_period_secs);

                        let mut micro_segment_interpolator =
                            LinearMicrosegmentStepInterpolator::new(
                                segment
                                    .unit_vector_dir
                                    .with_coord(relevant_coords.complement(), None)
                                    .abs(),
                                segment.displacement_su,
                                steps_per_su.with_coord(relevant_coords.complement(), None),
                            );
                        hwa::info!(
                            "[task_stepper] order_num:{:?} Trajectory interpolation START",
                            _order_num
                        );

                        // Micro-segments interpolation along segment
                        data_points.segment_starts(&trajectory);
                        loop {
                            // Micro-segment start

                            if let Some(estimated_position) = segment_iterator.next() {
                                data_points.interpolation_tick(&segment_iterator);

                                let w = (segment_iterator.dt() * hwa::math::ONE_MILLION).round();
                                if w.is_negligible() {
                                    hwa::info!("giving up for any reason");
                                    break;
                                }
                                let _has_more =
                                    micro_segment_interpolator.advance_to(estimated_position, w);

                                hwa::debug!(
                                    "[task_stepper] segment:{:?}|{:?} Trajectory micro-segment advanced: {:?} [{:?}] {} [{:?}] steps",
                                    data_points.current_segment_id(),
                                    data_points.current_micro_segment_id(),
                                    segment_iterator.ds(),
                                    micro_segment_interpolator
                                        .advanced_units()
                                        .map_nan_coords(relevant_coords, &hwa::math::ZERO),
                                    hwa::Contract::SPACE_UNIT_MAGNITUDE,
                                    micro_segment_interpolator
                                        .advanced_steps()
                                        .map_nan_coords(relevant_coords, &0),
                                );

                                #[cfg(feature = "verbose-timings")]
                                let t1 = embassy_time::Instant::now();

                                let _ = micro_segment_interpolator.state().clone();
                                let _ = relevant_coords;
                                data_points.micro_segment_ends();
                                if !_has_more {
                                    break;
                                }
                            } else {
                                // No advance
                                break;
                            }
                        }
                        data_points.segment_ends();
                        ////
                        //// TRAJECTORY INTERPOLATION END
                        ////
                        #[cfg(feature = "debug-motion")]
                        hwa::debug!(
                            "[task_stepper] order_num:{:?} Trajectory interpolation END",
                            _order_num
                        );

                        let _adv_steps = micro_segment_interpolator.advanced_steps();
                        let adv_pos = micro_segment_interpolator.advanced_units();
                        hwa::info!(
                            "[task_stepper] order_num:{:?} Trajectory advanced. vector displacement space: {:?} [{:#?}] {}, vlim: {:?} {}/s",
                            _order_num,
                            segment_iterator.current_position(),
                            adv_pos,
                            hwa::Contract::SPACE_UNIT_MAGNITUDE,
                            trajectory.v_lim,
                            hwa::Contract::SPACE_UNIT_MAGNITUDE
                        );

                        hwa::info!(
                            "[task_stepper] order_num:{:?} Trajectory advanced. vector displacement space: [{:#?}] steps",
                            _order_num,
                            _adv_steps.excluding_negligible()
                        );

                        let adv_delta = segment.unit_vector_dir.sign() * adv_pos;
                        let next_real_pos = (current_real_pos.space_pos
                            + adv_delta.map_nan(&hwa::math::ZERO))
                        .rdp(6);

                        let mut pos = current_real_pos.clone();
                        pos.update_from_space_coordinates(&next_real_pos);
                        motion_planner
                            .motion_status()
                            .update_current_position(_order_num, &pos);
                    }
                    _ => {
                        hwa::error!("Unable to compute motion plan. Discarding...");
                    }
                }
            }
            controllers::motion::ExecPlan::SetPosition(position, _channel, _order_num) => {
                motion_planner
                    .motion_status()
                    .update_current_position(_order_num, &position);
            }
            _ => {}
        }
        motion_planner.consume_current_plan(&event_bus).await;
        if motion_planner.num_queued().await == 0 {
            break;
        }
    }

    {
        #[allow(unused)]
        use gnuplot::{
            AutoOption, MultiplotFillDirection::Downwards, MultiplotFillOrder::RowsFirst, Tick,
        };
        use gnuplot::{AxesCommon, Figure};
        #[allow(unused)]
        use gnuplot::{DashType, PlotOption};

        let mut fg = Figure::new();

        cfg_if::cfg_if! {
            if #[cfg(feature="float-point-f64-impl")] {
                const MATH_PRECISION: &str = "[float point 64bits]";
            }
            else if #[cfg(feature="fixed-point-128-impl")] {
                const MATH_PRECISION: &str = "[fixed point 128bits]";
            }
            else {
                const MATH_PRECISION: &str = "[float point 32bits]";
            }
        }

        fg.set_multiplot_layout(2, 1)
            .set_title(
                format!(
                    "{} Double S-Curve velocity profile\n[{:?} segments, {:?} {} displacement, {:?} hz interp, {:?} hz sampling]",
                    MATH_PRECISION,
                    data_points.num_segments(),
                    data_points.total_displacement(),
                    hwa::Contract::SPACE_UNIT_MAGNITUDE,
                    hwa::Contract::MOTION_PLANNER_MICRO_SEGMENT_FREQUENCY,
                    hwa::Contract::STEP_PLANNER_CLOCK_FREQUENCY,
                ).as_str()
            )
            .set_scale(1.0, 1.0)
            .set_offset(0.0, 0.0)
            .set_multiplot_fill_order(RowsFirst, Downwards);

        fg.axes2d()
            .set_y_label(format!("Position ({})", hwa::Contract::SPACE_UNIT_MAGNITUDE).as_str(), &[])
            .points(data_points.segment_position_marks.times, data_points.segment_position_marks.points, &[PlotOption::Color("black")])
            .lines(data_points.interpolated_positions.times, data_points.interpolated_positions.points, &[PlotOption::Color("blue")])
            //.lines(data_points.time_discrete, data_points.pos_discrete, &[PlotOption::Color("gray")])
        ;
        fg.axes2d()
            .set_y_label(format!("Velocity ({}/s)", hwa::Contract::SPACE_UNIT_MAGNITUDE).as_str(), &[])
            .points(data_points.segment_velocity_marks.times, data_points.segment_velocity_marks.points, &[PlotOption::Color("black")])
            .lines(data_points.interpolated_velocities.times, data_points.interpolated_velocities.points, &[PlotOption::Color("red")])
            //.lines(data_points.time, data_points.spd, &[PlotOption::Color("green")])
            //.lines(data_points.time_discrete_deriv, data_points.spd_discrete, &[PlotOption::Color("gray")])
        ;
        #[cfg(not(test))]
        fg.show_and_keep_running().unwrap();
        _ = fg.save_to_pdf("plot.pdf", 10.0f32, 10.0f32);
    }

    #[cfg(not(test))]
    std::process::exit(0);
}

//#region "Machinery initialization"

pub fn initialization_error() {
    let msg = "Unable to start because SYS_ALARM raised at startup. Giving up...";
    hwa::error!("{}", msg);
    panic!("{}", msg);
}

//#endregion

//#region "Tests"

#[cfg(feature = "s-plot-bin")]
#[cfg(test)]
mod test {
    use super::*;
    use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex;
    use printhor_hwa_common::PersistentState;
    use std::marker::PhantomData;
    use std::sync::{Condvar, Mutex};

    struct Signaler {
        mutex: Mutex<bool>,
        condvar: Condvar,
    }

    impl Signaler {
        fn new() -> Self {
            Self {
                mutex: Mutex::new(false),
                condvar: Condvar::new(),
            }
        }

        fn wait(&self) {
            let mut signaled = self.mutex.lock().unwrap();
            while !*signaled {
                signaled = self.condvar.wait(signaled).unwrap();
            }
            *signaled = false;
        }
    }

    pub static SPLOT_STATE: PersistentState<CriticalSectionRawMutex, bool> = PersistentState::new();

    pub struct MockedExecutor {
        inner: embassy_executor::raw::Executor,
        not_send: PhantomData<*mut ()>,
        signaler: &'static Signaler,
    }

    impl MockedExecutor {
        /// Create a new Executor.
        pub fn new() -> Self {
            let signaler = Box::leak(Box::new(Signaler::new()));
            Self {
                inner: embassy_executor::raw::Executor::new(signaler as *mut Signaler as *mut ()),
                not_send: PhantomData,
                signaler,
            }
        }

        pub fn run(&'static mut self, init: impl FnOnce(embassy_executor::Spawner)) {
            init(self.inner.spawner());

            loop {
                unsafe { self.inner.poll() };
                if SPLOT_STATE.signaled() {
                    break;
                }
                self.signaler.wait()
            }
        }
    }

    /// The integration test entry-point
    #[embassy_executor::task(pool_size = 1)]
    async fn mocked_splot_main(spawner: embassy_executor::Spawner) {
        printhor_main(spawner, false).await;
        SPLOT_STATE.signal(true);
    }

    #[test]
    fn splot_test() {
        let executor = hwa::make_static_ref!("Executor", MockedExecutor, MockedExecutor::new());
        // 2. Spawn the [mocked_main] task
        executor.run(|spawner| {
            let _tk = spawner.must_spawn(mocked_splot_main(spawner));
        });
    }
}

//#endregion

1	//! Auxiliary commandline program to investigate, evaluate and verify and visualize motion planing
2	extern crate alloc;
3	extern crate core;
4
5	pub mod control;
6	pub mod helpers;
7	pub mod hwa;
8
9	mod instrumentation;
10
11	use hwa::HwiContract;
12
13	#[allow(unused)]
14	use hwa::RawHwiResource;
15
16	use control::motion;
17	use control::task_stepper::{
18	STEPPER_PLANNER_CLOCK_PERIOD_US, STEPPER_PLANNER_MICROSEGMENT_PERIOD_US,
19	};
20	use controllers::{LinearMicrosegmentStepInterpolator, SegmentIterator};
21	use hwa::controllers;
22	use motion::SCurveMotionProfile;
23	use printhor_hwa_common::CommChannel;
24
UNCOV 25	#[embassy_executor::main]	×
26	async fn main(spawner: embassy_executor::Spawner) {
27	printhor_main(spawner, false).await;
28	}
29
30	async fn printhor_main(spawner: embassy_executor::Spawner, _keep_feeding: bool) {	1✔
31	hwa::Contract::init_logger();	1✔
32	hwa::Contract::init_heap();	1✔
33
34	let c = instrumentation::machinery::init_splot(spawner).await;	1✔
35
36	let event_bus = c.event_bus;	1✔
37	let motion_planner = c.motion_planner;	1✔
38		1✔
39	motion_planner.start(&event_bus).await;	1✔
40
41	cfg_if::cfg_if! {
42	if #[cfg(feature = "with-motion")] {
43	// Max speed
44	motion_planner.motion_config().set_max_speed(hwa::Contract::DEFAULT_MAX_SPEED_PS);	1✔
45		1✔
46	motion_planner.motion_config().set_max_accel(hwa::Contract::DEFAULT_MAX_ACCEL_PS);	1✔
47		1✔
48	motion_planner.motion_config().set_max_jerk(hwa::Contract::DEFAULT_MAX_JERK_PS);	1✔
49		1✔
50	motion_planner.motion_config().set_default_travel_speed(hwa::Contract::DEFAULT_TRAVEL_SPEED_PS);	1✔
51		1✔
52	motion_planner.motion_config().set_space_units_per_world_unit(hwa::Contract::DEFAULT_UNITS_PER_WU);	1✔
53		1✔
54	motion_planner.motion_config().set_micro_steps_per_axis(	1✔
55	hwa::make_vector!(x=16, y=16, z=16)	1✔
56	//hwa::Contract::DEFAULT_MICRO_STEPS_PER_AXIS	1✔
57	);	1✔
58		1✔
59	motion_planner.motion_config().set_world_center(	1✔
60	hwa::Contract::DEFAULT_WORLD_CENTER_WU	1✔
61	);	1✔
62		1✔
63	motion_planner.motion_config().set_world_size(	1✔
64	hwa::Contract::DEFAULT_WORLD_SIZE_WU	1✔
65	);	1✔
66		1✔
67	motion_planner.motion_config().set_nozzle_offset(	1✔
68	hwa::make_vector_real!(x=0.0, y=0.0,z=0.0)	1✔
69	);	1✔
70		1✔
71	motion_planner.motion_config().set_probe_offset(	1✔
72	hwa::make_vector_real!(x=0.0, y=0.0,z=0.0)	1✔
73	);	1✔
74		1✔
75	motion_planner.motion_config().set_flow_rate(100);	1✔
76	motion_planner.motion_config().set_speed_rate(100);	1✔
77		1✔
78	// Compute min speed. Really useful because of discretion effects	1✔
79	motion_planner.motion_config().compute_min_speed();	1✔
80		1✔
81	// Make homing unneeded	1✔
82	hwa::info!("Virtually homing");	1✔
83	{	1✔
84	let position = hwa::controllers::Position::new_with_world_projection(	1✔
85	&hwa::Contract::DEFAULT_WORLD_HOMING_POINT_WU,	1✔
86	);	1✔
87	motion_planner	1✔
88	.motion_status()	1✔
89	.update_last_planned_position(0, &position);	1✔
90	motion_planner	1✔
91	.motion_status()	1✔
92	.update_current_position(0, &position);	1✔
93	}	1✔
94	#[cfg(all(feature = "native", feature = "with-ps-on"))]	1✔
95	{	1✔
96	hwa::info!("Virtually powering on");	1✔
97	event_bus.publish_event(hwa::EventStatus::containing(hwa::EventFlags::ATX_ON)).await;	1✔
98	}
99	}
100	}
101
102	// Starting motion logic
103
104	{
105	let mut gcode_buff = instrumentation::gcode::GCodeBuffer::new();	1✔
106	//gcode_buff.append("G0 Z2.2000 F3000; pen up\n");	1✔
107	//gcode_buff.append("G0 X100.18164 Y105\n");	1✔
108	//gcode_buff.append("G0 Z1.2000 F3000; pen down\n");	1✔
109	gcode_buff.append("G92 X100.18164 Y105\n");	1✔
110	gcode_buff.append("G1 X99.63526 Y104.99686 F7000 S0\n");	1✔
111	gcode_buff.append("G1 X99.09117 Y104.98748 F7000 S0\n");	1✔
112	gcode_buff.append("G1 X98.54937 Y104.97192 F7000 S0\n");	1✔
113	gcode_buff.append("G1 X98.00986 Y104.95024 F7000 S0\n");	1✔
114	gcode_buff.append("G1 X97.47261 Y104.92249 F7000 S0\n");	1✔
115	gcode_buff.append("G1 X96.93764 Y104.88873 F7000 S0\n");	1✔
116	gcode_buff.append("G1 X96.40494 Y104.84902 F7000 S0\n");	1✔
117	gcode_buff.append("G1 X95.87449 Y104.80339 F7000 S0\n");	1✔
118	gcode_buff.append("G1 X95.34630 Y104.75190 F7000 S0\n");	1✔
119	gcode_buff.append("G1 X94.82036 Y104.69461 F7000 S0\n");	1✔
120	gcode_buff.append("G1 X94.29131 Y104.63086 F7000 S0\n");	1✔
121	gcode_buff.append("G1 X93.76473 Y104.56131 F7000 S0\n");	1✔
122	gcode_buff.append("G1 X93.24062 Y104.48598 F7000 S0\n");	1✔
123	gcode_buff.append("G1 X92.71898 Y104.40494 F7000 S0\n");	1✔
124	gcode_buff.append("G1 X92.19980 Y104.31823 F7000 S0\n");	1✔
125	gcode_buff.append("G1 X91.68308 Y104.22590 F7000 S0\n");	1✔
126	gcode_buff.append("G1 X91.16881 Y104.12798 F7000 S0\n");	1✔
127	gcode_buff.append("G1 X90.65699 Y104.02452 F7000 S0\n");	1✔
128	gcode_buff.append("G1 X90.14763 Y103.91555 F7000 S0\n");	1✔
129	gcode_buff.append("G1 X89.64071 Y103.80113 F7000 S0\n");	1✔
130	gcode_buff.append("G1 X89.13232 Y103.68032 F7000 S0\n");	1✔
131	gcode_buff.append("G1 X88.62661 Y103.55409 F7000 S0\n");	1✔
132	gcode_buff.append("G1 X88.12357 Y103.42248 F7000 S0\n");	1✔
133	gcode_buff.append("G1 X87.62321 Y103.28552 F7000 S0\n");	1✔
134		1✔
135	let mut parser =	1✔
136	control::GCodeLineParser::new(instrumentation::gcode::BufferStream::new(gcode_buff));	1✔
137
138	loop {
139	match parser.next_gcode(CommChannel::Internal).await {	21✔
140	Ok(gcode) => {	21✔
141	if motion_planner	21✔
142	.plan(CommChannel::Internal, &gcode, false, &event_bus)	21✔
143	.await	21✔
144	.is_err()	21✔
145	{
146	break;	1✔
147	}	20✔
148	}
NEW 149	Err(_error) => {	×
NEW 150	break;	×
151	}
152	}
153	}
154	}
155
156	let micro_segment_period_secs: hwa::math::Real =	1✔
157	hwa::math::Real::from_lit(STEPPER_PLANNER_MICROSEGMENT_PERIOD_US as i64, 6).rdp(6);	1✔
158		1✔
159	let _sampling_time: hwa::math::Real =	1✔
160	hwa::math::Real::from_lit(STEPPER_PLANNER_CLOCK_PERIOD_US as i64, 6).rdp(6);	1✔
161		1✔
162	let mut data_points = instrumentation::datapoints::DataPoints::new();	1✔
163	loop {
164	match motion_planner.next_plan(&event_bus).await {	20✔
165	controllers::motion::ExecPlan::Segment(mut segment, _channel, _order_num) => {	19✔
166	let current_real_pos = motion_planner.motion_status().get_current_position();	19✔
167	let position_offset = segment.src_pos - current_real_pos.space_pos;	19✔
168	hwa::info!(	19✔
169	"[task_stepper] order_num:{:?} Correcting offset [{:?}] {}",	19✔
170	_order_num,
171	position_offset,
172	hwa::Contract::SPACE_UNIT_MAGNITUDE,
173	);
174	segment.fix_deviation(	19✔
175	&position_offset,	19✔
176	motion_planner.motion_config().get_flow_rate_as_real(),	19✔
177	);	19✔
178	match SCurveMotionProfile::compute(	19✔
179	segment.displacement_su,	19✔
180	segment.speed_enter_su_s,	19✔
181	segment.speed_exit_su_s,	19✔
182	&segment.constraints,	19✔
183	false,	19✔
184	) {	19✔
185	Ok(trajectory) => {	19✔
186	// The relevant coords (those that will move)	19✔
187	let mut relevant_coords = hwa::math::CoordSel::empty();	19✔
188	// The relevant coords that will move forward	19✔
189	let mut relevant_coords_dir_fwd = hwa::math::CoordSel::empty();	19✔
190	segment.unit_vector_dir.foreach_values(\|coord, val\| {	38✔
191	relevant_coords.set(coord, !val.is_negligible());	38✔
192	relevant_coords_dir_fwd.set(coord, val.is_defined_positive());	38✔
193	});	38✔
194	hwa::debug!("Relevant coords: [{:?}]", relevant_coords);	19✔
195	hwa::debug!("Relevant coords_dir_fwd: [{:?}]", relevant_coords_dir_fwd);	19✔
196
197	let steps_per_su = motion_planner	19✔
198	.motion_config()	19✔
199	.get_units_per_space_magnitude()	19✔
200	* motion_planner.motion_config().get_micro_steps_as_vector();	19✔
201		19✔
202	let mut segment_iterator =	19✔
203	SegmentIterator::new(&trajectory, micro_segment_period_secs);	19✔
204		19✔
205	let mut micro_segment_interpolator =	19✔
206	LinearMicrosegmentStepInterpolator::new(	19✔
207	segment	19✔
208	.unit_vector_dir	19✔
209	.with_coord(relevant_coords.complement(), None)	19✔
210	.abs(),	19✔
211	segment.displacement_su,	19✔
212	steps_per_su.with_coord(relevant_coords.complement(), None),	19✔
213	);	19✔
214	hwa::info!(	19✔
215	"[task_stepper] order_num:{:?} Trajectory interpolation START",	19✔
216	_order_num
217	);
218
219	// Micro-segments interpolation along segment
220	data_points.segment_starts(&trajectory);	19✔
221	loop {
222	// Micro-segment start
223
224	if let Some(estimated_position) = segment_iterator.next() {	69✔
225	data_points.interpolation_tick(&segment_iterator);	69✔
226		69✔
227	let w = (segment_iterator.dt() * hwa::math::ONE_MILLION).round();	69✔
228	if w.is_negligible() {	69✔
NEW 229	hwa::info!("giving up for any reason");	×
NEW 230	break;	×
231	}	69✔
232	let _has_more =	69✔
233	micro_segment_interpolator.advance_to(estimated_position, w);	69✔
234		69✔
235	hwa::debug!(	69✔
NEW 236	"[task_stepper] segment:{:?}\|{:?} Trajectory micro-segment advanced: {:?} [{:?}] {} [{:?}] steps",	×
NEW 237	data_points.current_segment_id(),	×
NEW 238	data_points.current_micro_segment_id(),	×
NEW 239	segment_iterator.ds(),	×
NEW 240	micro_segment_interpolator	×
NEW 241	.advanced_units()	×
NEW 242	.map_nan_coords(relevant_coords, &hwa::math::ZERO),	×
NEW 243	hwa::Contract::SPACE_UNIT_MAGNITUDE,	×
NEW 244	micro_segment_interpolator	×
NEW 245	.advanced_steps()	×
NEW 246	.map_nan_coords(relevant_coords, &0),	×
247	);
248
249	#[cfg(feature = "verbose-timings")]
250	let t1 = embassy_time::Instant::now();
251
252	let _ = micro_segment_interpolator.state().clone();	69✔
253	let _ = relevant_coords;	69✔
254	data_points.micro_segment_ends();	69✔
255	if !_has_more {	69✔
256	break;	19✔
257	}	50✔
258	} else {
259	// No advance
NEW 260	break;	×
261	}
262	}
263	data_points.segment_ends();	19✔
264	////	19✔
265	//// TRAJECTORY INTERPOLATION END	19✔
266	////	19✔
267	#[cfg(feature = "debug-motion")]	19✔
268	hwa::debug!(	19✔
269	"[task_stepper] order_num:{:?} Trajectory interpolation END",	19✔
270	_order_num	19✔
271	);	19✔
272		19✔
273	let _adv_steps = micro_segment_interpolator.advanced_steps();	19✔
274	let adv_pos = micro_segment_interpolator.advanced_units();	19✔
275	hwa::info!(	19✔
276	"[task_stepper] order_num:{:?} Trajectory advanced. vector displacement space: {:?} [{:#?}] {}, vlim: {:?} {}/s",	19✔
277	_order_num,	19✔
278	segment_iterator.current_position(),	19✔
279	adv_pos,
280	hwa::Contract::SPACE_UNIT_MAGNITUDE,
281	trajectory.v_lim,
282	hwa::Contract::SPACE_UNIT_MAGNITUDE
283	);
284
285	hwa::info!(	19✔
286	"[task_stepper] order_num:{:?} Trajectory advanced. vector displacement space: [{:#?}] steps",	19✔
287	_order_num,	19✔
288	_adv_steps.excluding_negligible()	19✔
289	);
290
291	let adv_delta = segment.unit_vector_dir.sign() * adv_pos;	19✔
292	let next_real_pos = (current_real_pos.space_pos	19✔
293	+ adv_delta.map_nan(&hwa::math::ZERO))	19✔
294	.rdp(6);	19✔
295		19✔
296	let mut pos = current_real_pos.clone();	19✔
297	pos.update_from_space_coordinates(&next_real_pos);	19✔
298	motion_planner	19✔
299	.motion_status()	19✔
300	.update_current_position(_order_num, &pos);	19✔
301	}
302	_ => {
NEW 303	hwa::error!("Unable to compute motion plan. Discarding...");	×
304	}
305	}
306	}
307	controllers::motion::ExecPlan::SetPosition(position, _channel, _order_num) => {	1✔
308	motion_planner	1✔
309	.motion_status()	1✔
310	.update_current_position(_order_num, &position);	1✔
311	}	1✔
312	_ => {}	×
313	}
314	motion_planner.consume_current_plan(&event_bus).await;	20✔
315	if motion_planner.num_queued().await == 0 {	20✔
316	break;	1✔
317	}	19✔
318	}
319
320	{	1✔
321	#[allow(unused)]	1✔
322	use gnuplot::{	1✔
323	AutoOption, MultiplotFillDirection::Downwards, MultiplotFillOrder::RowsFirst, Tick,
324	};	1✔
325	use gnuplot::{AxesCommon, Figure};	1✔
326	#[allow(unused)]	1✔
327	use gnuplot::{DashType, PlotOption};	1✔
328		1✔
329	let mut fg = Figure::new();	1✔
330		1✔
331	cfg_if::cfg_if! {	1✔
332	if #[cfg(feature="float-point-f64-impl")] {	1✔
333	const MATH_PRECISION: &str = "[float point 64bits]";	1✔
334	}	1✔
335	else if #[cfg(feature="fixed-point-128-impl")] {	1✔
336	const MATH_PRECISION: &str = "[fixed point 128bits]";	1✔
337	}	1✔
338	else {	1✔
339	const MATH_PRECISION: &str = "[float point 32bits]";	1✔
340	}	1✔
341	}	1✔
342		1✔
343	fg.set_multiplot_layout(2, 1)	1✔
344	.set_title(	1✔
345	format!(	1✔
346	"{} Double S-Curve velocity profile\n[{:?} segments, {:?} {} displacement, {:?} hz interp, {:?} hz sampling]",	1✔
347	MATH_PRECISION,	1✔
348	data_points.num_segments(),	1✔
349	data_points.total_displacement(),	1✔
350	hwa::Contract::SPACE_UNIT_MAGNITUDE,	1✔
351	hwa::Contract::MOTION_PLANNER_MICRO_SEGMENT_FREQUENCY,	1✔
352	hwa::Contract::STEP_PLANNER_CLOCK_FREQUENCY,	1✔
353	).as_str()	1✔
354	)	1✔
355	.set_scale(1.0, 1.0)	1✔
356	.set_offset(0.0, 0.0)	1✔
357	.set_multiplot_fill_order(RowsFirst, Downwards);	1✔
358		1✔
359	fg.axes2d()	1✔
360	.set_y_label(format!("Position ({})", hwa::Contract::SPACE_UNIT_MAGNITUDE).as_str(), &[])	1✔
361	.points(data_points.segment_position_marks.times, data_points.segment_position_marks.points, &[PlotOption::Color("black")])	1✔
362	.lines(data_points.interpolated_positions.times, data_points.interpolated_positions.points, &[PlotOption::Color("blue")])	1✔
363	//.lines(data_points.time_discrete, data_points.pos_discrete, &[PlotOption::Color("gray")])	1✔
364	;	1✔
365	fg.axes2d()	1✔
366	.set_y_label(format!("Velocity ({}/s)", hwa::Contract::SPACE_UNIT_MAGNITUDE).as_str(), &[])	1✔
367	.points(data_points.segment_velocity_marks.times, data_points.segment_velocity_marks.points, &[PlotOption::Color("black")])	1✔
368	.lines(data_points.interpolated_velocities.times, data_points.interpolated_velocities.points, &[PlotOption::Color("red")])	1✔
369	//.lines(data_points.time, data_points.spd, &[PlotOption::Color("green")])	1✔
370	//.lines(data_points.time_discrete_deriv, data_points.spd_discrete, &[PlotOption::Color("gray")])	1✔
371	;	1✔
372	#[cfg(not(test))]	1✔
373	fg.show_and_keep_running().unwrap();	1✔
374	_ = fg.save_to_pdf("plot.pdf", 10.0f32, 10.0f32);	1✔
375	}	1✔
376		1✔
377	#[cfg(not(test))]	1✔
378	std::process::exit(0);	1✔
379	}	1✔
380
381	//#region "Machinery initialization"
382
NEW 383	pub fn initialization_error() {	×
NEW 384	let msg = "Unable to start because SYS_ALARM raised at startup. Giving up...";	×
NEW 385	hwa::error!("{}", msg);	×
NEW 386	panic!("{}", msg);	×
387	}
388
389	//#endregion
390
391	//#region "Tests"
392
393	#[cfg(feature = "s-plot-bin")]
394	#[cfg(test)]
395	mod test {
396	use super::*;
397	use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex;
398	use printhor_hwa_common::PersistentState;
399	use std::marker::PhantomData;
400	use std::sync::{Condvar, Mutex};
401
402	struct Signaler {
403	mutex: Mutex<bool>,
404	condvar: Condvar,
405	}
406
407	impl Signaler {
408	fn new() -> Self {	1✔
409	Self {	1✔
410	mutex: Mutex::new(false),	1✔
411	condvar: Condvar::new(),	1✔
412	}	1✔
413	}	1✔
414
415	fn wait(&self) {	2✔
416	let mut signaled = self.mutex.lock().unwrap();	2✔
417	while !*signaled {	3✔
418	signaled = self.condvar.wait(signaled).unwrap();	1✔
419	}	1✔
420	*signaled = false;	2✔
421	}	2✔
422	}
423
424	pub static SPLOT_STATE: PersistentState<CriticalSectionRawMutex, bool> = PersistentState::new();
425
426	pub struct MockedExecutor {
427	inner: embassy_executor::raw::Executor,
428	not_send: PhantomData<*mut ()>,
429	signaler: &'static Signaler,
430	}
431
432	impl MockedExecutor {
433	/// Create a new Executor.
434	pub fn new() -> Self {	1✔
435	let signaler = Box::leak(Box::new(Signaler::new()));	1✔
436	Self {	1✔
437	inner: embassy_executor::raw::Executor::new(signaler as mut Signaler as mut ()),	1✔
438	not_send: PhantomData,	1✔
439	signaler,	1✔
440	}	1✔
441	}	1✔
442
443	pub fn run(&'static mut self, init: impl FnOnce(embassy_executor::Spawner)) {	1✔
444	init(self.inner.spawner());	1✔
445
446	loop {
447	unsafe { self.inner.poll() };	3✔
448	if SPLOT_STATE.signaled() {	3✔
449	break;	1✔
450	}	2✔
451	self.signaler.wait()	2✔
452	}
453	}	1✔
454	}
455
456	/// The integration test entry-point
457	#[embassy_executor::task(pool_size = 1)]	1✔
458	async fn mocked_splot_main(spawner: embassy_executor::Spawner) {	1✔
459	printhor_main(spawner, false).await;	1✔
460	SPLOT_STATE.signal(true);	1✔
461	}	1✔
462
463	#[test]
464	fn splot_test() {	1✔
465	let executor = hwa::make_static_ref!("Executor", MockedExecutor, MockedExecutor::new());	1✔
466	// 2. Spawn the [mocked_main] task	1✔
467	executor.run(\|spawner\| {	1✔
468	let _tk = spawner.must_spawn(mocked_splot_main(spawner));	1✔
469	});	1✔
470	}	1✔
471	}
472
473	//#endregion

cbruiz / printhor / 13753907916

Source File Press 'n' to go to next uncovered line, 'b' for previous

Source File
Press 'n' to go to next uncovered line, 'b' for previous