org.robokind.api.motion.servos.ServoRobot Maven / Gradle / Ivy
/*
* Copyright 2011 Hanson Robokind LLC.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
package org.robokind.api.motion.servos;
import java.beans.PropertyChangeEvent;
import java.util.*;
import java.util.Map.Entry;
import java.util.logging.Level;
import java.util.logging.Logger;
import org.robokind.api.common.config.VersionProperty;
import org.robokind.api.common.position.NormalizedDouble;
import org.robokind.api.common.property.PropertyChangeAction;
import org.robokind.api.common.property.PropertyChangeMonitor;
import org.robokind.api.common.services.ServiceContext;
import org.robokind.api.motion.AbstractRobot;
import org.robokind.api.motion.Joint;
import org.robokind.api.motion.Robot;
import org.robokind.api.motion.servos.ServoController.ServoId;
import org.robokind.api.motion.servos.config.ServoConfig;
import org.robokind.api.motion.servos.config.ServoControllerConfig;
import org.robokind.api.motion.servos.config.ServoRobotConfig;
import org.robokind.api.motion.servos.utils.EmptyServoJoint;
import org.robokind.api.motion.servos.utils.ServoJointAdapter;
/**
* Robot implementation using Servos.
*
* @author Matthew Stevenson
*/
public class ServoRobot extends AbstractRobot {
private final static Logger theLogger = Logger.getLogger(ServoRobot.class.getName());
/**
* Controller type version name.
*/
public final static String VERSION_NAME = "Servo Robot Implementation";
/**
* Controller type version number.
*/
public final static String VERSION_NUMBER = "1.0";
/**
* Controller type VersionProperty.
*/
public final static VersionProperty VERSION = new VersionProperty(VERSION_NAME, VERSION_NUMBER);
private Map myServoIdMap;
private Map myServoJointAdapters;
private Map myControllers;
private List myControllerList;
private List myMissingJoints;
private PropertyChangeMonitor myChangeMonitor;
private boolean myConnectionFlag;
private ServoRobotConfig myRobotConfig;
/**
* Creates a new ServoRobot with the given BundleContext and RobotConfig.
* @param config Robot's configuration parameters
*/
public ServoRobot(ServoRobotConfig config){
super(config.getRobotId());
myRobotConfig = config;
myServoIdMap = new HashMap();
myControllers = new HashMap();
myControllerList = new ArrayList();
myServoJointAdapters = new HashMap();
myMissingJoints = new ArrayList();
myConnectionFlag = false;
Map ids = config.getIdMap();
if(ids == null){
throw new NullPointerException();
}
for(Entry e : ids.entrySet()){
Joint.Id jointId = e.getKey();
ServoController.ServoId servoId = e.getValue();
if(jointId == null || servoId == null){
throw new NullPointerException();
}
Robot.JointId jId = new Robot.JointId(getRobotId(), jointId);
myServoIdMap.put(jId, servoId);
}
for(ServoControllerContext scc : config.getControllerContexts()){
if(scc == null){
throw new NullPointerException();
}
ServoController controller = scc.getServoController();
ServoJointAdapter adapter = scc.getServoJointAdapter();
if(controller == null || adapter == null){
throw new NullPointerException();
}
ServoController.Id scId = controller.getId();
myControllers.put(scId, controller);
myControllerList.add(controller);
myServoJointAdapters.put(scId, adapter);
}
initChangeMonitor();
setEnabled(true);
}
@Override
public void setEnabled(boolean val) {
super.setEnabled(val);
for(ServoController sc : myControllerList){
sc.setEnabled(val);
}
}
private void initChangeMonitor(){
myChangeMonitor = new PropertyChangeMonitor();
PropertyChangeAction pca = new PropertyChangeAction() {
@Override
protected void run(PropertyChangeEvent event) {
jointsChanged(event);
}
};
myChangeMonitor.addAction(ServoController.PROP_SERVOS, pca);
myChangeMonitor.addAction(ServoController.PROP_SERVO_ADD, pca);
myChangeMonitor.addAction(ServoController.PROP_SERVO_REMOVE, pca);
for(ServoController c : myControllerList){
c.addPropertyChangeListener(myChangeMonitor);
}
}
@Override
public boolean connect(){
for(ServoController controller : myControllerList){
try{
controller.connect();
}catch(Throwable t){
theLogger.log(Level.WARNING, "Unable to connect to ServoController.", t);
}
}
setServos();
boolean oldVal = myConnectionFlag;
myConnectionFlag = true;
firePropertyChange(PROP_CONNECTED, oldVal, myConnectionFlag);
return true;
}
private void setServos(){
clearJoints();
myMissingJoints.clear();
for(Entry e : myServoIdMap.entrySet()){
JointId jId = e.getKey();
ServoId sId = e.getValue();
ServoController.Id scId = sId.getControllerId();
ServoController sc = myControllers.get(scId);
ServoJointAdapter sja = myServoJointAdapters.get(scId);
if(sc == null || sja == null){
myMissingJoints.add(jId);
continue;
}
Servo s = sc.getServo(sId);
if(s == null){
myMissingJoints.add(jId);
continue;
}
ServoJoint j = sja.getJoint(jId.getJointId(), s);
if(j == null){
myMissingJoints.add(jId);
continue;
}
addJoint(j);
}
for(JointId robotJointId : myMissingJoints){
Joint.Id jointId = robotJointId.getJointId();
ServoId sId = myServoIdMap.get(robotJointId);
ServoController.Id scId = sId.getControllerId();
ServoConfig servoConf = null;
for(ServoControllerContext scc : myRobotConfig.getControllerContexts()){
ServiceContext<
ServoController,
ServoControllerConfig,?> context = scc.myConnectionContext;
if(context == null){
continue;
}
ServoControllerConfig controllerConfig = context.getServiceConfiguration();
ServoController.Id controllerId = controllerConfig.getServoControllerId();
if(!scId.equals(controllerId)){
continue;
}
Map servoConfs = controllerConfig.getServoConfigs();
if(!servoConfs.containsKey(sId.getServoId())){
continue;
}
servoConf = servoConfs.get(sId.getServoId());
}
if(servoConf == null){
String name = "Joint - " + jointId.toString();
ServoJoint j = new EmptyServoJoint(jointId, name, new NormalizedDouble(0.5));
addJoint(j);
}else{
String name = servoConf.getName();
int min = servoConf.getMinPosition();
int max = servoConf.getMaxPosition();
ServoJoint j = new EmptyServoJoint(jointId, name, new NormalizedDouble(0.5), min, max);
addJoint(j);
}
}
}
@Override
public void disconnect(){
for(ServoController jc : myControllerList){
jc.disconnect();
}
boolean oldVal = myConnectionFlag;
myConnectionFlag = false;
firePropertyChange(PROP_CONNECTED, oldVal, myConnectionFlag);
}
@Override
public boolean isConnected(){
return myConnectionFlag;
}
@Override
public void move(RobotPositionMap positions, long lenMillisec) {
if(!isConnected() || !isEnabled()){
return;
}
if(myJointMap == null){
throw new NullPointerException();
}
for(Entry e : positions.entrySet()){
Robot.JointId id = e.getKey();
ServoJoint j = myJointMap.get(id);
if(j == null){
continue;
}
NormalizedDouble pos = e.getValue();
j.setGoalPosition(pos);
}
move(lenMillisec, positions.keySet().toArray(new Robot.JointId[0]));
}
private void move(long lenMillisec, Robot.JointId... ids) {
Map> map = getServoIds(ids);
for(Entry> e : map.entrySet()){
ServoController.Id scId = e.getKey();
List sIds = e.getValue();
moveController(scId, sIds, lenMillisec);
}
}
private void moveController(
ServoController.Id controllerId,
List servoIds, long lenMillisec){
ServoController sc = myControllers.get(controllerId);
if(sc == null){
return;
}
Class idType = sc.getServoIdClass();
ServoId[] ids = new ServoId[servoIds.size()];
int i=0;
for(ServoId sId : servoIds){
Object id = sId.getServoId();
if(!idType.isAssignableFrom(id.getClass())){
continue;
}
ids[i] = sId;
i++;
}
sc.moveServos(ids, i, 0, lenMillisec);
}
/**
* Returns an unmodifiable Map of ServoController.Ids and ServoControllers.
* @return unmodifiable Map of ServoController.Ids and ServoControllers
*/
public Map getControllers(){
return Collections.unmodifiableMap(myControllers);
}
/**
* Returns a List of ServoControllers.
* @return List of ServoControllers
*/
public List getControllerList(){
return myControllerList;
}
private Map> getServoIds(JointId...ids){
Map> map = new HashMap();
for(JointId jointId : ids){
if(!jointId.getRobotId().equals(this.getRobotId())){
continue;
}
ServoController.ServoId sId = myServoIdMap.get(jointId);
if(sId == null){
continue;
}
List servoIds = map.get(sId.getControllerId());
if(servoIds == null){
servoIds = new ArrayList();
map.put(sId.getControllerId(), servoIds);
}
servoIds.add(sId);
}
return map;
}
private void jointsChanged(PropertyChangeEvent event){
setServos();
}
/**
* Contains the objects used for loading and creating a ServoController.
* @param Type of Servo to use
*/
public static class ServoControllerContext>{
private ServiceContext<
ServoController,
ServoControllerConfig,?> myConnectionContext;
private ServoJointAdapter myServoJointAdapter;
private boolean myInitializedFlag;
/**
* Creates a new ServoControllerContext.
* @param connContext ConnectionContext for the ServoController
* @param jointAdapter JointAdapter for creating Joints from Servos
*/
public ServoControllerContext(ServiceContext<
ServoController,ServoControllerConfig,?> connContext,
ServoJointAdapter jointAdapter){
if(connContext == null || jointAdapter == null){
throw new NullPointerException();
}
myInitializedFlag = false;
myConnectionContext = connContext;
myServoJointAdapter = jointAdapter;
}
/**
* Initializes the ServoControllerContext, creating the ServoController.
*/
public void initialize(){
if(myInitializedFlag){
return;
}
try{
myConnectionContext.buildService();
}catch(Exception ex){
theLogger.log(Level.WARNING, "Unable to build Service.", ex);
return;
}
ServoController controller = myConnectionContext.getService();
if(controller == null){
return;
}
myInitializedFlag = true;
}
/**
* Returns the ServoControllerContext's ServoController
* @return ServoControllerContext's ServoController
*/
public ServoController getServoController(){
if(!myInitializedFlag){
initialize();
}
return myConnectionContext.getService();
}
/**
* Returns the associated JointAdapter
* @return associated JointAdapter
*/
public ServoJointAdapter getServoJointAdapter(){
return myServoJointAdapter;
}
}
}
© 2015 - 2025 Weber Informatics LLC | Privacy Policy