// :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;
};
}
