// :3 Listen, here we will write the function that will decode only ONE order and will return some data, IF it's a sensors packet... static void decode_bro_input (const bro_fist_t * input_packet, bro_fist_t * output_packet, engines_t * motors ) { U8 temp_port; output_packet->port = input_packet->port; output_packet->operation = input_packet->operation; switch (input_packet->operation) { case LIGHT_SENSOR: decode_bro_port (input_packet->port, &temp_port); output_packet->data = (float) ecrobot_get_light_sensor(temp_port); break; case TOUCH_SENSOR: decode_bro_port (input_packet->port, &temp_port); output_packet->data = (float) ecrobot_get_touch_sensor(temp_port); break; case SOUND_SENSOR: decode_bro_port (input_packet->port, &temp_port); output_packet->data = (float) ecrobot_get_sound_sensor(temp_port); break; case RADAR_SENSOR: decode_bro_port (input_packet->port, &temp_port); output_packet->data = (float) ecrobot_get_sonar_sensor(temp_port); break; case TACHO_COUNT: decode_bro_port (input_packet->port, &temp_port); output_packet->data = (float) nxt_motor_get_count(temp_port); break; case SET_POWER: decode_bro_port (input_packet->port, &temp_port); switch (temp_port) { case NXT_PORT_A: motors->first.speed_control_type = RAW_POWER; motors->first.powers[0] = input_packet->data; output_packet->data = (float) nxt_motor_get_count(NXT_PORT_A); output_packet->timestamp = (float) systick_get_ms(); break; case NXT_PORT_B: motors->second.speed_control_type = RAW_POWER; motors->second.powers[0] = input_packet->data; break; case NXT_PORT_C: motors->third.speed_control_type = RAW_POWER; motors->third.powers[0] = input_packet->data; break; }; break; default: //Nothing HERE break; }; }
// :3 Listen, here we will write the function that will decode only ONE order and will return some data, IF it's a sensors packet... static void decode_bro_input (const bro_fist_t * input_packet, bro_fist_t * output_packet, engines_t * motors) { U8 temp_port; output_packet->port = input_packet->port; output_packet->operation = input_packet->operation; switch (input_packet->operation) { case LIGHT_SENSOR: decode_bro_port (input_packet->port, &temp_port); output_packet->data = (float) ecrobot_get_light_sensor(temp_port); break; case TOUCH_SENSOR: decode_bro_port (input_packet->port, &temp_port); output_packet->data = (float) ecrobot_get_touch_sensor(temp_port); break; case SOUND_SENSOR: decode_bro_port (input_packet->port, &temp_port); output_packet->data = (float) ecrobot_get_sound_sensor(temp_port); break; case RADAR_SENSOR: decode_bro_port (input_packet->port, &temp_port); output_packet->data = (float) ecrobot_get_sonar_sensor(temp_port); break; case TACHO_COUNT: decode_bro_port (input_packet->port, &temp_port); output_packet->data = (float) nxt_motor_get_count(temp_port); break; case SET_POWER: decode_bro_port (input_packet->port, &temp_port); switch (temp_port) { case NXT_PORT_A: motors->first.speed_control_type = RAW_POWER; motors->first.powers[0] = input_packet->data; output_packet->data = (float) nxt_motor_get_count(temp_port); output_packet->time = (long) ecrobot_get_systick_ms(); break; case NXT_PORT_B: motors->second.speed_control_type = RAW_POWER; motors->second.powers[0] = input_packet->data; output_packet->data = (float) nxt_motor_get_count(temp_port); output_packet->time = (long) ecrobot_get_systick_ms(); break; case NXT_PORT_C: motors->third.speed_control_type = RAW_POWER; motors->third.powers[0] = input_packet->data; output_packet->data = (float) nxt_motor_get_count(temp_port); output_packet->time = (long) ecrobot_get_systick_ms(); break; }; break; case SET_SPEED: motors->first.speed_control_type = PID_CONTROLLED; motors->second.speed_control_type = PID_CONTROLLED; motors->first.speed_ref[setSpeedCounter] = input_packet->data; motors->second.speed_ref[setSpeedCounter] = input_packet->data; if(setSpeedCounter == 0){ motors->first.distance_ref[setSpeedCounter] = input_packet->data2; motors->second.distance_ref[setSpeedCounter] = input_packet->data2; }else{ motors->first.distance_ref[setSpeedCounter] = motors->first.distance_ref[setSpeedCounter -1] + input_packet->data2; motors->second.distance_ref[setSpeedCounter] = motors->second.distance_ref[setSpeedCounter -1] + input_packet->data2; } setSpeedCounter++; break; default: break; }; }