Skip to content

Commit

Permalink
little polish
Browse files Browse the repository at this point in the history
  • Loading branch information
gperry committed May 23, 2018
1 parent f446bc4 commit 1e50e56
Show file tree
Hide file tree
Showing 3 changed files with 68 additions and 51 deletions.
11 changes: 9 additions & 2 deletions src/main/java/org/myrobotlab/service/Arduino.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2304,11 +2304,18 @@ public void attachEncoderControl(EncoderControl encoder) {

}

@Override
// callback for generated method from arduinoMsg.schema
public EncoderData publishEncoderPosition(Integer deviceId, Integer position) {
EncoderData data = new EncoderData(getDeviceName(deviceId), position);
log.info("Encoder position. {}" , data);
((Amt203Encoder) getDevice(deviceId)).onEncoderData(data);
// DO WE BOTH PUBLISH & CALLBACK ?
((EncoderControl) getDevice(deviceId)).onEncoderData(data);
invoke("publishEncoderPosition", data);
return data;
}

@Override
public EncoderData publishEncoderPosition(EncoderData data) {
return data;
}

Expand Down
106 changes: 58 additions & 48 deletions src/main/java/org/myrobotlab/service/RoboClaw.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -342,7 +342,7 @@ public void attach(Attachable service) throws Exception {
// FIXME - use SerialDevice
serial = (Serial) service;
serial.setTimeout(40);

// here we check and warn regarding config - but
// it "might" be right if the user has customized it
// this works well - the user controls all config
Expand Down Expand Up @@ -1273,7 +1273,7 @@ public EncoderData readEncoderM1() {
// FIXME - not really thread safe !
// what pairs serial recv data with the calling thread
// or what prevents another thread from "clearing" the buffer !
EncoderData ed = null;
EncoderData ed = null;
try {
// FIXME by making a blocking read sendPacket method
// sendPacket(data[], byte1...)
Expand All @@ -1282,34 +1282,34 @@ public EncoderData readEncoderM1() {
sendPacket(address, 16);

byte[] data = new byte[7];

// read uses timeout set globally
serial.read(data);
ed = new EncoderData(getName(),bytes4ToLong(data));
ed = new EncoderData(getName(), bytes4ToLong(data));

log.info("ret {}", Serial.bytesToHex(data));
log.info("{} ", ed);

invoke("publishEncoderM1", ed);

} catch (Exception e) {
log.error(e.getMessage(), e);
}
return ed;
}

public EncoderData publishEncoderM1(EncoderData data) {
return data;
}

public EncoderData publishEncoderM2(EncoderData data) {
return data;
}

public static long bytes4ToLong(byte[] data) {
return bytes4ToLong(data, 0);
}

public static long bytes4ToLong(byte[] data, int start) {
return (data[start] & 0xFF) << 24 | (data[start + 1] & 0xFF) << 16 | (data[start + 2] & 0xFF) << 8 | (data[start + 3] & 0xFF);
}
Expand Down Expand Up @@ -1337,33 +1337,48 @@ public static int bytes2ToInt(byte[] data, int start) {
Bit7 - Reserved
* </pre>
*/
public EncoderData readEncoderM2() {
// FIXME - not really thread safe !
// what pairs serial recv data with the calling thread
// or what prevents another thread from "clearing" the buffer !
EncoderData ed = null;
public EncoderData readEncoderM2() {

EncoderData ed = null;

byte[] data = sendReadPacket(7, address, 17);
if (data != null) {
ed = new EncoderData(getName(), bytes4ToLong(data));
invoke("publishEncoderM1", ed);
}

return ed;
}

/**
* synchronous send and receive - if not enough bytes, or a timeout is reached
* a null byte buffer will be returned. If the same number of bytes are
* returned as requested then the byte array is returned. Synchronized on the
* single serial resource.
*
* @param readDataSize
* @param sendData
* @return
*/
synchronized public byte[] sendReadPacket(int bytesRequested, int... sendData) {
try {
// FIXME by making a blocking read sendPacket method
// sendPacket(data[], byte1...)
// you can't mess with serial outside the "sendPacket"/"readPacket"

// clear buffer
serial.clear();
sendPacket(address, 17);

byte[] data = new byte[7];

byte[] data = new byte[bytesRequested];
sendPacket(sendData);
// read uses timeout set globally
serial.read(data);
ed = new EncoderData(getName(),bytes4ToLong(data));
int bytesRead = serial.read(data);

log.info("ret {}", Serial.bytesToHex(data));
log.info("{} ", ed);

invoke("publishEncoderM1", ed);
if (bytesRead == bytesRequested) {
return data;
}

} catch (Exception e) {
log.error(e.getMessage(), e);
}
return ed;
return null;
}

/**
Expand Down Expand Up @@ -2068,8 +2083,7 @@ public void readM2PID() {
sendPacket(address, 64);
// TODO lock - timeout - return value & publish
}



/**
* <pre>
65 - Buffered Drive M1 with signed Speed, Accel, Deccel and Position
Expand All @@ -2087,7 +2101,6 @@ public void speedAccelDeccelPositionM1(int accel, int speed, int deccel, int pos
// TODO lock - timeout - return value & publish
}


/**
* <pre>
66 - Buffered Drive M2 with signed Speed, Accel, Deccel and Position
Expand Down Expand Up @@ -2129,7 +2142,6 @@ public void bufferedDriveM1M2WithSignedSpeedAccelDeccelPosition(int accelM1, int
buffer);
}


@Override
public void onConnect(String portName) {
log.info("onConnect from port {}", portName);
Expand All @@ -2152,7 +2164,7 @@ public EncoderData publishEncoderData(EncoderData Data) {
// TODO Auto-generated method stub
return null;
}

public static void main(String[] args) {
try {
/*
Expand Down Expand Up @@ -2201,21 +2213,20 @@ public static void main(String[] args) {
// start the services
// Runtime.start("gui", "SwingGui");
RoboClaw rc = (RoboClaw) Runtime.start("roboclaw", "RoboClaw");

rc.connect(port);

boolean done = false;

while (!done) {
rc.speedAccelDeccelPositionM2(2000,4000,4000,7000,1);
rc.speedAccelDeccelPositionM2(2000, 4000, 4000, 7000, 1);
sleep(1000);
}



if (done) {
return;
}
}

MotorPort m1 = (MotorPort) Runtime.start("m1", "MotorPort");
MotorPort m2 = (MotorPort) Runtime.start("m2", "MotorPort");
// Joystick joy = (Joystick) Runtime.start("joy", "Joystick");
Expand All @@ -2224,7 +2235,7 @@ public static void main(String[] args) {
// doesnt matter for usb connection
// roboclaw.setAddress(128);
// roboclaw.setAddress(129);

// configure services
m1.setPort("m1");
m2.setPort("m2");
Expand All @@ -2248,7 +2259,7 @@ public static void main(String[] args) {
// roboclaw.restoreDefaults();

// roboclaw.resetQuadratureEncoderCounters();

// m1.stop();
// m2.stop();
rc.readEncoderM1();
Expand All @@ -2262,24 +2273,24 @@ public static void main(String[] args) {

rc.readEncoderM1();
rc.readEncoderM1();

rc.resetQuadratureEncoderCounters();

rc.readEncoderM1();
rc.readEncoderM1();


// roboclaw.readEncoderCount();
// roboclaw.read
rc.speedAccelDeccelPositionM1(500,500,500,10000,1);
rc.speedAccelDeccelPositionM1(500, 500, 500, 10000, 1);
rc.driveM1WithSignedDutyAndAccel(255, 255);

rc.readEncoderM1();
rc.readEncoderM1();

m1.move(0);

// public void bufferedDriveM1WithSignedSpeedAccelDeccelPosition(int accel, int speed, int deccel, int pos, int buffer)
// public void bufferedDriveM1WithSignedSpeedAccelDeccelPosition(int
// accel, int speed, int deccel, int pos, int buffer)

// speed up the motor
for (int i = 0; i < 100; ++i) {
Expand All @@ -2296,7 +2307,7 @@ public static void main(String[] args) {
double pwr = i * .01;
log.info("power {}", pwr);
m1.move(pwr);

sleep(100);
}

Expand All @@ -2317,5 +2328,4 @@ public static void main(String[] args) {
}
}


}
2 changes: 1 addition & 1 deletion src/main/java/org/myrobotlab/service/interfaces/EncoderController.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,6 @@ public interface EncoderController extends Attachable {
// > setZeroPoint/deviceId
public void setZeroPoint(Integer deviceId);
// < publishEncoderPosition/deviceId/b16 position
public EncoderData publishEncoderPosition(Integer deviceId, Integer position);
public EncoderData publishEncoderPosition(EncoderData data);

}

0 comments on commit 1e50e56

Please sign in to comment.