7 #include "RobotDrive.h"
10 #include "GenericHID.h"
13 #include "NetworkCommunication/UsageReporting.h"
15 #include "WPIErrors.h"
18 #define max(x, y) (((x) > (y)) ? (x) : (y))
20 const int32_t RobotDrive::kMaxNumberOfMotors;
35 m_frontLeftMotor = NULL;
36 m_frontRightMotor = NULL;
37 m_rearRightMotor = NULL;
38 m_rearLeftMotor = NULL;
55 m_rearLeftMotor =
new Jaguar(leftMotorChannel);
56 m_rearRightMotor =
new Jaguar(rightMotorChannel);
57 for (int32_t i=0; i < kMaxNumberOfMotors; i++)
59 m_invertedMotors[i] = 1;
62 m_deleteSpeedControllers =
true;
76 uint32_t frontRightMotor, uint32_t rearRightMotor)
79 m_rearLeftMotor =
new Jaguar(rearLeftMotor);
80 m_rearRightMotor =
new Jaguar(rearRightMotor);
81 m_frontLeftMotor =
new Jaguar(frontLeftMotor);
82 m_frontRightMotor =
new Jaguar(frontRightMotor);
83 for (int32_t i=0; i < kMaxNumberOfMotors; i++)
85 m_invertedMotors[i] = 1;
88 m_deleteSpeedControllers =
true;
102 if (leftMotor == NULL || rightMotor == NULL)
104 wpi_setWPIError(NullParameter);
105 m_rearLeftMotor = m_rearRightMotor = NULL;
108 m_rearLeftMotor = leftMotor;
109 m_rearRightMotor = rightMotor;
110 for (int32_t i=0; i < kMaxNumberOfMotors; i++)
112 m_invertedMotors[i] = 1;
114 m_deleteSpeedControllers =
false;
120 m_rearLeftMotor = &leftMotor;
121 m_rearRightMotor = &rightMotor;
122 for (int32_t i=0; i < kMaxNumberOfMotors; i++)
124 m_invertedMotors[i] = 1;
126 m_deleteSpeedControllers =
false;
141 if (frontLeftMotor == NULL || rearLeftMotor == NULL || frontRightMotor == NULL || rearRightMotor == NULL)
143 wpi_setWPIError(NullParameter);
146 m_frontLeftMotor = frontLeftMotor;
147 m_rearLeftMotor = rearLeftMotor;
148 m_frontRightMotor = frontRightMotor;
149 m_rearRightMotor = rearRightMotor;
150 for (int32_t i=0; i < kMaxNumberOfMotors; i++)
152 m_invertedMotors[i] = 1;
154 m_deleteSpeedControllers =
false;
161 m_frontLeftMotor = &frontLeftMotor;
162 m_rearLeftMotor = &rearLeftMotor;
163 m_frontRightMotor = &frontRightMotor;
164 m_rearRightMotor = &rearRightMotor;
165 for (int32_t i=0; i < kMaxNumberOfMotors; i++)
167 m_invertedMotors[i] = 1;
169 m_deleteSpeedControllers =
false;
178 if (m_deleteSpeedControllers)
180 delete m_frontLeftMotor;
181 delete m_rearLeftMotor;
182 delete m_frontRightMotor;
183 delete m_rearRightMotor;
185 delete m_safetyHelper;
202 float leftOutput, rightOutput;
203 static bool reported =
false;
206 nUsageReporting::report(nUsageReporting::kResourceType_RobotDrive, GetNumMotors(), nUsageReporting::kRobotDrive_ArcadeRatioCurve);
212 float value = log(-curve);
213 float ratio = (value - m_sensitivity)/(value + m_sensitivity);
214 if (ratio == 0) ratio =.0000000001;
215 leftOutput = outputMagnitude / ratio;
216 rightOutput = outputMagnitude;
220 float value = log(curve);
221 float ratio = (value - m_sensitivity)/(value + m_sensitivity);
222 if (ratio == 0) ratio =.0000000001;
223 leftOutput = outputMagnitude;
224 rightOutput = outputMagnitude / ratio;
228 leftOutput = outputMagnitude;
229 rightOutput = outputMagnitude;
243 if (leftStick == NULL || rightStick == NULL)
245 wpi_setWPIError(NullParameter);
248 TankDrive(leftStick->GetY(), rightStick->GetY(), squaredInputs);
253 TankDrive(leftStick.GetY(), rightStick.GetY(), squaredInputs);
266 GenericHID *rightStick, uint32_t rightAxis,
bool squaredInputs)
268 if (leftStick == NULL || rightStick == NULL)
270 wpi_setWPIError(NullParameter);
273 TankDrive(leftStick->GetRawAxis(leftAxis), rightStick->GetRawAxis(rightAxis), squaredInputs);
277 GenericHID &rightStick, uint32_t rightAxis,
bool squaredInputs)
279 TankDrive(leftStick.GetRawAxis(leftAxis), rightStick.GetRawAxis(rightAxis), squaredInputs);
291 static bool reported =
false;
294 nUsageReporting::report(nUsageReporting::kResourceType_RobotDrive, GetNumMotors(), nUsageReporting::kRobotDrive_Tank);
299 leftValue =
Limit(leftValue);
300 rightValue =
Limit(rightValue);
303 if (leftValue >= 0.0)
305 leftValue = (leftValue * leftValue);
309 leftValue = -(leftValue * leftValue);
311 if (rightValue >= 0.0)
313 rightValue = (rightValue * rightValue);
317 rightValue = -(rightValue * rightValue);
336 ArcadeDrive(stick->GetY(), stick->GetX(), squaredInputs);
351 ArcadeDrive(stick.GetY(), stick.GetX(), squaredInputs);
368 float moveValue = moveStick->GetRawAxis(moveAxis);
369 float rotateValue = rotateStick->GetRawAxis(rotateAxis);
371 ArcadeDrive(moveValue, rotateValue, squaredInputs);
389 float moveValue = moveStick.GetRawAxis(moveAxis);
390 float rotateValue = rotateStick.GetRawAxis(rotateAxis);
392 ArcadeDrive(moveValue, rotateValue, squaredInputs);
404 static bool reported =
false;
407 nUsageReporting::report(nUsageReporting::kResourceType_RobotDrive, GetNumMotors(), nUsageReporting::kRobotDrive_ArcadeStandard);
412 float leftMotorOutput;
413 float rightMotorOutput;
415 moveValue =
Limit(moveValue);
416 rotateValue =
Limit(rotateValue);
421 if (moveValue >= 0.0)
423 moveValue = (moveValue * moveValue);
427 moveValue = -(moveValue * moveValue);
429 if (rotateValue >= 0.0)
431 rotateValue = (rotateValue * rotateValue);
435 rotateValue = -(rotateValue * rotateValue);
441 if (rotateValue > 0.0)
443 leftMotorOutput = moveValue - rotateValue;
444 rightMotorOutput = max(moveValue, rotateValue);
448 leftMotorOutput = max(moveValue, -rotateValue);
449 rightMotorOutput = moveValue + rotateValue;
454 if (rotateValue > 0.0)
456 leftMotorOutput = - max(-moveValue, rotateValue);
457 rightMotorOutput = moveValue + rotateValue;
461 leftMotorOutput = moveValue - rotateValue;
462 rightMotorOutput = - max(-moveValue, -rotateValue);
486 static bool reported =
false;
489 nUsageReporting::report(nUsageReporting::kResourceType_RobotDrive, GetNumMotors(), nUsageReporting::kRobotDrive_MecanumCartesian);
500 double wheelSpeeds[kMaxNumberOfMotors];
501 wheelSpeeds[kFrontLeftMotor] = xIn + yIn + rotation;
502 wheelSpeeds[kFrontRightMotor] = -xIn + yIn - rotation;
503 wheelSpeeds[kRearLeftMotor] = -xIn + yIn + rotation;
504 wheelSpeeds[kRearRightMotor] = xIn + yIn - rotation;
508 uint8_t syncGroup = 0x80;
510 m_frontLeftMotor->
Set(wheelSpeeds[kFrontLeftMotor] * m_invertedMotors[kFrontLeftMotor] * m_maxOutput, syncGroup);
511 m_frontRightMotor->
Set(wheelSpeeds[kFrontRightMotor] * m_invertedMotors[kFrontRightMotor] * m_maxOutput, syncGroup);
512 m_rearLeftMotor->
Set(wheelSpeeds[kRearLeftMotor] * m_invertedMotors[kRearLeftMotor] * m_maxOutput, syncGroup);
513 m_rearRightMotor->
Set(wheelSpeeds[kRearRightMotor] * m_invertedMotors[kRearRightMotor] * m_maxOutput, syncGroup);
517 m_safetyHelper->Feed();
535 static bool reported =
false;
538 nUsageReporting::report(nUsageReporting::kResourceType_RobotDrive, GetNumMotors(), nUsageReporting::kRobotDrive_MecanumPolar);
543 magnitude =
Limit(magnitude) * sqrt(2.0);
545 double dirInRad = (direction + 45.0) * 3.14159 / 180.0;
546 double cosD = cos(dirInRad);
547 double sinD = sin(dirInRad);
549 double wheelSpeeds[kMaxNumberOfMotors];
550 wheelSpeeds[kFrontLeftMotor] = sinD * magnitude + rotation;
551 wheelSpeeds[kFrontRightMotor] = cosD * magnitude - rotation;
552 wheelSpeeds[kRearLeftMotor] = cosD * magnitude + rotation;
553 wheelSpeeds[kRearRightMotor] = sinD * magnitude - rotation;
557 uint8_t syncGroup = 0x80;
559 m_frontLeftMotor->
Set(wheelSpeeds[kFrontLeftMotor] * m_invertedMotors[kFrontLeftMotor] * m_maxOutput, syncGroup);
560 m_frontRightMotor->
Set(wheelSpeeds[kFrontRightMotor] * m_invertedMotors[kFrontRightMotor] * m_maxOutput, syncGroup);
561 m_rearLeftMotor->
Set(wheelSpeeds[kRearLeftMotor] * m_invertedMotors[kRearLeftMotor] * m_maxOutput, syncGroup);
562 m_rearRightMotor->
Set(wheelSpeeds[kRearRightMotor] * m_invertedMotors[kRearRightMotor] * m_maxOutput, syncGroup);
566 m_safetyHelper->Feed();
594 wpi_assert(m_rearLeftMotor != NULL && m_rearRightMotor != NULL);
596 uint8_t syncGroup = 0x80;
598 if (m_frontLeftMotor != NULL)
599 m_frontLeftMotor->
Set(
Limit(leftOutput) * m_invertedMotors[kFrontLeftMotor] * m_maxOutput, syncGroup);
600 m_rearLeftMotor->
Set(
Limit(leftOutput) * m_invertedMotors[kRearLeftMotor] * m_maxOutput, syncGroup);
602 if (m_frontRightMotor != NULL)
603 m_frontRightMotor->
Set(-
Limit(rightOutput) * m_invertedMotors[kFrontRightMotor] * m_maxOutput, syncGroup);
604 m_rearRightMotor->
Set(-
Limit(rightOutput) * m_invertedMotors[kRearRightMotor] * m_maxOutput, syncGroup);
608 m_safetyHelper->Feed();
632 double maxMagnitude = fabs(wheelSpeeds[0]);
634 for (i=1; i<kMaxNumberOfMotors; i++)
636 double temp = fabs(wheelSpeeds[i]);
637 if (maxMagnitude < temp) maxMagnitude = temp;
639 if (maxMagnitude > 1.0)
641 for (i=0; i<kMaxNumberOfMotors; i++)
643 wheelSpeeds[i] = wheelSpeeds[i] / maxMagnitude;
653 double cosA = cos(angle * (3.14159 / 180.0));
654 double sinA = sin(angle * (3.14159 / 180.0));
655 double xOut = x * cosA - y * sinA;
656 double yOut = x * sinA + y * cosA;
669 void RobotDrive::SetInvertedMotor(MotorType motor,
bool isInverted)
671 if (motor < 0 || motor > 3)
673 wpi_setWPIError(InvalidMotorIndex);
676 m_invertedMotors[motor] = isInverted ? -1 : 1;
687 m_sensitivity = sensitivity;
696 m_maxOutput = maxOutput;
701 void RobotDrive::SetExpiration(
float timeout)
703 m_safetyHelper->SetExpiration(timeout);
706 float RobotDrive::GetExpiration()
711 bool RobotDrive::IsAlive()
713 return m_safetyHelper->
IsAlive();
716 bool RobotDrive::IsSafetyEnabled()
721 void RobotDrive::SetSafetyEnabled(
bool enabled)
726 void RobotDrive::GetDescription(
char *desc)
728 sprintf(desc,
"RobotDrive");
731 void RobotDrive::StopMotor()
733 if (m_frontLeftMotor != NULL) m_frontLeftMotor->
Disable();
734 if (m_frontRightMotor != NULL) m_frontRightMotor->
Disable();
735 if (m_rearLeftMotor != NULL) m_rearLeftMotor->
Disable();
736 if (m_rearRightMotor != NULL) m_rearRightMotor->
Disable();
void MecanumDrive_Polar(float magnitude, float direction, float rotation)
void Drive(float outputMagnitude, float curve)
void RotateVector(double &x, double &y, double angle)
virtual void Set(float speed, uint8_t syncGroup=0)=0
void SetSafetyEnabled(bool enabled)
void TankDrive(GenericHID *leftStick, GenericHID *rightStick, bool squaredInputs=true)
virtual void SetLeftRightMotorOutputs(float leftOutput, float rightOutput)
void HolonomicDrive(float magnitude, float direction, float rotation)
void Normalize(double *wheelSpeeds)
void ArcadeDrive(GenericHID *stick, bool squaredInputs=true)
RobotDrive(uint32_t leftMotorChannel, uint32_t rightMotorChannel)
void SetSensitivity(float sensitivity)
void SetMaxOutput(double maxOutput)
void MecanumDrive_Cartesian(float x, float y, float rotation, float gyroAngle=0.0)
static void UpdateSyncGroup(uint8_t syncGroup)