private void SetSpeed(GTI.PWMOutput motor, GTI.DigitalOutput direction, int speed, bool isLeft)
{
if (speed == 0)
{
direction.Write(false);
motor.Set(FEZCerbot.MOTOR_BASE_FREQUENCY, 0.01);
}
else if (speed < 0)
{
direction.Write(isLeft ? true : false);
motor.Set(FEZCerbot.MOTOR_BASE_FREQUENCY, speed / -100.0);
}
else
{
direction.Write(isLeft ? false : true);
motor.Set(FEZCerbot.MOTOR_BASE_FREQUENCY, speed / 100.0);
}
}
/// <summary>
/// Sets PWM to the tone frequency and starts it.
/// </summary>
/// <param name="tone"></param>
private void SetTone(Tone tone)
{
tunePWM.Active = false;
if (tone.freq == 0)
{
tunePWM.Active = false;
return;
}
tunePWM.Set((int)tone.freq, 0.5);
tunePWM.Active = true;
}
/// <summary></summary>
/// <param name="socketNumber">The socket that this module is plugged in to.</param>
public MotorDriverL298(int socketNumber)
{
this.Frequency = 25000;
Socket socket = Socket.GetSocket(socketNumber, true, this, null);
socket.EnsureTypeIsSupported(new char[] { 'P' }, this);
m_Pwm1 = new GTI.PWMOutput(socket, Socket.Pin.Seven, false, this);
m_Pwm2 = new GTI.PWMOutput(socket, Socket.Pin.Eight, false, this);
m_Direction1 = new GTI.DigitalOutput(socket, Socket.Pin.Nine, false, this);
m_Direction2 = new GTI.DigitalOutput(socket, Socket.Pin.Six, false, this);
m_Pwm1.Set(Frequency, 0);
m_Pwm2.Set(Frequency, 0);
}
/// <summary>
/// Used to set a motor's speed.
/// <param name="_motorSide">The motor <see cref="Motor"/> you are setting the speed for.</param>
/// <param name="_newSpeed"> The new speed that you want to set the current motor to.</param>
/// </summary>
public void MoveMotor(Motor _motorSide, int _newSpeed)
{
// Make sure the speed is within an acceptable range.
if (_newSpeed > 100 || _newSpeed < -100)
{
new ArgumentException("New motor speed outside the acceptable range (-100-100)", "_newSpeed");
}
//////////////////////////////////////////////////////////////////////////////////
// Motor1
//////////////////////////////////////////////////////////////////////////////////
if (_motorSide == Motor.Motor2)
{
// Determine the direction we are going to go.
if (_newSpeed == 0)
{
//if (m_lastSpeed1 == 0)
m_Direction1.Write(false);
m_Pwm1.Set(Frequency, 0.01);
}
else if (_newSpeed < 0)
{
// Set direction and power.
m_Direction1.Write(true);
/////////////////////////////////////////////////////////////////////////////
// Quick fix for current PWM issue
double fix = (double)((100 - System.Math.Abs(_newSpeed)) / 100.0);
if (fix >= 1.0)
{
fix = 0.99;
}
if (fix <= 0.0)
{
fix = 0.01;
}
/////////////////////////////////////////////////////////////////////////////
m_Pwm1.Set(Frequency, fix);
}
else
{
// Set direction and power.
m_Direction1.Write(false);
/////////////////////////////////////////////////////////////////////////////
// Quick fix for current PWM issue
double fix = (double)(_newSpeed / 100.0);
if (fix >= 1.0)
{
fix = 0.99;
}
if (fix <= 0.0)
{
fix = 0.01;
}
/////////////////////////////////////////////////////////////////////////////
m_Pwm1.Set(Frequency, fix);
}
// Save our speed
m_lastSpeed1 = _newSpeed;
}
//////////////////////////////////////////////////////////////////////////////////
// Motor2
//////////////////////////////////////////////////////////////////////////////////
else
{
// Determine the direction we are going to go.
if (_newSpeed == 0)
{
//if( m_lastSpeed2 == 0)
m_Direction2.Write(false);
m_Pwm2.Set(Frequency, 0.01);
}
else if (_newSpeed < 0)
{
// Set direction and power.
m_Direction2.Write(true);
/////////////////////////////////////////////////////////////////////////////
// Quick fix for current PWM issue
double fix = (double)((100 - System.Math.Abs(_newSpeed)) / 100.0);
if (fix >= 1.0)
{
fix = 0.99;
}
if (fix <= 0.0)
{
fix = 0.01;
}
/////////////////////////////////////////////////////////////////////////////
m_Pwm2.Set(Frequency, fix);
}
else
{
// Set direction and power.
m_Direction2.Write(false);
/////////////////////////////////////////////////////////////////////////////
// Quick fix for current PWM issue
double fix = (double)(_newSpeed / 100.0);
if (fix >= 1.0)
{
fix = 0.99;
}
if (fix <= 0.0)
{
fix = 0.01;
}
/////////////////////////////////////////////////////////////////////////////
m_Pwm2.Set(Frequency, fix);
}
// Save our speed
m_lastSpeed2 = _newSpeed;
}
//////////////////////////////////////////////////////////////////////////////////
}