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

Add remote asserts

parent 5d9e0000

remote_gui/config.h

+3 −1
Original line number Diff line number Diff line
@@ -8,7 +8,8 @@ typedef struct { 
        uint8_t received_packets;  // RO

        uint8_t dip_switch_bits;   // RO

        uint8_t buttons_bits;      // RO

        uint16_t derivations;      // RO

        uint8_t error_code;        // RO

        uint8_t derivations;       // RO

        uint16_t fin_deriv_thres;  // CFG

        uint16_t battery;    	   // RO

    } general;
@@ -26,6 +27,7 @@ typedef struct { 
    } camera;

    struct Servo {

        uint16_t pulse_width_us;    // SET

        uint16_t midpoint_us;    	// CFG

        uint16_t c_lin;             // CFG

        uint16_t c_sq;              // CFG

        uint16_t c_thresh;          // CFG

remote_gui/config_module.py

+11 −9
Original line number Diff line number Diff line
@@ -19,6 +19,7 @@ class General: 
    received_packets: int = 0

    dip_switch_bits: int = 0

    buttons_bits: int = 0

    error_code: int = 0

    derivations: int = 0

    fin_deriv_thres: int = 0

    battery: int = 0
@@ -49,14 +50,15 @@ class Camera: 
@dataclass

class Servo:

    pulse_width_us: int = 0

    midpoint_us: int = 0

    c_lin: int = 0

    c_sq: int = 0

    c_thresh: int = 0



    cfg_members: List[str] = field(init=False)

    cfg_format: str = 'HHH'

    cfg_format: str = 'HHHH'

    def __post_init__(self):

        self.cfg_members: List[str] = ['c_lin', 'c_sq', 'c_thresh']

        self.cfg_members: List[str] = ['midpoint_us', 'c_lin', 'c_sq', 'c_thresh']



@dataclass

class Regl:
@@ -99,15 +101,15 @@ class ConfigStruct: 


    @staticmethod

    def deserialize(data: bytes) -> "ConfigStruct":

        fmt = "128H 128H B B B B B B H H H B B B B B B H H H H H H H H H H H H H H H H H H H H H "

        fmt = "128H 128H B B B B B B B B H H B B B B B B H H H H H H H H H H H H H H H H H H H H H H "

        unpacked = struct.unpack(fmt, data)

        return ConfigStruct(

            camera_1_frame=list(unpacked[0:128]),

            camera_2_frame=list(unpacked[128:256]),

            general=General(*unpacked[256:265]),

            camera=Camera(*unpacked[265:275]),

            servo=Servo(*unpacked[275:279]),

            regl=Regl(*unpacked[279:287]),

            sonic=Sonic(*unpacked[287:291]),

            separator=unpacked[291],

            general=General(*unpacked[256:266]),

            camera=Camera(*unpacked[266:276]),

            servo=Servo(*unpacked[276:281]),

            regl=Regl(*unpacked[281:289]),

            sonic=Sonic(*unpacked[289:293]),

            separator=unpacked[293],

        )

remote_gui/enums.py

+20 −0
Original line number Diff line number Diff line
@@ -33,6 +33,26 @@ class SumacherState(Enum): 
    AFTER_FINISH = 5

    BOX_STOP = 6



class ErrorCode(Enum):

    NO_ERR =        0  # no error

    COMM_SIZE_BUF = 1  # sometimes internal uart callbacks die, TODO write custom uart api

    COMM_STATE =    2  # Cannot perform that in this state

    COMM_SIZE =     3  # bad data size

    COMM_SEND =     4  # data sending too fast

    COMM_API =      5  # bad return status from UART API

    COMM_UNKNOWN =  6  # unknown command received

    REGL_RANGE =    7  #  out of hard limit range

    SERVO_RANGE =   8  #  out of hard limit range

    CAM_EXPO =      9  # camera exposition out of range

    LED_IDX =       10  # set led index bad

    SCH_FIND_TR =   11  # find treshold index out of range

    SCH_FIN =       12  # check finish index out of range

    SCH_L_IDX =     13  # left line index out of range

    SCH_R_IDX =     14  # right line index out of range

    SCH_M_IDX =     15  # mid line index out of range

    NOT_IMPL =      16  # not implemented





def get_config_packet_type(dataset_name: str):

    if dataset_name == "General":

        return PacketType.CONFIG_GENERAL

remote_gui/gui.py

+3 −1
Original line number Diff line number Diff line
@@ -13,7 +13,7 @@ from keyboard import Keyboard 
# Wifi

UDP_IP = "0.0.0.0"  # Listen on all available interfaces

UDP_PORT = 3333

BYTES_BRCAST = 572

BYTES_BRCAST = 574

car_ip = "0.0.0.0"



# Fps
@@ -258,6 +258,8 @@ def update_values(new_config: ConfigStruct, old_config: ConfigStruct, what: str) 
            val = CarState(value).name

        elif field == "sumacher_state":

            val = SumacherState(value).name

        elif field == "error_code":

            val = ErrorCode(value).name

        dpg.set_value(get_field_tag(what, field), f"{field}: {val}")

src/firmware/source/Schumacher.c

+21 −16
Original line number Diff line number Diff line
@@ -96,7 +96,7 @@ void process_camera_data(struct CameraData* data) { 
size_t find_treshold(struct HorizontalData* data, int from, int to, size_t _default) {

    int increment = (from <= to) ? 1 : -1;

    for (int i = from; i != to; i += increment) {

    	custom_assert(i >= 0 && i <= 127);

    	custom_assert(i >= 0 && i <= 127, EC_SCH_FIND_TR);

        if (data->camera_data.derived_frame[i] >= data->derivation_treshold) {

            return i;

        }
@@ -127,6 +127,9 @@ float horizontal_error(struct HorizontalData* data) { 
	size_t mid_index = (left_line_index + right_line_index + 1) / 2;

	float error = (left_line_index + right_line_index) / 2.0f - data->true_mid_index;



	custom_assert(left_line_index >= 0 && left_line_index <= 127, EC_SCH_L_IDX);

	custom_assert(left_line_index >= 0 && left_line_index <= 127, EC_SCH_R_IDX);

	custom_assert(mid_index >= 0 && mid_index <= 127, EC_SCH_M_IDX);

	data->mid_index = mid_index;

	data->left_line_index = left_line_index;

	data->right_line_index = right_line_index;
@@ -135,7 +138,7 @@ float horizontal_error(struct HorizontalData* data) { 
}



float vertical_distance(struct VerticalData* data) {

	custom_assert(false);

	custom_assert(false, EC_NOT_IMPL);

	return 0.0;

}
@@ -144,6 +147,7 @@ bool check_finish(struct HorizontalData* data, int derivation_count) { 
    int start = data->left_line_index + data->finish_line_ignore_padding;

    int end = data->right_line_index - data->finish_line_ignore_padding;

    for (size_t i = start; i <= end; i++) {

    	custom_assert(i >= 0 && i <= 127, EC_SCH_FIN);

        derivations += data->camera_data.derived_frame[i] >= data->derivation_treshold;

    }
@@ -152,19 +156,21 @@ bool check_finish(struct HorizontalData* data, int derivation_count) { 
}



uint16_t compute_servo(float error) {

	const float C_LINEAR = 8;

	const float C_SQUARE = 1.0;

	const float SQUARE_TRESHOLD = 6;

	const float c_lin = (float)config_struct.servo.c_lin;

	const float c_sq = config_struct.servo.c_sq / 10.0f;

	const uint16_t threshold = config_struct.servo.c_thresh;



	error = C_LINEAR * error

			+ (abs(error) >= SQUARE_TRESHOLD ? C_SQUARE * error * error : 0);

	error = c_lin * error

			+ (abs(error) >= threshold ?

					(c_sq * error * error * (error > 0.0f ? 1.0f : -1.0f)) :

					0.0f);



	return 1500u + clamp((int) error, -200, 200);

	return config_struct.servo.midpoint_us + clamp((int) error, -200, 200);

}



uint16_t compute_regl(float error, float* distance) {

	if (distance != NULL) {

		custom_assert(false);

		custom_assert(false, EC_NOT_IMPL);

	}



	return config_struct.regl.speed_straight_us;
@@ -181,7 +187,7 @@ 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;

	uint8_t* sumacher_state = &config_struct.general.sumacher_state;



    memcpy(horizontal_data.camera_data.camera_frame,

    		config_struct.camera_1_frame,
@@ -207,7 +213,6 @@ void sumacher_on_new_frame(bool has_two_cams) { 
		horizontal_data.finish_line_ignore_padding = LINE_FIX;



		was_init = true;



		return;

	}
@@ -224,12 +229,12 @@ void sumacher_on_new_frame(bool has_two_cams) { 
		count_cube_detection = 0;

	}



	if (saw_finish || sumacher_state >= SUMACHER_AFTER_FINISH) {

	if (saw_finish || *sumacher_state >= SUMACHER_AFTER_FINISH) {

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

			sumacher_state = SUMACHER_BOX_STOP;

			*sumacher_state = SUMACHER_BOX_STOP;

			regl_duty = 1000; // FULL BREAK

		} else if (sumacher_state != SUMACHER_BOX_STOP) {

			sumacher_state = SUMACHER_AFTER_FINISH;

		} else if (*sumacher_state != SUMACHER_BOX_STOP) {

			*sumacher_state = SUMACHER_AFTER_FINISH;

			regl_duty = config_struct.regl.speed_after_finish_us;

		}

	}
@@ -242,5 +247,5 @@ void sumacher_on_new_frame(bool has_two_cams) { 
	config_struct.camera.left_line_idx_1 = horizontal_data.left_line_index;

	config_struct.camera.right_line_idx_1 = horizontal_data.right_line_index;

	config_struct.camera.mid_idx_1 = horizontal_data.mid_index;

	config_struct.camera.error_1 = error;

	config_struct.camera.error_1 = (uint16_t)error;

}