Example 1 with Motor
use of lego.lib.Motor in project jop by jop-devel.
the class Demo2 method main.
/**
* @param args
*/
public static void main(String[] args) {
new RtThread(10, 1 * 100) {
public void run() {
up = true;
flag = false;
value = 10;
speed = 0x400;
while (true) {
if (state == STATE_ALL) {
counter++;
if ((counter % value) == 0) {
if (flag) {
flag = false;
} else {
flag = true;
}
}
counter1++;
if ((counter1 % speed) == 0) {
if (speaker_up) {
value++;
if (value >= 50) {
speaker_up = false;
value--;
}
} else {
value--;
if (value <= 10) {
speaker_up = true;
value++;
}
}
}
} else {
flag = false;
}
if (flag) {
Speaker.write(true);
} else
Speaker.write(false);
waitForNextPeriod();
}
}
};
new RtThread(10, 100 * 1000) {
public void run() {
val = LED0;
up = true;
while (true) {
if (state == STATE_ALL) {
if (up) {
switch(val) {
case LED0:
val = LED1;
break;
case LED1:
val = LED2;
break;
case LED2:
val = LED3;
break;
case LED3:
{
up = false;
val = LED2;
break;
}
default:
val = LED0;
break;
}
} else {
switch(val) {
case LED0:
{
up = true;
val = LED1;
break;
}
case LED1:
val = LED0;
break;
case LED2:
val = LED1;
break;
default:
val = LED0;
break;
}
}
} else if (state != STATE_LINEFOLLOWER) {
val = 0;
}
FutureUse.writePins(val);
waitForNextPeriod();
}
}
};
new RtThread(10, 50 * 1000) {
public void run() {
int forwardCount = 0;
System.out.println("Main thread ready.");
MOTORS = new Motor[] { new Motor(0), new Motor(1) };
freeValue = 0;
stop = true;
while (true) {
for (int i = 0; i < 4; i++) if (Buttons.getButton(i)) {
state = i + 1;
//System.out.println(state);
Leds.setLed(1, (i & 1) != 0);
Leds.setLed(2, (i & 2) != 0);
break;
}
stop = !DigitalInputs.getDigitalInput(2);
if ((Buttons.getButtons() != 0) || stop)
freeValue = Sensors.readSensor(IR_SENSOR);
if (stop)
for (int i = 0; i < 2; i++) MOTORS[i].setState(Motor.STATE_OFF);
switch(state) {
case STATE_OFF:
{
for (int i = 0; i < 2; i++) MOTORS[i].setState(Motor.STATE_OFF);
break;
}
case STATE_LINEFOLLOWER:
{
if (!stop) {
int val = Sensors.readSensor(IR_SENSOR);
//System.out.println(val);
// XXX
boolean black = val > 285;
//boolean black = val > freeValue - 5;
MOTORS[MOTOR_LEFT].setDutyCyclePercentage(60);
MOTORS[MOTOR_RIGHT].setDutyCyclePercentage(60);
if (black) {
MOTORS[MOTOR_RIGHT].setState(Motor.STATE_FORWARD);
MOTORS[MOTOR_LEFT].setState(Motor.STATE_BRAKE);
FutureUse.writePins(LED0 | LED3);
} else {
MOTORS[MOTOR_LEFT].setState(Motor.STATE_FORWARD);
MOTORS[MOTOR_RIGHT].setState(Motor.STATE_BRAKE);
FutureUse.writePins(LED1 | LED2);
}
}
break;
}
case STATE_TOUCHSENSOR:
case STATE_IRSENSOR:
case STATE_ALL:
{
if (!stop) {
for (int i = 0; i < 2; i++) MOTORS[i].setMotorPercentage(Motor.STATE_FORWARD, true, 40);
int totalDifference = Sensors.readSensor(IR_SENSOR) - freeValue;
if (forwardCount >= 60) {
if (MOTORS[0].readNormalizedBackEMF()[1] <= 5) {
Leds.setLeds(0x9);
turnback(false);
}
}
if (((state == STATE_IRSENSOR) || (state == STATE_ALL)) && (Math.abs(totalDifference) >= IR_SENSOR_THRESHOLD)) {
turnback(true);
} else if ((state == STATE_TOUCHSENSOR || state == STATE_ALL) && (forwardCount >= 6) && (DigitalInputs.getDigitalInput(1))) {
turnback(false);
}
}
break;
}
}
{
boolean goingForward = false;
for (int i = 0; i < 2; i++) {
if (MOTORS[i].getState() == Motor.STATE_FORWARD) {
goingForward = true;
break;
}
}
forwardCount = goingForward ? forwardCount + 1 : 0;
}
waitForNextPeriod();
}
}
void turnback(boolean fast) {
int turnspeed, turnsleep;
boolean dirLeft = false;
if (fast) {
turnspeed = 80;
turnsleep = 300;
} else {
turnspeed = 70;
turnsleep = 400;
}
speed = !fast ? 0x400 : 0x50;
dirLeft = (Native.rd(Const.IO_US_CNT) & 1) != 0;
for (int i = 0; i < 2; i++) MOTORS[i].setMotorPercentage(Motor.STATE_BACKWARD, false, 100);
RtThread.sleepMs(100);
for (int i = 0; i < 2; i++) MOTORS[i].setMotorPercentage(Motor.STATE_BACKWARD, false, turnspeed);
RtThread.sleepMs(turnsleep - 100);
MOTORS[dirLeft ? MOTOR_LEFT : MOTOR_RIGHT].setMotorPercentage(Motor.STATE_BACKWARD, false, 70);
MOTORS[dirLeft ? MOTOR_RIGHT : MOTOR_LEFT].setState(Motor.STATE_OFF);
RtThread.sleepMs(600);
speed = 0x400;
Leds.setLed(0, false);
Leds.setLed(3, false);
}
};
RtThread.startMission();
}Example 2 with Motor
use of lego.lib.Motor in project jop by jop-devel.
the class LegoBoardTest method main.
/**
* @param args
*/
public static void main(String[] args) {
System.out.println("Initializing.");
/* Motor.setMotor(0, Motor.STATE_FORWARD, true, Motor.MAX_DUTYCYCLE);
Motor.setMotor(1, Motor.STATE_FORWARD, true, Motor.MAX_DUTYCYCLE);
Motor.setMotor(2, Motor.STATE_FORWARD, true, Motor.MAX_DUTYCYCLE);
//Native.wr(-1 << 1, Motor.IO_OUTPUT_MOTOR[1]);
*/
if (KNIGHT_RIDER_DEMO) {
val = LED0;
up = true;
new RtThread(10, 100 * 1000) {
public void run() {
for (; ; ) {
knightRiderLoop();
waitForNextPeriod();
}
}
};
}
if (SPEAKER_DEMO) {
up = true;
flag = false;
value = 10;
new RtThread(10, 1 * 1000) {
public void run() {
for (; ; ) {
speakerLoop();
waitForNextPeriod();
}
}
};
}
new RtThread(10, 1000 * 1000) {
public void run() {
do {
System.out.println("New measurement...");
System.out.println();
if (FREEMEMORY) {
System.out.print("Free memory: ");
System.out.println(GC.freeMemory());
}
if (BUTTONS) {
for (int i = 0; i < 4; i++) {
// uncomment this to have fun with javac
//System.out.println("Button " + i + ": " + Buttons.getButton(i));
// uncomment this to have fun with JOP
//System.out.println("Button " + i + ": " + new Boolean(Buttons.getButton(i)));
System.out.print("Button ");
System.out.print(i);
System.out.print(": ");
System.out.println(Buttons.getButton(i) ? "Down" : "Up");
}
}
if (DIGITALINPUTS) {
for (int i = 0; i < 3; i++) {
System.out.print("Digital input ");
System.out.print(i);
System.out.print(": ");
System.out.println(DigitalInputs.getDigitalInput(i) ? "1" : "0");
}
//output.append("Digital input ").append(i).append(": ").append(DigitalInputs.getDigitalInput(i) ? "1" : "0").append("\n");
}
if (FUTUREUSE) {
System.out.print("Unknown input: 0x");
System.out.println(Integer.toHexString((FutureUse.readPins())));
}
if (LEDS) {
Leds.setLeds(-1);
}
if (MICROPHONE) {
System.out.print("Microphone: ");
System.out.println(Microphone.readMicrophone());
}
if (MOTORS) {
Motor.synchronizedReadBackEMF();
for (int i = 0; i < 2; i++) {
// MS: two times new to read the back EMF!!!
int[] backEMF = new Motor(i).getSynchronizedBackEMF();
System.out.print("Motor ");
System.out.print(i);
System.out.print(" back-emf measurement: ");
System.out.print(backEMF[0] - 0x100);
System.out.print(", ");
System.out.println(backEMF[1] - 0x100);
}
}
if (SENSORS) {
Sensors.synchronizedReadSensors();
for (int i = 0; i < 3; i++) {
System.out.print("Analog sensor ");
System.out.print(i);
System.out.print(" : ");
System.out.print(Sensors.getBufferedSensor(i));
System.out.print(" (");
System.out.print(Sensors.readSensorValueAsPercentage(i));
System.out.println("%)");
}
}
if (PLD_RAW_INPUT) {
System.out.print("PLD raw input: ");
System.out.println(Native.rd(Const.IO_LEGO + 7));
}
System.out.println();
System.out.println("Measurement finished.");
if (!REPEAT)
break;
waitForNextPeriod();
} while (true);
}
};
RtThread.startMission();
System.out.println("Started.");
}