public AutomaticDrive(MotorController motorController,
ServoController servoController,
DistanceMeasurementSensor distanceMeasurementSensor)
{
_motorController = motorController;
_servoController = servoController;
_distanceMeasurementSensor = distanceMeasurementSensor;
}
public Motor(MotorController motorController, int motorNumber)
{
MotorController = motorController;
MotorNumber = motorNumber;
int motorPwmPin = 0, motorIn2Pin = 0, motorIn1Pin = 0;
if (motorNumber == 0)
{
motorPwmPin = 8;
motorIn1Pin = 9;
motorIn2Pin = 10;
}
else if (motorNumber == 1)
{
motorPwmPin = 13;
motorIn1Pin = 12;
motorIn2Pin = 11;
}
else if (motorNumber == 2)
{
motorPwmPin = 2;
motorIn1Pin = 3;
motorIn2Pin = 4;
}
else if (motorNumber == 3)
{
motorPwmPin = 7;
motorIn1Pin = 6;
motorIn2Pin = 5;
}
else
{
throw new Exception("Motor must be between 1 and 4 inclusive");
}
MotorPwmPin = motorPwmPin;
MotorIn1Pin = motorIn2Pin;
MotorIn2Pin = motorIn1Pin;
}
public void Run(MotorAction command)
{
if (MotorController == null)
{
return;
}
if (command == MotorAction.FORWARD)
{
MotorController.SetPin(MotorIn2Pin, 0);
MotorController.SetPin(MotorIn1Pin, 1);
}
else if (command == MotorAction.BACKWARD)
{
MotorController.SetPin(MotorIn1Pin, 0);
MotorController.SetPin(MotorIn2Pin, 1);
}
else if (command == MotorAction.RELEASE)
{
MotorController.SetPin(MotorIn1Pin, 0);
MotorController.SetPin(MotorIn2Pin, 0);
}
}
