void CompassController::Move()
{
double speed, wheel_Direction;
// normal compass drive stuff here
speed = CalculateSpeed();
wheel_Direction = CalculateWheelDirection();
// adjust the front of the robot to where the nose should be
double y2 = m_controller.GetRawAxis(4) * -1, x2 = m_controller.GetRawAxis(3);
double rotation = 0;
// if we're asked to point in a particular direction, do it
if (fabs(__hypot(x2, y2)) > 0.25)
{
m_noseDirection = (atan2(y2, x2) * (180/M_PI) - 90.0 );
}
else if (m_spinEvent.DoEvent())
{
// use the buttons to spin around
// idea: it may be a useful idea to eventually drift back
// to the 'real' nose position if the person lets go of the
// buttons, so the bot doesn't just keep turning unexpectedly
double big = m_bigButtonAngle;
double small = m_smallButtonAngle;
// go left
if (m_controller.GetRawButton(7))
m_noseDirection += big;
// go right
if (m_controller.GetRawButton(8))
m_noseDirection -= big;
// go left a tiny bit
if (m_controller.GetRawButton(5))
m_noseDirection += small;
// go right a tiny bit
if (m_controller.GetRawButton(6))
m_noseDirection -= small;
}
rotation = m_nosePointer.GetRotation(m_noseDirection);
m_driveController->Move(speed, wheel_Direction, rotation, m_controller.GetRawButton(1));
}
/*
Calculates the parameters for an individual wheel
Set as inline function for speed
*/
static inline
void CalculateWheel(
double &magnitude, double &angle,
double Vtx, double Vty, double w,
double Rx, double Ry)
{
double Vx = Vtx - w*Ry;
double Vy = Vty + w*Rx;
// return as polar coordinates
magnitude = __hypot(Vx, Vy);
angle = (atan2(Vy, Vx)*180)/M_PI - 90.0;
angle = fmod(angle, 360);
if (angle < 0) angle = angle + 360;
}
void SwerveDrive::Stop()
{
double y = m_stick.GetY() * -1, x = m_stick.GetX();
double speed = __hypot(x, y);
double angle = (atan2(y, x) * (180/M_PI) - 90.0 );
double lf_angle, lr_angle, rf_angle, rr_angle;
if (fabs(speed) < 0.00001)
{
// pick a default if the driver isn't pointing
lf_angle = 45;
lr_angle = 315;
rf_angle = 135;
rr_angle = 225;
}
else
{
// translate the direction the driver is pointing
angle = PositionInformation::GetInstance()->TranslateFieldToRobotAngle(angle) - 90;
lf_angle = lr_angle = rf_angle = rr_angle = angle;
}
// find the shortest path to that spot, and do it
ShortestPath(speed, lf_angle, m_chassis->servo_lf.GetCurrentAngle(), m_pick_alt_lf, m_pickEvent_lf);
ShortestPath(speed, lr_angle, m_chassis->servo_lr.GetCurrentAngle(), m_pick_alt_lr, m_pickEvent_lr);
ShortestPath(speed, rf_angle, m_chassis->servo_rf.GetCurrentAngle(), m_pick_alt_rf, m_pickEvent_rf);
ShortestPath(speed, rr_angle, m_chassis->servo_rr.GetCurrentAngle(), m_pick_alt_rr, m_pickEvent_rr);
m_chassis->servo_lf.SetAngle(lf_angle);
m_chassis->servo_lr.SetAngle(lr_angle);
m_chassis->servo_rf.SetAngle(rf_angle);
m_chassis->servo_rr.SetAngle(rr_angle);
m_chassis->motor_lf.SetSpeed(0);
m_chassis->motor_lr.SetSpeed(0);
m_chassis->motor_rf.SetSpeed(0);
m_chassis->motor_rr.SetSpeed(0);
}
double CompassController::CalculateSpeed()
{
return __hypot(m_controller.GetX(), m_controller.GetY()*-1);
}
double
__cabs (double _Complex z)
{
return __hypot (__real__ z, __imag__ z);
}
double hypot( double arg1, double arg2) {
return __hypot( arg1, arg2 );
}
