/*
* To change this template, choose Tools | Templates
* and open the template in the editor.
*/
package com.test.simplecarusorobot;
import ca.teamdave.caruso.AbstractRobot;
import ca.teamdave.caruso.actuator.Actuator;
import ca.teamdave.caruso.sensor.Sensor;
import com.test.linearrobot.LrNames;
import java.util.Hashtable;
/**
*
* @author leigh
*/
public class LinearRobotSimulated implements AbstractRobot {
// Robot State variables
private double pos = 0.0;
private double speed = 0.0;
private double accel = 0.0;
// Robot target variables
private double destPos = 0.0;
private double destSpeed = 0.0;
private double destAccel = 0.0;
// Actuator output state variables
private double outputNormalized = 0.0;
// Useful constants
private final double dTime = 0.05; // step time in seconds
private final double mass = 50; // robot mass in kg
private final double maxOutputForce = 48; // Max force output in newtons
private final double friction = 25; // Friction in N/(m/s)
public Hashtable createSensors() {
final LinearRobotSimulated me = this;
Hashtable res = new Hashtable();
res.put(LrNames.POSITION_SENSOR, new Sensor(Double.class){
protected Object retrieveValue() {
return new Double(me.pos);
}
});
res.put(LrNames.ROBOT_SPEED, new Sensor(Double.class){
protected Object retrieveValue() {
return new Double(me.speed);
}
});
res.put(LrNames.ROBOT_ACCEL, new Sensor(Double.class){
protected Object retrieveValue() {
return new Double(me.accel);
}
});
return res;
}
public Hashtable createActuators() {
final LinearRobotSimulated me = this;
Hashtable res = new Hashtable();
res.put(LrNames.OUTPUT_FORCE, new Actuator(Double.class) {
protected Object retriveValue() {
return new Double(me.outputNormalized);
}
protected void assignValue(Object val) {
me.outputNormalized = ((Double) val).doubleValue();
if (me.outputNormalized > 1.0) {
me.outputNormalized = 1.0;
} else if (me.outputNormalized < -1.0) {
me.outputNormalized = -1.0;
}
}
});
res.put(LrNames.DESIRED_POSITION, new Actuator(Double.class) {
protected Object retriveValue() {
return new Double(me.destPos);
}
protected void assignValue(Object val) {
me.destPos = ((Double)val).doubleValue();
}
});
res.put(LrNames.DESIRED_SPEED, new Actuator(Double.class) {
protected Object retriveValue() {
return new Double(me.destSpeed);
}
protected void assignValue(Object val) {
me.destSpeed = ((Double)val).doubleValue();
}
});
res.put(LrNames.DESIRED_ACCEL, new Actuator(Double.class) {
protected Object retriveValue() {
return new Double(me.destAccel);
}
protected void assignValue(Object val) {
me.destAccel = ((Double)val).doubleValue();
}
});
return res;
}
public void robotInit() {
// .. do nothing...?
}
public void periodicUpdate() {
double outputForce = this.maxOutputForce * this.outputNormalized;
double frictionForce = -this.speed * this.friction;
this.accel = (frictionForce + outputForce)/this.mass;
this.speed += this.accel * this.dTime;
this.pos += this.speed * this.dTime;
}
}