Hi
I build the following wire guidance sensor succesfully
http://www.philohome.com/sensors/fil...uide-schem.gif
I removed the D4,D5,D6 part and put a pullup to +5V on the collector of Q1
This is giving me a fine DC voltage according to the position of the wire to the 2 coils
So far so good
Can anyone help me to convert the following LEGO NQC code to PBP ?
I use HPWM 1 for the left motor, HPWM 2 for the right motor on a 16F877
ADCin 0 has the wire guidance sensor as input (which is the wire sensor)
Portd.0 is the fwd/rev Hbridge control for motor 1 (Left)
Portd.1 is the fwd/rev Hbridge control for motor 2 (Right)
Here is the NQC code
#define Wireguide SENSOR_1
#define Left OUT_A
#define Right OUT_C
#define Threshold1 10
#define Threshold2 25
#define HysterVal 8
task main ()
{
int Offset;
int Hyster=0;
int Diff;
// The wire guidance sensor is used
// the same way as a Light sensor
SetSensor (Wireguide, SENSOR_LIGHT);
// Reads the "no-signal" response. The sensor must
// be centered on the wire, or generator be off
Offset = Wireguide;
Wait(200);
// Starts driving
OnFwd (Left+Right);
while (true)
{
// Reads error signal
Diff = Wireguide - Offset - Hyster;
if (Diff >= -Threshold1 && Diff <= Threshold1)
// We are centered - drive straight
{
OnFwd (Left+Right);
Hyster = 0;
}
else
{
if (Diff > 0)
{
if(Diff > Threshold2)
// Very off center: turn in place
{
OnRev (Left);
OnFwd (Right);
}
else
{
// Slightly off center: shallow turn
Off (Left);
OnFwd (Right);
}
// sets hysteresis to avoid over-turning
Hyster = HysterVal;
}
else
{
if(Diff < -Threshold2)
{
OnRev (Right);
OnFwd (Left);
}
else
{
Off (Right);
OnFwd (Left);
}
Hyster = -HysterVal;
}
}
}
}
Bookmarks