Hello,
i am doing a robot, with servo motor and ultrasonic sensor. But when i use the 2 at the same time, nothing work. Servo and US cant’ work together.
help plz.
us sensor : http://www.parallax.com/dl/docs/prod/acc/28015-PING-v1.3.pdf
my code :
//US method
public static int USdetect(TristatePort _pin)
{
const int TicksPerMicrosecond = (int)TimeSpan.TicksPerMillisecond / 1000;
_pin.Active = true;
_pin.Write(true);
Thread.Sleep(5);
_pin.Write(false);
_pin.Active = false;
bool high = false;
do {
high = _pin.Read();
}
while (!high);
long startTime = System.DateTime.Now.Ticks;
while (high) {
high = _pin.Read();
}
int ticks = (int)(System.DateTime.Now.Ticks - startTime)>>1;
int microSeconds = (ticks / TicksPerMicrosecond);
return (microSeconds / 29);}
//servo method
public static void ServoStop(PWM servoBase)
{
servoBase.Set(false);}
public static void ServoMove(float angle, PWM servoBase,bool precis)
{ float MinAngle = 0;
float MaxAngle = 180;
double MaxUs = 2.40;
if (angle > MaxAngle)
angle = MaxAngle;
if (angle < MinAngle)
angle = MinAngle;
if (precis == true && angle==0)
{for (int i=90; i >= angle; i--){
servoBase.SetPulse(20000000, (uint)((MaxUs - (0.01 * i)+0.02) * 1000000));
Thread.Sleep(10);}}
else if (precis == true && angle == 180)
{for (int i = 90; i <= angle; i++){
servoBase.SetPulse(20000000, (uint)((MaxUs - (0.0111 * i)) * 1000000));
Thread.Sleep(10);}}
else if (angle == 0)
{
servoBase.SetPulse(20000000, 2450000);
Thread.Sleep(1000);
ServoStop(servoBase);
}
else if (angle == 180)
{
servoBase.SetPulse(20000000, 500000);
Thread.Sleep(1000);
ServoStop(servoBase);
}
else
{
servoBase.SetPulse(20000000, (uint)((MaxUs - (0.0111 * angle)) * 1000000));
Thread.Sleep(1000);
ServoStop(servoBase);}}
public static void Main()
{
bool Play = true;
TristatePort _pin = new TristatePort((Cpu.Pin)FEZ_Pin.Digital.Di52, false, false, Port.ResistorMode.Disabled);
PWM servoBase = new PWM((PWM.Pin)FEZ_Pin.PWM.Di9);
while (Play==true)
{
USdetect(_pin);
ServoMove(180, servoBase, false);
Thread.Sleep(1000);
ServoMove(90, servoBase, false);
Thread.Sleep(1000);
}
}