currently i am working with Lejos 0.8.5 on a robot balancing itself (like a segway).
The problem i am experiencing is that sometimes the motors suddenly stop working for about half a second (looks like they just don't do anything in this time). To me it looks like this happens in "extreme" oscilating situations. Meaning the motors power is set alternately to a high postive and then to a high negative value (abs(power) may be 100 nearly all the time). After having stopped they continue like nothing happened.
Here is - i am sorry for that - a poorly made video of what happens:
http://euve19772.vserver.de/misc/nxt_abrupt_stops.wmv
I am using a Hitechnic gyrosensor in combination with a PD-Controller implementation to calculate the power given to the motors setPower-command.
- Code: Select all
import lejos.nxt.Motor;
import lejos.nxt.MotorPort;
import lejos.nxt.SensorPort;
import lejos.nxt.Sound;
import lejos.nxt.addon.GyroSensor;
public class Main {
// Define the ports used
private static final SensorPort PORT_GYROSENSOR = SensorPort.S1;
private static final MotorPort PORT_LEFTMOTOR = MotorPort.B;
private static final MotorPort PORT_RIGHTMOTOR = MotorPort.C;
// Define the details of the gyrosensors offset measurement
private static final int GYRO_OFFSET_SUM_ITERATIONS = 10;
private static final int GYRO_OFFSET_SUM_TIME_TO_WAIT = 100;
// Define the factors for the PD-Controller
// Currently they are taken from a NXC project: http://nxt.meinblock.eu/projektdateien/segway/segway_programm.nxc
// Obviously these values have to be adjusted
private static final float GA_FACTOR = -1/16F; // -1/16F
private static final float GV_FACTOR = -5/8F; // -5/8F
private static final float TA_FACTOR = -1/4F; // -1/4F
private static final float TV_FAKTOR = -25/2F; // -25/2F
private static final int CONSTANT_POWER = 46; // 46
private static final int TIME_TO_WAIT = 5; // 5;
private static GyroSensor myGyrosensor = new GyroSensor(PORT_GYROSENSOR);
private static Motor myLeftMotor = new Motor(PORT_LEFTMOTOR);
private static Motor myRightMotor = new Motor(PORT_RIGHTMOTOR);
/**
* Balancing data class
*
* Its only purpose is to store the data
*/
private static class BalancingData {
BalancingData() {
// Measure offset
this.gyrosensorOffset = getGyroOffset();
this.tachoAnglePrevious = 0;
}
int gyroAngle;
int gyroAngularVelocity;
int tachoAngle;
int tachoAngularVelocity;
int tachoAnglePrevious;
int gyrosensorOffset;
};
/**
* Get the offset by measuring the gyrosensors value multiple times while the NXT is not moved.
* @return Measured offset
*/
private static int getGyroOffset() {
myGyrosensor.readValue();
int offsetSum = 0;
for (int i = 0; i < GYRO_OFFSET_SUM_ITERATIONS; i++) {
offsetSum += myGyrosensor.readValue();
try {
Thread.sleep(GYRO_OFFSET_SUM_TIME_TO_WAIT);
} catch (InterruptedException e) {
System.out.println("INT WHILE MEASURE OF GO");
Sound.beep();
break;
}
}
int gyrosensorOffset = offsetSum / GYRO_OFFSET_SUM_ITERATIONS;
return gyrosensorOffset;
}
/**
* Calculates the power required to balance the NXT
* @param Balancing data
* @return Motor power required to balance the NXT
*/
private static int motorLeistungErmitteln(BalancingData d) {
int enginePower;
// Gyro angular velocity
d.gyroAngularVelocity = myGyrosensor.readValue() - d.gyrosensorOffset;
// Gyro angle
d.gyroAngle += d.gyroAngularVelocity;
// Tachometer angle
d.tachoAngle = (myLeftMotor.getTachoCount() + myRightMotor.getTachoCount()) / 2;
// Tachometer angular velocity
d.tachoAngularVelocity = d.tachoAngle - d.tachoAnglePrevious;
d.tachoAnglePrevious = d.tachoAngle;
// Calculate power (PD-Controller)
enginePower = (int) ( d.gyroAngle * GA_FACTOR
+ d.gyroAngularVelocity * GV_FACTOR
+ d.tachoAngle * TA_FACTOR
+ d.tachoAngularVelocity * TV_FAKTOR);
// Add a constant value to calculated power
enginePower += Math.signum(enginePower) * CONSTANT_POWER;
return enginePower;
}
public static void main(String[] args) {
BalancingData meinSegway = new BalancingData();
int enginePower;
// Disable motors speed regulation
myLeftMotor.regulateSpeed(false);
myRightMotor.regulateSpeed(false);
// Disable motors smooth accerlation
myLeftMotor.smoothAcceleration(false);
myRightMotor.smoothAcceleration(false);
// Enable motors
myLeftMotor.forward();
myRightMotor.forward();
// Variables used to 'smooth' power
//int current = 0;
//int previous = 0;
while(true) {
enginePower = motorLeistungErmitteln(meinSegway);
/*
// Smooth power
current = enginePower > 100 ? 100 : enginePower;
current = enginePower < -100 ? -100 : enginePower;
enginePower = (1*current+3*previous)/4;
previous = enginePower;
*/
myLeftMotor.setPower(enginePower);
myRightMotor.setPower(enginePower);
// Print out high power values
if(Math.abs(enginePower)>100)
{
System.out.println("L: " +enginePower);
}
// Wait in order to regulate how fast the NXTs reacts
try {
Thread.sleep(TIME_TO_WAIT);
} catch (InterruptedException e) {
System.out.println("INT WHILE WAIT");
Sound.beep();
break;
}
}
// This should never be reached!
}
}
So my questions are:
Is there something like a software/hardware motor mechanism that stops the motor as some kind of protection (I couldn't find anything in the Lejos docs/code so far)? Or do i miss anything else here? I spent plenty of hours to figure out what the problem is but failed so far.
Is there any segway-like project for lejos published which uses only the motors and one gyrosensor? So far i only found "Gelway", but couldn't compile its code. Another project was balancing bot, but this seems tho use two gyrosensors. The only working project i found so far, was written in NXC (http://nxt.meinblock.eu/projektdateien/ ... ogramm.nxc), but controlling the motor seems to be pretty much different in NXC.
I appreciate any hints
