Add movement class to python
This commit is contained in:
Konstantin Lampalzer
parent
2c5f283a46
commit
d4c49a0a51
18 changed files with 240 additions and 53 deletions
3
client_s2/.idea/misc.xml
generated
3
client_s2/.idea/misc.xml
generated
|
|
@ -1,4 +1,7 @@
|
|||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<project version="4">
|
||||
<component name="ProjectRootManager" version="2" project-jdk-name="Python 3.9" project-jdk-type="Python SDK" />
|
||||
<component name="PyCharmProfessionalAdvertiser">
|
||||
<option name="shown" value="true" />
|
||||
</component>
|
||||
</project>
|
||||
|
|
@ -82,3 +82,20 @@ message OdometryReadResponse {
|
|||
double orientation = 5;
|
||||
}
|
||||
|
||||
message DriveDistanceRequest {
|
||||
Header header = 1;
|
||||
double distance_m = 2;
|
||||
double velocity_m_s = 3;
|
||||
}
|
||||
|
||||
message TurnDegreesRequest {
|
||||
Header header = 1;
|
||||
double angle_degrees = 2;
|
||||
double velocity_rad_s = 3;
|
||||
}
|
||||
|
||||
message DriveRequest {
|
||||
Header header = 1;
|
||||
double linear_velocity_m_s = 2;
|
||||
double angular_velocity_rad_s = 3;
|
||||
}
|
||||
|
|
@ -11,11 +11,11 @@ class CompLibClient(object):
|
|||
|
||||
@staticmethod
|
||||
def send(data: bytes, size: int) -> bytes:
|
||||
# with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as sock:
|
||||
# sock.connect((TCP_SOCKET_HOST, TCP_SOCKET_PORT))
|
||||
with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as sock:
|
||||
sock.connect((TCP_SOCKET_HOST, TCP_SOCKET_PORT))
|
||||
|
||||
with socket.socket(socket.AF_UNIX, socket.SOCK_STREAM) as sock:
|
||||
sock.connect(UNIX_SOCKET_PATH)
|
||||
# with socket.socket(socket.AF_UNIX, socket.SOCK_STREAM) as sock:
|
||||
# sock.connect(UNIX_SOCKET_PATH)
|
||||
|
||||
sock.sendall(size.to_bytes(1, byteorder='big'))
|
||||
sock.sendall(data)
|
||||
|
|
|
|||
|
|
@ -14,7 +14,7 @@ _sym_db = _symbol_database.Default()
|
|||
|
||||
|
||||
|
||||
DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n\rCompLib.proto\x12\x07\x43ompLib\"\x1e\n\x06Header\x12\x14\n\x0cmessage_type\x18\x01 \x01(\t\"3\n\x06Status\x12\x12\n\nsuccessful\x18\x01 \x01(\x08\x12\x15\n\rerror_message\x18\x02 \x01(\t\"1\n\x0eGenericRequest\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\"S\n\x0fGenericResponse\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\x12\x1f\n\x06status\x18\x02 \x01(\x0b\x32\x0f.CompLib.Status\">\n\x1b\x45ncoderReadPositionsRequest\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\"w\n\x1c\x45ncoderReadPositionsResponse\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\x12\x1f\n\x06status\x18\x02 \x01(\x0b\x32\x0f.CompLib.Status\x12\x15\n\tpositions\x18\x03 \x03(\x05\x42\x02\x10\x01\"?\n\x1c\x45ncoderReadVelocitiesRequest\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\"y\n\x1d\x45ncoderReadVelocitiesResponse\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\x12\x1f\n\x06status\x18\x02 \x01(\x0b\x32\x0f.CompLib.Status\x12\x16\n\nvelocities\x18\x03 \x03(\x01\x42\x02\x10\x01\"9\n\x16IRSensorsEnableRequest\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\":\n\x17IRSensorsDisableRequest\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\":\n\x17IRSensorsReadAllRequest\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\"n\n\x18IRSensorsReadAllResponse\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\x12\x1f\n\x06status\x18\x02 \x01(\x0b\x32\x0f.CompLib.Status\x12\x10\n\x04\x64\x61ta\x18\x03 \x03(\rB\x02\x10\x01\"U\n\x15MotorsSetPowerRequest\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\x12\x0c\n\x04port\x18\x02 \x01(\r\x12\r\n\x05power\x18\x03 \x01(\x01\"U\n\x15MotorsSetSpeedRequest\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\x12\x0c\n\x04port\x18\x02 \x01(\r\x12\r\n\x05speed\x18\x03 \x01(\x01\"6\n\x13OdometryReadRequest\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\"\x95\x01\n\x14OdometryReadResponse\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\x12\x1f\n\x06status\x18\x02 \x01(\x0b\x32\x0f.CompLib.Status\x12\x12\n\nx_position\x18\x03 \x01(\x01\x12\x12\n\ny_position\x18\x04 \x01(\x01\x12\x13\n\x0borientation\x18\x05 \x01(\x01\x62\x06proto3')
|
||||
DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n\rCompLib.proto\x12\x07\x43ompLib\"\x1e\n\x06Header\x12\x14\n\x0cmessage_type\x18\x01 \x01(\t\"3\n\x06Status\x12\x12\n\nsuccessful\x18\x01 \x01(\x08\x12\x15\n\rerror_message\x18\x02 \x01(\t\"1\n\x0eGenericRequest\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\"S\n\x0fGenericResponse\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\x12\x1f\n\x06status\x18\x02 \x01(\x0b\x32\x0f.CompLib.Status\">\n\x1b\x45ncoderReadPositionsRequest\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\"w\n\x1c\x45ncoderReadPositionsResponse\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\x12\x1f\n\x06status\x18\x02 \x01(\x0b\x32\x0f.CompLib.Status\x12\x15\n\tpositions\x18\x03 \x03(\x05\x42\x02\x10\x01\"?\n\x1c\x45ncoderReadVelocitiesRequest\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\"y\n\x1d\x45ncoderReadVelocitiesResponse\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\x12\x1f\n\x06status\x18\x02 \x01(\x0b\x32\x0f.CompLib.Status\x12\x16\n\nvelocities\x18\x03 \x03(\x01\x42\x02\x10\x01\"9\n\x16IRSensorsEnableRequest\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\":\n\x17IRSensorsDisableRequest\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\":\n\x17IRSensorsReadAllRequest\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\"n\n\x18IRSensorsReadAllResponse\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\x12\x1f\n\x06status\x18\x02 \x01(\x0b\x32\x0f.CompLib.Status\x12\x10\n\x04\x64\x61ta\x18\x03 \x03(\rB\x02\x10\x01\"U\n\x15MotorsSetPowerRequest\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\x12\x0c\n\x04port\x18\x02 \x01(\r\x12\r\n\x05power\x18\x03 \x01(\x01\"U\n\x15MotorsSetSpeedRequest\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\x12\x0c\n\x04port\x18\x02 \x01(\r\x12\r\n\x05speed\x18\x03 \x01(\x01\"6\n\x13OdometryReadRequest\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\"\x95\x01\n\x14OdometryReadResponse\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\x12\x1f\n\x06status\x18\x02 \x01(\x0b\x32\x0f.CompLib.Status\x12\x12\n\nx_position\x18\x03 \x01(\x01\x12\x12\n\ny_position\x18\x04 \x01(\x01\x12\x13\n\x0borientation\x18\x05 \x01(\x01\"a\n\x14\x44riveDistanceRequest\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\x12\x12\n\ndistance_m\x18\x02 \x01(\x01\x12\x14\n\x0cvelocity_m_s\x18\x03 \x01(\x01\"d\n\x12TurnDegreesRequest\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\x12\x15\n\rangle_degrees\x18\x02 \x01(\x01\x12\x16\n\x0evelocity_rad_s\x18\x03 \x01(\x01\"l\n\x0c\x44riveRequest\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\x12\x1b\n\x13linear_velocity_m_s\x18\x02 \x01(\x01\x12\x1e\n\x16\x61ngular_velocity_rad_s\x18\x03 \x01(\x01\x62\x06proto3')
|
||||
|
||||
|
||||
|
||||
|
|
@ -34,6 +34,9 @@ _MOTORSSETPOWERREQUEST = DESCRIPTOR.message_types_by_name['MotorsSetPowerRequest
|
|||
_MOTORSSETSPEEDREQUEST = DESCRIPTOR.message_types_by_name['MotorsSetSpeedRequest']
|
||||
_ODOMETRYREADREQUEST = DESCRIPTOR.message_types_by_name['OdometryReadRequest']
|
||||
_ODOMETRYREADRESPONSE = DESCRIPTOR.message_types_by_name['OdometryReadResponse']
|
||||
_DRIVEDISTANCEREQUEST = DESCRIPTOR.message_types_by_name['DriveDistanceRequest']
|
||||
_TURNDEGREESREQUEST = DESCRIPTOR.message_types_by_name['TurnDegreesRequest']
|
||||
_DRIVEREQUEST = DESCRIPTOR.message_types_by_name['DriveRequest']
|
||||
Header = _reflection.GeneratedProtocolMessageType('Header', (_message.Message,), {
|
||||
'DESCRIPTOR' : _HEADER,
|
||||
'__module__' : 'CompLib_pb2'
|
||||
|
|
@ -146,6 +149,27 @@ OdometryReadResponse = _reflection.GeneratedProtocolMessageType('OdometryReadRes
|
|||
})
|
||||
_sym_db.RegisterMessage(OdometryReadResponse)
|
||||
|
||||
DriveDistanceRequest = _reflection.GeneratedProtocolMessageType('DriveDistanceRequest', (_message.Message,), {
|
||||
'DESCRIPTOR' : _DRIVEDISTANCEREQUEST,
|
||||
'__module__' : 'CompLib_pb2'
|
||||
# @@protoc_insertion_point(class_scope:CompLib.DriveDistanceRequest)
|
||||
})
|
||||
_sym_db.RegisterMessage(DriveDistanceRequest)
|
||||
|
||||
TurnDegreesRequest = _reflection.GeneratedProtocolMessageType('TurnDegreesRequest', (_message.Message,), {
|
||||
'DESCRIPTOR' : _TURNDEGREESREQUEST,
|
||||
'__module__' : 'CompLib_pb2'
|
||||
# @@protoc_insertion_point(class_scope:CompLib.TurnDegreesRequest)
|
||||
})
|
||||
_sym_db.RegisterMessage(TurnDegreesRequest)
|
||||
|
||||
DriveRequest = _reflection.GeneratedProtocolMessageType('DriveRequest', (_message.Message,), {
|
||||
'DESCRIPTOR' : _DRIVEREQUEST,
|
||||
'__module__' : 'CompLib_pb2'
|
||||
# @@protoc_insertion_point(class_scope:CompLib.DriveRequest)
|
||||
})
|
||||
_sym_db.RegisterMessage(DriveRequest)
|
||||
|
||||
if _descriptor._USE_C_DESCRIPTORS == False:
|
||||
|
||||
DESCRIPTOR._options = None
|
||||
|
|
@ -187,4 +211,10 @@ if _descriptor._USE_C_DESCRIPTORS == False:
|
|||
_ODOMETRYREADREQUEST._serialized_end=1139
|
||||
_ODOMETRYREADRESPONSE._serialized_start=1142
|
||||
_ODOMETRYREADRESPONSE._serialized_end=1291
|
||||
_DRIVEDISTANCEREQUEST._serialized_start=1293
|
||||
_DRIVEDISTANCEREQUEST._serialized_end=1390
|
||||
_TURNDEGREESREQUEST._serialized_start=1392
|
||||
_TURNDEGREESREQUEST._serialized_end=1492
|
||||
_DRIVEREQUEST._serialized_start=1494
|
||||
_DRIVEREQUEST._serialized_end=1602
|
||||
# @@protoc_insertion_point(module_scope)
|
||||
|
|
|
|||
57
client_s2/compLib/Movement.py
Normal file
57
client_s2/compLib/Movement.py
Normal file
|
|
@ -0,0 +1,57 @@
|
|||
import compLib.CompLib_pb2 as CompLib_pb2
|
||||
from compLib.CompLibClient import CompLibClient
|
||||
|
||||
|
||||
class Movement(object):
|
||||
"""High level class to control movement of the robot
|
||||
"""
|
||||
|
||||
@staticmethod
|
||||
def drive_distance(distance: float, speed: float):
|
||||
"""
|
||||
Drive a given distance with a certain speed.
|
||||
Positive distance and speed with result in forward motion. Everything else will move backwards.
|
||||
:param distance: Distance in meters
|
||||
:param speed: Speed in meters per second
|
||||
:return: None
|
||||
"""
|
||||
request = CompLib_pb2.DriveDistanceRequest()
|
||||
request.header.message_type = request.DESCRIPTOR.full_name
|
||||
request.distance_m = distance
|
||||
request.velocity_m_s = speed
|
||||
|
||||
response = CompLib_pb2.GenericResponse()
|
||||
response.ParseFromString(CompLibClient.send(request.SerializeToString(), request.ByteSize()))
|
||||
|
||||
@staticmethod
|
||||
def turn_degrees(degrees: float, speed: float):
|
||||
"""
|
||||
Turn specified degrees with a given speed.
|
||||
Positive degrees and speed with result in counter-clockwise motion. Everything else will be clockwise
|
||||
:param degrees: Degrees between -180 and 180
|
||||
:param speed: Speed in radians per second
|
||||
:return: None
|
||||
"""
|
||||
request = CompLib_pb2.TurnDegreesRequest()
|
||||
request.header.message_type = request.DESCRIPTOR.full_name
|
||||
request.angle_degrees = degrees
|
||||
request.velocity_rad_s = speed
|
||||
|
||||
response = CompLib_pb2.GenericResponse()
|
||||
response.ParseFromString(CompLibClient.send(request.SerializeToString(), request.ByteSize()))
|
||||
|
||||
@staticmethod
|
||||
def drive(linear: float, angular: float):
|
||||
"""
|
||||
Non-blocking way to perform a linear and angular motion at the same time.
|
||||
:param linear: Linear speed in meters per second
|
||||
:param angular: Angular speed in radians per second
|
||||
:return: None
|
||||
"""
|
||||
request = CompLib_pb2.DriveDistanceRequest()
|
||||
request.header.message_type = request.DESCRIPTOR.full_name
|
||||
request.linear_velocity_m_s = linear
|
||||
request.angular_velocity_rad_s = angular
|
||||
|
||||
response = CompLib_pb2.GenericResponse()
|
||||
response.ParseFromString(CompLibClient.send(request.SerializeToString(), request.ByteSize()))
|
||||
Binary file not shown.
Binary file not shown.
BIN
client_s2/compLib/__pycache__/Movement.cpython-39.pyc
Normal file
BIN
client_s2/compLib/__pycache__/Movement.cpython-39.pyc
Normal file
Binary file not shown.
|
|
@ -33,15 +33,57 @@ def main():
|
|||
# from compLib.IRSensor import IRSensor
|
||||
# IRSensor.read_all()
|
||||
#
|
||||
from compLib.Encoder import Encoder
|
||||
Encoder.read_all_positions()
|
||||
Encoder.read_all_velocities()
|
||||
# from compLib.Encoder import Encoder
|
||||
# print(Encoder.read_all_positions())
|
||||
# print(Encoder.read_all_velocities())
|
||||
|
||||
# from compLib.Motor import Motor
|
||||
# Motor.speed(0, 50)
|
||||
# Motor.speed(3, -50)
|
||||
#
|
||||
# import time
|
||||
# time.sleep(2)
|
||||
#
|
||||
# Motor.speed(0, 0)
|
||||
# Motor.speed(3, -0)
|
||||
|
||||
# time.sleep(10)
|
||||
# Motor.power(0, 0)
|
||||
# Motor.power(3, 0)
|
||||
|
||||
# import math
|
||||
# from compLib.Movement import Movement
|
||||
# Movement.turn_degrees(90, math.pi * 2)
|
||||
# Movement.turn_degrees(-90, math.pi * 2)
|
||||
#
|
||||
# Movement.turn_degrees(90, math.pi * 2)
|
||||
# Movement.turn_degrees(90, -math.pi * 2)
|
||||
#
|
||||
# Movement.turn_degrees(90, math.pi * 2)
|
||||
# Movement.turn_degrees(-90, -math.pi * 2)
|
||||
|
||||
# from compLib.Movement import Movement
|
||||
# Movement.drive_distance(0.1, 0.5)
|
||||
# Movement.drive_distance(-0.1, 0.5)
|
||||
#
|
||||
# Movement.drive_distance(0.1, 0.5)
|
||||
# Movement.drive_distance(0.1, -0.5)
|
||||
#
|
||||
# Movement.drive_distance(0.1, 0.5)
|
||||
# Movement.drive_distance(-0.1, -0.5)
|
||||
|
||||
from compLib.Movement import Movement
|
||||
import math
|
||||
Movement.drive_distance(0.5, 0.5)
|
||||
Movement.turn_degrees(90, math.pi * 2)
|
||||
|
||||
Movement.drive_distance(0.5, 0.5)
|
||||
Movement.turn_degrees(90, math.pi * 2)
|
||||
|
||||
Movement.drive_distance(0.5, 0.5)
|
||||
Movement.turn_degrees(90, math.pi * 2)
|
||||
|
||||
Movement.drive_distance(0.5, 0.5)
|
||||
Movement.turn_degrees(90, math.pi * 2)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
|
|
|
|||
|
|
@ -5,6 +5,7 @@
|
|||
#include <cstdint>
|
||||
#include <array>
|
||||
#include <thread>
|
||||
#include <mutex>
|
||||
|
||||
#include "include/PID.hpp"
|
||||
#include "include/Robot.hpp"
|
||||
|
|
@ -46,6 +47,7 @@ private:
|
|||
std::array<double, ROBOT_MOTOR_COUNT> speed_targets{0};
|
||||
|
||||
std::thread speed_control_thread;
|
||||
std::recursive_mutex speed_control_mutex;
|
||||
};
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -21,14 +21,18 @@ public:
|
|||
|
||||
std::array<double, ROBOT_MOTOR_COUNT> get_velocities_rad_s();
|
||||
|
||||
std::array<double, ROBOT_MOTOR_COUNT> get_velocities_ticks_s();
|
||||
|
||||
private:
|
||||
Encoders() = default;
|
||||
|
||||
Cache position_cache{ROBOT_ENCODER_RATE_HZ};
|
||||
Cache velocity_cache{ROBOT_ENCODER_RATE_HZ};
|
||||
Cache ticks_cache{ROBOT_ENCODER_RATE_HZ};
|
||||
|
||||
std::array<int32_t, ROBOT_MOTOR_COUNT> cached_positions = {0};
|
||||
std::array<double, ROBOT_MOTOR_COUNT> cached_velocities_rad_s = {0};
|
||||
std::array<double, ROBOT_MOTOR_COUNT> cached_velocities_ticks_s = {0};
|
||||
};
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -7,9 +7,9 @@
|
|||
#define ROBOT_IR_RATE_HZ 250
|
||||
|
||||
#define ROBOT_MOTOR_COUNT 4
|
||||
#define ROBOT_MOTOR_SPEED_CONTROL_KP 8.5
|
||||
#define ROBOT_MOTOR_SPEED_CONTROL_KI 31.0
|
||||
#define ROBOT_MOTOR_SPEED_CONTROL_KD 0.085
|
||||
#define ROBOT_MOTOR_SPEED_CONTROL_KP 15.0
|
||||
#define ROBOT_MOTOR_SPEED_CONTROL_KI 5.0
|
||||
#define ROBOT_MOTOR_SPEED_CONTROL_KD 0.0
|
||||
#define ROBOT_MOTOR_SPEED_CONTROL_RAMP 4 * M_PI
|
||||
#define ROBOT_MOTOR_SPEED_CONTROL_RATE_HZ 250
|
||||
|
||||
|
|
@ -20,6 +20,7 @@
|
|||
#define ROBOT_ODOMETRY_CONTROLLER_RIGHT_MULT -1.0
|
||||
|
||||
#define ROBOT_ENCODER_RATE_HZ 500
|
||||
#define ROBOT_ENCODER_MAX_CHANGE_RAD_S M_PI_4
|
||||
|
||||
#define ROBOT_WHEEL_RADIUS_MM 35.5
|
||||
#define ROBOT_WHEEL_RADIUS_M (ROBOT_WHEEL_RADIUS_MM / 1000.0)
|
||||
|
|
@ -34,13 +35,13 @@
|
|||
|
||||
#define ROBOT_GO_TO_DISTANCE_RATE_HZ 200
|
||||
#define ROBOT_GO_TO_DISTANCE_ALPHA_P 2.0
|
||||
#define ROBOT_GO_TO_DISTANCE_VELOCITY_P 10.0
|
||||
#define ROBOT_GO_TO_DISTANCE_VELOCITY_P 5.0
|
||||
#define ROBOT_GO_TO_DISTANCE_END_M 0.01 // stop approx 1 cm before target
|
||||
#define ROBOT_GO_TO_DISTANCE_MIN_VEL 0.075
|
||||
|
||||
#define ROBOT_GO_TO_ANGLE_RATE_HZ 200
|
||||
#define ROBOT_GO_TO_ANGLE_END_RAD (0.5 * (M_PI / 180.0)) // stop 0.5 deg before target
|
||||
#define ROBOT_GO_TO_ANGLE_ALPHA_P 0.5
|
||||
#define ROBOT_GO_TO_ANGLE_MIN_VEL (M_PI / 2.0)
|
||||
#define ROBOT_GO_TO_ANGLE_ALPHA_P 0.9
|
||||
#define ROBOT_GO_TO_ANGLE_MIN_VEL M_PI_2
|
||||
|
||||
#endif //COMPLIB_SERVER_ROBOT_HPP
|
||||
|
|
|
|||
|
|
@ -11,11 +11,13 @@
|
|||
#define US_IN_S (1000 * MS_IN_S)
|
||||
|
||||
void ClosedLoopMotorController::set_power(uint8_t port, double power) {
|
||||
std::lock_guard<std::recursive_mutex> lock(speed_control_mutex);
|
||||
control_modes.at(port) = ControlMode::POWER;
|
||||
Motors::set_power(port, power);
|
||||
}
|
||||
|
||||
void ClosedLoopMotorController::set_speed(uint8_t port, double speed_ms) {
|
||||
std::lock_guard<std::recursive_mutex> lock(speed_control_mutex);
|
||||
speed_targets.at(port) = speed_ms;
|
||||
if (control_modes.at(port) != ControlMode::SPEED) {
|
||||
pids.at(port) = PID(
|
||||
|
|
@ -35,15 +37,17 @@ ClosedLoopMotorController::ClosedLoopMotorController() {
|
|||
while (true) {
|
||||
std::this_thread::sleep_for(sleep_duration);
|
||||
|
||||
std::lock_guard<std::recursive_mutex> lock(speed_control_mutex);
|
||||
bool should_control_speed = std::find(control_modes.begin(), control_modes.end(), ControlMode::SPEED) != control_modes.end();
|
||||
if (!should_control_speed) {
|
||||
continue;
|
||||
}
|
||||
|
||||
auto speeds_ms = Encoders::getInstance().get_velocities_rad_s();
|
||||
auto speeds_rad_s = Encoders::getInstance().get_velocities_rad_s();
|
||||
for (int i = 0; i < ROBOT_MOTOR_COUNT; i++) {
|
||||
if (control_modes.at(i) == ControlMode::SPEED) {
|
||||
double power = pids.at(i)(speed_targets.at(i), speeds_ms.at(i));
|
||||
spdlog::debug("CLMC: {}", i);
|
||||
double power = pids.at(i)(speed_targets.at(i), speeds_rad_s.at(i));
|
||||
Motors::set_power(i, power);
|
||||
}
|
||||
}
|
||||
|
|
@ -54,7 +58,7 @@ void ClosedLoopMotorController::generate_step_response_data() {
|
|||
auto console = spdlog::stdout_color_mt("generate_step_response_data");
|
||||
console->set_pattern("%v");
|
||||
|
||||
auto input = 25.0;
|
||||
auto input = 30.0;
|
||||
|
||||
speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) = input * ROBOT_ODOMETRY_CONTROLLER_LEFT_MULT;
|
||||
speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) = input * ROBOT_ODOMETRY_CONTROLLER_RIGHT_MULT;
|
||||
|
|
@ -70,21 +74,21 @@ void ClosedLoopMotorController::generate_step_response_data() {
|
|||
std::this_thread::sleep_for(std::chrono::seconds(1));
|
||||
|
||||
auto start_time = std::chrono::steady_clock::now();
|
||||
for (int i = 0; i < 150; ++i) {
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
||||
for (int i = 0; i < 1000; ++i) {
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(3));
|
||||
double delta_seconds = std::chrono::duration_cast<std::chrono::duration<double, std::micro>>(std::chrono::steady_clock::now() - start_time).count() / US_IN_S;
|
||||
auto output = Encoders::getInstance().get_velocities_rad_s().at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT);
|
||||
console->debug("{:03.4f}; {:03.4f}; {:03.4f}", delta_seconds, input, output);
|
||||
console->info("{:03.4f}; {:03.4f}; {:03.4f}", delta_seconds, input, output);
|
||||
}
|
||||
|
||||
input = 35.0;
|
||||
input = 40.0;
|
||||
speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) = input * ROBOT_ODOMETRY_CONTROLLER_LEFT_MULT;
|
||||
speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) = input * ROBOT_ODOMETRY_CONTROLLER_RIGHT_MULT;
|
||||
for (int i = 0; i < 150; ++i) {
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
||||
for (int i = 0; i < 1000; ++i) {
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(3));
|
||||
double delta_seconds = std::chrono::duration_cast<std::chrono::duration<double, std::micro>>(std::chrono::steady_clock::now() - start_time).count() / US_IN_S;
|
||||
auto output = Encoders::getInstance().get_velocities_rad_s().at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT);
|
||||
console->debug("{:03.4f}; {:03.4f}; {:03.4f}", delta_seconds, input, output);
|
||||
console->info("{:03.4f}; {:03.4f}; {:03.4f}", delta_seconds, input, output);
|
||||
}
|
||||
|
||||
speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) = 0;
|
||||
|
|
@ -102,10 +106,10 @@ void ClosedLoopMotorController::generate_tuned_step_response_data() {
|
|||
speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) = input * ROBOT_ODOMETRY_CONTROLLER_LEFT_MULT;
|
||||
speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) = input * ROBOT_ODOMETRY_CONTROLLER_RIGHT_MULT;
|
||||
pids.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) = PID(
|
||||
8.5, 31.0, 0.085,
|
||||
5.5, 50.0, 0.0,
|
||||
4 * M_PI, 100.0);
|
||||
pids.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) = PID(
|
||||
8.5, 31.0, 0.085,
|
||||
5.5, 50.0, 0.0,
|
||||
4 * M_PI, 100.0);
|
||||
control_modes.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) = ControlMode::SPEED;
|
||||
control_modes.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) = ControlMode::SPEED;
|
||||
|
|
@ -113,31 +117,31 @@ void ClosedLoopMotorController::generate_tuned_step_response_data() {
|
|||
std::this_thread::sleep_for(std::chrono::seconds(2));
|
||||
|
||||
auto start_time = std::chrono::steady_clock::now();
|
||||
for (int i = 0; i < 500; ++i) {
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
||||
for (int i = 0; i < 1500; ++i) {
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(3));
|
||||
double delta_seconds = std::chrono::duration_cast<std::chrono::duration<double, std::micro>>(std::chrono::steady_clock::now() - start_time).count() / US_IN_S;
|
||||
auto output = Encoders::getInstance().get_velocities_rad_s().at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT);
|
||||
console->debug("{:03.4f}; {:03.4f}; {:03.4f}", delta_seconds, input, output);
|
||||
console->info("{:03.4f}; {:03.4f}; {:03.4f}", delta_seconds, input, output);
|
||||
}
|
||||
|
||||
input = 4 * M_PI;
|
||||
speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) = input * ROBOT_ODOMETRY_CONTROLLER_LEFT_MULT;
|
||||
speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) = input * ROBOT_ODOMETRY_CONTROLLER_RIGHT_MULT;
|
||||
for (int i = 0; i < 500; ++i) {
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
||||
for (int i = 0; i < 1500; ++i) {
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(3));
|
||||
double delta_seconds = std::chrono::duration_cast<std::chrono::duration<double, std::micro>>(std::chrono::steady_clock::now() - start_time).count() / US_IN_S;
|
||||
auto output = Encoders::getInstance().get_velocities_rad_s().at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT);
|
||||
console->debug("{:03.4f}; {:03.4f}; {:03.4f}", delta_seconds, input, output);
|
||||
console->info("{:03.4f}; {:03.4f}; {:03.4f}", delta_seconds, input, output);
|
||||
}
|
||||
|
||||
input = M_PI;
|
||||
speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) = input * ROBOT_ODOMETRY_CONTROLLER_LEFT_MULT;
|
||||
speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) = input * ROBOT_ODOMETRY_CONTROLLER_RIGHT_MULT;
|
||||
for (int i = 0; i < 500; ++i) {
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
||||
for (int i = 0; i < 1500; ++i) {
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(3));
|
||||
double delta_seconds = std::chrono::duration_cast<std::chrono::duration<double, std::micro>>(std::chrono::steady_clock::now() - start_time).count() / US_IN_S;
|
||||
auto output = Encoders::getInstance().get_velocities_rad_s().at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT);
|
||||
console->debug("{:03.4f}; {:03.4f}; {:03.4f}", delta_seconds, input, output);
|
||||
console->info("{:03.4f}; {:03.4f}; {:03.4f}", delta_seconds, input, output);
|
||||
|
||||
input += 0.01;
|
||||
speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) = input * ROBOT_ODOMETRY_CONTROLLER_LEFT_MULT;
|
||||
|
|
@ -169,8 +173,8 @@ void ClosedLoopMotorController::calibrate_wheel_ticks(double turns, double ticks
|
|||
|
||||
set_power(port, adjusted_power);
|
||||
|
||||
spdlog::debug("{:03.4f}; {:03.4f}; {:03.4f};",
|
||||
ticks, target_ticks, adjusted_power);
|
||||
spdlog::info("{:03.4f}; {:03.4f}; {:03.4f};",
|
||||
ticks, target_ticks, adjusted_power);
|
||||
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(2));
|
||||
ticks = Encoders::getInstance().get_positions().at(port);
|
||||
|
|
|
|||
|
|
@ -23,9 +23,25 @@ std::array<double, ROBOT_MOTOR_COUNT> Encoders::get_velocities_rad_s() {
|
|||
auto velocity_ticks_s = mathUtils::from_bytes<int16_t>(data + i * 2, 2);
|
||||
auto velocity_rot_s = velocity_ticks_s / ROBOT_TICKS_PER_TURN;
|
||||
auto velocity_rad_s = velocity_rot_s * 2 * M_PI;
|
||||
cached_velocities_rad_s.at(i) = velocity_rad_s;
|
||||
|
||||
auto change = velocity_rad_s - cached_velocities_rad_s.at(i);
|
||||
change = mathUtils::limit_max(change, -ROBOT_ENCODER_MAX_CHANGE_RAD_S, ROBOT_ENCODER_MAX_CHANGE_RAD_S);
|
||||
cached_velocities_rad_s.at(i) = cached_velocities_rad_s.at(i) + change;
|
||||
}
|
||||
velocity_cache.update();
|
||||
}
|
||||
return cached_velocities_rad_s;
|
||||
}
|
||||
|
||||
std::array<double, ROBOT_MOTOR_COUNT> Encoders::get_velocities_ticks_s() {
|
||||
if (ticks_cache.is_expired()) {
|
||||
uint8_t data[ROBOT_MOTOR_COUNT * 2] = {0};
|
||||
Spi::getInstance().read_array(Spi::MOTOR_1_VEL_H, 2 * ROBOT_MOTOR_COUNT, data);
|
||||
for (int i = 0; i < ROBOT_MOTOR_COUNT; ++i) {
|
||||
auto velocity_ticks_s = mathUtils::from_bytes<int16_t>(data + i * 2, 2);
|
||||
cached_velocities_ticks_s.at(i) = velocity_ticks_s;
|
||||
}
|
||||
ticks_cache.update();
|
||||
}
|
||||
return cached_velocities_ticks_s;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -26,8 +26,9 @@ void GoToController::diff_drive_inverse_kinematics(double v_m_s, double w_rad_s)
|
|||
void GoToController::drive_distance(double distance_m, double v_m_s) {
|
||||
auto sleep_duration = std::chrono::microseconds(US_IN_S / ROBOT_GO_TO_DISTANCE_RATE_HZ);
|
||||
|
||||
if (distance_m < 0 && v_m_s > 0) {
|
||||
v_m_s *= -1;
|
||||
if (distance_m < 0 || v_m_s < 0) {
|
||||
distance_m = -fabs(distance_m);
|
||||
v_m_s = -fabs(v_m_s);
|
||||
}
|
||||
|
||||
auto was_enabled = OdometryController::getInstance().is_enabled();
|
||||
|
|
@ -74,8 +75,9 @@ void GoToController::turn_degrees(double angle_deg, double v_rad_s) {
|
|||
auto angle_rad = mathUtils::wrap_angle_to_pi(angle_deg * (M_PI / 180.0));
|
||||
auto sleep_duration = std::chrono::microseconds(US_IN_S / ROBOT_GO_TO_ANGLE_RATE_HZ);
|
||||
|
||||
if (angle_rad < 0 && v_rad_s > 0) {
|
||||
v_rad_s *= -1;
|
||||
if (angle_rad < 0 || v_rad_s < 0) {
|
||||
angle_rad = -fabs(angle_rad);
|
||||
v_rad_s = -fabs(v_rad_s);
|
||||
}
|
||||
|
||||
auto was_enabled = OdometryController::getInstance().is_enabled();
|
||||
|
|
@ -91,12 +93,12 @@ void GoToController::turn_degrees(double angle_deg, double v_rad_s) {
|
|||
|
||||
if (fabs(angle_to_target) > M_PI_2) {
|
||||
diff_drive_inverse_kinematics(0.0, v_rad_s);
|
||||
spdlog::debug("{:03.4f}; {:03.4f};", current_angle, angle_to_target);
|
||||
spdlog::debug("TURN: {:03.4f}; {:03.4f};", current_angle, angle_to_target);
|
||||
} else {
|
||||
auto speed_mult = mathUtils::limit_max(angle_to_target * ROBOT_GO_TO_ANGLE_ALPHA_P, -1.0, 1.0);
|
||||
auto adjusted_speed = mathUtils::limit_min(fabs(v_rad_s) * speed_mult, ROBOT_GO_TO_ANGLE_MIN_VEL);
|
||||
diff_drive_inverse_kinematics(0.0, adjusted_speed);
|
||||
spdlog::debug("{:03.4f}; {:03.4f}; {:03.4f};", current_angle, angle_to_target, adjusted_speed);
|
||||
spdlog::debug("TURN: {:03.4f}; {:03.4f}; {:03.4f};", current_angle, angle_to_target, adjusted_speed);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -80,10 +80,10 @@ OdometryController::OdometryController() {
|
|||
current_odometry = Odometry(new_x, new_y, new_theta);
|
||||
|
||||
|
||||
spdlog::info("{:03.4f} {:03.4f} {:03.4f} {:03.4f} {:03.4f} {:03.4f}",
|
||||
current_position_left, current_position_right,
|
||||
distance_left, distance_right,
|
||||
v, w);
|
||||
spdlog::debug("ODOM: {:03.4f} {:03.4f} {:03.4f} {:03.4f} {:03.4f} {:03.4f}",
|
||||
current_position_left, current_position_right,
|
||||
distance_left, distance_right,
|
||||
v, w);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,6 +1,7 @@
|
|||
#include "include/PID.hpp"
|
||||
#include <chrono>
|
||||
#include <algorithm>
|
||||
#include <spdlog/spdlog.h>
|
||||
|
||||
#define MS_IN_S 1000.0
|
||||
#define US_IN_S (1000.0 * MS_IN_S)
|
||||
|
|
@ -35,12 +36,15 @@ double PID::operator()(double setpoint, double process_variable) {
|
|||
// Tustin transform of the integral part
|
||||
double integral = integral_prev + I * delta_seconds * 0.5f * (error + error_prev);
|
||||
double derivative = D * ((error - error_prev) / delta_seconds);
|
||||
integral = std::clamp(integral, -limit, limit);
|
||||
|
||||
double output = proportional + integral + derivative;
|
||||
|
||||
// anti-windup - limit the output variable
|
||||
output = std::min(std::max(-limit, output), limit);
|
||||
// spdlog::info("E{} P{} I{} D{} O{} EP{} DS{}", error, proportional, integral, derivative, output, error_prev, delta_seconds);
|
||||
output = std::clamp(output, -limit, limit);
|
||||
spdlog::debug("PID: E{:03.4f} P{:03.4f} I{:03.4f} D{:03.4f} O{:03.4f} EP{:03.4f} DS{:03.4f}, SP{:03.4f} PV{:03.4f}",
|
||||
error, proportional, integral, derivative, output, error_prev, delta_seconds, setpoint, process_variable
|
||||
);
|
||||
|
||||
integral_prev = integral;
|
||||
setpoint_prev = setpoint;
|
||||
|
|
|
|||
|
|
@ -14,7 +14,7 @@ using namespace std;
|
|||
int main(int argc, char *argv[]) {
|
||||
Reset::reset_robot();
|
||||
spdlog::set_pattern("%H:%M:%S.%e %^%-8l%$: %v");
|
||||
spdlog::set_level(spdlog::level::debug);
|
||||
spdlog::set_level(spdlog::level::info);
|
||||
|
||||
UnixSocketServer unixSocketServer;
|
||||
TCPSocketServer tcpSocketServer;
|
||||
|
|
@ -39,7 +39,11 @@ int main(int argc, char *argv[]) {
|
|||
|
||||
// GoToGoalController::go_to_point(2, 0, 0.2);
|
||||
// GoToController::drive_distance(-4, 0.2);
|
||||
// GoToController::turn_degrees(180, M_PI);
|
||||
// GoToController::turn_degrees(90, M_PI);
|
||||
// this_thread::sleep_for(std::chrono::milliseconds(100));
|
||||
// GoToController::turn_degrees(-90, M_PI);
|
||||
|
||||
|
||||
// while (true) {
|
||||
// this_thread::sleep_for(std::chrono::milliseconds(10));
|
||||
// ClosedLoopMotorController::getInstance().set_power(0, 100);
|
||||
|
|
@ -69,6 +73,7 @@ int main(int argc, char *argv[]) {
|
|||
|
||||
// ClosedLoopMotorController::getInstance().generate_step_response_data();
|
||||
// ClosedLoopMotorController::getInstance().generate_tuned_step_response_data();
|
||||
// this_thread::sleep_for(std::chrono::milliseconds(1000));
|
||||
std::this_thread::sleep_for(std::chrono::hours(12));
|
||||
return 0;
|
||||
}
|
||||
|
|
|
|||
Reference in a new issue