////////////////////////////////////////////////////////////////////////////////
//1. check robot stability
//2. get robot's direction
////////////////////////////////////////////////////////////////////////////////
void RobotPrepare(void) {
//1. make robot stop
typeWheelSpeed speed;
typeMPUSensor mpuAccel;
u8 retryTimes=0;
static typeCoordinate startCoordinate, endCoordinate;
float diffX, diffY;
float angle;
MotorRunEnable(true);
speed.left = 0;
speed.right = 0;
SetWheelGivenSpeed(speed);
speed = GetWheelSpeed();
while ((speed.left > 1) || (speed.right > 1)); {
vTaskDelay(50);
speed = GetWheelSpeed();
}
MotorRunEnable(false);
//2. read MPU accel sensor, check robot's stalibility
mpuAccel.accelX = 20;
mpuAccel.accelY = 20;
while ( retryTimes< ROBOT_STABLE_RETRY_THR ){
mpuAccel = ReadMPUSensor();
if ((FloatAbs(mpuAccel.accelX)<ROBOT_STABLE_ACCEL_X_THR) && (FloatAbs(mpuAccel.accelY)<ROBOT_STABLE_ACCEL_X_THR)){
retryTimes++;
}
else {
retryTimes = 0;
}
vTaskDelay(50);
}
//3. rotate to find x min compass
startCoordinate = GetCoordinate();
MotorRunEnable(true);
speed.left = 10;
speed.right = 10;
SetWheelGivenSpeed(speed);
vTaskDelay(4000);
speed.left = 0;
speed.right = 0;
SetWheelGivenSpeed(speed);
endCoordinate = GetCoordinate();
diffX = endCoordinate.x - startCoordinate.x;
diffY = endCoordinate.y - startCoordinate.y;
angle =(180/3.14)*acos(diffX / sqrt(diffY*diffY + diffX*diffX));
if (diffY < 0) {
angle = 0-angle;
}
SetRobotAngle(angle);
//4.
}
//0 - 360 degree
float GetLineDirectionX(float x0, float y0, float x1, float y1) {
float angle;
float eLong;
float comX, comY;
comX = x1-x0;
comY = y1-y0;
eLong = sqrt(comX*comX + comY*comY);
angle = (180/M_PI) * acos(FloatAbs(comX)/eLong); //0 to 90 degree
if ((comX>=0) && (comY>=0)) { //1
angle = angle;
}
if ((comX<=0) && (comY>=0)) { //2
angle = 180 - angle;
}
if ((comX<=0) && (comY<=0)) { //3
angle = 180 + angle;
}
if ((comX>=0) && (comY<=0)) { //4
angle = 360 - angle;
}
return angle;
}
int whichSide(float x, float y){
typeCoordinate start = GetCoordinate();
// typeCoordinate start = getNowPos();
// float lineDir = GetLineDirection(start.x, start.y, x, y);
// float robotDir = CalibrateNorth2_Y();
float lineDir = GetLineDirectionX(start.x, start.y, x, y);
float robotDir = CalibrateNorth2X();
float turnangle = lineDir - robotDir;
if (turnangle < -180) turnangle += 360;
if (turnangle > 180) turnangle -= 360;
if(FloatAbs(turnangle) <= 25){
if(turnangle > 0)
return 1; //right side
if(turnangle < 0)
return -1; //left side
}
if(FloatAbs(turnangle) > 25){
ControlRobotRotate(turnangle, MAX_ROTATE_SPEED);
}
return 0; // on the line
}
void rotateTo(float x, float y, float speed,int flag) {
typeCoordinate start = GetCoordinate();
// float lineDir = GetLineDirection(start.x, start.y, x, y);
// float robotDir = CalibrateNorth2_Y();
float lineDir = GetLineDirectionX(start.x, start.y, x, y);
float robotDir = CalibrateNorth2X();
float turnangle = lineDir - robotDir;
if (turnangle < -180) turnangle += 360 ;
if (turnangle > 180) turnangle -= 360;
if(flag == ROTATE_LARGER_15)
{
if(FloatAbs(turnangle) <= 15)
return;
}
if(flag == ROTATE_LARGER_25) //used for runStraight
{
if(FloatAbs(turnangle) <= 25)
return;
}
ControlRobotRotate(turnangle, speed);
}
///////////////////////////////
/////////////////////////////////////////////////
//angle: range from -180 to 180
////////////////////////////////////////////////////////////////////////////////
void ControlRobotRotate(float angle, float speed) {
float givenAngle;
float timeInterval;
if (FloatAbs(speed) > MAX_ROTATE_SPEED) {
if (speed<0) {
speed = 0 - MAX_ROTATE_SPEED;
}
else {
speed = MAX_ROTATE_SPEED;
}
}
if (angle<0) {
givenAngle = -angle/180*PI;
SetLeftWheelGivenSpeed(speed);
SetRightWheelGivenSpeed(0-speed);
}
else {
givenAngle = angle/180*PI;
SetLeftWheelGivenSpeed(0-speed);
SetRightWheelGivenSpeed(speed);
}
//timeInterval = 1000*givenAngle*(WHEEL_L_R_DISTANCE)/(2*speed); //(L = n*PI*r/180)
//vTaskDelay((unsigned long)timeInterval);
//////////////////////////////////////////////////////////////////////////////
//1��=0.0174532922222222����
//d=60mm
//1��Ƕ��൱��������0.534070742mm
//�������ÿ����Ӧ��������570.4113257715211������, 4��Ƶ
//1��=1218.56������
//��1��2�ȵ�����
//////////////////////////////////////////////////////////////////////////////
uint32_t angle2Pulse;
extern int64_t pulseAccL, pulseAccR;
angle2Pulse = (uint32_t)(angle*121856/100); //����������121856/100��������ݣ���������ڲ������
pulseAccL = 0;
pulseAccR = 0;
while(pulseAccL < angle2Pulse) {
vTaskDelay(5);
}
SetLeftWheelGivenSpeed(0);
SetRightWheelGivenSpeed(0);
}
float r2rAngle(float lineDir, float myDir){
float turnangle = lineDir - myDir;
if (turnangle < -180) turnangle += 360;
if (turnangle > 180) turnangle -= 360;
return FloatAbs(turnangle);
}
bool RobotInRange(float x0, float y0, float x1, float y1) {
if ((FloatAbs(x1-x0)>0.03f) || (FloatAbs(y1-y0)>0.03f)){
return false;
}
return true;
}
/******************************************************************************
* Examples:
* a: Jim Jim Jim
* other: Mary Mary Mary
* in: [happy-for Jim Mary] [sorry-for J. M.] [gloating J. M.]
* (positive-emotion) (negative-emotion) (positive-emotion)
* attitude: like-human like-human like-human
* weight: WEIGHT_DEFAULT WEIGHT_DEFAULT -WEIGHT_DEFAULT
* o.e.c.: positive-emotion negative-emotion negative-emotion
* att: [like-human J. M. WD] [like-human J. M. WD] [like-human J. M.
* -WD]
*
* todo: Generalize this to work for all the related like/love attitudes as well
* as friends/enemies relations.
* The intensity rules are more complex: something like
* value of actor2's emotion = value of actor1's emotion x
* value of how much actor2 likes actor1.
******************************************************************************/
void UA_Emotion_FortunesOfOthers(Actor *ac, Ts *ts, Obj *a, Obj *in,
Obj *other, Float weight,
Obj *other_emot_class)
{
int found;
Float weight1;
Obj *other_emot_class1;
ObjList *causes, *objs, *atts, *p, *q;
/* Relate <a>'s emotion to <a>'s attitudes. */
if (0.0 != (weight1 = UA_FriendAttitude(ts, a, other, 1, &atts))) {
if (FloatSign(weight1) == FloatSign(weight)) {
/* The input emotion agrees with known attitudes. */
ContextSetRSN(ac->cx, RELEVANCE_TOTAL, SENSE_TOTAL, NOVELTY_HALF);
ContextAddMakeSenseReasons(ac->cx, atts);
} else {
/* The input emotion disagrees with known attitudes. */
ContextSetRSN(ac->cx, RELEVANCE_TOTAL, SENSE_LITTLE, NOVELTY_TOTAL);
ContextAddNotMakeSenseReasons(ac->cx, atts);
}
} else {
/* Attitude of <a> toward <other> is unknown. */
ContextSetRSN(ac->cx, RELEVANCE_TOTAL, SENSE_MOSTLY, NOVELTY_MOSTLY);
UA_Infer(ac->cx->dc, ac->cx, ts,
L(N("like-human"), a, other, NumberToObj(weight), E), in);
}
/* Relate <a>'s emotion to <other>'s emotion.
* found = <other>'s emotion implied by <a>'s emotion is already known,
* excluding motivating emotions.
*/
objs = RD(ts, L(other_emot_class, other, E), 0);
found = 0;
for (p = objs; p; p = p->next) {
if (ISA(N("motivation"), I(p->obj, 0))) continue;
if ((causes = CAUSES(p->obj, ac->cx))) {
for (q = causes; q; q = q->next) {
if (!ISA(N("active-goal"), I(q->obj, 0))) {
found = 1;
ContextSetRSN(ac->cx, RELEVANCE_TOTAL, SENSE_TOTAL, NOVELTY_HALF);
ContextAddMakeSenseReason(ac->cx, q->obj);
}
}
} else {
found = 1;
ContextSetRSN(ac->cx, RELEVANCE_TOTAL, SENSE_TOTAL, NOVELTY_HALF);
ContextAddMakeSenseReason(ac->cx, p->obj);
}
ObjListFree(causes);
}
ObjListFree(objs);
if (!found) {
/* <other>'s emotion implied by <a>'s emotion is not yet known. */
ContextSetRSN(ac->cx, RELEVANCE_TOTAL, SENSE_MOSTLY, NOVELTY_MOSTLY);
if (other_emot_class == N("positive-emotion")) {
other_emot_class1 = N("happiness");
} else if (other_emot_class == N("negative-emotion")) {
other_emot_class1 = N("sadness");
} else {
other_emot_class1 = other_emot_class;
}
UA_Infer(ac->cx->dc, ac->cx, ts,
L(other_emot_class1, other, NumberToObj(FloatAbs(weight)), E),
in);
}
/* todo: Relate <a>'s emotion to <other>'s goal. */
}
