Permalink
Browse files

first attempt of uniform motorcontrol & motorcontroller

  • Loading branch information...
gperry
gperry committed Oct 11, 2017
1 parent 54cab1d commit d3a88ebc366ed5bc6ee5ab503d45a40e15080a9c
@@ -486,30 +486,35 @@ public void motorMove(MotorControl mc) {
Class<?> type = mc.getClass();
double powerOutput = mc.getPowerOutput();
// FIXME - do not count on MotorControl's MotorControl's getPowerOutput
// to produce the correct values for MotorController
// double powerOutput = mc.getPowerOutput();
// this is guaranteed to be between -1.0 and 1.0
double powerLevel = mc.getPowerLevel();
if (Motor.class == type) {
Motor motor = (Motor) mc;
if (motor.getPwmFreq() == null) {
motor.setPwmFreq(defaultMotorPwmFreq);
setPWMFreq(motor.getPwrPin(), motor.getPwmFreq());
}
setPinValue(motor.getDirPin(), (powerOutput < 0) ? MOTOR_BACKWARD : MOTOR_FORWARD);
setPinValue(motor.getPwrPin(), powerOutput);
setPinValue(motor.getDirPin(), (powerLevel < 0) ? MOTOR_BACKWARD : MOTOR_FORWARD);
setPinValue(motor.getPwrPin(), powerLevel);
} else if (MotorDualPwm.class == type) {
MotorDualPwm motor = (MotorDualPwm) mc;
log.info(String.format("Adafrutit16C Motor DualPwm motorMove, powerOutput = %s", powerOutput));
log.info(String.format("Adafrutit16C Motor DualPwm motorMove, powerOutput = %s", powerLevel));
if (motor.getPwmFreq() == null) {
motor.setPwmFreq(defaultMotorPwmFreq);
setPWMFreq(motor.getLeftPwmPin(), motor.getPwmFreq());
setPWMFreq(motor.getRightPwmPin(), motor.getPwmFreq());
}
if (powerOutput < 0) {
if (powerLevel < 0) {
setPinValue(motor.getLeftPwmPin(), 0);
setPinValue(motor.getRightPwmPin(), Math.abs(powerOutput / 255));
} else if (powerOutput > 0) {
setPinValue(motor.getRightPwmPin(), Math.abs(powerLevel / 255));
} else if (powerLevel > 0) {
setPinValue(motor.getRightPwmPin(), 0);
setPinValue(motor.getLeftPwmPin(), Math.abs(powerOutput / 255));
setPinValue(motor.getLeftPwmPin(), Math.abs(powerLevel / 255));
} else {
setPinValue(motor.getRightPwmPin(), 0);
setPinValue(motor.getLeftPwmPin(), 0);
@@ -21,11 +21,13 @@
import org.myrobotlab.logging.LoggerFactory;
import org.myrobotlab.logging.Logging;
import org.myrobotlab.logging.LoggingFactory;
import org.myrobotlab.math.Mapper;
import org.myrobotlab.service.interfaces.I2CControl;
import org.myrobotlab.service.interfaces.I2CController;
import org.myrobotlab.service.interfaces.MotorControl;
import org.myrobotlab.service.interfaces.MotorController;
import org.myrobotlab.service.RasPi;
import org.myrobotlab.service.abstracts.AbstractMotorController;
import org.slf4j.Logger;
/**
@@ -37,7 +39,7 @@
* https://learn.adafruit.com/adafruit-dc-and-stepper-motor-hat-for-raspberry-pi/overview
*/
public class AdafruitMotorHat4Pi extends Service implements I2CControl, MotorController {
public class AdafruitMotorHat4Pi extends AbstractMotorController implements I2CControl {
/** version of the library */
static public final String VERSION = "0.9";
@@ -152,6 +154,7 @@ public AdafruitMotorHat4Pi(String n) {
super(n);
refreshControllers();
subscribe(Runtime.getInstance().getName(), "registered", this.getName(), "onRegistered");
powerMapper = new Mapper(-1.0, 1.0, -.98, 0.98);
}
public void onRegistered(ServiceInterface s) {
@@ -266,7 +269,7 @@ public void motorMove(MotorControl mc) {
Class<?> type = mc.getClass();
double powerOutput = mc.getPowerOutput();
double powerOutput = powerMapper.calcOutput(mc.getPowerLevel ());
// log.info(String.format("powerOutput = %.3f", powerOutput));
// Clamp powerOutput between -1 and 1
@@ -35,6 +35,7 @@
import org.myrobotlab.logging.Level;
import org.myrobotlab.logging.Logging;
import org.myrobotlab.logging.LoggingFactory;
import org.myrobotlab.math.Mapper;
import org.myrobotlab.service.data.DeviceMapping;
import org.myrobotlab.service.data.Pin;
import org.myrobotlab.service.data.PinData;
@@ -66,6 +67,8 @@
NeoPixelController, UltrasonicSensorController, PortConnector, RecordControl, SerialRelayListener, PortListener, PortPublisher {
private static final long serialVersionUID = 1L;
Mapper motorPowerMapper = new Mapper(-1.0, 1.0, -255.0, 255.0);
public static class I2CDeviceMap {
public transient I2CControl control;
@@ -1045,7 +1048,7 @@ public void motorMove(MotorControl mc) {
Class<?> type = mc.getClass();
double powerOutput = mc.getPowerOutput();
double powerOutput = motorPowerMapper.calcInput(mc.getPowerLevel());
if (Motor.class == type) {
Motor config = (Motor) mc;
@@ -25,7 +25,7 @@
public MotorHat4Pi(String n) {
super(n);
mapPower(-1, 1, -1, 1);
//mapPower(-1, 1, -1, 1);
refreshControllers();
subscribe(Runtime.getInstance().getName(), "registered", this.getName(), "onRegistered");
}
@@ -6,12 +6,13 @@
import java.util.List;
import java.util.Set;
import org.myrobotlab.framework.Service;
import org.myrobotlab.framework.ServiceType;
import org.myrobotlab.framework.interfaces.Attachable;
import org.myrobotlab.logging.LoggerFactory;
import org.myrobotlab.logging.Logging;
import org.myrobotlab.logging.LoggingFactory;
import org.myrobotlab.math.Mapper;
import org.myrobotlab.service.abstracts.AbstractMotorController;
import org.myrobotlab.service.interfaces.MotorControl;
import org.myrobotlab.service.interfaces.MotorController;
import org.myrobotlab.service.interfaces.PortConnector;
@@ -30,7 +31,7 @@
* @author GroG
*
*/
public class Sabertooth extends Service implements PortConnector, MotorController {
public class Sabertooth extends AbstractMotorController implements PortConnector, MotorController {
private static final long serialVersionUID = 1L;
@@ -76,6 +77,7 @@ public Sabertooth(String n) {
// add motor ports the sabertooth supports
ports.add("m1");
ports.add("m2");
powerMapper = new Mapper(-1.0, 1.0, -127, 127);
}
public void connect(String port) throws Exception {
@@ -231,17 +233,19 @@ public void motorMove(MotorControl mc) {
MotorPort motor = motors.get(mc.getName());
String port = motor.getPort();
double pwr = motor.getPowerLevel();
int power = (int) (pwr * 127);
/// double pwr = motor.getPowerLevel();
int power = (int)powerMapper.calcOutput(mc.getPowerLevel());
// int power = (int) (pwr * 127);
if (port.equals("m1")) {
if (pwr >= 0) {
if (power >= 0) {
driveForwardMotor1(power);
} else {
driveBackwardsMotor1(Math.abs(power));
}
} else if (port.equals("m2")) {
if (pwr >= 0) {
if (power >= 0) {
driveForwardMotor2(power);
} else {
driveBackwardsMotor2(Math.abs(power));
@@ -298,10 +302,16 @@ static public ServiceType getMetaData() {
*/
@Override
public void connect(String port, int rate, int databits, int stopbits, int parity) throws Exception {
if (serial == null){
if (serial == null) {
serial = (Serial) startPeer("serial");
}
log.info("{} opening serial port {}|{}|{}|{}", port, rate, databits, stopbits, parity);
serial.open(port, rate, databits, stopbits, parity);
// not much choice here :(
// sabertooth is not 'readable' and connecting serial is almost always
// an asynchronous process - since we have no way to verify the port is open
// we sadly must
sleep(3000);
}
public void detach(MotorControl device) {
@@ -382,7 +392,6 @@ public void attach(Attachable service) throws Exception {
public boolean isAttached(Attachable service) {
return motors.containsKey(service.getName()) || (serial != null && serial.getName().equals(service.getName()));
}
@Override
public void connect(String port, int rate) throws Exception {
@@ -403,12 +412,11 @@ public void detach() {
}
}
public static void main(String[] args) {
try {
LoggingFactory.init("INFO");
boolean virtual = false;
//////////////////////////////////////////////////////////////////////////////////
// Sabertooth.py
@@ -422,19 +430,19 @@ public static void main(String[] args) {
String port = "/dev/ttyUSB0";
// start optional virtual serial service, used for test
if (virtual){
// use static method Serial.connectVirtualUart to create
// a virtual hardware uart for the serial service to
// connect to
Serial uart = Serial.connectVirtualUart(port);
uart.logRecv(true); // dump bytes sent from sabertooth
if (virtual) {
// use static method Serial.connectVirtualUart to create
// a virtual hardware uart for the serial service to
// connect to
Serial uart = Serial.connectVirtualUart(port);
uart.logRecv(true); // dump bytes sent from sabertooth
}
// start the services
Runtime.start("gui","SwingGui");
Sabertooth sabertooth = (Sabertooth)Runtime.start("sabertooth","Sabertooth");
MotorPort m1 = (MotorPort)Runtime.start("m1","MotorPort");
MotorPort m2 = (MotorPort)Runtime.start("m2","MotorPort");
Joystick joy = (Joystick)Runtime.start("joy","Joystick");
Runtime.start("gui", "SwingGui");
Sabertooth sabertooth = (Sabertooth) Runtime.start("sabertooth", "Sabertooth");
MotorPort m1 = (MotorPort) Runtime.start("m1", "MotorPort");
MotorPort m2 = (MotorPort) Runtime.start("m2", "MotorPort");
Joystick joy = (Joystick) Runtime.start("joy", "Joystick");
// Arduino arduino = (Arduino)Runtime.start("arduino","Arduino");
// configure services
@@ -446,39 +454,38 @@ public static void main(String[] args) {
sabertooth.attach(m1);
sabertooth.attach(m2);
m1.attach(joy.getAxis("y"));
// m2.attach(arduino.getPin("A4"));
m2.attach(joy.getAxis("rz"));
// m2.attach(arduino.getPin("A4"));
// FIXME - sabertooth.attach(motor1) & sabertooth.attach(motor2)
// FIXME - motor1.attach(joystick) !
sabertooth.connect(port);
m1.stop();
m2.stop();
// m1.stop();
// m2.stop();
boolean done = true;
if (done){
if (done) {
return;
}
// speed up the motor
for (int i = 0; i < 100; ++i){
for (int i = 0; i < 100; ++i) {
double pwr = i * .01;
log.info("power {}", pwr);
m1.move(pwr);
sleep(100);
}
sleep(1000);
// slow down the motor
for (int i = 100; i > 0; --i){
for (int i = 100; i > 0; --i) {
double pwr = i * .01;
log.info("power {}", pwr);
m1.move(pwr);
sleep(100);
}
// move motor clockwise
m1.move(0.3);
@@ -489,12 +496,12 @@ public static void main(String[] args) {
m1.move(-0.3);
sleep(1);
m1.stop();
// TODO - stopAndLock
} catch (Exception e) {
Logging.logError(e);
}
}
}
@@ -86,7 +86,10 @@
double maxPower = 1.0;
double minPower = -1.0;
Mapper powerMap = new Mapper(-1.0, 1.0, -255.0, 255.0);
// grog: THIS SHOULD ONLY BE USED FOR INVERTED AND INPUT LIMITS !!!!
// SHOULD NOT BE USED FOR RANGE MAPPING - RANGE MAPPING SHOULD ONLY BE
// IN MOTORCONTROLLER !!!
Mapper powerMap = new Mapper(-1.0, 1.0, -1.0, 1.0);
// position
double currentPos = 0;
@@ -126,15 +129,30 @@ public MotorController getController() {
public double getPowerLevel() {
return powerLevel;
}
/*
// FIXME - remove !!! - no need or desire to map inside controller !
@Override
public double getPowerOutput() {
return powerMap.calcOutput(powerLevel);
}
// FIXME - remove !!! - no need or desire to map inside controller !
public Mapper getPowerMap() {
return powerMap;
}
// FIXME - remove !!! - no need or desire to map inside controller !
public void mapPower(double minX, double maxX, double minY, double maxY) {
powerMap = new Mapper(minX, maxX, minY, maxY);
broadcastState();
}
*/
@Override
public boolean isAttached(MotorController controller) {
@@ -157,11 +175,7 @@ public void mapEncoder(double minX, double maxX, double minY, double maxY) {
broadcastState();
}
public void mapPower(double minX, double maxX, double minY, double maxY) {
powerMap = new Mapper(minX, maxX, minY, maxY);
broadcastState();
}
@Override
// not relative ! - see moveStep
public void move(double power) {
@@ -203,7 +217,9 @@ public void moveTo(double newPos) {
@Override
public void setInverted(boolean invert) {
powerMap.setInverted(invert);
controller.motorMove(this);
// controller.motorMove(this); - motor should not be
// told to move for setting
// inverted
broadcastState();
}
@@ -0,0 +1,17 @@
package org.myrobotlab.service.abstracts;
import org.myrobotlab.framework.Service;
import org.myrobotlab.math.Mapper;
import org.myrobotlab.service.interfaces.MotorController;
public abstract class AbstractMotorController extends Service implements MotorController {
private static final long serialVersionUID = 1L;
protected Mapper powerMapper = null;
public AbstractMotorController(String reservedKey) {
super(reservedKey);
}
}
Oops, something went wrong.

1 comment on commit d3a88eb

@MatsMRL

This comment has been minimized.

Show comment
Hide comment
@MatsMRL

MatsMRL Oct 11, 2017

Contributor

Looks good. Made some minor changes, like removing unused imports, and updated the Mapper in AdafruitMotorHat4Pi to be (-1.0, 1.0, -1.0, 1.0).

Contributor

MatsMRL commented on d3a88eb Oct 11, 2017

Looks good. Made some minor changes, like removing unused imports, and updated the Mapper in AdafruitMotorHat4Pi to be (-1.0, 1.0, -1.0, 1.0).

Please sign in to comment.