int main(int argc, char** argv)
{
Motor* motor = new Motor(0);
Encoder* encoder = new Encoder(motor);
PIDController* pidController = new PIDController(motor, encoder, POSITION_REV, 1, 0, 0);
pidController->setSetpoint(10);
pidController->enable();
Motor* motor2 = new Motor(1);
Encoder* encoder2 = new Encoder(motor2);
PIDController* pidController2 = new PIDController(motor2, encoder2, SPEED, 0, 0, 0, .4/20);
pidController2->setSetpoint(20);
pidController2->enable();
int ticks = 0;
while(!pidController->onTarget())
{
pidController->update();
pidController2->update();
std::cout << "tick:\t" << ++ticks << "\tcurrent position:\t" << encoder->getPosition() << "\tcurrent speed:\t" << encoder->getSpeed() << "\n";
std::cout << "tick:\t" << ticks << "\tcurrent speed:\t" << encoder2->getSpeed() << "\n";
}
std::cout << "done with position " << encoder->getPosition() << "\n";
return 0;
}
/*
* motionType parameter:
* 1 - move forward
* 2 - move backward
* 3 - turn left
* 4 - turn right
*/
void MotorController::updatePositionAndHeading(int motionType)
{
double leftMotorPositionDelta = leftEncoder.getPosition() - _leftMotorLastPosition;
double rightMotorPositionDelta = rightEncoder.getPosition() - _rightMotorLastPosition;
_leftMotorLastPosition = leftEncoder.getPosition();
_rightMotorLastPosition = rightEncoder.getPosition();
double deltaPosition = (leftMotorPositionDelta + rightMotorPositionDelta) / 2;
if (motionType == 1 || motionType == 2)
{
_position.setX(_position.getX() + deltaPosition * COS[round(_heading)]);
_position.setY(_position.getY() + deltaPosition * SIN[round(_heading)]);
// If robot turns slightly to left when moving forward or backward
// if (leftMotorPositionDelta < rightMotorPositionDelta)
// {
// double average = rightMotorPositionDelta - leftMotorPositionDelta;
// double radians = average / ROTATION_CIRCUMFERENCE;
// double degrees = radians * RAD_TO_DEG;
// // _heading = motionType == 1 ? _heading + degrees : _heading - degrees;
// }
// else
// {
// double average = leftMotorPositionDelta - rightMotorPositionDelta;
// double radians = average / ROTATION_CIRCUMFERENCE;
// double degrees = radians * RAD_TO_DEG;
// // _heading = motionType == 1 ? _heading - degrees : _heading + degrees;
// }
}
else
{
// Robot turns left
if (motionType == 3)
{
double average = (rightMotorPositionDelta - leftMotorPositionDelta) / 2;
double radians = average / ROTATION_RADIUS;
double degrees = radians * RAD_TO_DEG;
_heading += degrees;
}
else
{
double average = (leftMotorPositionDelta - rightMotorPositionDelta) / 2;
double radians = average / ROTATION_RADIUS;
double degrees = radians * RAD_TO_DEG;
_heading -= degrees;
}
}
_heading = _heading < 0 ? 360 + _heading : _heading;
_heading = _heading > 360 ? _heading - 360 : _heading;
}
/*
* Move the robot backward and update the value of the
* move backward command which happens to be the first
* in the commands list, reason for being called
*/
void MotorController::doMoveBackward()
{
fuzzyMove(-1, -1);
_leftMotorPosition += _leftMotorLastPosition - leftEncoder.getPosition();
_rightMotorPosition += _rightMotorLastPosition - rightEncoder.getPosition();
updatePositionAndHeading(2);
double average = (_leftMotorPosition + _rightMotorPosition) / 2;
if (average >= _commands.getFirstValue())
{
_commands.updateFirstValue(0);
}
}
/*
* Move the robot forward and update the value of the
* move forward command which happens to be the first
* in the commands list, reason for being called
*/
void MotorController::doMoveForward()
{
fuzzyMove(1, 1);
_leftMotorPosition += leftEncoder.getPosition() - _leftMotorLastPosition;
_rightMotorPosition += rightEncoder.getPosition() - _rightMotorLastPosition;
updatePositionAndHeading(1);
double average = (_leftMotorPosition + _rightMotorPosition) / 2;
if (average >= _commands.getFirstValue())
{
_commands.updateFirstValue(0);
}
}
/*
* Turn the robot to the right and update the value of the
* turn right command which happens to be the first
* in the commands list, reason for being called
*/
void MotorController::doTurnRight()
{
// When moving in place half down the speed of the motors
// as they compose because they're moving in different Headings
fuzzyMove(1, -1);
_leftMotorPosition += leftEncoder.getPosition() - _leftMotorLastPosition;
_rightMotorPosition += rightEncoder.getPosition() - _rightMotorLastPosition;
updatePositionAndHeading(4);
double average = (_leftMotorPosition - _rightMotorPosition) / 2;
double radians = average / ROTATION_RADIUS;
double degrees = radians * RAD_TO_DEG;
if (degrees >= _commands.getFirstValue())
{
_commands.updateFirstValue(0);
}
}