Commit 5d9e0000 authored by Vojtěch Szotkowski's avatar Vojtěch Szotkowski
Browse files

Add after finish schumacher state management

parent acc3d128

src/firmware/source/Schumacher.c

+11 −7
Original line number Diff line number Diff line
@@ -181,6 +181,8 @@ void sumacher_on_new_frame(bool has_two_cams) { 
	static struct HorizontalData horizontal_data = {0U};

	static struct VerticalData vertical_data = {0U};



	uint8_t* sumacher_state = config_struct.general.sumacher_state;



    memcpy(horizontal_data.camera_data.camera_frame,

    		config_struct.camera_1_frame,

			sizeof(FrameVal) * 128);
@@ -211,7 +213,7 @@ void sumacher_on_new_frame(bool has_two_cams) { 


	float error = horizontal_error(&horizontal_data);

	float boarder_distance = has_two_cams ? vertical_distance(&vertical_data) : 0.0f;

	saw_finish = saw_finish || check_finish(&horizontal_data, config_struct.general.fin_deriv_thres);

	saw_finish = check_finish(&horizontal_data, config_struct.general.fin_deriv_thres);



	uint16_t servo_duty = compute_servo(error);

	uint16_t regl_duty = compute_regl(error, has_two_cams ? &boarder_distance : NULL);
@@ -222,12 +224,14 @@ void sumacher_on_new_frame(bool has_two_cams) { 
		count_cube_detection = 0;

	}



	if (saw_finish) {

		config_struct.general.sumacher_state = SUMACHER_AFTER_FINISH;

		regl_duty =

				count_cube_detection >= config_struct.sonic.count_cube_thresh

				? 1000u

				: config_struct.regl.speed_after_finish_us;

	if (saw_finish || sumacher_state >= SUMACHER_AFTER_FINISH) {

		if (count_cube_detection >= config_struct.sonic.count_cube_thresh) {

			sumacher_state = SUMACHER_BOX_STOP;

			regl_duty = 1000; // FULL BREAK

		} else if (sumacher_state != SUMACHER_BOX_STOP) {

			sumacher_state = SUMACHER_AFTER_FINISH;

			regl_duty = config_struct.regl.speed_after_finish_us;

		}

	}



    set_servo(servo_duty);

src/firmware/source/firmware.c

+4 −10
Original line number Diff line number Diff line
@@ -27,9 +27,9 @@ ConfigStruct config_struct = { 
	},

	.regl = {

		.pulse_width_us = 1500,

		.speed_straight_us = 1580,

		.speed_straight_us = 1550,

		.speed_after_finish_us = 1540,

		.speed_turn = 1565,

		.speed_turn = 1550,

		.error_turn_config = 3,

	},

	.sonic = {
@@ -110,7 +110,6 @@ int main(void) { 


    if (!has_remote) {

		config_struct.general.state = CAR_STATE_RUN_FULL;

		config_struct.general.state = CAR_STATE_RUN_SERVO;



		reset_new_frames();

		while (!get_num_of_new_frames())
@@ -148,16 +147,11 @@ int main(void) { 


    	switch(config_struct.general.state) {

    		case CAR_STATE_RESET:

    			config_struct.general.sumacher_state = SUMACHER_STRAIGHT;

    			config_struct.general.battery = bat_voltage_mV_blocking();

    			// Delay

    			for (int i = 0 ; i < 5000000; ++i)

    				__asm volatile ("nop");

    			reset_new_frames();

    			while (!get_num_of_new_frames())

    				__asm volatile ("nop");

    			reset_new_frames();

    			if (has_remote) {

    				send_config_data();

    			}

    			break;

    		case CAR_STATE_IDLE:

    		case CAR_STATE_MANUAL: