//////////////////////////////////////////////////////////////////////////////// //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. */ }