Environment.c
Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012 #include "Environment.h"
00013
00014
00018 Environment::Environment(ENV_POS* d) {
00019
00020
00021 this->discretization.x = d->x;
00022 this->discretization.y = d->y;
00023
00024
00025
00026
00027
00028 BioloidServo::scanBioloidBus();
00029
00030 servo_x = new BioloidServo(6, 180, 450);
00031 servo_y = new BioloidServo(2, 320, 700);
00032
00033
00034
00035
00036
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
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
00062 actionReturn->reward = 0;
00063
00064
00065 BoardController::enableSerialBioloidInterface();
00066
00067
00068 if (this->isValidAction(&this->currentPos, action)) {
00069
00070
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
00082 if (newPos.x != this->currentPos.x || newPos.y != this->currentPos.y) {
00083
00084 r->enableSensor();
00085
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
00092 servo_x->setPosition(newPos.x, this->discretization.x);
00093 servo_y->setPosition(newPos.y, this->discretization.y);
00094
00095 r->disableSensor();
00096
00097
00098 this->currentPos.x = newPos.x;
00099 this->currentPos.y = newPos.y;
00100
00101
00102 actionReturn->reward = r->getFeedback();
00103
00104 }
00105 } else {
00106 BoardController::blinkLED((1<<LED_WALK), 4, 300);
00107 }
00108
00109
00110
00111 actionReturn->state.x = this->currentPos.x;
00112 actionReturn->state.y = this->currentPos.y;
00113
00114 BoardController::powerOnLED((1<<LED_WALK), false);
00115
00116
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
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
00166 if (action != UP && action != DOWN && action != LEFT && action != RIGHT) return false;
00167
00168
00169 if (pos->x < 0 || pos->x >= this->discretization.x ||
00170 pos->y < 0 || pos->y >= this->discretization.y)
00171 return false;
00172
00173
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