tearabite
/
eaglebot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

latest

This commit is contained in:
Scott Barnes 2024-03-24 18:14:57 -05:00
parent 6395d3f2f7
commit a07cf86719
32 changed files with 621 additions and 470 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

403
src/main/java/com/tearabite/Board.java
View File

@ -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)
}

83
src/main/java/com/tearabite/Main.java
View File

@ -7,74 +7,49 @@ package com.tearabite;
import com.pi4j.Pi4J; import com.pi4j.Pi4J;
import com.pi4j.context.Context; import com.pi4j.context.Context;
import com.pi4j.io.gpio.digital.DigitalOutput; import com.pi4j.context.ContextBuilder;
import com.pi4j.io.gpio.digital.DigitalState;
import com.pi4j.platform.Platforms;
import com.pi4j.util.Console; import com.pi4j.util.Console;
import com.tearabite.bot.Arm;
import com.tearabite.hardware.Board;
import java.lang.reflect.InvocationTargetException; import java.lang.reflect.InvocationTargetException;
/**
* @author Scott Barnes
*/
public class Main { public class Main {
private static final int PIN_LED = 22; // PIN 15 = BCM 22
private static final Console console = new Console(); private static final Console console = new Console();
/** public static void main(String[] args) {
* @param args the command line arguments
*/
public static void main(String[] args) throws Exception {
console.box("Hello Rasbian world !"); console.box("Hello Rasbian world !");
Context pi4j = null;
try { try {
pi4j = Pi4J.newAutoContext(); new Main().run();
new Main().run(pi4j);
} catch (InvocationTargetException e) { } catch (InvocationTargetException e) {
console.println("Error: " + e.getTargetException().getMessage()); console.println("Error: " + e.getTargetException().getMessage());
} catch (Exception e) { } catch (Exception e) {
console.println("Error: " + e.getMessage()); console.println("Error: " + e.getMessage());
e.printStackTrace(); e.printStackTrace();
} finally { }
if (pi4j != null) { }
private void run() throws Exception {
ContextBuilder pi4jb = Pi4J.newContextBuilder();
Context pi4j = pi4jb.autoDetect().build();
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(); 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);
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);
}
}
} }

10
src/main/java/com/tearabite/Robot.java
View File

@ -1,10 +0,0 @@
package com.tearabite;
public class Robot {
private Drive drive;
public Robot() {
}
}

52
src/main/java/com/tearabite/base/Component.java Normal file
View File

@ -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();
}
}
}

25
src/main/java/com/tearabite/base/I2CDevice.java Normal file
View File

@ -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) {};
}

51
src/main/java/com/tearabite/bot/Arm.java Normal file
View File

@ -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);
}
}

4
src/main/java/com/tearabite/bot/Drive.java Normal file
View File

@ -0,0 +1,4 @@
package com.tearabite.bot;
public class Drive {
}

13
src/main/java/com/tearabite/bot/Robot.java Normal file
View File

@ -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) {
}
}

39
src/main/java/com/tearabite/hardware/Board.java Normal file
View File

@ -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);
}
}

147
src/main/java/com/tearabite/hardware/BoardCore.java Normal file
View File

@ -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);
}
}
}

4
src/main/java/com/tearabite/DcMotor.java → src/main/java/com/tearabite/hardware/DcMotor.java
View File

