void photovore(void)
{
//initialize values
long unsigned int wheel_left=500;
long unsigned int wheel_right=500;
unsigned int threshold = 8;
//autocalibrate makes both start at equal values (so must start in same lighting)
update_sensors();
if (sensor_left>sensor_right)
auto_calib_R=sensor_left-sensor_right;
else
auto_calib_L=sensor_right-sensor_left;
while(1)
{
//get sensor data
update_sensors();
//detects more light on left side of robot
if(sensor_left > sensor_right && (sensor_left - sensor_right) > threshold)
{//go left
wheel_left=550;
wheel_right=550;
}
//detects more light on right side of robot
else if(sensor_right > sensor_left && (sensor_right - sensor_left) > threshold)
{//go right
wheel_left=850;
wheel_right=850;
}
//light is about equal on both sides
else
{//go straight
wheel_left=600;
wheel_right=800;
}
//output data to USB (use hyperterminal to view it)
rprintf("L_Sensor=%d, L_wheel=%d%d, R_Sensor=%d, R_wheel=%d%d\r\n",sensor_left, wheel_left, sensor_right, wheel_right);
//command servos
wheel_left(wheel_left);
wheel_right(wheel_right);
}
}
// Maintains the system
// Rules are hardcoded for now.
void maintain_system(unsigned long current_time)
{
if ((current_time - sensor_last_time) / 1000 > SENSOR_UPDATE_TIME || current_time == -1){
sensor_last_time = current_time;
#ifdef DEBUG
Serial.println("Updating sensor values");
#endif
update_sensors(SENSOR_PIN);
}
if ((current_time - system_last_time) / 1000 > ACTIVATION_TIME || current_time == -1){
system_last_time = current_time;
// Maintain Ale Zone (Bottom Chamber)
float zone_temp = zone_temperature(BOTTOMCHAMBER_ADDR);
if (zone_temp != INVALID_TEMP) {
if (zone_temp > (bottom_temp_setting + bottom_temp_overshoot) &&
bottom_zone_state == IDLE){
digitalWrite(BOTTOMCHAMBER, LOW); // Enable cooling
bottom_zone_state = COOLING;
}
if (zone_temp <= (bottom_temp_setting - bottom_temp_undershoot) &&
bottom_zone_state == COOLING){
digitalWrite(BOTTOMCHAMBER, HIGH); // Disable cooling
bottom_zone_state = IDLE;
}
}
// Maintain Lager Zone (Top Chamber)
// TODO
// Maintain Cooling System
zone_temp = zone_temperature(GLYCOL_ADDR);
if ( zone_temp != INVALID_TEMP){
if (zone_temp > (glycol_temp_setting + glycol_temp_overshoot) &&
glycol_state == IDLE){
#ifdef DEBUG
Serial.print("Enabling cooling for glycol: Temperature: ");
Serial.println(zone_temp);
#endif
digitalWrite(AIRCON, LOW); // Enable cooling
glycol_state = COOLING;
}
if ( zone_temp < (glycol_temp_setting - glycol_temp_undershoot) &&
glycol_state == COOLING) {
#ifdef DEBUG
Serial.print("Disabling cooling for glycol: Temperature: ");
Serial.println(zone_temp);
#endif
digitalWrite(AIRCON, HIGH); // Disable cooling
glycol_state = IDLE;
}
}
}
}
// Set I2C address and init I2C
void FC_IMU::init() {
_quaternion = FC_Quaternion();
set_clock_source(0x01);
gyro_set_range(0x00);
accel_set_range(0x00);
set_sleep(false);
set_baseline();
TIMSK2 |= (1 << TOIE2);
update_sensors();
}
int simulate_seconds(pState state, int seconds){
if (state == NULL){
return 1;
}
if ( (seconds < 10 ) || (seconds > 8000) ){
return 1;
}
for (int i=0;i<seconds;++i){
if (state->power == ON){
if (state->heater == ON){
state->currentTemp = (double)state->currentTemp + 1;
if ((double)state->currentTemp > ((double)state->setTemp + 5) ){
state->heater = OFF;
}
} else {
if ((double)state->currentTemp > (double)state->ambientTemp){
state->currentTemp = ((double)state->currentTemp - 0.25);
}
if (state->currentTemp < ((double)state->setTemp - 5)){
state->heater = ON;
}
}
}else {
if (state->currentTemp > (double)state->ambientTemp){
state->currentTemp = (double)state->currentTemp - 0.25;
}
}
update_sensors(state);
// only run program if there is a program.
if (state->currentStep > -1){
check_program(state);
if (state->finished){
return 2;
}
}
if (state->currentTime%60 == 0){
add_history(state);
}
state->currentTime = state->currentTime + 1;
}
//end simulate seconds
return 0;
}
void EXP_Class::adv ( void ){
//cout << "enter advance" << endl;
if( viewing ) stop_iterations_loop( );
update_sensors( );//here you compute the input vector
update_controllers ( ); //here you update the controllers and you generate a new output vector
if( viewing ) param->eye->view_eye_movement();
update_world(); //Here you call the set eye fucntion to upadte the eye position given the output vector
//manage_collisions ( );
compute_fitness();
iter++;
//cout << "leave advance" << endl;
}
void robot_class::TeleopPeriodic() {
update_sensors();
}
void robot_class::AutonomousPeriodic() {
update_sensors();
}
void CServer::robots_refresh()
{
u32 j, i;
std::vector<u32> robots_erase_list;
struct sRobot robot;
/*check for refresh time*/
if (this->time > this->time_refresh)
{
#ifdef VISUALISATION_IN_SERVER
/*
for (j = 0; j < robots.size(); j++)
visualisation_update(robots[j]);
*/
visualisation_update_all(&robots);
#endif
this->time_refresh = this->dt + get_ms_time();
for (j = 0; j < robots.size(); j++)
{
switch (robots[j].request)
{
case REQUEST_ROBOT_RESPAWN:
respawn(&robots[j]);
update_sensors(j);
break;
case REQUEST_ROBOT_DELETE:
robots_erase_list.push_back(j);
break;
case REQUEST_ROBOT_ADD_RED_PHEROMONE:
robot.type = ROBOT_TYPE_RED_PHEROMONE;
robot.parameter_int = 0.0;
robot.parameter_f = 0.0;
robot.reward = 0.0;
for (i = 0; i < ROBOT_SPACE_DIMENSION; i++)
robot.position[i] = robots[j].position[i];
if (robots[j].sensors[ROBOT_SENSOR_RED_PHEROMONE_DISTANCE_IDX] > robots[j].colision_distance)
add_new_robot(robot);
break;
case REQUEST_ROBOT_ADD_GREEN_PHEROMONE:
robot.type = ROBOT_TYPE_GREEN_PHEROMONE;
robot.parameter_int = 0.0;
robot.parameter_f = 0.0;
robot.reward = 0.0;
for (i = 0; i < ROBOT_SPACE_DIMENSION; i++)
robot.position[i] = robots[j].position[i];
if (robots[j].sensors[ROBOT_SENSOR_GREEN_PHEROMONE_DISTANCE_IDX] > robots[j].colision_distance)
add_new_robot(robot);
break;
case REQUEST_ROBOT_ADD_BLUE_PHEROMONE:
robot.type = ROBOT_TYPE_BLUE_PHEROMONE;
robot.parameter_int = 0.0;
robot.parameter_f = 0.0;
robot.reward = 0.0;
for (i = 0; i < ROBOT_SPACE_DIMENSION; i++)
robot.position[i] = robots[j].position[i];
if (robots[j].sensors[ROBOT_SENSOR_BLUE_PHEROMONE_DISTANCE_IDX] > robots[j].colision_distance)
add_new_robot(robot);
break;
case REQUEST_ROBOT_ADD_PATH:
robot.type = ROBOT_TYPE_PATH;
robot.parameter_int = 0.0;
robot.parameter_f = 0.0;
robot.reward = 0.0;
for (i = 0; i < ROBOT_SPACE_DIMENSION; i++)
robot.position[i] = robots[j].position[i];
if (robots[j].sensors[ROBOT_SENSOR_BLUE_PHEROMONE_DISTANCE_IDX] > robots[j].colision_distance)
add_new_robot(robot);
break;
default:
break;
}
if (robots[j].type&ROBOT_MOVEABLE_FLAG)
{
if (robots[j].type == ROBOT_TYPE_RED_ROBOT)
red_score+= robots[j].fitness;
if (robots[j].type == ROBOT_TYPE_GREEN_ROBOT)
green_score+= robots[j].fitness;
if (robots[j].type == ROBOT_TYPE_BLUE_ROBOT)
blue_score+= robots[j].fitness;
update_position(j);
update_sensors(j);
}
robots[j].request = REQUEST_NULL;
}
}
sprintf(text, "red %6.2f , green %6.2f , blue %6.2f", red_score, green_score, blue_score);
for (j = 0; j < robots_erase_list.size(); j++)
{
u32 idx = robots_erase_list[j];
robots.erase(robots.begin() + idx);
delete c_robots[idx];
c_robots.erase(c_robots.begin() + idx);
}
}
void stabilization(void){
update_sensors();
compute_motors_speed();
update_all_motors_speed();
}
static void handle_sensors(struct trainsrv_state *state, struct sensor_state sens) {
update_sensors(state, sens);
}
