//--------------------------------------------------------------------------------
bool EditLine::checkBallCollide(Ball *ball){
V3 lineLocalVec(x2-x1, y2-y1, 0);
//get the angle between the ball and one end of the wall
float angleCurrent1 = checkAngle(ball->x , ball->y , x1,y1, lineLocalVec);
float angleCurrent2 = checkAngle(ball->x , ball->y , x2,y2, lineLocalVec);
//lets get the angle between the ball and one end of the wall
float angleFuture1 = checkAngle(ball->x+ball->force.x, ball->y+ball->force.y ,x1,y1, lineLocalVec);
float angleFuture2 = checkAngle(ball->x+ball->force.x, ball->y+ball->force.y ,x2,y2, lineLocalVec);
if(diffSign(angleCurrent1,angleFuture1) && diffSign(angleCurrent2,angleFuture2)){
float d1x = ball->x - x1;
float d1y = ball->y - y1;
float d2x = ball->x - x2;
float d2y = ball->y - y2;
float wallLength = lineLocalVec.getLength();
if( (sqrt(d1x*d1x + d1y*d1y) < wallLength) && (sqrt(d2x*d2x + d2y*d2y) < wallLength)){
return true;
}
else return false;
}
else return false;
}
bool OperatorTrigonometric::executeAngle(double &value)
{
value = angleAdjust(value);
if (!checkAngle(value)) {
setErrMsg("Invalid trigonometric angle");
return false;
}
value = angle2radian(value);
return executeRadian(value);
}
bool OperatorTrigonometricArc::executeAngle(double &value)
{
if (!checkValue(value)) {
setErrMsg("Invalid arc trigonometric value");
return false;
}
if (!executeRadian(value)) {
return false;
}
value = radian2angle(value);
if (!checkAngle(value)) {
setErrMsg("Invalid trigonometric angle");
return false;
}
return true;
}
//returns true if p is inside the polygon and false otherwise
//pathological cases where p is on a vertex or edge are not
//explicitly handled and result in undefined behavior
bool isInside(const Point& p)
{
if(m_pts.size() < 2)
return false;
double angle = 0;
Point p1, p2;
for(size_t i = 0; i < m_pts.size(); ++i)
{
p1 = m_pts.at(i);
p1 -= p;
p2 = m_pts.at((i+1)%m_pts.size());
p2 -= p;
angle += checkAngle(p1.x(), p1.y(), p2.x(), p2.y());
}
if(fabs(angle) < PI)
return false;
else
return true;
}
void AIStateSearching::setPeriodAngles()
{
m_vSearchAngles.push_back(m_fEnterAngle);
int maxTrigger = 0;
for (int i = 1; i < m_vSearchPeriods.size(); i++)
{
float tempangle = static_cast <float> (rand()) / static_cast <float> (RAND_MAX / 360);
while (checkAngle(tempangle))
{
if (++maxTrigger > 1000)
break;
tempangle = static_cast <float> (rand()) / static_cast <float> (RAND_MAX / 360);
}
m_vSearchAngles.push_back(tempangle);
}
}
int main()
{
double *time;
double **angles;
int n;
double slope;
double intercept;
CMobot mobot;
angles = (double**)malloc(sizeof(double*)*4);
mobot.connect();
if(!mobot.isConnected()) {
ERRMSG("Connect failed");
return -1;
}
mobot.setJointSpeeds(45, 45, 45, 45);
mobot.resetToZero();
mobot.moveContinuousNB(ROBOT_FORWARD, ROBOT_HOLD, ROBOT_HOLD, ROBOT_BACKWARD);
mobot.recordAnglesBegin(time,
angles[0],
angles[1],
angles[2],
angles[3],
0.1);
#ifndef _WIN32
sleep(12);
#else
Sleep(12000);
#endif
printf("Sleep done.\n");
mobot.recordAnglesEnd(n);
printf("Ended Recording.\n");
mobot.stopAllJoints();
printf("Joints Stopped.\n");
/* Check end angle. */
double angle;
mobot.getJointAngle(ROBOT_JOINT1, angle);
if(ABS(angle - 45*12) > 20) {
ERRMSG("Mobot movement speed incorrect");
return -1;
}
/* Perform least squares regression */
double sum_y = 0;
double sum_y_2 = 0; // sum(y^2)
double sum_x = 0;
double sum_x_2 = 0;
double sum_xy = 0;
double angle_mean = 0;
int i;
for(i = 0; i < n; i++) {
sum_y += angles[0][i];
sum_y_2 += angles[0][i]*angles[0][i];
sum_x += time[i];
sum_x_2 += time[i] * time[i];
sum_xy += angles[0][i] * time[i];
angle_mean += angles[0][i]/n;
}
intercept = ((sum_y * sum_x_2) - (sum_x * sum_xy)) / (((double)n)*sum_x_2 - sum_x*sum_x);
slope = ((double)n * sum_xy - sum_x * sum_y)/ (((double)n)*sum_x_2 - sum_x*sum_x);
printf("Slope is %lf\n", slope);
if(checkAngle(slope, 45)) {
ERRMSG("Slope incorrect");
return -1;
}
/* Check the residual */
double SS_tot = 0;
double SS_err = 0;
double R_2 = 0;
double diff;
for(i = 0; i < n; i++) {
SS_tot += pow(angles[0][i] - angle_mean, 2);
SS_err += pow(angles[0][i] - (slope*time[i] + intercept), 2);
}
R_2 = 1 - SS_err/SS_tot;
printf("R squared: %lf\n", R_2);
if(R_2 < 0.97) {
ERRMSG("Recorded data too noisy");
}
/* Now do it again but faster */
mobot.setJointSpeeds(60, 60, 60, 60);
mobot.resetToZero();
mobot.moveContinuousNB(ROBOT_FORWARD, ROBOT_HOLD, ROBOT_HOLD, ROBOT_BACKWARD);
mobot.recordAnglesBegin(time,
angles[0],
angles[1],
angles[2],
angles[3],
0.1);
#ifndef _WIN32
sleep(12);
#else
Sleep(12);
#endif
printf("Sleep done.\n");
mobot.recordAnglesEnd(n);
printf("Ended Recording.\n");
mobot.stopAllJoints();
printf("Joints Stopped.\n");
/* Check end angle. */
mobot.getJointAngle(ROBOT_JOINT1, angle);
if(ABS(angle - 60*12) > 30) {
ERRMSG("Mobot movement speed incorrect");
return -1;
}
/* Perform least squares regression */
sum_y = 0;
sum_y_2 = 0; // sum(y^2)
sum_x = 0;
sum_x_2 = 0;
sum_xy = 0;
angle_mean = 0;
for(i = 0; i < n; i++) {
sum_y += angles[0][i];
sum_y_2 += angles[0][i]*angles[0][i];
sum_x += time[i];
sum_x_2 += time[i] * time[i];
sum_xy += angles[0][i] * time[i];
angle_mean += angles[0][i]/n;
}
intercept = ((sum_y * sum_x_2) - (sum_x * sum_xy)) / (((double)n)*sum_x_2 - sum_x*sum_x);
slope = ((double)n * sum_xy - sum_x * sum_y)/ (((double)n)*sum_x_2 - sum_x*sum_x);
printf("Slope is %lf\n", slope);
if(checkAngle(slope, 60)) {
ERRMSG("Slope incorrect");
return -1;
}
/* Check the residual */
SS_tot = 0;
SS_err = 0;
R_2 = 0;
for(i = 0; i < n; i++) {
SS_tot += pow(angles[0][i] - angle_mean, 2);
SS_err += pow(angles[0][i] - (slope*time[i] + intercept), 2);
}
R_2 = 1 - SS_err/SS_tot;
printf("R squared: %lf\n", R_2);
if(R_2 < 0.97) {
ERRMSG("Recorded data too noisy");
}
return 0;
}
int main()
{
double angles[4];
CMobot mobot;
mobot.connect();
if(!mobot.isConnected()) {
ERRMSG("Connect failed");
return -1;
}
mobot.disconnect();
mobot.connect();
if(!mobot.isConnected()) {
ERRMSG("Reconnect failed");
return -1;
}
mobot.setJointSpeeds(45, 45, 45, 45);
mobot.resetToZero();
/* Begin testing. First, rotate joint 2 */
mobot.moveJointToNB(ROBOT_JOINT2, -85);
/* This should take about 2 seconds */
#ifndef _WIN32
usleep(1000000);
#else
Sleep(1000);
#endif
if (!mobot.isMoving()) {
ERRMSG("Error: Robot should still be moving.");
return -1;
}
#ifndef _WIN32
usleep(1000000);
#else
Sleep(1000);
#endif
if (mobot.isMoving()) {
ERRMSG("Error: Robot did not reach joint angle in time.");
return -1;
}
mobot.getJointAngle(ROBOT_JOINT2, angles[0]);
if(checkAngle(angles[0], -85)) {
ERRMSG("Angle mismatch");
return -1;
}
/*Now rotate joint 3 */
mobot.moveJointTo(ROBOT_JOINT3, 70);
mobot.moveJointTo(ROBOT_JOINT1, 45);
mobot.moveJointTo(ROBOT_JOINT2, 20);
/* Check the joint angles */
mobot.getJointAngles(angles[0], angles[1], angles[2], angles[3]);
if(checkAngle(angles[0], 45)) {
ERRMSG("Angle Mismatch");
return -1;
}
if(checkAngle(angles[1], 20)) {
ERRMSG("Angle Mismatch");
return -1;
}
if(checkAngle(angles[2], 70)) {
ERRMSG("Angle Mismatch");
return -1;
}
if(checkAngle(angles[3], 0)) {
ERRMSG("Angle Mismatch");
return -1;
}
mobot.disconnect();
return 0;
}
