void UIJoypad::RecalcDigitalPosition()
{
needRecalcDigital = false;
if(!currentPos.x && !currentPos.y)
{
digitalVector.x = 0;
digitalVector.y = 0;
return;
}
float ang = GetStickAngle();
if(ang > PI/8 && ang < PI - PI/8)
{
digitalVector.x = 1.0f;
}
else if(ang < PI*2 - PI/8 && ang > PI + PI/8)
{
digitalVector.x = -1.0f;
}
else
{
digitalVector.x = 0;
}
if(ang < PI/2 - PI/8 || ang > PI*2 - PI/2 + PI/8)
{
digitalVector.y = -1.0f;
}
else if(ang < PI*2 - PI/2 - PI/8 && ang > PI/2 + PI/8)
{
digitalVector.y = 1.0f;
}
else
{
digitalVector.y = 0;
}
// Logger::Info("x = %f, y = %f", digitalVector.x, digitalVector.y);
}
void ArcadeDriver::Drive2() {
float oneMotorAngle=60.0;
float backwardsActiveAngle=60.0;
float backwardsOneMotorAngle=45.0;
float angle;
float speed;
float tolerance=0.15;
float x=m_Stick->GetRawAxis(1);
float y=m_Stick->GetRawAxis(2);
float leftSpeed = 0.0;
float rightSpeed = 0.0;
if (fabs(x)< tolerance)
x=0.0;
if (fabs(y)< tolerance)
y=0.0;
angle=GetStickAngle();
speed=float (sqrt(double (x*x)+ double (y*y)));
if (angle<oneMotorAngle) {
//Move forward to the left
//Right Motor forward
//Left Motor forward then to stop
leftSpeed = -speed*(oneMotorAngle - angle)/oneMotorAngle; //LeftMotor Forward is (-); LeftMotor backward is (+)
rightSpeed = speed; // RightMotor forward is (+); RightMotor backward is (-)
} else if (angle<=90.0) {
leftSpeed = speed*(angle-oneMotorAngle)/(90.0-oneMotorAngle);
rightSpeed = speed;
} else if (angle<180.0-backwardsActiveAngle) {
leftSpeed = 0.0;
rightSpeed = 0.0;
} else if (angle<180.0-backwardsOneMotorAngle) {
leftSpeed = 0.0;
rightSpeed = -speed;
} else if (angle<180.0) {
leftSpeed = speed*(angle-(180.0-backwardsOneMotorAngle))/backwardsOneMotorAngle;
rightSpeed = -speed;
} else if (angle<180.0+backwardsOneMotorAngle) {
leftSpeed = speed;
rightSpeed = -speed*(180.0+backwardsOneMotorAngle-angle)/backwardsOneMotorAngle;
} else if (angle<180.0+backwardsActiveAngle) {
leftSpeed = speed;
rightSpeed = 0.0;
} else if (angle<270.0) {
leftSpeed = 0.0;
rightSpeed = 0.0;
} else if (angle<360-oneMotorAngle) {
leftSpeed = -speed;
rightSpeed = -speed*((360.0-oneMotorAngle)-angle)/(90.0-oneMotorAngle);
} else {
leftSpeed = -speed;
rightSpeed = speed*(angle-(360.0-oneMotorAngle))/oneMotorAngle;
}
if (m_Stick->GetRawAxis(3) > 0.8)
{
//Pull left trigger, get slower and more precise movement
leftSpeed /= 3;
rightSpeed /= 3;
}
m_LeftMotor->SetSpeed(leftSpeed); //Left motor inverted since first writing this function
m_RightMotor->SetSpeed(rightSpeed * -1.0);
}
