Motors working
This commit is contained in:
Scott Barnes
parent
b8e18136b1
commit
6395d3f2f7
5
pom.xml
5
pom.xml
|
|
@ -29,6 +29,11 @@
|
|||
</properties>
|
||||
|
||||
<dependencies>
|
||||
<dependency>
|
||||
<groupId>com.pi4j</groupId>
|
||||
<artifactId>pi4j-plugin-linuxfs</artifactId>
|
||||
<version>${pi4j.version}</version>
|
||||
</dependency>
|
||||
<dependency>
|
||||
<groupId>org.slf4j</groupId>
|
||||
<artifactId>slf4j-api</artifactId>
|
||||
|
|
|
|||
|
|
@ -0,0 +1,403 @@
|
|||
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)
|
||||
}
|
||||
|
||||
|
|
@ -0,0 +1,23 @@
|
|||
package com.tearabite;
|
||||
|
||||
public class DcMotor {
|
||||
private final Board board;
|
||||
private final int index;
|
||||
private int multiplier = 1;
|
||||
|
||||
public DcMotor(Board board, int index) {
|
||||
this.board = board;
|
||||
this.index = index;
|
||||
}
|
||||
|
||||
public void setPower(double power) {
|
||||
int speed = (int) (100 * power * multiplier);
|
||||
this.board.setMotor(index, speed);
|
||||
}
|
||||
|
||||
public void setDirection(Direction direction) {
|
||||
this.multiplier = Direction.Reversed.equals(direction) ? -1 : 1;
|
||||
}
|
||||
|
||||
public enum Direction { Forward, Reversed }
|
||||
}
|
||||
|
|
@ -11,11 +11,11 @@ import com.pi4j.io.gpio.digital.DigitalOutput;
|
|||
import com.pi4j.io.gpio.digital.DigitalState;
|
||||
import com.pi4j.platform.Platforms;
|
||||
import com.pi4j.util.Console;
|
||||
|
||||
import java.lang.reflect.InvocationTargetException;
|
||||
|
||||
/**
|
||||
*
|
||||
* @author luca
|
||||
* @author Scott Barnes
|
||||
*/
|
||||
public class Main {
|
||||
|
||||
|
|
@ -44,34 +44,37 @@ public class Main {
|
|||
}
|
||||
|
||||
private void run(Context pi4j) throws Exception {
|
||||
Platforms platforms = pi4j.platforms();
|
||||
Board board = new Board();
|
||||
DcMotor a = board.getMotor(1);
|
||||
DcMotor b = board.getMotor(2);
|
||||
DcMotor c = board.getMotor(4);
|
||||
DcMotor d = board.getMotor(3);
|
||||
|
||||
console.box("Pi4J PLATFORMS");
|
||||
console.println();
|
||||
platforms.describe().print(System.out);
|
||||
console.println();
|
||||
a.setDirection(DcMotor.Direction.Forward);
|
||||
b.setDirection(DcMotor.Direction.Reversed);
|
||||
c.setDirection(DcMotor.Direction.Reversed);
|
||||
d.setDirection(DcMotor.Direction.Forward);
|
||||
|
||||
var ledConfig = DigitalOutput.newConfigBuilder(pi4j)
|
||||
.id("led")
|
||||
.name("LED Flasher")
|
||||
.address(PIN_LED)
|
||||
.shutdown(DigitalState.LOW)
|
||||
.initial(DigitalState.LOW)
|
||||
.provider("pigpio-digital-output");
|
||||
|
||||
var led = pi4j.create(ledConfig);
|
||||
int counter = 0;
|
||||
while (counter < 50) {
|
||||
if (led.equals(DigitalState.HIGH)) {
|
||||
led.low();
|
||||
System.out.println("low");
|
||||
} else {
|
||||
led.high();
|
||||
System.out.println("high");
|
||||
}
|
||||
for (int i = 0; i < 5; i++) {
|
||||
a.setPower(100f);
|
||||
Thread.sleep(500);
|
||||
a.setPower(0f);
|
||||
Thread.sleep(500);
|
||||
counter++;
|
||||
}
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -0,0 +1,10 @@
|
|||
package com.tearabite;
|
||||
|
||||
public class Robot {
|
||||
|
||||
private Drive drive;
|
||||
|
||||
public Robot() {
|
||||
|
||||
}
|
||||
}
|
||||
Binary file not shown.
Binary file not shown.
|
|
@ -1,2 +1,6 @@
|
|||
module-info.class
|
||||
com/tearabite/Board.class
|
||||
com/tearabite/DcMotor.class
|
||||
com/tearabite/Main.class
|
||||
module-info.class
|
||||
com/tearabite/Robot.class
|
||||
com/tearabite/DcMotor$Direction.class
|
||||
|
|
|
|||
|
|
@ -1,2 +1,3 @@
|
|||
/Users/scott/Developer/EagleBot/eaglebot/src/main/java/com/tearabite/Board.java
|
||||
/Users/scott/Developer/EagleBot/eaglebot/src/main/java/module-info.java
|
||||
/Users/scott/Developer/EagleBot/eaglebot/src/main/java/com/tearabite/Main.java
|
||||
|
|
|
|||
Loading…
Reference in New Issue