static void reset(void)
{
int i,j;
int channel;
robot_name = robot_get_name();
robot_num = robot_name[1];
char connector_name[15];
float khepera3_matrix[9][2] =
{ {0, 0}, {-20000, 20000}, {-50000, 50000}, {-70001, 70000}, {70001, -70000},
{50000, -50000}, {20000, -20000}, {0, 0}, {0, 0} };
// { {-5000, -5000}, {-20000, 40000}, {-30000, 50000}, {-70000, 70000}, {70000, -60000},
// {50000, -40000}, {40000, -20000}, {-5000, -5000}, {-10000, -10000} };
char sensors_name[12];
// float (*temp_matrix)[2];
range = RANGE;
sensor_number = 9;
sprintf(sensors_name, "ds0");
range = 2000;
// Enable the Distance sensors and Braitenberg weights
for (i = 0; i < sensor_number; i++) {
sensors[i] = robot_get_device(sensors_name);
distance_sensor_enable(sensors[i], TIME_STEP);
if ((i + 1) >= 10) {
sensors_name[2] = '1';
sensors_name[3]++;
if ((i + 1) == 10) {
sensors_name[3] = '0';
sensors_name[4] = (char) '\0';
}
} else {
sensors_name[2]++;
}
for (j = 0; j < 2; j++) {
matrix[i][j] = khepera3_matrix[i][j];
}
}
// Enable the Connectors
for (i = 0; i< MAX_CONNECTOR_NUMBER; i++) {
// Construct the names of connectors
sprintf(connector_name, "con%d", i+1);
//robot_console_printf("connector %d: %s \n", i, connector_name);
// Activate the connectors
connectors[i] = robot_get_device(connector_name);
connector_enable_presence(connectors[i], TIME_STEP);
// Internal state
connector_status[i] = UNLOCKED;
}
//
// connectors[0] = robot_get_device("con1");
// connectors[1] = robot_get_device("con2");
// connector_enable_presence(connectors[0], TIME_STEP);
// connector_enable_presence(connectors[1], TIME_STEP);
//
// for (i = 0; i< MAX_CONNECTOR_NUMBER; i++) {
// connector_status[i] = UNLOCKED;
// }
// Basic speed at 0
speed_bias[0] = INITIAL_SPEED;
speed_bias[1] = INITIAL_SPEED;
// Enable keyboard
robot_keyboard_enable(TIME_STEP);
// Enable the Arm servo
arm_servo = robot_get_device("arm_servo");
servo_enable_position(arm_servo, TIME_STEP);
//robot_console_printf("The %s robot is reset, it uses %d sensors\n",robot_name, sensor_number);
// Enable the emitter/receiver, gps ------------%
commEmitter = robot_get_device("rs232_out");
commReceiver = robot_get_device("rs232_in");
gps = robot_get_device("gps");
/* If the channel is not the good one, we change it. */
channel = emitter_get_channel(commEmitter);
if (channel != COMMUNICATION_CHANNEL) {
emitter_set_channel(commEmitter, COMMUNICATION_CHANNEL);
}
receiver_enable(commReceiver, TIME_STEP);
gps_enable(gps, TIME_STEP);
found_robot = 0;
return;
}
static void schedule(enum sys_message msg)
{
// battery related
if (rtca_time.sys > adc_check_next) {
adc_read();
adc_check_next = rtca_time.sys + adc_check_interval;
if ((stat.v_raw > 400) && (stat.v_raw < 550)) {
if (CHARGING_STOPPED) {
if (stat.should_charge) {
CHARGE_DISABLE;
stat.should_charge = false;
adc_check_next = rtca_time.sys + 300;
} else {
if (stat.v_bat < 390) {
CHARGE_ENABLE;
stat.should_charge = true;
charge_start = rtca_time.sys;
}
}
} else {
if (rtca_time.sys > charge_start + 36000) {
CHARGE_DISABLE;
stat.should_charge = false;
adc_check_next = rtca_time.sys + 3600;
}
}
} else {
CHARGE_DISABLE;
stat.should_charge = false;
}
}
// GPS related
if (rtca_time.sys > gps_trigger_next) {
switch (gps_next_state) {
case MAIN_GPS_IDLE:
if (s.gps_loop_interval > s.gps_warmup_interval + 30) {
// when gps has OFF intervals
gps_disable();
gps_trigger_next = rtca_time.sys + s.gps_loop_interval - s.gps_warmup_interval - 2;
}
gps_next_state = MAIN_GPS_START;
break;
case MAIN_GPS_START:
gps_next_state = MAIN_GPS_INIT;
gps_enable();
break;
case MAIN_GPS_INIT:
if (s.gps_loop_interval > s.gps_warmup_interval + 30) {
// gps had a power off interval
gps_trigger_next = rtca_time.sys + s.gps_warmup_interval - s.gps_invalidate_interval - 3;
uart0_tx_str((char *)gps_init, 51);
} else {
// gps was on all the time
gps_trigger_next = rtca_time.sys + s.gps_loop_interval - s.gps_invalidate_interval - 3;
if (rtca_time.sys > gps_reinit_next) {
gps_reinit_next = rtca_time.sys + gps_reinit_interval;
uart0_tx_str((char *)gps_init, 51);
}
}
gps_next_state = MAIN_GPS_PDOP_RST;
// zero out all the old data
memset(&mc_f, 0, sizeof(mc_f));
break;
case MAIN_GPS_PDOP_RST:
gps_trigger_next = rtca_time.sys + s.gps_invalidate_interval - 1;
gps_next_state = MAIN_GPS_STORE;
mc_f.pdop = 9999;
break;
case MAIN_GPS_STORE:
if (mc_f.fix) {
// save all info to f-ram
store_pkt();
}
gps_next_state = MAIN_GPS_IDLE;
break;
}
}
// GPRS related
// force the HTTP POST from time to time
if (rtca_time.sys > gprs_tx_next) {
if (gprs_tx_trig & TG_NOW_MOVING) {
gprs_tx_next = rtca_time.sys + s.gprs_moving_tx_interval;
} else {
gprs_tx_next = rtca_time.sys + s.gprs_static_tx_interval;
}
sim900.flags |= TX_FIX_RDY;
}
if (sim900.flags & BLACKOUT) {
if (rtca_time.sys > gprs_blackout_lift) {
sim900.flags &= ~BLACKOUT;
}
}
if (((rtca_time.sys > gprs_trigger_next) || (sim900.flags & TX_FIX_RDY)) &&
!(sim900.flags & TASK_IN_PROGRESS)) {
// time to act
adc_read();
gprs_trigger_next = rtca_time.sys + s.gprs_loop_interval;
if (stat.v_bat > 350) {
#ifndef DEBUG_GPS
// if battery voltage is below ~3.4v
// the sim will most likely lock up while trying to TX
if (!(sim900.flags & BLACKOUT)) {
sim900_exec_default_task();
}
#endif
}
}
}
