/*
* 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();
}
void setup() {
Serial.begin(9600);
Serial.println("Serial communication established");
/*
SERVO_FRONT.attach(SERVO_FRONT_PIN);
SERVO_BACK.attach(SERVO_BACK_PIN);
SERVO_GRAB.attach(SERVO_GRAB_PIN);
*/
m1.attach(M1_EN_PIN, M1_INA_PIN, M1_INB_PIN);
m2.attach(M2_EN_PIN, M2_INA_PIN, M2_INB_PIN);
//roomba.start();
//roomba.safeMode();
//roomba.fullMode();
}