@ -1,4 +1,4 @@
package com.tearabite; package com.tearabite.hardware;
public class DcMotor { public class DcMotor {
private final Board board; private final Board board;
@ -12,7 +12,7 @@ public class DcMotor {
public void setPower(double power) { public void setPower(double power) {
int speed = (int) (100 * power * multiplier); int speed = (int) (100 * power * multiplier);
this.board.setMotor(index, speed); this.board.setMotorSpeed(index, speed);
} }
public void setDirection(Direction direction) { public void setDirection(Direction direction) {

211
src/main/java/com/tearabite/hardware/LX16A.java Normal file
View File

@ -0,0 +1,211 @@
package com.tearabite.hardware;
public class LX16A {
#include <Arduino.h>
#include <lx16a-servo.h>
// 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<buflen){
ok = false;
}
if (_debug) {
if (!ok) {
Serial.print("\n\n\tWrote:[ ");
for(int i=0;i<buflen;i++){
Serial.print(" " + String(buf[i]) + ", ");
}
Serial.print("] id " + String(MYID)
+ " cmd " + String(cmd) + " last cmd "
+ String(lastCommand));
Serial.print("\n\tGot :[ ");
for(int i=0;i<got;i++){
Serial.print(" " + String(ret[i]) + ", ");
}
Serial.print("] in "+String(millis()-t0)+"\n");
}
}
}
return ok;
}
bool LX16ABus::rcv(uint8_t cmd, uint8_t *params, int param_len, uint8_t MYID) {
// read back the expected response
uint32_t t0 = millis();
uint32_t tout = time(param_len + 6) + 30; // time in ms for the servo to think
int got = 0;
uint8_t sum = 0;
// if (_deepDebug)
// Serial.println("RCV: ");
int len = 7; // minimum length
while (got < len && ((millis() - t0) < tout)) {
if (available()) {
int ch = read();
// if (_deepDebug)
// Serial.println(" 0x%02x", ch);
switch (got) {
case 0:
case 1:
if (ch != 0x55) {
if (_debug)
Serial.println(" ERR (hdr expected 0x55) 0x"+String(ch,HEX)+" cmd "+String(cmd)+" ID "+String(MYID)+" PacketIndex "+String(got)+"\n");
return false;
}
break;
case 2:
if (ch != MYID && MYID != 0xfe) {
if (_debug)
Serial.println(" ERR (id)\n");
return false;
}
break;
case 3:
if (ch < 3 || ch > 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);
}
}

31
src/main/java/com/tearabite/hardware/Servo.java Normal file
View File

@ -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());
}
}

2
src/main/java/module-info.java
View File

@ -11,6 +11,8 @@ module com.tearabite.eaglebot {
// allow access to classes in the following namespaces for Pi4J annotation processing // allow access to classes in the following namespaces for Pi4J annotation processing
opens com.tearabite to com.pi4j; opens com.tearabite to com.pi4j;
opens com.tearabite.bot to com.pi4j;
opens com.tearabite.hardware to com.pi4j;
//exports com.tearabite; //exports com.tearabite;
} }

BIN
target/classes/com/tearabite/Main.class
View File

Binary file not shown.

BIN
target/classes/com/tearabite/base/Component.class Normal file
View File

Binary file not shown.

BIN
target/classes/com/tearabite/base/I2CDevice.class Normal file
View File

Binary file not shown.

BIN
target/classes/com/tearabite/bot/Arm.class Normal file
View File

Binary file not shown.

BIN
target/classes/com/tearabite/bot/Drive.class Normal file
View File

Binary file not shown.

BIN
target/classes/com/tearabite/bot/Robot.class Normal file
View File

Binary file not shown.

BIN
target/classes/com/tearabite/hardware/Board.class Normal file
View File

Binary file not shown.

BIN
target/classes/com/tearabite/hardware/BoardCore.class Normal file
View File

Binary file not shown.

BIN
target/classes/com/tearabite/hardware/DcMotor$Direction.class Normal file
View File

Binary file not shown.

BIN
target/classes/com/tearabite/hardware/DcMotor.class Normal file
View File

Binary file not shown.

BIN
target/classes/com/tearabite/hardware/Servo.class Normal file
View File

Binary file not shown.

BIN
target/classes/module-info.class
View File

Binary file not shown.

BIN
target/lib/eaglebot.jar
View File

Binary file not shown.

BIN
target/lib/jsch.jar Normal file
View File

Binary file not shown.

BIN
target/lib/pi4j-library-linuxfs.jar Normal file
View File

Binary file not shown.

BIN
target/lib/pi4j-plugin-linuxfs.jar Normal file
View File

Binary file not shown.

14
target/maven-status/maven-compiler-plugin/compile/default-compile/createdFiles.lst
View File

@ -1,6 +1,12 @@
com/tearabite/Board.class com/tearabite/hardware/Board.class
com/tearabite/DcMotor.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 com/tearabite/Main.class
module-info.class module-info.class
com/tearabite/Robot.class com/tearabite/hardware/Servo.class
com/tearabite/DcMotor$Direction.class com/tearabite/base/Component.class
com/tearabite/hardware/DcMotor.class
com/tearabite/hardware/BoardCore.class
com/tearabite/bot/Arm.class

10
target/maven-status/maven-compiler-plugin/compile/default-compile/inputFiles.lst
View File

@ -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/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/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