| |
Salut Philippe, hi everybody,
"Philippe Hurbain" <philohome@free.fr> writes:
> >
> > I am sorry if this is a repeat question. Did anyone successfully stabilize
> > the Legway using standard Lego light sensors ? Any links or proofs, code,
> > etc ? Thanks
>
> Have a look to YAL... http://news.lugnet.com/robotics/?n=22181
that was the straw that finally made me program LejWay,
a Lejos version. The hardware is the same with two sensors,
and the program is loosely based on yours. Tweaking was not easy,
but with a little luck the program below now manages to stand on
a sheet of white paper for long periods of time (several minutes).
I'm in a well-lit room and it's been standing in front of me while
I was typing, until it got some unexpected shade, I think.
It runs on NiMH and currently shows 7.7V.
import josx.platform.rcx.*;
public class LejWay
implements SensorConstants, LCDConstants // [sic]
{
public static void main (String[] args)
throws InterruptedException
{
int kd = 230;
int kp = 180;
int ki = 26;
int scale = 3000;
int pidMax = 255;
int decayInt = 6; // decay percentage of I per period
int tDelta = 16; // period in ms
Sensor s1 = Sensor.S1;
s1.setTypeAndMode (SENSOR_TYPE_RAW, SENSOR_MODE_RAW);
s1.activate();
Sensor s3 = Sensor.S3;
s3.setTypeAndMode (SENSOR_TYPE_RAW, SENSOR_MODE_RAW);
s3.activate();
Thread.sleep(20);
int offset = 0;
int errorInt = 0;
int errorOld = 0;
Thread.sleep (200);
int tNext = (int)System.currentTimeMillis() + tDelta;
while (Button.RUN.isPressed()) {
}
while (!Button.RUN.isPressed()) {
int error = s1.readRawValue() - s3.readRawValue() - offset;
int errorDiff = error - errorOld;
errorInt += error - errorInt * decayInt / 100;
errorOld = error;
int pid = (kd * errorDiff + kp * error + ki * errorInt) / scale;
LCD.setNumber (LCD_SIGNED, pid, LCD_DECIMAL_0);
LCD.refresh();
boolean forward = pid >= 0;
pid = forward ? pid : -pid;
if (pid > pidMax) {
pid = pidMax;
}
int tOn = pid * (tDelta-2) / pidMax;
int power = 7;
Motor.A.setPower (power);
Motor.C.setPower (power);
if (forward) {
Motor.A.forward();
Motor.C.forward();
} else {
Motor.A.backward();
Motor.C.backward();
}
int tEnd = (int)System.currentTimeMillis() + tOn;
while ((int)System.currentTimeMillis() < tEnd) {
Thread.yield();
}
Motor.A.stop();
Motor.C.stop();
while ((int)System.currentTimeMillis() < tNext) {
Thread.yield();
}
tNext += tDelta;
}
Motor.A.stop();
Motor.C.stop();
s1.passivate();
s3.passivate();
TextLCD.print ("stop");
Thread.sleep(300);
while (Button.RUN.isPressed()) {
}
Motor.A.flt();
Motor.C.flt();
}
}
Have fun
Jürgen
--
Jürgen Stuber <stuber@loria.fr>
http://www.loria.fr/~stuber/
|
|
Message has 1 Reply:
 | | Re: LejWay (Re: Legway with Standard Lego Light Sensors)
|
| Bonjour Jürgen, (...) I just tried LejWay, it works fine! (it was also the opportunity to install Lejos under BricxCC. Works great too...). The behaviour is somewhat different from BrickOS version, less shaky but it falls if tilt angle is too high. (...) (20 years ago, 17-Mar-04, to lugnet.robotics)
|
Message is in Reply To:
18 Messages in This Thread:
 
 
- Entire Thread on One Page:
- Nested:
All | Brief | Compact | Dots
Linear:
All | Brief | Compact
This Message and its Replies on One Page:
- Nested:
All | Brief | Compact | Dots
Linear:
All | Brief | Compact
|
|
|
|
