const bool RobotAction::turnFaceToHuman() {
/* Request current human pose */
PerceptionMgr legRequest;
legRequest.sensing = legDetection;
sendPerception(legRequest);
Sleep(sizeof(legDetection));
this->buzyWaitForMgr(250);
PeopleMgr targetPos;
getPeople(targetPos);
// Find the possible candidate of the target, the nearest one
double possibleCandidateX = 0.0, possibleCandidateY = 0.0;
if (targetPos.count == 0) {
cout << "> WARNING: No Human Candidate!" << endl;
return 1;
} else if(targetPos.count > 1) {
/* Find the person who is nearest to the robot */
float minX = 0.0, minY = 0.0, squaredDistance = 0.0, minSquaredDistance = 999999.0;
for (int i = 0; i < targetPos.count; i++) {
squaredDistance = pow(targetPos.x[i],2) + pow(targetPos.y[i],2);
if (squaredDistance <= minSquaredDistance) {
minSquaredDistance = squaredDistance;
minX = targetPos.x[i];
minY = targetPos.y[i];
}
}
possibleCandidateX = minX / 100.0;
possibleCandidateY = minY / 100.0;
}
/* Prepare the angle to turn the head */
int goalDegree = -1 * static_cast< int >(atan2(possibleCandidateY, possibleCandidateX) / M_PI * 180.0);
if (abs(goalDegree) > 35) {
cout << "> WARNING: Head cannot turn to this angle, " << goalDegree << endl;
goalDegree = 35;
} else if (abs(goalDegree) < -35) {
cout << "> WARNING: Head cannot turn to this angle, " << goalDegree << endl;
goalDegree = -35;
}
cout << "> Turning Face To " << goalDegree << endl;
return this->turningFace(goalDegree);
}
int main(void){
// set for 16 MHz clock
CPU_PRESCALE(0);
usb_init();
//D0->4 Output Bits for Transistors to individual displays
DDRD = 0b01011111;
PORTD = 0b00000000;
//C0->7 Initially setting bits to input. Will set to output to turn on.
DDRC = 0b00000000;
PORTC = 0b00000000;
//DDRC = 0b11111111;
DDRF = 0b00000000;
PORTF = 0b11101010;
TCCR1B = 0b00000011;
TIFR1 = 0b00000001;
TIMSK1 = 0b00000001;
//sei();
//Turn on one of the columns for interupt purposes.
PORTF &= ~(1<<2); //Setting Low
DDRF |= (1<<2); //Set to Output
uint8_t peeps = 0;
uint16_t hourly = 0;
peeps = getPeople();
hourly = getHourly();
while (1) {
//.26 ticks per second, we'll say .25 for simplicity.
displayMultiF((float)peeps*(float)hourly*ticks/60.0f/60.0f/4.0f);
}
}
const bool RobotAction::movingToAroundOfHuman(const int& speed, const float& distance, const double& angle2FrontOfHuman) {
this->turnFaceToHuman();
Sleep(1000);
/* Request current robot pose */
MessageFreqMgr requestOdo;
sendMessageFreq(requestOdo);
Sleep(sizeof(requestOdo));
this->buzyWaitForMgr(250);
OdometryMgr curPos;
getOdometry(curPos);
/* Request current human pose */
PerceptionMgr legRequest;
legRequest.sensing = legDetection;
sendPerception(legRequest);
Sleep(sizeof(legDetection));
this->buzyWaitForMgr(250);
PeopleMgr targetPos;
getPeople(targetPos);
/* Request body direction */
PerceptionHAEMgr requestBody;
requestBody.sensing = bodyDirectionCont;
sendPerceptionHAE(requestBody);
Sleep(sizeof(requestBody));
this->buzyWaitForMgr(250);
HAEMgr receivedBodyDir;
getHAE(receivedBodyDir);
/* Prepare subgoal message */
// Find the possible candidate of the target, the nearest one
double possibleCandidateX = 0.0, possibleCandidateY = 0.0;
if (targetPos.count == 0) {
cout << "> WARNING: No Human Candidate!" << endl;
return 1;
} else if(targetPos.count > 1) {
/* Find the person who is nearest to the robot */
float minX = 0.0, minY = 0.0, squaredDistance = 0.0, minSquaredDistance = 999999.0;
for (int i = 0; i < targetPos.count; i++) {
squaredDistance = pow(targetPos.x[i],2) + pow(targetPos.y[i],2);
if (squaredDistance <= minSquaredDistance) {
minSquaredDistance = squaredDistance;
minX = targetPos.x[i];
minY = targetPos.y[i];
}
}
possibleCandidateX = minX / 100.0;
possibleCandidateY = minY / 100.0;
}
double dist_robot2human = sqrt(pow(possibleCandidateX / 100.0, 2) + pow(possibleCandidateY / 100.0, 2));
double theta_BD = (-1 * receivedBodyDir.body_direction_cont);
double theta_robotHeading = atan2(possibleCandidateY, possibleCandidateX) * 180 / M_PI;
/* Coordinate Transformation */
CoordTrans coordinateTrans;
// From human coordinate (B) to robot coordinate (R)
double X_g_H = distance * cos(angle2FrontOfHuman / 180 * M_PI), Y_g_H = distance * sin(angle2FrontOfHuman / 180 * M_PI), theta_g_H = 180.0 + angle2FrontOfHuman;
double X_g_R = 0.0, Y_g_R = 0.0, theta_g_R = 0.0;
coordinateTrans.trans2D(X_g_H, Y_g_H, theta_g_H,
dist_robot2human * cos(theta_BD / 180 * M_PI), dist_robot2human * sin(theta_BD / 180 * M_PI), 180.0 + theta_BD - theta_robotHeading,
X_g_R, Y_g_R, theta_g_R);
// From robot coordinate to world coordinate (W)
double X_g_W = 0.0, Y_g_W = 0.0, theta_g_W = 0.0;
coordinateTrans.trans2D(X_g_R, Y_g_R, theta_g_R,
curPos.x - 0.0, curPos.y - 0.0, -1 * curPos.theta,
X_g_W, Y_g_W, theta_g_W);
/* For visualization, used for debug */
// Transform the centor point of human
//double X_c_H = 0.0, Y_c_H = 0.0, Y_c_R, Y_c_W, X_c_R, X_c_W;
//coordinateTrans.trans2D(X_c_H, Y_c_H, theta_g_H,
// dist_robot2human * cos(theta_BD / 180 * M_PI), dist_robot2human * sin(theta_BD / 180 * M_PI), 180.0 + theta_BD - theta_robotHeading,
// X_c_R, Y_c_R, theta_g_R);
//coordinateTrans.trans2D(X_c_R, Y_c_R, theta_g_R,
// curPos.x - 0.0, curPos.y - 0.0, -1 * curPos.theta,
// X_c_W, Y_c_W, theta_g_W);
//int imgX_g = 0, imgY_g = 0, imgX_c = 0, imgY_c = 0;
//coordinateTrans.plane2Img(X_g_W, Y_g_W, imgX_g, imgY_g);
//coordinateTrans.plane2Img(X_c_W, Y_c_W, imgX_c, imgY_c);
//coordinateTrans.insertPoint(imgX_g, imgY_g);
//coordinateTrans.insertPoint(imgX_c, imgY_c);
//coordinateTrans.drawPoint();
SubgoalMgr goal;
goal.theta = theta_g_W;
goal.x = X_g_W;
goal.y = Y_g_W;
/* Print information for debug */
cout << "BodyDirection: " << receivedBodyDir.body_direction_cont << endl;
cout << "X_Goal: " << goal.x << ", Y_Goal: " << goal.y << ", Theta_Goal: " << goal.theta << endl;
/* Send subgoal message */
sendSubgoal(goal);
naviExecuting = true;
Sleep(sizeof(goal) + 500);
this->getCurrentTime();
this->turningFace(0);
Result_Navi resultNaviStatus;
getResultNavi(resultNaviStatus);
while (this->executingTime() <= timeout) {
if (naviExecuting == false || resultNaviStatus.current_status == FINISHED)
return true;
getResultNavi(resultNaviStatus);
Sleep(50);
}
// Timeout
cout << "> WARNING: Action, MovingToFrontOfHuman, timeout" << endl;
naviExecuting = false;
return false;
}
const bool RobotAction::forwardApproach(const int& speed, const double& distance) {
this->turningFace(0);
/* Request current robot pose */
MessageFreqMgr requestOdo;
sendMessageFreq(requestOdo);
Sleep(sizeof(requestOdo));
this->buzyWaitForMgr(500);
OdometryMgr curPos;
getOdometry(curPos);
/* Request current human pose */
PerceptionMgr legRequest;
legRequest.sensing = legDetection;
sendPerception(legRequest);
Sleep(sizeof(legDetection));
this->buzyWaitForMgr(500);
PeopleMgr targetPos;
getPeople(targetPos);
/* Prepare subgoal message */
if (targetPos.count == 0) {
cout << "> WARNING: No Human!" << endl;
return 1;
} else if(targetPos.count > 1) {
/* Find the person who is nearest to the robot */
float minX = 0.0, minY = 0.0, squaredDistance = 0.0, minSquaredDistance = 999999.0;
for (int i = 0; i < targetPos.count; i++) {
squaredDistance = pow(targetPos.x[i],2) + pow(targetPos.y[i],2);
if (squaredDistance <= minSquaredDistance) {
minSquaredDistance = squaredDistance;
minX = targetPos.x[i];
minY = targetPos.y[i];
}
}
targetPos.x[0] = minX;
targetPos.y[0] = minY;
}
double dX = (targetPos.x[0] / 100.0) - curPos.x;
double dY = (targetPos.y[0] / 100.0) - curPos.y;
if (sqrt(pow(dX, 2) + pow(dY, 2)) < 1.5) {
cout << "> WARNING: Too close to human!" << endl;
return false;
}
SubgoalMgr goal;
goal.theta = atan2(dY, dX) * 180 / M_PI;
goal.x = curPos.x + distance * cos(goal.theta / 180 * M_PI);
goal.y = curPos.y + distance * sin(goal.theta / 180 * M_PI);
cout << "Xr: " << curPos.x << ", Yr: " << curPos.y << endl;
cout << "Xh: " << targetPos.x[0] << ", Yh: " << targetPos.y[0] << endl;
cout << "Xg: " << goal.x << ", Yg: " << goal.y << endl;
/* Send subgoal message */
sendSubgoal(goal);
naviExecuting = true;
Sleep(sizeof(goal) + 500);
this->getCurrentTime();
Result_Navi resultNaviStatus;
getResultNavi(resultNaviStatus);
while (this->executingTime() <= timeout) {
if (naviExecuting == false || resultNaviStatus.current_status == FINISHED)
return true;
getResultNavi(resultNaviStatus);
Sleep(50);
}
// Timeout
cout << "> WARNING: Action, ForwardApproach, timeout" << endl;
naviExecuting = false;
return false;
}
