com.mytechia.robobo.rob.DefaultRob Maven / Gradle / Ivy
Go to download
Show more of this group Show more artifacts with this name
Show all versions of robobo-rob-interface Show documentation
Show all versions of robobo-rob-interface Show documentation
Control library and protocol for the Robobo-ROB robotic platform.
/*******************************************************************************
*
* Copyright 2016 Mytech Ingenieria Aplicada
* Copyright 2016 Julio Gómez
*
* This file is part of Robobo ROB Interface Library.
*
* Robobo ROB Interface Library is free software: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Robobo ROB Interface Library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with Robobo ROB Interface Library. If not, see obstacles = new ArrayList();
private final List bumps = new ArrayList();
private final List falls = new ArrayList();
private final List gaps = new ArrayList();
private final List irSensors = new ArrayList();
private final List leds = new ArrayList();
private MotorStatus panMotor, tiltMotor, leftMotor, rightMotor;
private WallConnectionStatus wallConnectonStatus= new WallConnectionStatus();
private final DispatcherRobStatusListener dispatcherRobStatusListener = new DispatcherRobStatusListener();
private final DispatcherStopWarningListener dispatcherStopWarningListener = new DispatcherStopWarningListener();
private StopWarningType lastStopWarning;
public DefaultRob(IRobComm roboCom){
if(roboCom==null){
throw new NullPointerException("The parameter roboCom is required");
}
initObstacles();
initBumps();
initFalls();
initGaps();
initIrSensors();
initMotors();
initLeds();
this.roboCom = roboCom;
this.roboCom.addRobStatusListener(this);
this.roboCom.addStopWarningListener(this);
}
private void initIrSensors() {
for (IRSentorStatusId irSensor: IRSentorStatusId.values()) {
irSensors.add(new IRSensorStatus(irSensor));
}
}
private void initLeds() {
for (LedStatus.LedStatusId led: LedStatus.LedStatusId.values()) {
leds.add(new LedStatus(led));
}
}
private void initMotors() {
this.panMotor = new MotorStatus(MotorStatusId.Pan);
this.tiltMotor = new MotorStatus(MotorStatusId.Tilt);
this.leftMotor = new MotorStatus(MotorStatusId.Left);
this.rightMotor = new MotorStatus(MotorStatusId.Right);
}
private void initGaps() {
for(GapStatusId id: GapStatusId.values()){
gaps.add(new GapStatus(id));
}
}
private void initFalls() {
for(FallStatusId id: FallStatusId.values()){
falls.add(new FallStatus(id));
}
}
private void initObstacles() {
for (ObstacleSensorStatusId id: ObstacleSensorStatusId.values()){
obstacles.add(new ObstacleSensorStatus(id));
}
}
private void initBumps() {
for (BumpStatusId id: BumpStatusId.values()){
bumps.add(new BumpStatus(id));
}
}
@Override
public void robStatus(RobStatusMessage robStatusMessage) {
Date updateDate= new Date(System.currentTimeMillis());
this.updateGaps(robStatusMessage, updateDate);
this.updateIRSensorStatus(robStatusMessage, updateDate);
this.updateFalls(robStatusMessage, updateDate);
this.updateBumps(robStatusMessage, updateDate);
this.updateObstacles(robStatusMessage, updateDate);
this.updateAllMotorStatus(robStatusMessage, updateDate);
this.updateBateryStatus(robStatusMessage, updateDate);
this.updateWallConnection(robStatusMessage, updateDate);
}
private void updateWallConnection(RobStatusMessage robStatusMessage, Date updateDate) {
this.wallConnectonStatus.setWallConnetion(robStatusMessage.getWallConnection());
this.wallConnectonStatus.setLastUpdate(updateDate);
this.dispatcherRobStatusListener.fireStatusWallConnection(wallConnectonStatus);
}
private void updateBateryStatus(RobStatusMessage robStatusMessage, Date updateDate) {
int batteryLevel = robStatusMessage.getBatteryLevel();
this.battery.setBattery(calcBattery(batteryLevel));
this.battery.setLastUpdate(updateDate);
this.dispatcherRobStatusListener.fireStatusBattery(battery);
}
private void updateGaps(RobStatusMessage robStatusMessage, Date updateDate) {
byte gapsValue = robStatusMessage.getGaps();
int indexGap=1;
for (int i = 0; i < GapStatusId.values().length; i++) {
byte gapValue = readBitAtPosition(gapsValue, indexGap);
GapStatus gap = gaps.get(i);
gap.setGap((gapValue == 1));
gap.setLastUpdate(updateDate);
indexGap+=2;
}
this.dispatcherRobStatusListener.fireStatusGaps(new ArrayList(this.gaps));
}
private void updateIRSensorStatus(RobStatusMessage robStatusMessage, Date updateDate) {
short[] irs = robStatusMessage.getIrs();
if ((irs == null) || (irs.length == 0)) {
return;
}
List listIrSensorStatus= new ArrayList();
for (int i = 0; i < irs.length; i++) {
final IRSensorStatus irSensorStatus = irSensors.get(i);
irSensorStatus.setDistance(irs[i]);
irSensorStatus.setLastUpdate(updateDate);
listIrSensorStatus.add(irSensorStatus);
}
this.dispatcherRobStatusListener.fireStatusIRSensorStatus(listIrSensorStatus);
}
private byte readBitAtPosition(byte value, int position) {
byte mask = 0x1;
byte readedBit = (byte) ((value >> position) & mask);
return readedBit;
}
private void updateFalls(RobStatusMessage robStatusMessage, Date updateDate) {
byte fallsValue = robStatusMessage.getFalls();
int indexFall=1;
for (int i = 0; i < FallStatusId.values().length; i++) {
byte fallValue = readBitAtPosition(fallsValue, indexFall);
FallStatus fall = falls.get(i);
fall.setFall((fallValue == 1));
fall.setLastUpdate(updateDate);
indexFall+=2;
}
this.dispatcherRobStatusListener.fireStatusFalls(new ArrayList(this.falls));
}
private void updateBumps(RobStatusMessage robStatusMessage, Date updateDate) {
// short[] bumpsValues = robStatusMessage.getBumps();
//
// if ((bumpsValues == null) || (bumpsValues.length == 0)) {
// return;
// }
//
// for (int i = 0; i < bumpsValues.length; i++) {
//
// BumpStatus bump = bumps.get(i);
//
// bump.setDistance(bumpsValues[i]);
//
// bump.setLastUpdate(updateDate);
//
// }
}
private void updateObstacles(RobStatusMessage robStatusMessage, Date updateDate) {
// short[] obstablesValues = robStatusMessage.getObstacles();
//
// if ((obstablesValues == null) || (obstablesValues.length == 0)) {
// return;
// }
//
// for (int i = 0; i < obstablesValues.length; i++) {
//
// ObstacleSensorStatus obstacle = obstacles.get(i);
//
// obstacle.setDistance(obstablesValues[i]);
//
// obstacle.setLastUpdate(updateDate);
//
// }
}
private void updateAllMotorStatus(RobStatusMessage roStatusMessage, Date updateDate) {
int[] motorAngle = roStatusMessage.getMotorAngles();
short[] motorVelocities = roStatusMessage.getMotorVelocities();
int[] motorVoltages = roStatusMessage.getMotorVoltages();
if ((motorAngle.length == MOTOR_COUNT) && (motorVelocities.length == MOTOR_COUNT)
&& (motorVoltages.length == MOTOR_COUNT)) {
updateMotorStatus(panMotor, 0, motorAngle, motorVelocities, motorVoltages);
updateMotorStatus(tiltMotor, 1, motorAngle, motorVelocities, motorVoltages);
updateMotorStatus(leftMotor, 2, motorAngle, motorVelocities, motorVoltages);
updateMotorStatus(rightMotor, 3, motorAngle, motorVelocities, motorVoltages);
}
this.dispatcherRobStatusListener.fireStatusMotorPan(panMotor);
this.dispatcherRobStatusListener.fireStatusMotorTilt(tiltMotor);
this.dispatcherRobStatusListener.fireStatusMotorsMT(leftMotor, rightMotor);
}
private void updateMotorStatus(
MotorStatus ms, int index,
int[] motorAngle, short[] motorVelocities, int[] motorVoltages) {
ms.setVariationAngle(convertAngleROB2OBO(motorAngle[index]));
ms.setAngularVelocity(motorVelocities[index]);
ms.setVoltage(motorVoltages[index]);
}
/**
* Sets the color of a led
* @param led Led index (1-7)
* @param color Led color
* @throws InternalErrorException
*/
@Override
public void setLEDColor(int led, Color color) throws InternalErrorException, IllegalArgumentException {
if ((color.getRed()>255)||(color.getGreen()>255)||(color.getBlue()>255)){
throw new IllegalArgumentException("Invalid color");
}
LedStatus s ;
try {
s = new LedStatus(LedStatus.LedStatusId.values()[led-1]);
s.setColor(color.getRed(), color.getGreen(), color.getBlue());
this.dispatcherRobStatusListener.fireStatusLeds(s);
} catch (IndexOutOfBoundsException e){
throw new IllegalArgumentException("Invalid led id");
}
this.roboCom.setLEDColor(led, color.getRed(), color.getGreen(), color.getBlue());
}
@Override
public void setLEDsMode(LEDsModeEnum mode) throws InternalErrorException {
this.roboCom.setLEDsMode(mode.code);
}
/**
* Moves the motors by degrees
* @param mode Movement Mode R_L
* @param angVelR Angular Speed of the right Motor
* @param angleR Angle of the right Motor
* @param angVelL Angular Speed of the left Motor
* @param angleL Angle of the left Motor
* @throws InternalErrorException
*/
@Override
public void moveMT(MoveMTMode mode, int angVelR, int angleR, int angVelL, int angleL) throws InternalErrorException {
this.roboCom.moveMT(mode.getMode(), limitAngVel(angVelL, MAX_ANG_VEL, MIN_ANG_VEL), convertAngleOBO2ROB(angleL), limitAngVel(angVelR, MAX_ANG_VEL, MIN_ANG_VEL), convertAngleOBO2ROB(angleR));
}
/**
* Moves the motors by time
* @param mode Movement Mode R_L
* @param angVelR Angular speed of the right motor
* @param angVelL Angular speed of the left motor
* @param time Time in milliseconds
* @throws InternalErrorException
*/
@Override
public void moveMT(MoveMTMode mode, int angVelR, int angVelL, long time) throws InternalErrorException {
this.roboCom.moveMT(mode.getMode(), limitAngVel(angVelL, MAX_ANG_VEL, MIN_ANG_VEL), limitAngVel(angVelR, MAX_ANG_VEL, MIN_ANG_VEL), time);
}
/**
*
* @param angVel the velocity (0-255)
* @param angle the angle (0-90)
* @throws InternalErrorException
*/
@Override
public void movePan(int angVel, int angle) throws InternalErrorException {
this.roboCom.movePan(angVel, convertAngleOBO2ROB(limitAngle(angle, MAX_PAN_ANGLE, MIN_PAN_ANGLE)));
}
@Override
public void moveTilt(int angVel, int angle) throws InternalErrorException {
this.roboCom.moveTilt(angVel, convertAngleOBO2ROB(limitAngle(angle, MAX_TILT_ANGLE, MIN_TILT_ANGLE)));
}
@Override
public void movePanTilt(int angVelPan, int anglePan, int angVelTilt, int angleTilt) throws InternalErrorException {
this.roboCom.movePanTilt(angVelPan, convertAngleOBO2ROB(limitAngle(anglePan, MAX_PAN_ANGLE, MIN_PAN_ANGLE)),
angVelTilt, convertAngleOBO2ROB(limitAngle(angleTilt, MAX_TILT_ANGLE, MIN_TILT_ANGLE)));
}
@Override
public void resetPanTiltOffset() throws InternalErrorException {
this.roboCom.resetPanTiltOffset();
}
@Override
public void setRobStatusPeriod(int period) throws InternalErrorException {
this.roboCom.setRobStatusPeriod(period);
}
@Override
public void setOperationMode(byte operationMode) throws InternalErrorException {
this.roboCom.setOperationMode(operationMode);
}
@Override
public void configureInfrared(byte infraredId, byte commandCode, byte dataByteLow, byte dataByteHigh) throws InternalErrorException {
this.roboCom.infraredConfiguration(infraredId, commandCode, dataByteLow, dataByteHigh);
}
@Override
public void configureMotorSControlValue(byte motorId, int startki, int perturbationski, int stopki) throws CommunicationException {
this.roboCom.setControlValues(motorId, startki, perturbationski, stopki);
}
@Override
public void maxValueMotors(int m1Tension, int m1Time, int m2Tension, int m2Time, int panTension, int panTime,
int tiltTension, int tiltTime) throws InternalErrorException {
this.roboCom.maxValueMotors(m1Tension, m1Time, m2Tension, m2Time, panTension, panTime, tiltTension, tiltTime);
}
@Override
public List getLastStatusMotors() {
ArrayList motors = new ArrayList();
motors.add(panMotor);
motors.add(tiltMotor);
motors.add(leftMotor);
motors.add(rightMotor);
return motors;
}
@Override
public List getLastStatusIRs() {
return new ArrayList(this.irSensors);
}
@Override
public List getLastGapsStatus() {
return new ArrayList(this.gaps);
}
@Override
public List getLastStatusFalls() {
return new ArrayList(this.falls);
}
@Override
public List getLastStatusBumps() {
return new ArrayList(this.bumps);
}
@Override
public List getLastStatusObstacles() {
return new ArrayList(this.obstacles);
}
@Override
public BatteryStatus getLastStatusBattery() {
return this.battery;
}
@Override
public StopWarningType getLastStopWarning() {
return lastStopWarning;
}
@Override
public void addRobStatusListener(IRobStatusListener listener) {
dispatcherRobStatusListener.subscribetoContentChanges(listener);
}
@Override
public void addStopWarningListener(IStopWarningListener listener) {
dispatcherStopWarningListener.subscribetoStopWarnings(listener);
}
@Override
public void removeStopWarningListener(IStopWarningListener listener) {
dispatcherStopWarningListener.unsubscribeFromStopWarnings(listener);
}
@Override
public void removeRobStatusListener(IRobStatusListener listener) {
dispatcherRobStatusListener.unsubscribeFromContentChanges(listener);
}
private int convertAngleOBO2ROB(int angle) {
return angle*ANGLE_CONVERSION_FACTOR;
}
private int convertAngleROB2OBO(int angle) {
return angle/ANGLE_CONVERSION_FACTOR;
}
private int limitAngVel(int angVel, short max, short min) {
if (angVel > max)
return max;
else if (angVel < min)
return 0;
else
return angVel;
}
private int limitAngle(int angle, int max, int min) {
if (angle > max) {
return max;
}
else if (angle < min) {
return min;
}
else {
return angle;
}
}
private int calcBattery(int value){
if (value > max_battery){
max_battery = value;
}else if (value < min_battery){
min_battery = value;
}
return Math.round(((float)(value-min_battery)/(float)(max_battery-min_battery))*100.0f);
}
@Override
public void robCommunicationError(CommunicationException ex) {
dispatcherRobStatusListener.fireInternalError(ex);
}
@Override
public void stopWarning(StopWarningMessage sw) {
lastStopWarning = sw.getMessage();
dispatcherStopWarningListener.fireStatusBattery(sw);
}
}