float squareroot(float x)
{
const float epsilon= .00001;
float guess = 1.0;
while(absolutevalue (guess * guess - x) >= epsilon)
guess = ( x / guess + guess ) / 2.0;
return guess;
}
int main(void)
{
float f1=-23.5,f2=65.0,f3=-8.0,result;
int i1=-150;
result=absolutevalue(f1);
printf("Result = %.2f\n",result);
printf("f1=%.2f\n",f1);
result=absolutevalue(f2)+absolutevalue(f3);
printf("Result =%.2f\n",result);
result=absolutevalue((float)i1);
printf("Result = %.2f\n",result);
result=absolutevalue(i1);
printf("Result = %.2f\n",result);
printf("%.2f\n",absolutevalue(-6.0)/4);
return 0;
}
bool EntropyDrive::DriveRobotTrig(float MoveValue, float RotateValue) {
float LeftMotors = 0;
float RightMotors = 0;
float OutsideWheels = 0;
float InsideWheels = 0;
float Hypot = 0;
float AbsMoveValue = 0;
float AbsRotateValue = 0;
float CompMoveValuePlus=0.99;
float CompMoveValueMinus=0.60;
float CompRotateValuePlus=0.99;
float CompRotateValueMinus=0.99;
//Normalize Joystick inputs
if (MoveValue >= 0.0)
{
MoveValue=MoveValue/CompMoveValuePlus;
}
else
{
MoveValue=MoveValue/CompMoveValueMinus;
}
if (RotateValue>=0.0)
{
RotateValue=RotateValue/CompRotateValuePlus;
}
else
{
RotateValue=RotateValue/CompRotateValueMinus;
}
MoveValue=Limit(MoveValue);
RotateValue=Limit(RotateValue);
AbsMoveValue=absolutevalue(MoveValue);
AbsRotateValue= absolutevalue(RotateValue);
//Theta = atanf(AbsMoveValue/(AbsRotateValue+0.000001));
//Theta = asinf(0.2);
Hypot = sqrt(AbsMoveValue*AbsMoveValue+AbsRotateValue*AbsRotateValue);
OutsideWheels = AbsMoveValue*(AbsMoveValue/Hypot);
InsideWheels = AbsMoveValue*( 1- (AbsRotateValue/Hypot));
//Scale Motor inputs
if (RotateValue<=0.0)
{
LeftMotors = InsideWheels * MoveValue/AbsMoveValue;
RightMotors = OutsideWheels * MoveValue/AbsMoveValue;
}
else
{
LeftMotors = OutsideWheels * MoveValue/AbsMoveValue;
RightMotors = InsideWheels * MoveValue/AbsMoveValue;
}
//Command motors
wpiDrive->SetLeftRightMotorOutputs( -1*LeftMotors,-1*RightMotors );
return true;
}
