// Esta funcion toma tres muestras del sonar y se queda con la media
int obtener_distancia()
{
int sensor1, sensor2, sensor3, sensor;
sensor1 = ecrobot_get_sonar_sensor(SONAR_PORT);
systick_wait_ms(20);
sensor2 = ecrobot_get_sonar_sensor(SONAR_PORT);
systick_wait_ms(20);
sensor3 = ecrobot_get_sonar_sensor(SONAR_PORT);
systick_wait_ms(20);
sensor = (sensor1+sensor2+sensor3)/3;
// if(sensor == 255) sensor = 0;
return (sensor);
}
// :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;
};
}
//*****************************************************************************
// ���� : sonar_alert
// �� : ����
// �Ԃ�l : 1(��Q������)/0(��Q������)
// �T�v : �����g�Z���T�ɂ���Q�����m
//*****************************************************************************
static int sonar_alert(void)
{
static unsigned int counter = 0;
static int alert = 0;
signed int distance;
if (++counter == 40/4) /* ��40msec���ɏ�Q�����m */
{
/* v
* �����g�Z���T�ɂ�鋗��������́A�����g�̌��������Ɉˑ����܂��B
* NXT�̏ꍇ�́A40msec�����x���o����̍ŒZ������ł��B
*/
distance = ecrobot_get_sonar_sensor(NXT_PORT_S2);
if ((distance <= SONAR_ALERT_DISTANCE) && (distance >= 0))
{
alert = 1; /* ��Q�������m */
}
else
{
alert = 0; /* ��Q������ */
}
counter = 0;
}
return alert;
}
void sonar_sensor_measurement(void)
{
// Variables used for distance measurement
S32 distance_left = 0;
S32 distance_right = 0;
int measurements = 0;
// Display menu
reset_sonar_measurement_menu();
while(true)
{
// Update values
distance_left += ecrobot_get_sonar_sensor(SONAR_SENSOR_FRONT);
distance_right += ecrobot_get_sonar_sensor(SONAR_SENSOR_REAR);
// Calculate average of new and previous
distance_left /= 2;
distance_right /= 2;
// Wait when over some measurements
if(measurements++ % MEASUREMENT_WAIT_INTERVAL == 0)
{
// Update the display
display_sonar_sensor_measurements(distance_left, distance_right);
systick_wait_ms(MEASUREMENT_WAIT);
}
// Chek if the enter button is pressed for long enough time to exit
if(ecrobot_is_ENTER_button_pressed())
{
// Time when the button was pressed
int enter_button_pressed_start_time = systick_get_ms();
// Continusly check if enought time has past to exit
while(ecrobot_is_ENTER_button_pressed())
{
if(enter_button_pressed_start_time + ENTER_BUTTON_EXIT_TIMEOUT <
systick_get_ms())
{
return;
}
}
}
}
}
double getPositionFromWall(UNICYCLE_CONTROLLER *uc) {
sonar = ecrobot_get_sonar_sensor(NXT_PORT_S4);
if(sonar < 0)
uc->cPos = uc->pPos;
else
uc->cPos = 0.01*sonar;
return sensor_model(uc);
}
//超音波センサ状態検出関数
void sonarcheck(void)
{
if(ecrobot_get_sonar_sensor(NXT_PORT_S2) <= target_sonar) //超音波センサの値が目標値以下か判断しフラグ変更
{
sonarflag = 1;
}
else
sonarflag = 0;
}
//*****************************************************************************
// TASK : tsk1
// DESCRIPTION : 40msec periodical Task and detects obstacles in front of
// NXTway-GS by using a sonar sensor
//*****************************************************************************
void tsk1(VP_INT exinf)
{
obstacle_flag = 0; /* no obstacles */
if ((nxtway_gs_mode == CONTROL_MODE) &&
(ecrobot_get_sonar_sensor(PORT_SONAR) <= MIN_DISTANCE)){
obstacle_flag = 1; /* obstacle detected */
}
ext_tsk(); /* terminates this task */
}
//bluetoothログ送信関数
void logSend(S8 data1, S8 data2, S16 adc1, S16 adc2, S16 adc3, S16 adc4){
U8 data_log_buffer[32];
*((U32 *)(&data_log_buffer[0])) = (U32)systick_get_ms();
*(( S8 *)(&data_log_buffer[4])) = (S8)data1;
*(( S8 *)(&data_log_buffer[5])) = (S8)data2;
*((U16 *)(&data_log_buffer[6])) = (U16)ecrobot_get_light_sensor(NXT_PORT_S3);
*((S32 *)(&data_log_buffer[8])) = (S32)nxt_motor_get_count(0);
*((S32 *)(&data_log_buffer[12])) = (S32)nxt_motor_get_count(1);
*((S32 *)(&data_log_buffer[16])) = (S32)nxt_motor_get_count(2);
*((S16 *)(&data_log_buffer[20])) = (S16)adc1;
*((S16 *)(&data_log_buffer[22])) = (S16)adc2;
*((S16 *)(&data_log_buffer[24])) = (S16)adc3;
*((S16 *)(&data_log_buffer[26])) = (S16)adc4;
*((S32 *)(&data_log_buffer[28])) = (S32)ecrobot_get_sonar_sensor(NXT_PORT_S2);
ecrobot_send_bt_packet(data_log_buffer, 32);
}
int getDistancia()
{
int auxDist = ecrobot_get_sonar_sensor(SONAR_PORT);
auxDist=(ecrobot_get_sonar_sensor(SONAR_PORT)+auxDist)/2;
return auxDist;
}
void getsonarvalue(void)
{
sonarvalue = ecrobot_get_sonar_sensor(NXT_PORT_S2);
}
// :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;
};
}