Skip to content

Commit

Permalink
Inmoov2 intermediate 2 (#1406)
Browse files Browse the repository at this point in the history 
* update

* drupp update
  • Loading branch information
@supertick
supertick authored Feb 12, 2024
1 parent 7c38cb9 commit c40d0f9
Show file tree
Hide file tree
Showing 6 changed files with 66 additions and 66 deletions.
90 changes: 45 additions & 45 deletions src/main/java/org/myrobotlab/service/DruppNeck.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,24 +18,25 @@
* @author kwatters
*
*/
public class DruppNeck extends Service<DruppNeckConfig>
{
public class DruppNeck extends Service<DruppNeckConfig> {

private static final long serialVersionUID = 1L;
// 3 servos for the drupp neck
public transient ServoControl up;
public transient ServoControl middle;
public transient ServoControl down;

// this is an offset angle that is added to the solution from the IK solver
public double upOffset = 90;
public double middleOffset = 120 + 90;
public double downOffset = -120 + 90;
protected transient ServoControl up;
protected transient ServoControl middle;
protected transient ServoControl down;

public DruppNeck(String n, String id) {
super(n, id);
}

public void startService() {
super.startService();
up = (ServoControl) startPeer("up");
middle = (ServoControl) startPeer("middle");
down = (ServoControl) startPeer("down");
}

private DruppIKSolver solver = new DruppIKSolver();

/**
Expand All @@ -44,13 +45,13 @@ public DruppNeck(String n, String id) {
* down servos.
*
* @param roll
* degrees
* degrees
* @param pitch
* degrees
* degrees
* @param yaw
* degrees
* degrees
* @throws Exception
* boom
* boom
*
*/
public void moveTo(double roll, double pitch, double yaw) throws Exception {
Expand All @@ -62,9 +63,9 @@ public void moveTo(double roll, double pitch, double yaw) throws Exception {
// TODO: if the solver fails, should we catch this exception ?
double[] result = solver.solve(rollRad, pitchRad, yawRad);
// convert to degrees
double upDeg = MathUtils.radToDeg(result[0]) + upOffset;
double middleDeg = MathUtils.radToDeg(result[1]) + middleOffset;
double downDeg = MathUtils.radToDeg(result[2]) + downOffset;
double upDeg = MathUtils.radToDeg(result[0]) + config.upOffset;
double middleDeg = MathUtils.radToDeg(result[1]) + config.middleOffset;
double downDeg = MathUtils.radToDeg(result[2]) + config.downOffset;
// Ok, servos can only (typically) move from 0 to 180.. if any of the angles
// are
// negative... we can't move there.. let's log a warning
Expand All @@ -73,14 +74,12 @@ public void moveTo(double roll, double pitch, double yaw) throws Exception {
// but for the drupp neck, if you've installed it correctly,
// all servos can go from 0 to 180...
if (upDeg < 0 || middleDeg < 0 || downDeg < 0 || upDeg > 180 || middleDeg > 180 || downDeg > 180) {
log.warn("Target Position out of range! {} Pitch {} Yaw {} -> Up {} Middle {} Down {}", roll, pitch, yaw,
MathUtils.round(upDeg, 3), MathUtils.round(middleDeg, 3),
log.warn("Target Position out of range! {} Pitch {} Yaw {} -> Up {} Middle {} Down {}", roll, pitch, yaw, MathUtils.round(upDeg, 3), MathUtils.round(middleDeg, 3),
MathUtils.round(downDeg, 3));
// Skipping this movement as it's likely unstable!
return;
}
log.info("Input Roll {} Pitch {} Yaw {} -> Up {} Middle {} Down {}", roll, pitch, yaw, MathUtils.round(upDeg, 3),
MathUtils.round(middleDeg, 3), MathUtils.round(downDeg, 3));
log.info("Input Roll {} Pitch {} Yaw {} -> Up {} Middle {} Down {}", roll, pitch, yaw, MathUtils.round(upDeg, 3), MathUtils.round(middleDeg, 3), MathUtils.round(downDeg, 3));
// we should probably track the last moved to position.
up.moveTo(upDeg);
middle.moveTo(middleDeg);
Expand Down Expand Up @@ -144,53 +143,54 @@ public void setServos(ServoControl up, ServoControl middle, ServoControl down) {
}

public double getUpOffset() {
return upOffset;
return config.upOffset;
}

public void setUpOffset(double upOffset) {
this.upOffset = upOffset;
this.config.upOffset = upOffset;
}

public double getMiddleOffset() {
return middleOffset;
return config.middleOffset;
}

public void setMiddleOffset(double middleOffset) {
this.middleOffset = middleOffset;
this.config.middleOffset = middleOffset;
}

public double getDownOffset() {
return downOffset;
return config.downOffset;
}

public void setDownOffset(double downOffset) {
this.downOffset = downOffset;
this.config.downOffset = downOffset;
}

public static void main(String[] args) throws Exception {
LoggingFactory.init("INFO");
// To use the drup service you need to configure and attach the servos
// then set them on the service.
Runtime.start("gui", "SwingGui");
Runtime.start("python", "Python");
Servo up = (Servo) Runtime.start("up", "Servo");
Servo middle = (Servo) Runtime.start("middle", "Servo");
Servo down = (Servo) Runtime.start("down", "Servo");
up.setPin(6);
middle.setPin(5);
down.setPin(4);
// String port = "COM4";
String port = "VIRTUAL_COM_PORT";
VirtualArduino va1 = (VirtualArduino) Runtime.start("va1", "VirtualArduino");
va1.connect(port);
Arduino ard = (Arduino) Runtime.start("ard", "Arduino");
ard.connect(port);
ard.attach(up);
ard.attach(middle);
ard.attach(down);
// Runtime.start("python", "Python");
// Servo up = (Servo) Runtime.start("up", "Servo");
// Servo middle = (Servo) Runtime.start("middle", "Servo");
// Servo down = (Servo) Runtime.start("down", "Servo");
// up.setPin(6);
// middle.setPin(5);
// down.setPin(4);
// // String port = "COM4";
// String port = "VIRTUAL_COM_PORT";
// VirtualArduino va1 = (VirtualArduino) Runtime.start("va1",
// "VirtualArduino");
// va1.connect(port);
// Arduino ard = (Arduino) Runtime.start("ard", "Arduino");
// ard.connect(port);
// ard.attach(up);
// ard.attach(middle);
// ard.attach(down);
// Create the drupp service
DruppNeck neck = (DruppNeck) Runtime.start("neck", "DruppNeck");
neck.setServos(up, middle, down);
Runtime.start("webgui", "WebGui");
// neck.setServos(up, middle, down);
// neck.moveTo(0, 0, 0);
// neck.moveTo(0, 0, -45);
// neck.moveTo(0, 0, 45);
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/org/myrobotlab/service/Hd44780.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -687,7 +687,7 @@ public void preShutdown() {
public static void main(String[] args) {
try {
LoggingFactory.init(Level.INFO);
Platform.setVirtual(false);
Runtime.getInstance().setVirtual(false);

Runtime.start("webgui", "WebGui");
Pcf8574 pcf = (Pcf8574) Runtime.start("pcf8574t", "Pcf8574");
Expand Down
28 changes: 11 additions & 17 deletions src/main/java/org/myrobotlab/service/HtmlFilter.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,27 +36,23 @@ public HtmlFilter(String n, String id) {

// helper function to add html tags
public String addHtml(String text) {
HtmlFilterConfig c = (HtmlFilterConfig) config;
return c.preHtmlTag + text + c.postHtmlTag;
return config.preHtmlTag + text + config.postHtmlTag;
}

public void addTextListener(TextListener service) {
attachTextListener(service);
}

public String getPostHtmlTag() {
HtmlFilterConfig c = (HtmlFilterConfig) config;
return c.postHtmlTag;
return config.postHtmlTag;
}

public String getPreHtmlTag() {
HtmlFilterConfig c = (HtmlFilterConfig) config;
return c.preHtmlTag;
return config.preHtmlTag;
}

public boolean isStripHtml() {
HtmlFilterConfig c = (HtmlFilterConfig) config;
return c.stripHtml;
return config.stripHtml;
}

@Override
Expand All @@ -67,20 +63,20 @@ public void onText(String text) {

@Override
public String processText(String text) {
HtmlFilterConfig c = (HtmlFilterConfig) config;


invoke("publishRawText", text);

String processedText = text;

if (c.stripHtml) {
if (config.stripHtml) {
// clean text
processedText = stripHtml(text);
} else {
processedText = addHtml(text);
}

if (c.stripUrls) {
if (config.stripUrls) {
processedText = stripUrls(processedText);
}

Expand All @@ -104,8 +100,8 @@ public String publishText(String text) {
* - a string to append to the text
*/
public void setPostHtmlTag(String postHtmlTag) {
HtmlFilterConfig c = (HtmlFilterConfig) config;
c.postHtmlTag = postHtmlTag;

config.postHtmlTag = postHtmlTag;
}

/**
Expand All @@ -115,8 +111,7 @@ public void setPostHtmlTag(String postHtmlTag) {
* - a string to prepend to the text.
*/
public void setPreHtmlTag(String preHtmlTag) {
HtmlFilterConfig c = (HtmlFilterConfig) config;
c.preHtmlTag = preHtmlTag;
config.preHtmlTag = preHtmlTag;
}

/**
Expand All @@ -127,8 +122,7 @@ public void setPreHtmlTag(String preHtmlTag) {
* - if true, all content between &lt;and &gt; will be removed.
*/
public void setStripHtml(boolean stripHtml) {
HtmlFilterConfig c = (HtmlFilterConfig) config;
c.stripHtml = stripHtml;
config.stripHtml = stripHtml;
}

// helper function to strip html tags.
Expand Down
4 changes: 2 additions & 2 deletions src/main/java/org/myrobotlab/service/HttpClient.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,7 +54,7 @@
import org.myrobotlab.logging.LoggerFactory;
import org.myrobotlab.logging.LoggingFactory;
import org.myrobotlab.net.InstallCert;
import org.myrobotlab.service.config.ServiceConfig;
import org.myrobotlab.service.config.HttpClientConfig;
import org.myrobotlab.service.data.HttpData;
import org.myrobotlab.service.interfaces.HttpDataListener;
import org.myrobotlab.service.interfaces.HttpResponseListener;
Expand All @@ -74,7 +74,7 @@
* - Proxies proxies proxies ! -
* https://memorynotfound.com/configure-http-proxy-settings-java/
*/
public class HttpClient<C extends ServiceConfig> extends Service<C> implements TextPublisher {
public class HttpClient<C extends HttpClientConfig> extends Service<C> implements TextPublisher {

public final static Logger log = LoggerFactory.getLogger(HttpClient.class);

Expand Down
2 changes: 1 addition & 1 deletion src/main/java/org/myrobotlab/service/Mqtt.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -570,7 +570,7 @@ public void messageArrived(String topic, MqttMessage message) throws MqttExcepti
// 4. describe new instance for me
// FIXME why isn't this using Gateway.getDescribeMessage()?
Message describe = Message.createMessage(String.format("%s@%s", getName(), getId()), "runtime@" + remoteId, "describe",
new Object[] { Gateway.FILL_UUID_MAGIC_VAL, new DescribeQuery(Platform.getLocalInstance().getId(), uuid) });
new Object[] { Gateway.FILL_UUID_MAGIC_VAL, new DescribeQuery(Runtime.getInstance().getId(), uuid) });
describe.sendingMethod = "onConnect";
sendRemote(describe);

Expand Down
6 changes: 6 additions & 0 deletions src/main/java/org/myrobotlab/service/config/DruppNeckConfig.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,12 @@
import org.myrobotlab.framework.Plan;

public class DruppNeckConfig extends ServiceConfig {

// this is an offset angle that is added to the solution from the IK solver
public double upOffset = 90;
public double middleOffset = 120 + 90;
public double downOffset = -120 + 90;


@Override
public Plan getDefault(Plan plan, String name) {
Expand Down

0 comments on commit c40d0f9

Please sign in to comment.