diff --git a/src/main/java/com/tearabite/Board.java b/src/main/java/com/tearabite/Board.java deleted file mode 100644 index 5be68a3..0000000 --- a/src/main/java/com/tearabite/Board.java +++ /dev/null @@ -1,403 +0,0 @@ -package com.tearabite; - -import com.pi4j.Pi4J; -import com.pi4j.context.Context; -import com.pi4j.context.ContextBuilder; -import com.pi4j.io.i2c.I2C; -import com.pi4j.io.i2c.I2CConfig; -import com.pi4j.io.i2c.I2CProvider; -import com.pi4j.library.pigpio.PiGpio; - -public class Board { - - - private static final int __ADC_BAT_ADDR = 0; - private static final int __SERVO_ADDR = 21; - private static final int __MOTOR_ADDR = 31; - private static final int __SERVO_ADDR_CMD = 40; - - private final int[]__motor_speed = new int[] { 0, 0, 0, 0 }; - private final int[] __servo_angle = new int[] { 0, 0, 0, 0, 0, 0 }; - private final int[] __servo_pulse = new int[] { 0, 0, 0, 0, 0, 0 }; - private static final int __I2C = 0x1; - private static final int __I2C_ADDR = 0x7A; - -// GPIO.setwarnings(False) -// GPIO.setmode(GPIO.BOARD) - - private static final int __RGB_COUNT = 2; - private static final int __RGB_PIN = 12; - private static final int __RGB_FREQ_HZ = 800000; - private static final int __RGB_DMA = 10; - private static final int __RGB_BRIGHTNESS = 120; - private static final int __RGB_CHANNEL = 0; - private static final boolean __RGB_INVERT = false; - private final Context pi4j; - private final I2C device; - - -// RGB = PixelStrip(__RGB_COUNT, __RGB_PIN, __RGB_FREQ_HZ, __RGB_DMA, __RGB_INVERT, __RGB_BRIGHTNESS, __RGB_CHANNEL) -//RGB.begin() -// for i in range(RGB.numPixels()): -// RGB.setPixelColor(i, PixelColor(0,0,0)) -// RGB.show() - - - - public Board() { - ContextBuilder pi4jb = Pi4J.newContextBuilder(); - this.pi4j = pi4jb.autoDetect().build(); - I2CProvider i2CProvider = pi4j.provider("linuxfs-i2c"); - I2CConfig i2cConfig = I2C.newConfigBuilder(pi4j).id("HiwonderMasterPi").bus(__I2C).device(__ADC_BAT_ADDR).build(); - device = i2CProvider.create(i2cConfig); - } - - public int setMotor(int index, int speed) { - if (index < 1 || index > 4) { - throw new IllegalArgumentException(String.format("Invalid motor num: %d", index)); - } - - speed = Math.max(Math.min(speed, 100), -100); - - byte reg = (byte) (__MOTOR_ADDR + --index); - device.writeRegister(reg, speed); - this.__motor_speed[index] = speed; - return __motor_speed[index]; - } - - public DcMotor getMotor(int index) { - return new DcMotor(this, index); - } - -// -// def getMotor(index): -// if index < 1 or index > 4: -// raise AttributeError("Invalid motor num: %d"%index) -// index = index - 1 -// return __motor_speed[index] -// -// def setPWMServoAngle(index, angle): -// if servo_id < 1 or servo_id > 6: -// raise AttributeError("Invalid Servo ID: %d"%servo_id) -// index = servo_id - 1 -// angle = 180 if angle > 180 else angle -// angle = 0 if angle < 0 else angle -// reg = __SERVO_ADDR + index -// with SMBus(__I2C) as bus: -// try: -// msg = i2c_msg.write(__I2C_ADDR, [reg, angle]) -// bus.i2c_rdwr(msg) -// __servo_angle[index] = angle -// __servo_pulse[index] = int(((200 * angle) / 9) + 500) -// -// except: -// msg = i2c_msg.write(__I2C_ADDR, [reg, angle]) -// bus.i2c_rdwr(msg) -// __servo_angle[index] = angle -// __servo_pulse[index] = int(((200 * angle) / 9) + 500) -// -// return __servo_angle[index] -// -// def setPWMServoPulse(servo_id, pulse = 1500, use_time = 1000): -// if servo_id< 1 or servo_id > 6: -// raise AttributeError("Invalid Servo ID: %d" %servo_id) -// deviation_data = yaml_handle.get_yaml_data(yaml_handle.Deviation_file_path) -// index = servo_id - 1 -// pulse += deviation_data[str(servo_id)] -// pulse = 500 if pulse < 500 else pulse -// pulse = 2500 if pulse > 2500 else pulse -// use_time = 0 if use_time < 0 else use_time -// use_time = 30000 if use_time > 30000 else use_time -// buf = [__SERVO_ADDR_CMD, 1] + list(use_time.to_bytes(2, 'little')) + [servo_id,] + list(pulse.to_bytes(2, 'little')) -// -// with SMBus(__I2C) as bus: -// try: -// msg = i2c_msg.write(__I2C_ADDR, buf) -// bus.i2c_rdwr(msg) -// __servo_pulse[index] = pulse -// __servo_angle[index] = int((pulse - 500) * 0.09) -// except BaseException as e: -// print(e) -// msg = i2c_msg.write(__I2C_ADDR, buf) -// bus.i2c_rdwr(msg) -// __servo_pulse[index] = pulse -// __servo_angle[index] = int((pulse - 500) * 0.09) -// -// return __servo_pulse[index] -// -// def setPWMServosPulse(args): -// ''' time,number, id1, pos1, id2, pos2...''' -// deviation_data = yaml_handle.get_yaml_data(yaml_handle.Deviation_file_path) -// arglen = len(args) -// servos = args[2:arglen:2] -// pulses = args[3:arglen:2] -// use_time = args[0] -// use_time = 0 if use_time < 0 else use_time -// use_time = 30000 if use_time > 30000 else use_time -// servo_number = args[1] -// buf = [__SERVO_ADDR_CMD, servo_number] + list(use_time.to_bytes(2, 'little')) -// dat = zip(servos, pulses) -// for (s, p) in dat: -// buf.append(s) -// p += deviation_data[str(s)] -// p = 500 if p < 500 else p -// p = 2500 if p > 2500 else p -// buf += list(p.to_bytes(2, 'little')) -// __servo_pulse[s-1] = p -// __servo_angle[s-1] = int((p - 500) * 0.09) -// -// with SMBus(__I2C) as bus: -// try: -// msg = i2c_msg.write(__I2C_ADDR, buf) -// bus.i2c_rdwr(msg) -// except: -// msg = i2c_msg.write(__I2C_ADDR, buf) -// bus.i2c_rdwr(msg) -// -// -// def getPWMServoAngle(servo_id): -// if servo_id < 1 or servo_id > 6: -// raise AttributeError("Invalid Servo ID: %d"%servo_id) -// index = servo_id - 1 -// return __servo_pulse[index] -// -// def getPWMServoPulse(servo_id): -// if servo_id < 1 or servo_id > 6: -// raise AttributeError("Invalid Servo ID: %d"%servo_id) -// index = servo_id - 1 -// return __servo_pulse[index] -// -// def getBattery(): -// ret = 0 -// with SMBus(__I2C) as bus: -// try: -// msg = i2c_msg.write(__I2C_ADDR, [__ADC_BAT_ADDR,]) -// bus.i2c_rdwr(msg) -// read = i2c_msg.read(__I2C_ADDR, 2) -// bus.i2c_rdwr(read) -// ret = int.from_bytes(bytes(list(read)), 'little') -// -// except: -// msg = i2c_msg.write(__I2C_ADDR, [__ADC_BAT_ADDR,]) -// bus.i2c_rdwr(msg) -// read = i2c_msg.read(__I2C_ADDR, 2) -// bus.i2c_rdwr(read) -// ret = int.from_bytes(bytes(list(read)), 'little') -// -// return ret -// -// def setBuzzer(new_state): -// GPIO.setup(31, GPIO.OUT) -// GPIO.output(31, new_state) -// -// def setBusServoID(oldid, newid): -// """ -// 配置舵机id号, 出厂默认为1 -// :param oldid: 原来的id， 出厂默认为1 -// :param newid: 新的id -// """ -// serial_serro_wirte_cmd(oldid, LOBOT_SERVO_ID_WRITE, newid) -// -// def getBusServoID(id=None): -// """ -// 读取串口舵机id -// :param id: 默认为空 -// :return: 返回舵机id -// """ -// -// while True: -// if id is None: # 总线上只能有一个舵机 -// serial_servo_read_cmd(0xfe, LOBOT_SERVO_ID_READ) -// else: -// serial_servo_read_cmd(id, LOBOT_SERVO_ID_READ) -// # 获取内容 -// msg = serial_servo_get_rmsg(LOBOT_SERVO_ID_READ) -// if msg is not None: -// return msg -// -// def setBusServoPulse(id, pulse, use_time): -// """ -// 驱动串口舵机转到指定位置 -// :param id: 要驱动的舵机id -// :pulse: 位置 -// :use_time: 转动需要的时间 -// """ -// -// pulse = 0 if pulse < 0 else pulse -// pulse = 1000 if pulse > 1000 else pulse -// use_time = 0 if use_time < 0 else use_time -// use_time = 30000 if use_time > 30000 else use_time -// serial_serro_wirte_cmd(id, LOBOT_SERVO_MOVE_TIME_WRITE, pulse, use_time) -// -// def stopBusServo(id=None): -// ''' -// 停止舵机运行 -// :param id: -// :return: -// ''' -// serial_serro_wirte_cmd(id, LOBOT_SERVO_MOVE_STOP) -// -// def setBusServoDeviation(id, d=0): -// """ -// 调整偏差 -// :param id: 舵机id -// :param d: 偏差 -// """ -// serial_serro_wirte_cmd(id, LOBOT_SERVO_ANGLE_OFFSET_ADJUST, d) -// -// def saveBusServoDeviation(id): -// """ -// 配置偏差，掉电保护 -// :param id: 舵机id -// """ -// serial_serro_wirte_cmd(id, LOBOT_SERVO_ANGLE_OFFSET_WRITE) -// -// time_out = 50 -// def getBusServoDeviation(id): -// ''' -// 读取偏差值 -// :param id: 舵机号 -// :return: -// ''' -// # 发送读取偏差指令 -// count = 0 -// while True: -// serial_servo_read_cmd(id, LOBOT_SERVO_ANGLE_OFFSET_READ) -// # 获取 -// msg = serial_servo_get_rmsg(LOBOT_SERVO_ANGLE_OFFSET_READ) -// count += 1 -// if msg is not None: -// return msg -// if count > time_out: -// return None -// -// def setBusServoAngleLimit(id, low, high): -// ''' -// 设置舵机转动范围 -// :param id: -// :param low: -// :param high: -// :return: -// ''' -// serial_serro_wirte_cmd(id, LOBOT_SERVO_ANGLE_LIMIT_WRITE, low, high) -// -// def getBusServoAngleLimit(id): -// ''' -// 读取舵机转动范围 -// :param id: -// :return: 返回元祖 0： 低位 1： 高位 -// ''' -// -// while True: -// serial_servo_read_cmd(id, LOBOT_SERVO_ANGLE_LIMIT_READ) -// msg = serial_servo_get_rmsg(LOBOT_SERVO_ANGLE_LIMIT_READ) -// if msg is not None: -// count = 0 -// return msg -// -// def setBusServoVinLimit(id, low, high): -// ''' -// 设置舵机电压范围 -// :param id: -// :param low: -// :param high: -// :return: -// ''' -// serial_serro_wirte_cmd(id, LOBOT_SERVO_VIN_LIMIT_WRITE, low, high) -// -// def getBusServoVinLimit(id): -// ''' -// 读取舵机转动范围 -// :param id: -// :return: 返回元祖 0： 低位 1： 高位 -// ''' -// while True: -// serial_servo_read_cmd(id, LOBOT_SERVO_VIN_LIMIT_READ) -// msg = serial_servo_get_rmsg(LOBOT_SERVO_VIN_LIMIT_READ) -// if msg is not None: -// return msg -// -// def setBusServoMaxTemp(id, m_temp): -// ''' -// 设置舵机最高温度报警 -// :param id: -// :param m_temp: -// :return: -// ''' -// serial_serro_wirte_cmd(id, LOBOT_SERVO_TEMP_MAX_LIMIT_WRITE, m_temp) -// -// def getBusServoTempLimit(id): -// ''' -// 读取舵机温度报警范围 -// :param id: -// :return: -// ''' -// -// while True: -// serial_servo_read_cmd(id, LOBOT_SERVO_TEMP_MAX_LIMIT_READ) -// msg = serial_servo_get_rmsg(LOBOT_SERVO_TEMP_MAX_LIMIT_READ) -// if msg is not None: -// return msg -// -// def getBusServoPulse(id): -// ''' -// 读取舵机当前位置 -// :param id: -// :return: -// ''' -// while True: -// serial_servo_read_cmd(id, LOBOT_SERVO_POS_READ) -// msg = serial_servo_get_rmsg(LOBOT_SERVO_POS_READ) -// if msg is not None: -// return msg -// -// def getBusServoTemp(id): -// ''' -// 读取舵机温度 -// :param id: -// :return: -// ''' -// while True: -// serial_servo_read_cmd(id, LOBOT_SERVO_TEMP_READ) -// msg = serial_servo_get_rmsg(LOBOT_SERVO_TEMP_READ) -// if msg is not None: -// return msg -// -// def getBusServoVin(id): -// ''' -// 读取舵机电压 -// :param id: -// :return: -// ''' -// while True: -// serial_servo_read_cmd(id, LOBOT_SERVO_VIN_READ) -// msg = serial_servo_get_rmsg(LOBOT_SERVO_VIN_READ) -// if msg is not None: -// return msg -// -// def restBusServoPulse(oldid): -// # 舵机清零偏差和P值中位（500） -// serial_servo_set_deviation(oldid, 0) # 清零偏差 -// time.sleep(0.1) -// serial_serro_wirte_cmd(oldid, LOBOT_SERVO_MOVE_TIME_WRITE, 500, 100) # 中位 -// -//##掉电 -// def unloadBusServo(id): -// serial_serro_wirte_cmd(id, LOBOT_SERVO_LOAD_OR_UNLOAD_WRITE, 0) -// -//##读取是否掉电 -// def getBusServoLoadStatus(id): -// while True: -// serial_servo_read_cmd(id, LOBOT_SERVO_LOAD_OR_UNLOAD_READ) -// msg = serial_servo_get_rmsg(LOBOT_SERVO_LOAD_OR_UNLOAD_READ) -// if msg is not None: -// return msg -// -// setBuzzer(0) -// -//# setMotor(1, 60) -//# setMotor(2, 60) -//# setMotor(3, 60) -//# setMotor(4, 60) -} - diff --git a/src/main/java/com/tearabite/Main.java b/src/main/java/com/tearabite/Main.java index af9325e..70389e6 100644 --- a/src/main/java/com/tearabite/Main.java +++ b/src/main/java/com/tearabite/Main.java @@ -7,74 +7,49 @@ package com.tearabite; import com.pi4j.Pi4J; import com.pi4j.context.Context; -import com.pi4j.io.gpio.digital.DigitalOutput; -import com.pi4j.io.gpio.digital.DigitalState; -import com.pi4j.platform.Platforms; +import com.pi4j.context.ContextBuilder; import com.pi4j.util.Console; +import com.tearabite.bot.Arm; +import com.tearabite.hardware.Board; import java.lang.reflect.InvocationTargetException; -/** - * @author Scott Barnes - */ public class Main { - - private static final int PIN_LED = 22; // PIN 15 = BCM 22 private static final Console console = new Console(); - /** - * @param args the command line arguments - */ - public static void main(String[] args) throws Exception { + public static void main(String[] args) { console.box("Hello Rasbian world !"); - Context pi4j = null; try { - pi4j = Pi4J.newAutoContext(); - new Main().run(pi4j); + new Main().run(); } catch (InvocationTargetException e) { console.println("Error: " + e.getTargetException().getMessage()); } catch (Exception e) { console.println("Error: " + e.getMessage()); e.printStackTrace(); - } finally { - if (pi4j != null) { - pi4j.shutdown(); - } } } - private void run(Context pi4j) throws Exception { - Board board = new Board(); - DcMotor a = board.getMotor(1); - DcMotor b = board.getMotor(2); - DcMotor c = board.getMotor(4); - DcMotor d = board.getMotor(3); + private void run() throws Exception { + ContextBuilder pi4jb = Pi4J.newContextBuilder(); + Context pi4j = pi4jb.autoDetect().build(); - a.setDirection(DcMotor.Direction.Forward); - b.setDirection(DcMotor.Direction.Reversed); - c.setDirection(DcMotor.Direction.Reversed); - d.setDirection(DcMotor.Direction.Forward); - - for (int i = 0; i < 5; i++) { - a.setPower(100f); - Thread.sleep(500); - a.setPower(0f); - Thread.sleep(500); - - b.setPower(100f); - Thread.sleep(500); - b.setPower(0f); - Thread.sleep(500); - - c.setPower(100f); - Thread.sleep(500); - c.setPower(0f); - Thread.sleep(500); - - d.setPower(100f); - Thread.sleep(500); - d.setPower(0f); - Thread.sleep(500); + Board board = new Board(pi4j); + board.start(); + Arm arm = new Arm(board); + board.beep(250); + for(int i = 0; i < 5; i++) { + arm.reset(); + Thread.sleep(2000); + arm.extend(); + Thread.sleep(2000); + arm.spin(-1); + Thread.sleep(1000); + arm.spin(1); + Thread.sleep(1000); } + arm.reset(); + Thread.sleep(3000); + + pi4j.shutdown(); } } diff --git a/src/main/java/com/tearabite/Robot.java b/src/main/java/com/tearabite/Robot.java deleted file mode 100644 index 508897c..0000000 --- a/src/main/java/com/tearabite/Robot.java +++ /dev/null @@ -1,10 +0,0 @@ -package com.tearabite; - -public class Robot { - - private Drive drive; - - public Robot() { - - } -} diff --git a/src/main/java/com/tearabite/base/Component.java b/src/main/java/com/tearabite/base/Component.java new file mode 100644 index 0000000..137481c --- /dev/null +++ b/src/main/java/com/tearabite/base/Component.java @@ -0,0 +1,52 @@ +package com.tearabite.base; + +import com.pi4j.util.Console; + +import java.time.Duration; + +public abstract class Component { + protected static Console console = new Console(); + + protected Component() { + } + + /** + * Override this method to clean up all used resources + */ + public void reset() { + //nothing to do by default + } + + protected void logInfo(String msg, Object... args) { + console.println(String.format(msg, args)); + } + + protected void logError(String msg, Object... args) { + console.println(String.format(msg, args)); + } + + protected void logDebug(String msg, Object... args) { + console.println(String.format(msg, args)); + } + + protected void logException(String msg, Throwable exception){ + console.println(String.format(msg, exception.getMessage())); + } + + /** + * Utility function to sleep for the specified amount of milliseconds. + * An {@link InterruptedException} will be caught and ignored while setting the interrupt flag again. + * + * @param duration Time to sleep + */ + protected void delay(Duration duration) { + try { + long nanos = duration.toNanos(); + long millis = nanos / 1_000_000; + int remainingNanos = (int) (nanos % 1_000_000); + Thread.currentThread().sleep(millis, remainingNanos); + } catch (InterruptedException e) { + Thread.currentThread().interrupt(); + } + } +} \ No newline at end of file diff --git a/src/main/java/com/tearabite/base/I2CDevice.java b/src/main/java/com/tearabite/base/I2CDevice.java new file mode 100644 index 0000000..ef8848c --- /dev/null +++ b/src/main/java/com/tearabite/base/I2CDevice.java @@ -0,0 +1,25 @@ +package com.tearabite.base; + +import com.pi4j.context.Context; +import com.pi4j.io.i2c.I2C; +import com.pi4j.io.i2c.I2CProvider; + +public abstract class I2CDevice extends Component { + protected static final int DEFAULT_BUS = 0x01; + protected final I2C i2c; + + + protected I2CDevice(Context pi4j, int device, String name){ + I2CProvider i2CProvider = pi4j.provider("pigpio-i2c"); + i2c = i2CProvider.create(I2C.newConfigBuilder(pi4j) + .id("I2C-" + DEFAULT_BUS + "@" + device) + .name(name+ "@" + device) + .bus(DEFAULT_BUS) + .device(device) + .build()); + init(i2c); + logDebug("I2C device %s initialized", name); + } + + protected void init(I2C i2c) {}; +} diff --git a/src/main/java/com/tearabite/bot/Arm.java b/src/main/java/com/tearabite/bot/Arm.java new file mode 100644 index 0000000..dca8cbc --- /dev/null +++ b/src/main/java/com/tearabite/bot/Arm.java @@ -0,0 +1,51 @@ +package com.tearabite.bot; + +import com.tearabite.hardware.Board; +import com.tearabite.hardware.Servo; + +public class Arm { + private final Servo claw; + private final Servo wrist; + private final Servo elbow; + private final Servo shoulder; + private final Servo spin; + + public Arm(Board board) { + this.claw = board.getServo(0); + this.claw.scaleRange(0, 0.7); + + this.wrist = board.getServo(2); + this.wrist.scaleRange(-0.8, 0.8); + + this.elbow = board.getServo(3); + this.elbow.scaleRange(-0.6, 0.6); + + this.shoulder = board.getServo(4); + this.shoulder.scaleRange(-0.2, 1); + + this.spin = board.getServo(5); + double fudge = -0.01; + double width = 0.4; + this.spin.scaleRange(-width / 2 + fudge, width / 2 + fudge); + this.reset(); + } + + public void reset() { + this.claw.setPosition(-1); + this.wrist.setPosition(-0.3); + this.elbow.setPosition(1); + this.shoulder.setPosition(-1); + this.spin.setPosition(0); + } + + public void extend() { + this.claw.setPosition(0); + this.wrist.setPosition(0); + this.elbow.setPosition(0); + this.shoulder.setPosition(1); + } + + public void spin(double pos) { + this.spin.setPosition(pos); + } +} diff --git a/src/main/java/com/tearabite/bot/Drive.java b/src/main/java/com/tearabite/bot/Drive.java new file mode 100644 index 0000000..968ec5f --- /dev/null +++ b/src/main/java/com/tearabite/bot/Drive.java @@ -0,0 +1,4 @@ +package com.tearabite.bot; + +public class Drive { +} diff --git a/src/main/java/com/tearabite/bot/Robot.java b/src/main/java/com/tearabite/bot/Robot.java new file mode 100644 index 0000000..b73a2cc --- /dev/null +++ b/src/main/java/com/tearabite/bot/Robot.java @@ -0,0 +1,13 @@ +package com.tearabite.bot; + +import com.tearabite.hardware.Board; + +public class Robot { + + private Drive drive; + private Arm arm; + + public Robot(Board board) { + + } +} diff --git a/src/main/java/com/tearabite/hardware/Board.java b/src/main/java/com/tearabite/hardware/Board.java new file mode 100644 index 0000000..b56341c --- /dev/null +++ b/src/main/java/com/tearabite/hardware/Board.java @@ -0,0 +1,39 @@ +package com.tearabite.hardware; + +import com.pi4j.context.Context; + +public class Board extends BoardCore { + + public Board(Context pi4j) { + super(pi4j); + } + + public void setPwmServoAngle(int index, int angle) { + if (index < 0 || index >= 6) { + throw new IllegalArgumentException(String.format("Invalid servo num: %d", index)); + } + + this.servoAngles[index] = Math.max(Math.min(angle, 180), 0); + } + + public void setMotorSpeed(int index, int speed) { + if (index < 0 || index >= 4) { + throw new IllegalArgumentException(String.format("Invalid motor num: %d", index)); + } + + this.motorSpeeds[index] = Math.max(Math.min(speed, 100), -100); + } + + public void beep(int duration) { + this.beepUntil = System.currentTimeMillis() + duration; + } + + public DcMotor getMotor(int index) { + return new DcMotor(this, index); + } + + public Servo getServo(int index) { + return new Servo(this, (byte) index); + } +} + diff --git a/src/main/java/com/tearabite/hardware/BoardCore.java b/src/main/java/com/tearabite/hardware/BoardCore.java new file mode 100644 index 0000000..da354c0 --- /dev/null +++ b/src/main/java/com/tearabite/hardware/BoardCore.java @@ -0,0 +1,147 @@ +package com.tearabite.hardware; + +import com.pi4j.context.Context; +import com.pi4j.io.gpio.digital.DigitalOutput; +import com.pi4j.io.gpio.digital.DigitalState; +import com.pi4j.util.Console; +import com.tearabite.base.I2CDevice; + +import java.nio.ByteBuffer; + +import static java.lang.Thread.sleep; + +public class BoardCore extends I2CDevice implements Runnable { + + private static final int I2C_BAT_ADDR = 0; + private static final byte I2C_SERVO_ADDR = 21; + private static final int I2C_SERVO_CMD_ADDR = 40; + private static final int I2C_MOTOR_ADDR = 31; + private static final int I2C_BUS = 1; + private static final int I2C_DEVICE_ADDR = 0x7A; + private static final int BUZZER_PIN = 6; + + private static final int LOBOT_SERVO_FRAME_HEADER = 0x55; + private static final int LOBOT_SERVO_MOVE_TIME_WRITE = 1; + private static final int LOBOT_SERVO_MOVE_TIME_READ = 2; + private static final int LOBOT_SERVO_MOVE_TIME_WAIT_WRITE = 7; + private static final int LOBOT_SERVO_MOVE_TIME_WAIT_READ = 8; + private static final int LOBOT_SERVO_MOVE_START = 11; + private static final int LOBOT_SERVO_MOVE_STOP = 12; + private static final int LOBOT_SERVO_ID_WRITE = 13; + private static final int LOBOT_SERVO_ID_READ = 14; + private static final int LOBOT_SERVO_ANGLE_OFFSET_ADJUST = 17; + private static final int LOBOT_SERVO_ANGLE_OFFSET_WRITE = 18; + private static final int LOBOT_SERVO_ANGLE_OFFSET_READ = 19; + private static final int LOBOT_SERVO_ANGLE_LIMIT_WRITE = 20; + private static final int LOBOT_SERVO_ANGLE_LIMIT_READ = 21; + private static final int LOBOT_SERVO_VIN_LIMIT_WRITE = 22; + private static final int LOBOT_SERVO_VIN_LIMIT_READ = 23; + private static final int LOBOT_SERVO_TEMP_MAX_LIMIT_WRITE = 24; + private static final int LOBOT_SERVO_TEMP_MAX_LIMIT_READ = 25; + private static final int LOBOT_SERVO_TEMP_READ = 26; + private static final int LOBOT_SERVO_VIN_READ = 27; + private static final int LOBOT_SERVO_POS_READ = 28; + private static final int LOBOT_SERVO_OR_MOTOR_MODE_WRITE = 29; + private static final int LOBOT_SERVO_OR_MOTOR_MODE_READ = 30; + private static final int LOBOT_SERVO_LOAD_OR_UNLOAD_WRITE = 31; + private static final int LOBOT_SERVO_LOAD_OR_UNLOAD_READ = 32; + private static final int LOBOT_SERVO_LED_CTRL_WRITE = 33; + private static final int LOBOT_SERVO_LED_CTRL_READ = 34; + private static final int LOBOT_SERVO_LED_ERROR_WRITE = 35; + private static final int LOBOT_SERVO_LED_ERROR_READ = 36; + +// GPIO.setwarnings(False) +// GPIO.setmode(GPIO.BOARD) + + private static final int __RGB_COUNT = 2; + private static final int __RGB_PIN = 12; + private static final int __RGB_FREQ_HZ = 800000; + private static final int __RGB_DMA = 10; + private static final int __RGB_BRIGHTNESS = 120; + private static final int __RGB_CHANNEL = 0; + private static final boolean __RGB_INVERT = false; + private static final Console console = new Console(); + private final DigitalOutput buzzer; + + protected final int[] motorSpeeds = new int[] { 0, 0, 0, 0 }; + protected final int[] servoAngles = new int[] { 0, 0, 0, 0, 0, 0 }; + protected int battery; + protected long beepUntil; + + public BoardCore(Context pi4j) { + super(pi4j, I2C_DEVICE_ADDR, "MasterPiBoard"); + this.buzzer = pi4j.create(DigitalOutput.newConfigBuilder(pi4j) + .address(BUZZER_PIN) + .shutdown(DigitalState.LOW) + .initial(DigitalState.LOW) + .provider("pigpio-digital-output") + .build()); + } + + private void setMotor(int index, int speed) { + if (!i2c.isOpen()) { + return; + } + + byte reg = (byte) (I2C_MOTOR_ADDR + index); + i2c.writeRegister(reg, speed); + } + + private void setServo(int index, int angle) { + if (!i2c.isOpen()) { + return; + } + + byte reg = (byte) (I2C_SERVO_ADDR + index); + i2c.writeRegister(reg, angle); + } + + private void updateBattery() { + i2c.write(I2C_BAT_ADDR); + byte[] bytes = i2c.readNBytes(2); + this.battery = ByteBuffer.wrap(bytes).getShort(); + } + + private void startBeep() { + buzzer.high(); + } + + private void stopBeep() { + buzzer.low(); + } + + private void scan() { + for (int motorIndex = 0; motorIndex < 4; motorIndex++) { + this.setMotor(motorIndex, motorSpeeds[motorIndex]); + } + + for (int servoIndex = 0; servoIndex < 6; servoIndex++) { + this.setServo(servoIndex, servoAngles[servoIndex]); + } + + this.updateBattery(); + + if (System.currentTimeMillis() < this.beepUntil) { + this.startBeep(); + } else { + this.stopBeep(); + } + } + + public void start() { + new Thread(this).start(); + } + + @Override + public void run() { + try { + while (true) { + scan(); + sleep(20); + } + } catch (InterruptedException e) { + throw new RuntimeException(e); + } + } +} + diff --git a/src/main/java/com/tearabite/DcMotor.java b/src/main/java/com/tearabite/hardware/DcMotor.java similarity index 86% rename from src/main/java/com/tearabite/DcMotor.java rename to src/main/java/com/tearabite/hardware/DcMotor.java index 226f4fb..d4f6196 100644 --- a/src/main/java/com/tearabite/DcMotor.java +++ b/src/main/java/com/tearabite/hardware/DcMotor.java @@ -1,4 +1,4 @@ -package com.tearabite; +package com.tearabite.hardware; public class DcMotor { private final Board board; @@ -12,7 +12,7 @@ public class DcMotor { public void setPower(double power) { int speed = (int) (100 * power * multiplier); - this.board.setMotor(index, speed); + this.board.setMotorSpeed(index, speed); } public void setDirection(Direction direction) { diff --git a/src/main/java/com/tearabite/hardware/LX16A.java b/src/main/java/com/tearabite/hardware/LX16A.java new file mode 100644 index 0000000..3a920e5 --- /dev/null +++ b/src/main/java/com/tearabite/hardware/LX16A.java @@ -0,0 +1,211 @@ +package com.tearabite.hardware; + +public class LX16A { + #include +#include + + // write a command with the provided parameters +// returns true if the command was written without conflict onto the bus + bool LX16ABus::write_no_retry(uint8_t cmd, const uint8_t *params, int param_cnt, + uint8_t MYID) { + if (param_cnt < 0 || param_cnt > 4) + return false; + + // prepare packet in a buffer + int buflen = 6 + param_cnt; + uint8_t buf[buflen]; + uint8_t ret[buflen]; + buf[0] = 0x55; + buf[1] = 0x55; + buf[2] = MYID; + buf[3] = buflen - 3; + buf[4] = cmd; + for (int i = 0; i < param_cnt; i++) + buf[5 + i] = params[i]; + uint8_t cksum = 0; + for (int i = 2; i < buflen - 1; i++) + cksum += buf[i]; + buf[buflen - 1] = ~cksum; + +// clear input buffer + int junkCount = 0; + delayMicroseconds(timeus(2)); + while (available()) { + if (junkCount == 0) { +// if (_debug) +// Serial.print("\n\t\t[ "); + } +// if (_debug) +// Serial.print(" " + String(read()) + ", "); +// else + read(); + delayMicroseconds(timeus(3)); + junkCount++; + } + +// if (_debug && junkCount!=0) { +// Serial.print("]\n "); +// Serial.println( +// "\t\t Junk bytes = " + String(junkCount) + " id " + String(MYID) +// + " cmd " + String(cmd) + " last cmd " +// + String(lastCommand)); +// } + lastCommand = cmd; + // send command packet + uint32_t t0 = millis(); + if(!singlePinMode) + if (myTXFlagGPIO >= 0) { + digitalWrite(myTXFlagGPIO, 1); + } + delayMicroseconds(timeus(1)); + write(buf, buflen); + delayMicroseconds(timeus(1)); + if(!singlePinMode) + if (myTXFlagGPIO >= 0) { + digitalWrite(myTXFlagGPIO, 0); + } + // expect to read back command by virtue of single-pin loop-back + uint32_t tout = time(buflen+4) + 4; // 2ms margin + int got = 0; + bool ok = true; + if(!singlePinMode){ + if (_deepDebug) + Serial.println("RCV: "); + while ((got < buflen) && ((millis() - t0) < tout)) { + if (available()) { + ret[got] = read(); + if (ret[got] != buf[got]) { + ok = false; + } + got++; + } + } + if (got 7) { + if (_debug) + Serial.println(" ERR (len)\n"); + return false; + } + len = ch + 3; + if (len > param_len + 6) { + if (_debug) + Serial.println( + " ERR (param_len) got " + String(len) + + " expected " + + String((param_len + 6))); + return false; + } + break; + case 4: + if (ch != cmd) { + if (_debug) + Serial.println(" ERR (cmd)\n"); + return false; + } + break; + default: + if (got == len - 1) { + if ((uint8_t) ch == (uint8_t) ~sum) { + //if (_deepDebug) + // Serial.println(" OK\n"); + return true; + } else { + if (_debug) + Serial.println(" ERR (cksum!="+String(~sum)+")\n"); + return false; + } + } + if (got - 5 > param_len) { + if (_debug) + Serial.println(" ERR (cksum)\n"); + return false; + } + params[got - 5] = ch; + } + if (got > 1) + sum += ch; + got++; + } + } + if (_debug){ + long done = millis(); + Serial.println( + "Read TIMEOUT Expected " + String(len) + " got " + String(got) + + + " on cmd: " + String(cmd) + " id " + String(MYID) + +" took "+String(done-t0)); + } + return false; + } + // read sends a command to the servo and reads back the response into the params buffer. +// returns true if everything checks out correctly. + bool LX16ABus::read_no_retry(uint8_t cmd, uint8_t *params, int param_len, + uint8_t MYID) { + // send the read command + bool ok = write(cmd, NULL, 0, MYID); + if (!ok) { + if (_debug) + Serial.println( + "Command of read failed on cmd: " + String(cmd) + " id " + + String(MYID)); + return false; + } + + return rcv(cmd, params, param_len, MYID); + } +} diff --git a/src/main/java/com/tearabite/hardware/Servo.java b/src/main/java/com/tearabite/hardware/Servo.java new file mode 100644 index 0000000..94c61bd --- /dev/null +++ b/src/main/java/com/tearabite/hardware/Servo.java @@ -0,0 +1,31 @@ +package com.tearabite.hardware; + +public class Servo { + private final Board board; + private final byte index; + private double position; + private double min; + private double max; + + public Servo(Board board, byte index) { + this.board = board; + this.index = index; + } + + public void setPosition(double position) { + double remappedValue = ((position + 1) / 2) * (max - min) + min; + this.position = remappedValue; + double scaledValue = remappedValue * 180 + 90; + this.board.setPwmServoAngle(this.index, (int)scaledValue); + } + + public double getPosition() { + return this.position; + } + + public void scaleRange(double min, double max) { + this.min = min; + this.max = max; + this.setPosition(this.getPosition()); + } +} diff --git a/src/main/java/module-info.java b/src/main/java/module-info.java index 55b7c66..7291f19 100644 --- a/src/main/java/module-info.java +++ b/src/main/java/module-info.java @@ -11,6 +11,8 @@ module com.tearabite.eaglebot { // allow access to classes in the following namespaces for Pi4J annotation processing opens com.tearabite to com.pi4j; + opens com.tearabite.bot to com.pi4j; + opens com.tearabite.hardware to com.pi4j; //exports com.tearabite; } \ No newline at end of file diff --git a/target/classes/com/tearabite/Main.class b/target/classes/com/tearabite/Main.class index f3004b9..324d474 100644 Binary files a/target/classes/com/tearabite/Main.class and b/target/classes/com/tearabite/Main.class differ diff --git a/target/classes/com/tearabite/base/Component.class b/target/classes/com/tearabite/base/Component.class new file mode 100644 index 0000000..3fbad2a Binary files /dev/null and b/target/classes/com/tearabite/base/Component.class differ diff --git a/target/classes/com/tearabite/base/I2CDevice.class b/target/classes/com/tearabite/base/I2CDevice.class new file mode 100644 index 0000000..7ef32be Binary files /dev/null and b/target/classes/com/tearabite/base/I2CDevice.class differ diff --git a/target/classes/com/tearabite/bot/Arm.class b/target/classes/com/tearabite/bot/Arm.class new file mode 100644 index 0000000..6a31c9d Binary files /dev/null and b/target/classes/com/tearabite/bot/Arm.class differ diff --git a/target/classes/com/tearabite/bot/Drive.class b/target/classes/com/tearabite/bot/Drive.class new file mode 100644 index 0000000..74cf2c6 Binary files /dev/null and b/target/classes/com/tearabite/bot/Drive.class differ diff --git a/target/classes/com/tearabite/bot/Robot.class b/target/classes/com/tearabite/bot/Robot.class new file mode 100644 index 0000000..3ce5eda Binary files /dev/null and b/target/classes/com/tearabite/bot/Robot.class differ diff --git a/target/classes/com/tearabite/hardware/Board.class b/target/classes/com/tearabite/hardware/Board.class new file mode 100644 index 0000000..1d85d54 Binary files /dev/null and b/target/classes/com/tearabite/hardware/Board.class differ diff --git a/target/classes/com/tearabite/hardware/BoardCore.class b/target/classes/com/tearabite/hardware/BoardCore.class new file mode 100644 index 0000000..1bb7d93 Binary files /dev/null and b/target/classes/com/tearabite/hardware/BoardCore.class differ diff --git a/target/classes/com/tearabite/hardware/DcMotor$Direction.class b/target/classes/com/tearabite/hardware/DcMotor$Direction.class new file mode 100644 index 0000000..da39672 Binary files /dev/null and b/target/classes/com/tearabite/hardware/DcMotor$Direction.class differ diff --git a/target/classes/com/tearabite/hardware/DcMotor.class b/target/classes/com/tearabite/hardware/DcMotor.class new file mode 100644 index 0000000..ed3f4ab Binary files /dev/null and b/target/classes/com/tearabite/hardware/DcMotor.class differ diff --git a/target/classes/com/tearabite/hardware/Servo.class b/target/classes/com/tearabite/hardware/Servo.class new file mode 100644 index 0000000..9053664 Binary files /dev/null and b/target/classes/com/tearabite/hardware/Servo.class differ diff --git a/target/classes/module-info.class b/target/classes/module-info.class index 69d80af..8712710 100644 Binary files a/target/classes/module-info.class and b/target/classes/module-info.class differ diff --git a/target/lib/eaglebot.jar b/target/lib/eaglebot.jar index d1234e7..7904c49 100644 Binary files a/target/lib/eaglebot.jar and b/target/lib/eaglebot.jar differ diff --git a/target/lib/jsch.jar b/target/lib/jsch.jar new file mode 100644 index 0000000..c6fd21d Binary files /dev/null and b/target/lib/jsch.jar differ diff --git a/target/lib/pi4j-library-linuxfs.jar b/target/lib/pi4j-library-linuxfs.jar new file mode 100644 index 0000000..4dd24a2 Binary files /dev/null and b/target/lib/pi4j-library-linuxfs.jar differ diff --git a/target/lib/pi4j-plugin-linuxfs.jar b/target/lib/pi4j-plugin-linuxfs.jar new file mode 100644 index 0000000..aa1cce7 Binary files /dev/null and b/target/lib/pi4j-plugin-linuxfs.jar differ diff --git a/target/maven-status/maven-compiler-plugin/compile/default-compile/createdFiles.lst b/target/maven-status/maven-compiler-plugin/compile/default-compile/createdFiles.lst index d67dd8c..d678bf5 100644 --- a/target/maven-status/maven-compiler-plugin/compile/default-compile/createdFiles.lst +++ b/target/maven-status/maven-compiler-plugin/compile/default-compile/createdFiles.lst @@ -1,6 +1,12 @@ -com/tearabite/Board.class -com/tearabite/DcMotor.class +com/tearabite/hardware/Board.class +com/tearabite/bot/Robot.class +com/tearabite/base/I2CDevice.class +com/tearabite/bot/Drive.class +com/tearabite/hardware/DcMotor$Direction.class com/tearabite/Main.class module-info.class -com/tearabite/Robot.class -com/tearabite/DcMotor$Direction.class +com/tearabite/hardware/Servo.class +com/tearabite/base/Component.class +com/tearabite/hardware/DcMotor.class +com/tearabite/hardware/BoardCore.class +com/tearabite/bot/Arm.class diff --git a/target/maven-status/maven-compiler-plugin/compile/default-compile/inputFiles.lst b/target/maven-status/maven-compiler-plugin/compile/default-compile/inputFiles.lst index f804d08..18357c8 100644 --- a/target/maven-status/maven-compiler-plugin/compile/default-compile/inputFiles.lst +++ b/target/maven-status/maven-compiler-plugin/compile/default-compile/inputFiles.lst @@ -1,3 +1,11 @@ -/Users/scott/Developer/EagleBot/eaglebot/src/main/java/com/tearabite/Board.java +/Users/scott/Developer/EagleBot/eaglebot/src/main/java/com/tearabite/hardware/Board.java +/Users/scott/Developer/EagleBot/eaglebot/src/main/java/com/tearabite/bot/Arm.java +/Users/scott/Developer/EagleBot/eaglebot/src/main/java/com/tearabite/hardware/BoardCore.java +/Users/scott/Developer/EagleBot/eaglebot/src/main/java/com/tearabite/base/Component.java +/Users/scott/Developer/EagleBot/eaglebot/src/main/java/com/tearabite/base/I2CDevice.java /Users/scott/Developer/EagleBot/eaglebot/src/main/java/module-info.java /Users/scott/Developer/EagleBot/eaglebot/src/main/java/com/tearabite/Main.java +/Users/scott/Developer/EagleBot/eaglebot/src/main/java/com/tearabite/bot/Robot.java +/Users/scott/Developer/EagleBot/eaglebot/src/main/java/com/tearabite/hardware/DcMotor.java +/Users/scott/Developer/EagleBot/eaglebot/src/main/java/com/tearabite/bot/Drive.java +/Users/scott/Developer/EagleBot/eaglebot/src/main/java/com/tearabite/hardware/Servo.java