diff --git a/software/environment/rover/UDEV_rules/99-rover-usb-serial.rules b/software/environment/rover/UDEV_rules/99-rover-usb-serial.rules index 1c19eb2..9645ba8 100644 --- a/software/environment/rover/UDEV_rules/99-rover-usb-serial.rules +++ b/software/environment/rover/UDEV_rules/99-rover-usb-serial.rules @@ -14,8 +14,8 @@ SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{seria SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM2CZLZF", ENV{ID_USB_INTERFACE_NUM}=="03", SYMLINK+="rover/ttyIRIS_0_3" SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM2HPA6V", ENV{ID_USB_INTERFACE_NUM}=="00", SYMLINK+="rover/ttyIRIS_1_0" -SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM2HPA6V", ENV{ID_USB_INTERFACE_NUM}=="01", SYMLINK+="rover/ttyOdometry" -SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM2HPA6V", ENV{ID_USB_INTERFACE_NUM}=="02", SYMLINK+="rover/ttyTowerAndPanTilt" +SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM2HPA6V", ENV{ID_USB_INTERFACE_NUM}=="01", SYMLINK+="rover/ttyTowerAndPanTilt" +SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM2HPA6V", ENV{ID_USB_INTERFACE_NUM}=="02", SYMLINK+="rover/ttyOdometry" SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM2HPA6V", ENV{ID_USB_INTERFACE_NUM}=="03", SYMLINK+="rover/ttyIRIS_1_3" SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM34WE0R", ENV{ID_USB_INTERFACE_NUM}=="00", SYMLINK+="rover/ttyIRIS_2_0" diff --git a/software/firmware/tower_pan_tilt/tower_pan_tilt.ino b/software/firmware/tower_pan_tilt/tower_pan_tilt.ino index f7bd4b1..14736d6 100644 --- a/software/firmware/tower_pan_tilt/tower_pan_tilt.ino +++ b/software/firmware/tower_pan_tilt/tower_pan_tilt.ino @@ -38,12 +38,12 @@ bool communication_good = false; uint8_t message_count = 0; // Pan/tilt hard limits -const int pan_min = 1415; -const int pan_center = 1538; -const int pan_max = 1665; +const int pan_min = 1470; +const int pan_center = 1600; +const int pan_max = 1725; -const int tilt_min = 0; -const int tilt_center = 1900; +const int tilt_min = 1020; +const int tilt_center = 1820; const int tilt_max = 2400; // Pan/tilt positions @@ -57,6 +57,8 @@ Servo pan_servo; Servo tilt_servo; void setup() { +// Serial.begin(9600); +// while(!Serial); setup_hardware(); num_modbus_registers = sizeof(modbus_data) / sizeof(modbus_data[0]); @@ -82,8 +84,8 @@ void setup_hardware() { pan_servo.attach(HARDWARE::SERVO_PAN); tilt_servo.attach(HARDWARE::SERVO_TILT); - pan_servo.write(pan_center); - tilt_servo.write(tilt_center); + pan_servo.writeMicroseconds(pan_center); + tilt_servo.writeMicroseconds(tilt_center); pinMode(HARDWARE::LED_RED, OUTPUT); pinMode(HARDWARE::LED_GREEN, OUTPUT); @@ -124,8 +126,8 @@ void set_leds() { void set_pan_tilt_adjustments() { if (communication_good) { if (modbus_data[MODBUS_REGISTERS::CENTER_ALL]) { - pan_servo.write(constrain(pan_position, pan_min, pan_max)); - tilt_servo.write(constrain(tilt_position, tilt_min, tilt_max)); + pan_servo.writeMicroseconds(constrain(pan_position, pan_min, pan_max)); + tilt_servo.writeMicroseconds(constrain(tilt_position, tilt_min, tilt_max)); pan_position = pan_center; tilt_position = tilt_center; @@ -133,11 +135,15 @@ void set_pan_tilt_adjustments() { modbus_data[MODBUS_REGISTERS::CENTER_ALL] = 0; } - pan_position = pan_position - modbus_data[MODBUS_REGISTERS::PAN_ADJUST_POSITIVE] + modbus_data[MODBUS_REGISTERS::PAN_ADJUST_NEGATIVE]; - tilt_position = tilt_position + modbus_data[MODBUS_REGISTERS::TILT_ADJUST_POSITIVE] - modbus_data[MODBUS_REGISTERS::TILT_ADJUST_NEGATIVE]; + pan_position = constrain(pan_position - modbus_data[MODBUS_REGISTERS::PAN_ADJUST_POSITIVE] + modbus_data[MODBUS_REGISTERS::PAN_ADJUST_NEGATIVE], pan_min, pan_max); + tilt_position = constrain(tilt_position + modbus_data[MODBUS_REGISTERS::TILT_ADJUST_POSITIVE] - modbus_data[MODBUS_REGISTERS::TILT_ADJUST_NEGATIVE], tilt_min, tilt_max); - pan_servo.write(constrain(pan_position, pan_min, pan_max)); - tilt_servo.write(constrain(tilt_position, tilt_min, tilt_max)); + pan_servo.writeMicroseconds(pan_position); + tilt_servo.writeMicroseconds(tilt_position); + +// Serial.print(pan_position); +// Serial.print("\t"); +// Serial.println(tilt_position); modbus_data[MODBUS_REGISTERS::PAN_ADJUST_POSITIVE] = 0; modbus_data[MODBUS_REGISTERS::PAN_ADJUST_NEGATIVE] = 0; diff --git a/software/ros_packages/ground_station/src/Framework/InputSystems/JoystickControlSender.py b/software/ros_packages/ground_station/src/Framework/InputSystems/JoystickControlSender.py index 4d5bb07..c8ec883 100644 --- a/software/ros_packages/ground_station/src/Framework/InputSystems/JoystickControlSender.py +++ b/software/ros_packages/ground_station/src/Framework/InputSystems/JoystickControlSender.py @@ -16,7 +16,7 @@ from rover_control.msg import DriveCommandMessage, TowerPanTiltControlMessage GAME_CONTROLLER_NAME = "Logitech Logitech Extreme 3D Pro" DEFAULT_DRIVE_COMMAND_TOPIC = "/rover_control/command_control/ground_station_drive" -DEFAULT_PAN_TILT_COMMAND_TOPIC = "/tower/control" +DEFAULT_PAN_TILT_COMMAND_TOPIC = "/rover_control/pan_tilt/control" DRIVE_COMMAND_HERTZ = 15 @@ -31,9 +31,10 @@ CAMERA_CHANGE_TIME = 0.2 GUI_ELEMENT_CHANGE_TIME = 0.2 CAMERA_TOGGLE_CHANGE_TIME = 0.35 -PAN_TILT_X_AXIS_SCALAR = 5 +PAN_TILT_X_AXIS_SCALAR = 3 PAN_TILT_Y_AXIS_SCALAR = 10 + ##################################### # Controller Class Definition ##################################### diff --git a/software/ros_packages/rover_control/src/tower_pan_tilt_control/tower_and_pan_tilt_control.py b/software/ros_packages/rover_control/src/tower_pan_tilt_control/tower_and_pan_tilt_control.py index 4c74285..37cf6ac 100755 --- a/software/ros_packages/rover_control/src/tower_pan_tilt_control/tower_and_pan_tilt_control.py +++ b/software/ros_packages/rover_control/src/tower_pan_tilt_control/tower_and_pan_tilt_control.py @@ -16,16 +16,16 @@ from rover_control.msg import TowerPanTiltControlMessage ##################################### # Global Variables ##################################### -NODE_NAME = "drive_control" +NODE_NAME = "pan_tilt_and_tower_control" -DEFAULT_PORT = "/dev/ttyUSB0" +DEFAULT_PORT = "/dev/rover/ttyTowerAndPanTilt" DEFAULT_BAUD = 115200 DEFAULT_INVERT = False -DEFAULT_TOWER_PAN_TILT_CONTROL_TOPIC = "tower/control" +DEFAULT_TOWER_PAN_TILT_CONTROL_TOPIC = "pan_tilt/control" -NODE_ID = 1 +NODE_ID = 2 COMMUNICATIONS_TIMEOUT = 0.01 # Seconds diff --git a/software/ros_packages/rover_main/launch/ground_station/topic_transport_senders.launch b/software/ros_packages/rover_main/launch/ground_station/topic_transport_senders.launch index af7eaae..ec55c1c 100644 --- a/software/ros_packages/rover_main/launch/ground_station/topic_transport_senders.launch +++ b/software/ros_packages/rover_main/launch/ground_station/topic_transport_senders.launch @@ -6,7 +6,8 @@ - [{name: "/rover_control/command_control/ground_station_drive", compress: true, rate: 15.0}] + [{name: "/rover_control/command_control/ground_station_drive", compress: true, rate: 15.0}, + {name: "/rover_control/pan_tilt/control", compress: true, rate: 15.0}] @@ -18,7 +19,7 @@ {name: "/cameras/undercarriage/camera_control", compress: false, rate: 5.0}, {name: "/cameras/main_navigation/camera_control", compress: false, rate: 5.0}, {name: "/cameras/end_effector/camera_control", compress: false, rate: 5.0}, - {name: "/rover_status/update_requested", compress: false, rate: 5.0}] + {name: "/rover_status/update_requested", compress: false, rate: 5.0},] diff --git a/software/ros_packages/rover_main/launch/rover/control.launch b/software/ros_packages/rover_main/launch/rover/control.launch index 033ca9e..8acd219 100644 --- a/software/ros_packages/rover_main/launch/rover/control.launch +++ b/software/ros_packages/rover_main/launch/rover/control.launch @@ -32,5 +32,7 @@ + +