/*
* Updates the fields of current motor and sets the Heading
* of its pins
*/
void MotorController::init()
{
// Attach motors
leftMotor.attach(LEFT_MOTOR_IN_A_PIN, LEFT_MOTOR_IN_B_PIN, LEFT_MOTOR_PWM_PIN);
rightMotor.attach(RIGHT_MOTOR_IN_A_PIN, RIGHT_MOTOR_IN_B_PIN, RIGHT_MOTOR_PWM_PIN);
// Attach encoders
leftEncoder.attach(LEFT_ENCODER_INT_PIN, LEFT_ENCODER_DIGITAL_PIN);
rightEncoder.attach(RIGHT_ENCODER_INT_PIN, RIGHT_ENCODER_DIGITAL_PIN);
// Consider the starting point of the robot
// as its origin
_position.setX(0);
_position.setY(0);
// Suppose robot is heading along Y axis
_heading = 90;
// Initialize the number of commands to 0
_commandsCount = 0;
_processingNewCommand = true;
_leftMotorPosition = 0;
_leftMotorLastPosition = 0;
_rightMotorPosition = 0;
_rightMotorLastPosition = 0;
_leftMotorSpeed = 0;
_rightMotorSpeed = 0;
_leftMotorPWM = 0;
_rightMotorPWM = 0;
// Set vacuum pump pin as output
pinMode(VACUUM_PIN, OUTPUT);
stopVacuum();
}
