Environment.c

Go to the documentation of this file.
00001 /*
00002 *  C Implementation: Environment
00003 *
00004 * Description: Implements the environment where actions can be performed
00005 *
00006 *
00007 * Author: Michel Tokic <michel@tokic.com>, (C) 2009
00008 *
00009 * Copyright: See COPYING file that comes with this distribution
00010 *
00011 */
00012 #include "Environment.h"
00013 
00014 
00018 Environment::Environment(ENV_POS* d) {
00019 
00020      // set the discretization
00021      this->discretization.x = d->x;
00022      this->discretization.y = d->y;
00023 
00024      // initialize the wheel encoder
00025      //r = &FeedbackSensor();
00026 
00027      // initialize servos
00028      BioloidServo::scanBioloidBus();
00029 
00030      servo_x = new BioloidServo(6, 180, 450); // area of operations from 200-500
00031      servo_y = new BioloidServo(2, 320, 700); // area of operations from 450-600
00032      //servo_x = new BioloidServo(SERVO_X, 180, 450); // area of operations from 200-500
00033      //servo_y = new BioloidServo(SERVO_Y, 320, 700); // area of operations from 450-600
00034 
00035 
00036      // position servos to (0,0)
00037      this->currentPos.x = this->currentPos.y = 0;
00038      servo_x->setPosition(0, this->discretization.x);
00039      servo_y->setPosition(0, this->discretization.y);
00040      
00041 
00042      // instantiate FeedbackSensor
00043      r = new FeedbackSensor();
00044 }
00045 
00046 
00047 Environment::~Environment() {
00048      delete r;
00049 }
00050 
00054 void Environment::doAction (unsigned int action, unsigned int delayTime, ACTION_RETURN* actionReturn) { 
00055 
00056      ENV_POS newPos;
00057      this->getCurrentState(&newPos);
00058 
00059      BoardController::powerOnLED((1<<LED_WALK), true);
00060 
00061      // initialize reward
00062      actionReturn->reward = 0;
00063 
00064      // Enable Bioloid Bus
00065      BoardController::enableSerialBioloidInterface();
00066 
00067      // if action is valid => select action
00068      if (this->isValidAction(&this->currentPos, action)) {
00069 
00070           // compute successor state
00071           if (action == UP) {
00072                newPos.y--;
00073           } else if (action == DOWN) { 
00074                newPos.y++;
00075           } else if (action == LEFT) {
00076                newPos.x--;
00077           } else if (action == RIGHT) {
00078                newPos.x++;
00079           }
00080 
00081           // perform action
00082           if (newPos.x != this->currentPos.x || newPos.y != this->currentPos.y) {
00083 
00084                     r->enableSensor();
00085                     // enable bioloid bus
00086                     servo_x->setPosition(newPos.x, this->discretization.x);
00087                     servo_y->setPosition(newPos.y, this->discretization.y);
00088 
00089                     BoardController::msDelay(delayTime);
00090 
00091                     // retransmit for the case if an error occured the first time
00092                     servo_x->setPosition(newPos.x, this->discretization.x);
00093                     servo_y->setPosition(newPos.y, this->discretization.y);
00094 
00095                     r->disableSensor();
00096                     
00097                     // save new state information
00098                     this->currentPos.x = newPos.x;
00099                     this->currentPos.y = newPos.y;
00100      
00101                     // assemble actionReturn
00102                     actionReturn->reward = r->getFeedback();
00103 
00104           }
00105      } else {
00106           BoardController::blinkLED((1<<LED_WALK), 4, 300);
00107      }
00108 
00109 
00110      // return current state state
00111      actionReturn->state.x = this->currentPos.x;
00112      actionReturn->state.y = this->currentPos.y;
00113 
00114      BoardController::powerOnLED((1<<LED_WALK), false);
00115 
00116      // Disable Bioloid Bus and enable V24
00117      BoardController::disableSerialInterfaces();
00118 
00119 
00120 }
00121 
00123 void Environment::getCurrentState (ENV_POS* _pos) {
00124      _pos->x = this->currentPos.x;
00125      _pos->y = this->currentPos.y;
00126 }
00127 
00129 void Environment::setDiscretization(ENV_POS* _discretization) {
00130      this->discretization.x = _discretization->x;
00131      this->discretization.y = _discretization->y;
00132 }
00133 
00135 void Environment::getDiscretization(ENV_POS* _discretization) {
00136      _discretization->x = this->discretization.x;
00137      _discretization->y = this->discretization.y;
00138 }
00139 
00141 void Environment::beamRobotToState(ENV_POS* newPos) {
00142 
00143      //BoardController::enableSerialBioloidInterface();
00144 
00145      if (newPos->x >= 0 && newPos->x < discretization.x) {
00146           servo_x->setPosition(newPos->x, discretization.x);
00147           this->currentPos.x = newPos->x;
00148      } else {
00149           newPos->x = this->currentPos.x;
00150      }
00151 
00152      if (newPos->y >= 0 && newPos->x < discretization.y) {
00153           servo_y->setPosition(newPos->y, discretization.y);
00154           this->currentPos.y = newPos->y;
00155      } else {
00156           newPos->y = this->currentPos.y;
00157      }
00158 }
00159 
00163 bool Environment::isValidAction(ENV_POS* pos, unsigned char action) {
00164 
00165      // check for valid action
00166      if (action != UP && action != DOWN && action != LEFT && action != RIGHT) return false;
00167      
00168      // check if pos is within statespace range
00169      if (pos->x < 0 || pos->x >= this->discretization.x ||
00170          pos->y < 0 || pos->y >= this->discretization.y)
00171           return false;
00172 
00173      // check if successorstate is within the statespace
00174      if      (action == UP    && pos->y == 0) return false;
00175      else if (action == DOWN  && pos->y >= (this->discretization.y-1)) return false;
00176      else if (action == LEFT  && pos->x == 0) return false;
00177      else if (action == RIGHT && pos->x >= (this->discretization.x-1)) return false;
00178      
00179      return true;
00180 }
00181 
Generated on Fri Oct 8 17:10:07 2010 for Crawling Robot Microcontroller Software by  doxygen 1.6.3