/* for elevator auto mode */
static void elevator_cb(void *dummy)
{
static uint8_t prev = 0;
uint8_t val;
val = !sensor_get(0);
switch (elevator_state) {
case OFF:
break;
case WAIT_SENSOR:
if (val && !prev) {
delay = elevator_delay_up;
elevator_state = SENSOR_OK;
}
break;
case SENSOR_OK:
if (delay-- == 0) {
cs_set_consign(&mainboard.elevator.cs, elevator_pos_up);
elevator_state = WAIT_DOWN;
delay = elevator_delay_down;
}
break;
case WAIT_DOWN:
if (delay-- == 0) {
cs_set_consign(&mainboard.elevator.cs, elevator_pos_down);
elevator_state = WAIT_SENSOR;
}
break;
default:
break;
}
prev = val;
}
void agent_learn(void) {
// 既に学習済みならセンサーから値を取り込まない
if(visible[curY][curX]) return;
visible[curY][curX] = true;
uint8_t sensor_data = sensor_get();
for(int k = 0; k < 4; ++k) {
// 後ろのセンサーは付いていないので
if(k == 2) continue;
int ndir = (dir + k)%4;
// 壁があるとき
if(gbi(sensor_data, k)) {
wall[curY][curX][ndir] = true;
}
int ny = curY + dy[ndir];
int nx = curX + dx[ndir];
if(check_pos(ny, nx)) {
if(gbi(sensor_data, k)) {
wall[ny][nx][(ndir + 2)%4] = true;
}
}
}
}
int get_sensor_value(int sensor_id)
{
int val;
struct serialsosurce ss;
sensor_get(&ss,sensor_id,&val);
return val;
}
unsigned char *
var_sensors_table(struct variable *vp, oid *name, size_t *length, int exact,
size_t *var_len, WriteMethod **write_method)
{
int index;
struct sensor sensor;
struct sensordev sdev;
static u_long ulong_ret;
static unsigned char str[BUFSIZ];
sensor_refresh();
if (num_sensors == 0)
return (NULL);
if (header_simple_table(vp, name, length, exact, var_len,
write_method, num_sensors)
== MATCH_FAILED)
return (NULL);
index = name[*length-1]-1;
if (sensor_get(index, &sdev, &sensor))
return (NULL);
switch (vp->magic) {
case SENS_INDEX:
ulong_ret = name[*length-1];
return (unsigned char *) &ulong_ret;
case SENS_DESCR:
sensor_desc(index, &sensor, str, BUFSIZ);
*var_len = strlen(str);
return (unsigned char *) str;
case SENS_TYPE:
ulong_ret = sensor.type;
return (unsigned char *) &ulong_ret;
case SENS_DEVICE:
if ((*var_len = strlcpy(str, sdev.xname, BUFSIZ)) !=
strlen(sdev.xname))
return (NULL);
return (unsigned char *) str;
case SENS_VALUE:
sensor_value(&sensor, str);
*var_len = strlen(str);
return (unsigned char *) str;
case SENS_UNITS:
sensor_units(&sensor, str);
*var_len = strlen(str);
return (unsigned char *) str;
case SENS_STATUS:
ulong_ret = sensor.status;
return (unsigned char *) &ulong_ret;
default:
return (NULL);
}
/* NOTREACHED */
}
int sensorX_get(struct serialsosurce *ss, struct sensorX *sx)
{
int i;
for (i = 0; i < sx->n; i++) {
if(sensor_get(ss, sx->id[i], &sx->data[i]) == -1)
return -1;
}
return 0;
}
float T1_trans(int id)
{
int data;
float ret;
srand((unsigned)time(NULL));
sensor_get(ss,id,&data);
data+=rand()%1;
ret = (float)(data * 120/1000);
printf_log(id,data,ret);
return ret;
}
void beacon_calibre_pos(void)
{
sensorboard.flags &= ~DO_CS;
/* init beacon pos */
pwm_ng_set(BEACON_PWM, 100);
/* find rising edge of the mirror*/
wait_ms(100);
while (sensor_get(BEACON_POS_SENSOR));
wait_ms(100);
while (!sensor_get(BEACON_POS_SENSOR));
pwm_ng_set(BEACON_PWM, 0);
beacon_reset_pos();
pid_reset(&sensorboard.beacon.pid);
encoders_spi_set_value_beacon(BEACON_ENCODER, BEACON_OFFSET_CALIBRE);
cs_set_consign(&sensorboard.beacon.cs, 0);
sensorboard.flags |= DO_CS;
}
// The loop function is called in an endless loop
void loop()
{
// This artificial delay is kept here to avoid endless spinning
// of Arduino microcontroller. Modify it as per specific application needs.
delay(600);
// Give CPU cycles to OCStack to perform send/recv and other OCStack stuff
if (OCProcess() != OC_STACK_OK) {
OC_LOG(ERROR, TAG, ("OCStack process error"));
return;
}
sensor_get();
button_observer();
}
/* function called when cmd_sensor is parsed successfully */
static void cmd_sensor_parsed(void *parsed_result, void *data)
{
struct cmd_sensor_result *res = parsed_result;
uint8_t i, loop = 0;
if (!strcmp_P(res->arg1, PSTR("loop_show")))
loop = 1;
do {
printf_P(PSTR("SENSOR values: "));
for (i=0; i<SENSOR_MAX; i++) {
printf_P(PSTR("%d "), !!sensor_get(i));
}
printf_P(PSTR("\r\n"));
wait_ms(100);
} while (loop && !cmdline_keypressed());
}
/*
* must be called from start area.
* get 4 static columns and build a temple on the disc
*/
uint8_t strat_static_columns(uint8_t configuration)
{
uint8_t err;
uint8_t col1_present = 0, col4_present = 0;
uint16_t old_spdd, old_spda;
DEBUG(E_USER_STRAT, "%s(%d)", __FUNCTION__, configuration);
strat_get_speed(&old_spdd, &old_spda);
/* calibrate scanner */
i2c_sensorboard_scanner_calib();
i2c_mechboard_mode_harvest();
/* go straight. total distance is less than 5 meters */
strat_set_speed(1000, 1000);
trajectory_d_rel(&mainboard.traj, BIG_DIST);
/* when y > 50, break */
err = WAIT_COND_OR_TRAJ_END(y_is_more_than(500), TRAJ_FLAGS_STD);
if (TRAJ_SUCCESS(err)) /* we should not reach end */
ERROUT(END_ERROR);
else if (err)
ERROUT(err);
/* turn to 90° abs while going forward */
DEBUG(E_USER_STRAT, "turn now");
strat_set_speed(1000, 350);
trajectory_only_a_abs(&mainboard.traj, COLOR_A(90));
/* when y > 100, check the presence of column 4 */
err = WAIT_COND_OR_TRAJ_END(y_is_more_than(1000), TRAJ_FLAGS_STD);
if (TRAJ_SUCCESS(err)) /* we should not reach end */
ERROUT(END_ERROR);
else if (err)
ERROUT(err);
if (get_color() == I2C_COLOR_RED && sensor_get(S_COLUMN_RIGHT))
col4_present = 1;
if (get_color() == I2C_COLOR_GREEN && sensor_get(S_COLUMN_LEFT))
col4_present = 1;
/* when y > 120, check the presence of column 1 */
err = WAIT_COND_OR_TRAJ_END(y_is_more_than(1200), TRAJ_FLAGS_STD);
if (TRAJ_SUCCESS(err)) /* we should not reach end */
ERROUT(END_ERROR);
else if (err)
ERROUT(err);
if (get_color() == I2C_COLOR_RED && sensor_get(S_COLUMN_RIGHT))
col1_present = 1;
if (get_color() == I2C_COLOR_GREEN && sensor_get(S_COLUMN_LEFT))
col1_present = 1;
/* when y > 130, break */
err = WAIT_COND_OR_TRAJ_END(y_is_more_than(1300), TRAJ_FLAGS_STD);
if (TRAJ_SUCCESS(err)) /* we should not reach end */
ERROUT(END_ERROR);
else if (err)
ERROUT(err);
strat_infos.s_cols.flags |= STATIC_COL_LINE0_DONE;
DEBUG(E_USER_STRAT, "col4=%d col1=%d", col4_present, col1_present);
DEBUG(E_USER_STRAT, "have %d cols", get_column_count());
if (configuration == 0) {
if (get_column_count() > 2) {
configuration = 1;
if (col4_present || col1_present) {
strat_infos.s_cols.flags |=
STATIC_COL_LINE2_DONE;
}
else {
strat_infos.s_cols.flags |=
STATIC_COL_LINE1_DONE;
}
}
/* only 2 colums on the first line */
else {
/* all other colums are on line 1 */
if (col4_present && col1_present) {
configuration = 2;
strat_infos.s_cols.flags |=
STATIC_COL_LINE2_DONE;
}
/* only 2 columns on line 1, so there are also
* 2 on line 2 */
else if (col4_present || col1_present) {
configuration = 4;
strat_infos.s_cols.flags |=
STATIC_COL_LINE2_DONE;
}
/* all other columns are on line 2 */
else {
configuration = 3;
strat_infos.s_cols.flags |=
STATIC_COL_LINE1_DONE;
}
}
}
strat_infos.s_cols.configuration = configuration;
DEBUG(E_USER_STRAT, "use configuration %d", configuration);
if (configuration == 1) {
/* we already got 4 columns, go to the disc directly */
strat_set_speed(1500, 900);
trajectory_only_a_abs(&mainboard.traj, COLOR_A(0));
err = WAIT_COND_OR_TRAJ_END(x_is_more_than(1100), TRAJ_FLAGS_STD);
if (TRAJ_SUCCESS(err)) /* we should not reach end */
ERROUT(END_ERROR);
else if (err)
ERROUT(err);
}
else if (configuration == 2 /* go from line 0 to line 1 */) {
strat_set_speed(800, 1000);
/* relative is needed here */
trajectory_only_a_rel(&mainboard.traj, COLOR_A(-180));
err = WAIT_COND_OR_TRAJ_END(!y_is_more_than(1300), TRAJ_FLAGS_STD);
if (TRAJ_SUCCESS(err)) /* we should not reach end */
ERROUT(END_ERROR);
else if (err)
ERROUT(err);
strat_set_speed(1000, 600);
err = WAIT_COND_OR_TRAJ_END(!y_is_more_than(1100),
TRAJ_FLAGS_STD);
if (TRAJ_SUCCESS(err)) /* we should not reach end */
ERROUT(END_ERROR);
else if (err)
ERROUT(err);
}
else if (configuration == 3 /* go from line 0 to line 2 and there is 4 columns
on line 2*/) {
strat_set_speed(1000, 600);
/* relative is needed here */
trajectory_only_a_rel(&mainboard.traj, COLOR_A(-180));
err = WAIT_COND_OR_TRAJ_END(!y_is_more_than(1110), TRAJ_FLAGS_STD);
if (TRAJ_SUCCESS(err)) /* we should not reach end */
ERROUT(END_ERROR);
else if (err)
ERROUT(err);
}
else if (configuration == 4 /* go from line 0 to line 2 and there is 2 columns
on line 2 */) {
strat_set_speed(1000, 600);
/* relative is needed here */
trajectory_only_a_rel(&mainboard.traj, COLOR_A(-180));
err = WAIT_COND_OR_TRAJ_END(!y_is_more_than(600), TRAJ_FLAGS_STD);
if (TRAJ_SUCCESS(err)) /* we should not reach end */
ERROUT(END_ERROR);
else if (err)
ERROUT(err);
}
else {
trajectory_stop(&mainboard.traj);
}
ERROUT(END_TRAJ);
end:
strat_set_speed(old_spdd, old_spda);
return err;
}
