caperren/osurc_mars_rover_2017_2018
1
0
Fork 0
mirror of https://github.com/OSURoboticsClub/Rover_2017_2018.git synced 2025-11-08 10:11:14 +00:00
Code Issues Packages Projects Releases Wiki Activity

Got point and shoot control working. Needs zooming. Backup scale node added. Lots of random things, super tired.

Browse Source
This commit is contained in:
Corwin Perren
2018-08-11 07:12:06 -07:00
parent 12ed32ef54
commit 03cdd2e3a6
27 changed files with 768 additions and 130 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
software/environment/rover/UDEV_rules/99-rover-usb-serial.rules
View File

@@ -29,6 +29,6 @@ SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{seria
SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6001", ATTRS{serial}=="AH05K2Y8", SYMLINK+="rover/ttyARM"
##### Mappings from OMSI / EXPO driving with individual adapters
# SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6001", ATTRS{serial}=="AH05K2Y8", SYMLINK+="rover/ttyLEFT"
SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6015", ATTRS{serial}=="DA01OM1N", SYMLINK+="rover/ttyScale"
# SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6001", ATTRS{serial}=="A906H89E", SYMLINK+="rover/ttyRIGHT"
# SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6001", ATTRS{serial}=="A906H7P0", SYMLINK+="rover/ttyREAR"

4
software/firmware/freq_testing/freq_testing.ino
View File

@@ -9,7 +9,7 @@ void setup(){
void loop(){
digitalWrite(pin,LOW);
delay(mills);
delay(mills / 2.0);
digitalWrite(pin, HIGH);
delay(mills);
delay(mills / 2.0);
}

85
software/firmware/mining/mining.ino
View File

@@ -41,6 +41,13 @@ enum MOTORS {
TILT = 1,
};
// void shoot();
// void slowZoomIn();
// void slowZoomOut();
// void fullZoomIn();
// void fullZoomOut();
// void focus();
enum MODBUS_REGISTERS {
// Inputs
SET_POSITION_LIFT_POSITIVE = 0,
@@ -53,10 +60,24 @@ enum MODBUS_REGISTERS {
TARE = 7,
CALIBRATION_FACTOR = 8,
CHANGE_VIEW_MODE = 9,
ZOOM_IN = 10,
ZOOM_OUT = 11,
FULL_ZOOM_IN = 12,
FULL_ZOOM_OUT = 13,
FOCUS = 14,
SHOOT = 15,
// Outputs
CURRENT_POSITION_LIFT = 9,
CURRENT_POSITION_TILT = 10,
MEASURED_WEIGHT = 11
CURRENT_POSITION_LIFT = 16,
CURRENT_POSITION_TILT = 17,
MEASURED_WEIGHT = 18
};
enum CAMERA_VIEW_MODES {
NO_CHANGE = 0,
LCD = 1,
NETWORK = 2
};
////////// Global Variables //////////
@@ -71,7 +92,7 @@ float last_calibration_factor = -120000; //for the scale
// modbus stuff
const uint8_t node_id = 2;
const uint8_t mobus_serial_port_number = 2;
uint16_t modbus_data[] = {0, 0, 0, 0, 9999, 9999, 0, 0, 972, 0, 0, 0};
uint16_t modbus_data[] = {0, 0, 0, 0, 9999, 9999, 0, 0, 895, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
uint8_t num_modbus_registers = 0;
int8_t poll_state = 0;
bool communication_good = false;
@@ -89,7 +110,7 @@ Modbus slave(node_id, mobus_serial_port_number, HARDWARE::RS485_EN);
void setup() {
Serial.begin(9600); // debug
// while(!Serial);
while(!Serial);
setup_hardware();
num_modbus_registers = sizeof(modbus_data) / sizeof(modbus_data[0]);
@@ -103,6 +124,7 @@ void loop() {
set_motors();
set_scale();
poll_scale();
apply_camera_commands();
}
void setup_hardware() {
@@ -139,7 +161,7 @@ void setup_hardware() {
digitalWrite(HARDWARE::MOTOR_LIFT_EN, HIGH);
digitalWrite(HARDWARE::MOTOR_TILT_EN, HIGH);
digitalWrite(HARDWARE::CAMERA_VIDEO_EN, HIGH);
digitalWrite(HARDWARE::CAMERA_VIDEO_EN, LOW);
// Change motor PWM frequency so it's not in the audible range
analogWriteFrequency(HARDWARE::MOTOR_LIFT_PWM, 25000);
@@ -152,7 +174,7 @@ void setup_hardware() {
current_position_tilt = analogRead(HARDWARE::MOTOR_TILT_FB);
// setup scale
scale.set_scale(last_calibration_factor);
scale.set_scale();
scale.tare(); //Reset the scale to 0
}
@@ -233,7 +255,7 @@ void set_motors() {
}
void set_scale(){
float cal_factor = modbus_data[MODBUS_REGISTERS::CALIBRATION_FACTOR] * -100;
float cal_factor = modbus_data[MODBUS_REGISTERS::CALIBRATION_FACTOR] * 100;
if(cal_factor != last_calibration_factor){
scale.set_scale(cal_factor);
last_calibration_factor = cal_factor;
@@ -247,12 +269,57 @@ void set_scale(){
void poll_scale(){
if(modbus_data[MODBUS_REGISTERS::MEASURE] == 1){
// Serial.println(scale.get_units()*-1000);
Serial.println(scale.get_units()*-1000);
modbus_data[MODBUS_REGISTERS::MEASURED_WEIGHT] = scale.get_units()*-1000;
modbus_data[MODBUS_REGISTERS::MEASURE] = 0;
}
}
void apply_camera_commands(){
///// Camea video output control /////
switch (modbus_data[MODBUS_REGISTERS::CHANGE_VIEW_MODE]) {
case CAMERA_VIEW_MODES::LCD:
digitalWrite(HARDWARE::CAMERA_VIDEO_EN, HIGH);
break;
case CAMERA_VIEW_MODES::NETWORK:
digitalWrite(HARDWARE::CAMERA_VIDEO_EN, LOW);
break;
default:
break;
}
modbus_data[MODBUS_REGISTERS::CHANGE_VIEW_MODE] = CAMERA_VIEW_MODES::NO_CHANGE;
////// Zoom controls, process all, even if conflicting ///////
if(modbus_data[MODBUS_REGISTERS::ZOOM_IN]){
Serial.println("ZOOM IN");
modbus_data[MODBUS_REGISTERS::ZOOM_IN] = 0;
}else if(modbus_data[MODBUS_REGISTERS::ZOOM_OUT]){
Serial.println("ZOOM OUT");
modbus_data[MODBUS_REGISTERS::ZOOM_OUT] = 0;
}else if(modbus_data[MODBUS_REGISTERS::FULL_ZOOM_IN]){
Serial.println("FULL ZOOM IN");
modbus_data[MODBUS_REGISTERS::FULL_ZOOM_IN] = 0;
}
if(modbus_data[MODBUS_REGISTERS::FULL_ZOOM_OUT]){
Serial.println("FULL ZOOM OUT");
modbus_data[MODBUS_REGISTERS::FULL_ZOOM_OUT] = 0;
}
////// Focus and shoot controls ///////
if(modbus_data[MODBUS_REGISTERS::FOCUS]){
Serial.println("FOCUS");
modbus_data[MODBUS_REGISTERS::FOCUS] = 0;
}
if(modbus_data[MODBUS_REGISTERS::SHOOT]){
Serial.println("SHOOT");
modbus_data[MODBUS_REGISTERS::SHOOT] = 0;
}
///// camera process here
}
//---Set Motor Output---//
/*
Inputs: motor number, direction, pwm value

67
software/firmware/scale_tester/scale_tester.ino Normal file
View File

@@ -0,0 +1,67 @@
/*
Example using the SparkFun HX711 breakout board with a scale
By: Nathan Seidle
SparkFun Electronics
Date: November 19th, 2014
License: This code is public domain but you buy me a beer if you use this and we meet someday (Beerware license).
This is the calibration sketch. Use it to determine the calibration_factor that the main example uses. It also
outputs the zero_factor useful for projects that have a permanent mass on the scale in between power cycles.
Setup your scale and start the sketch WITHOUT a weight on the scale
Once readings are displayed place the weight on the scale
Press +/- or a/z to adjust the calibration_factor until the output readings match the known weight
Use this calibration_factor on the example sketch
This example assumes pounds (lbs). If you prefer kilograms, change the Serial.print(" lbs"); line to kg. The
calibration factor will be significantly different but it will be linearly related to lbs (1 lbs = 0.453592 kg).
Your calibration factor may be very positive or very negative. It all depends on the setup of your scale system
and the direction the sensors deflect from zero state
This example code uses bogde's excellent library: https://github.com/bogde/HX711
bogde's library is released under a GNU GENERAL PUBLIC LICENSE
Arduino pin 2 -> HX711 CLK
3 -> DOUT
5V -> VCC
GND -> GND
Most any pin on the Arduino Uno will be compatible with DOUT/CLK.
The HX711 board can be powered from 2.7V to 5V so the Arduino 5V power should be fine.
*/
#include "HX711.h"
#define DOUT 8
#define CLK 7
HX711 scale(DOUT, CLK);
float calibration_factor = 1314 00; //-7050 worked for my 440lb max scale setup
void setup() {
Serial.begin(9600);
Serial.println("HX711 calibration sketch");
Serial.println("Remove all weight from scale");
Serial.println("After readings begin, place known weight on scale");
Serial.println("Press + or a to increase calibration factor");
Serial.println("Press - or z to decrease calibration factor");
scale.set_scale();
scale.tare(); //Reset the scale to 0
long zero_factor = scale.read_average(); //Get a baseline reading
Serial.print("Zero factor: "); //This can be used to remove the need to tare the scale. Useful in permanent scale projects.
Serial.println(zero_factor);
}
void loop() {
scale.set_scale(calibration_factor); //Adjust to this calibration factor
Serial.print("Reading: ");
Serial.print(scale.get_units(), 4);
Serial.print(" Kg"); //Change this to kg and re-adjust the calibration factor if you follow SI units like a sane person
Serial.print(" calibration_factor: ");
Serial.print(calibration_factor);
Serial.println();
if(Serial.available())
{
char temp = Serial.read();
if(temp == '+' || temp == 'a')
calibration_factor += 100;
else if(temp == '-' || temp == 'z')
calibration_factor -= 100;
}
}

0
software/ros_packages/__init__.py Normal file
View File

12
software/ros_packages/ground_station/src/Framework/ArmSystems/ArmIndication.py
View File

@@ -63,6 +63,14 @@ FAULT_TO_STRING = {
15: "HOST COMM ERROR"
}
GRIPPER_MODES = {
0: "No Change",
1: "Normal",
2: "Pinch",
3: "Wide ",
4: "Scissor"
}
#####################################
# Controller Class Definition
@@ -106,6 +114,7 @@ class ArmIndication(QtCore.QObject):
thumb_current_updated__signal = QtCore.pyqtSignal(int)
middlefinger_current_updated__signal = QtCore.pyqtSignal(int)
gripper_reported_mode_updated__signal = QtCore.pyqtSignal(str)
gripper_reported_setpoint_updated__signal = QtCore.pyqtSignal(int)
def __init__(self, shared_objects):
@@ -155,6 +164,7 @@ class ArmIndication(QtCore.QObject):
self.arm_controls_wrist_roll_status_label = self.right_screen.arm_controls_wrist_roll_status_label # type:QtWidgets.QLabel
self.arm_controls_wrist_roll_faults_label = self.right_screen.arm_controls_wrist_roll_faults_label # type:QtWidgets.QLabel
self.gripper_reported_mode_label = self.right_screen.gripper_reported_mode_label # type:QtWidgets.QLabel
self.gripper_reported_setpoint_lcd_number = self.right_screen.gripper_reported_setpoint_lcd_number # type: QtWidgets.QLCDNumber
# ########## Get the settings instance ##########
@@ -209,6 +219,7 @@ class ArmIndication(QtCore.QObject):
self.thumb_current_updated__signal.connect(self.thumb_current_lcd_number.display)
self.middlefinger_current_updated__signal.connect(self.middlefinger_current_lcd_number.display)
self.gripper_reported_mode_updated__signal.connect(self.gripper_reported_mode_label.setText)
self.gripper_reported_setpoint_updated__signal.connect(self.gripper_reported_setpoint_lcd_number.display)
def on_arm_status_update_received__callback(self, data):
@@ -254,6 +265,7 @@ class ArmIndication(QtCore.QObject):
self.thumb_current_updated__signal.emit(data.thumb_current)
self.middlefinger_current_updated__signal.emit(data.middlefinger_current)
self.gripper_reported_mode_updated__signal.emit(GRIPPER_MODES[data.current_mode])
self.gripper_reported_setpoint_updated__signal.emit(data.current_finger_position)
@staticmethod

17
software/ros_packages/ground_station/src/Framework/InputSystems/XBOXControllerControlSender.py
View File

@@ -226,7 +226,7 @@ class XBOXControllerControlSender(QtCore.QThread):
self.last_right_bumper_state = 0
self.last_back_button_state = 0
self.gripper_control_mode = 0
self.gripper_control_mode = 1
self.gripper_control_mode_just_changed = False
self.send_new_gripper_mode = False
@@ -257,7 +257,7 @@ class XBOXControllerControlSender(QtCore.QThread):
"UNSET": [self.gripper_mode_pinch_stylesheet_update_ready__signal,
self.gripper_mode_wide_stylesheet_update_ready__signal,
self.gripper_mode_normal_stylesheet_update_ready__signal],
"ABS_MOVE": self.PINCH_MODE_ABSOLUTE_SET_POSITION
# "ABS_MOVE": self.PINCH_MODE_ABSOLUTE_SET_POSITION
}
}
@@ -314,15 +314,14 @@ class XBOXControllerControlSender(QtCore.QThread):
if self.xbox_current_control_state == self.XBOX_CONTROL_STATES.index("ARM"):
if self.last_left_bumper_state == 0 and left_bumper_state == 1:
self.gripper_control_mode = ((self.gripper_control_mode - 1) % len(self.GRIPPER_CONTROL_MODES))
self.gripper_control_mode_just_changed = True
# self.gripper_control_mode = ((self.gripper_control_mode - 1) % len(self.GRIPPER_CONTROL_MODES))
# self.gripper_control_mode_just_changed = True
self.last_left_bumper_state = 1
elif self.last_left_bumper_state == 1 and left_bumper_state == 0:
self.last_left_bumper_state = 0
if self.last_right_bumper_state == 0 and right_bumper_state == 1:
self.gripper_control_mode = ((self.gripper_control_mode + 1) % len(self.GRIPPER_CONTROL_MODES))
self.gripper_control_mode_just_changed = True
# self.gripper_control_mode = ((self.gripper_control_mode + 1) % len(self.GRIPPER_CONTROL_MODES))
# self.gripper_control_mode_just_changed = True
self.last_right_bumper_state = 1
elif self.last_right_bumper_state == 1 and right_bumper_state == 0:
self.last_right_bumper_state = 0
@@ -390,7 +389,7 @@ class XBOXControllerControlSender(QtCore.QThread):
should_publish_arm = True
should_publish_gripper = True
arm_control_message.wrist_roll = ((right_x_axis / THUMB_STICK_MAX) * WRIST_ROLL_SCALAR) * right_trigger_ratio
arm_control_message.wrist_roll = ((left_x_axis / THUMB_STICK_MAX) * BASE_SCALAR) * right_trigger_ratio
arm_control_message.wrist_pitch = (-(left_y_axis / THUMB_STICK_MAX) * WRIST_PITCH_SCALAR) * right_trigger_ratio
gripper_control_message.gripper_position_relative = (-(right_y_axis / THUMB_STICK_MAX) * GRIPPER_MOVEMENT_SCALAR) * right_trigger_ratio
@@ -408,7 +407,7 @@ class XBOXControllerControlSender(QtCore.QThread):
if self.last_back_button_state == 0 and back_state == 1:
gripper_control_message.should_home = True
gripper_control_message.gripper_mode = 1
gripper_control_message.gripper_mode = self.gripper_control_mode + 1
self.gripper_control_publisher.publish(gripper_control_message)
self.last_back_button_state = 1
elif self.last_back_button_state == 1 and back_state == 0:

38
software/ros_packages/ground_station/src/Framework/MiscSystems/MiningCore.py
View File

@@ -7,8 +7,9 @@ from PyQt5 import QtCore, QtWidgets
import logging
import rospy
from rover_control.msg import MiningStatusMessage, MiningControlMessage
from rover_control.msg import MiningStatusMessage, MiningControlMessage, CameraControlMessage
from rover_science.msg import SoilSensorStatusMessage
from std_msgs.msg import Float64
#####################################
# Global Variables
@@ -16,8 +17,12 @@ from rover_science.msg import SoilSensorStatusMessage
MINING_STATUS_TOPIC = "/rover_control/mining/status"
MINING_CONTROL_TOPIC = "/rover_control/mining/control"
SCALE_MEASUREMENT_TOPIC = "/rover_control/scale/measurement"
SOIL_PROBE_TOPIC = "/rover_science/soil_probe/data"
CAMERA_CONTROL_TOPIC = "/rover_control/camera/control"
TRAVEL_POSITION_LIFT = 110
TRAVEL_POSITION_TILT = 1023
@@ -71,6 +76,17 @@ class Mining(QtCore.QObject):
self.science_real_dielectric_lcd_number = self.left_screen.science_real_dielectric_lcd_number # type:QtWidgets.QLCDNumber
self.science_imaginary_dielectric_lcd_number = self.left_screen.science_imaginary_dielectric_lcd_number # type:QtWidgets.QLCDNumber
self.cam_lcd_output_button = self.left_screen.cam_lcd_output_button # type:QtWidgets.QPushButton
self.cam_network_output_button = self.left_screen.cam_network_output_button # type:QtWidgets.QPushButton
self.cam_zoom_in_button = self.left_screen.cam_zoom_in_button # type:QtWidgets.QPushButton
self.cam_zoom_out_button = self.left_screen.cam_zoom_out_button # type:QtWidgets.QPushButton
self.cam_lcd_output_button = self.left_screen.cam_lcd_output_button # type:QtWidgets.QPushButton
self.cam_lcd_output_button = self.left_screen.cam_lcd_output_button # type:QtWidgets.QPushButton
self.cam_lcd_output_button = self.left_screen.cam_lcd_output_button # type:QtWidgets.QPushButton
self.cam_lcd_output_button = self.left_screen.cam_lcd_output_button # type:QtWidgets.QPushButton
# ########## Get the settings instance ##########
self.settings = QtCore.QSettings()
@@ -85,8 +101,10 @@ class Mining(QtCore.QObject):
self.mining_status_message_received__callback)
self.soil_probe_subscriber = rospy.Subscriber(SOIL_PROBE_TOPIC, SoilSensorStatusMessage, self.on_soil_probe_message_received__callback)
self.scale_measurement_subscriber = rospy.Subscriber(SCALE_MEASUREMENT_TOPIC, Float64, self.on_scale_measurement_received__callback)
self.mining_control_publisher = rospy.Publisher(MINING_CONTROL_TOPIC, MiningControlMessage, queue_size=1)
self.camera_control_publisher = rospy.Publisher(CAMERA_CONTROL_TOPIC, CameraControlMessage, queue_size=1)
self.connect_signals_and_slots()
@@ -111,6 +129,9 @@ class Mining(QtCore.QObject):
self.real_dielectric_update_ready__signal.connect(self.science_real_dielectric_lcd_number.display)
self.imaginary_dielectric_update_ready__signal.connect(self.science_imaginary_dielectric_lcd_number.display)
self.cam_lcd_output_button.clicked.connect(self.on_cam_lcd_button_clicked__slot)
self.cam_network_output_button.clicked.connect(self.on_cam_network_button_clicked__slot)
def on_mining_set_cal_factor_clicked__slot(self):
message = MiningControlMessage()
@@ -162,11 +183,21 @@ class Mining(QtCore.QObject):
self.mining_control_publisher.publish(message)
def on_cam_lcd_button_clicked__slot(self):
message = CameraControlMessage()
message.camera_mode = 1
self.camera_control_publisher.publish(message)
def on_cam_network_button_clicked__slot(self):
message = CameraControlMessage()
message.camera_mode = 2
self.camera_control_publisher.publish(message)
def mining_status_message_received__callback(self, status):
status = status # type:MiningStatusMessage
self.tilt_position_update_ready__signal.emit(status.tilt_position)
self.lift_position_update_ready__signal.emit(status.lift_position)
self.weight_measurement_update_ready__signal.emit(status.measured_weight)
# self.weight_measurement_update_ready__signal.emit(status.measured_weight)
def on_soil_probe_message_received__callback(self, data):
self.temp_update_ready__signal.emit(data.temp_c)
@@ -175,3 +206,6 @@ class Mining(QtCore.QObject):
self.electrical_conductivity_update_ready__signal.emit(data.soil_electrical_conductivity)
self.real_dielectric_update_ready__signal.emit(data.real_dielectric_permittivity)
self.imaginary_dielectric_update_ready__signal.emit(data.imaginary_dielectric_permittivity)
def on_scale_measurement_received__callback(self, data):
self.weight_measurement_update_ready__signal.emit(data.data * 1000)

76
software/ros_packages/ground_station/src/Framework/MiscSystems/MiscArmCore.py
View File

@@ -26,7 +26,7 @@ POSITIONAL_TOLERANCE = 0.02
# Order is [base, shoulder, elbow, roll, wrist_pitch, wrist_roll]
ARM_STOW_PROCEDURE = [
[0.0, -0.035, -0.28, 0.0, 0.0, 0.0],
[0.0, -0.035, -0.28, 0.0, 0.0, 0.0], # Out in front of rover
[0.0, -0.035, -0.28, -0.25, 0.25, 0.0],
[0.0, -0.035, -0.5, -0.25, 0.25, 0.0],
[0.0, -0.25, -0.5, -0.25, 0.25, -0.25]
@@ -39,6 +39,18 @@ ARM_UNSTOW_PROCEDURE = [
[0.0, -0.035, -0.28, 0.0, 0.0, 0.0]
]
APPROACH_O2 = [
[0.0, -0.035, -0.28, 0.0, 0.0, 0.0], # Out in front of rover
[0.0, 0.0485472094896, -0.273685193022, -0.00102834607876, -0.17200773156, 0], # Correct positioning out to left side of rover
[-0.436250296246, 0.0485472094896, -0.273685193022, -0.00102834607876, -0.17200773156, 0] # Directly positioned, ready to grip
]
APPROACH_BEACON = [
[0.0, -0.035, -0.28, 0.0, 0.0, 0.0], # Out in front of rover
[0.0, 0.0361371382336, -0.325145836608, -0.00731261537597, -0.129662333807, 0.0569339269566], # Correct positioning out to right side of rover
[0.458505849045, 0.0361371382336, -0.325145633259, -0.00731200471916, -0.131140162948, 0.0920117742056]
]
#####################################
# UbiquitiRadioSettings Class Definition
@@ -59,6 +71,11 @@ class MiscArm(QtCore.QThread):
self.arm_controls_clear_faults_button = self.left_screen.arm_controls_clear_faults_button # type:QtWidgets.QPushButton
self.arm_controls_reset_motor_drivers_button = self.left_screen.arm_controls_reset_motor_drivers_button # type:QtWidgets.QPushButton
self.arm_controls_approach_o2_button = self.left_screen.arm_controls_approach_o2_button # type:QtWidgets.QPushButton
self.arm_controls_depart_o2_button = self.left_screen.arm_controls_depart_o2_button # type:QtWidgets.QPushButton
self.arm_controls_approach_beacon_button = self.left_screen.arm_controls_approach_beacon_button # type:QtWidgets.QPushButton
self.arm_controls_depart_beacon_button = self.left_screen.arm_controls_depart_beacon_button # type:QtWidgets.QPushButton
self.gripper_home_button = self.left_screen.gripper_home_button # type:QtWidgets.QPushButton
self.gripper_toggle_light_button = self.left_screen.gripper_toggle_light_button # type:QtWidgets.QPushButton
@@ -94,6 +111,11 @@ class MiscArm(QtCore.QThread):
self.should_stow_arm = False
self.should_unstow_arm = False
self.should_approach_o2 = False
self.should_depart_o2 = False
self.should_approach_beacon = False
self.should_depart_beacon = False
def run(self):
self.logger.debug("Starting MiscArm Thread")
@@ -107,6 +129,22 @@ class MiscArm(QtCore.QThread):
self.unstow_rover_arm()
self.should_unstow_arm = False
elif self.should_approach_o2:
self.approach_o2()
self.should_approach_o2 = False
elif self.should_depart_o2:
self.depart_o2()
self.should_depart_o2 = False
elif self.should_approach_beacon:
self.approach_beacon()
self.should_approach_beacon = False
elif self.should_depart_beacon:
self.depart_beacon()
self.should_depart_beacon = False
time_diff = time() - start_time
self.msleep(max(int(self.wait_time - time_diff), 0))
@@ -121,6 +159,22 @@ class MiscArm(QtCore.QThread):
for movement in ARM_UNSTOW_PROCEDURE:
self.process_absolute_move_command(movement)
def approach_o2(self):
for movement in APPROACH_O2:
self.process_absolute_move_command(movement)
def depart_o2(self):
for movement in reversed(APPROACH_O2):
self.process_absolute_move_command(movement)
def approach_beacon(self):
for movement in APPROACH_BEACON:
self.process_absolute_move_command(movement)
def depart_beacon(self):
for movement in reversed(APPROACH_BEACON):
self.process_absolute_move_command(movement)
def process_absolute_move_command(self, movement):
message = ArmControlMessage()
@@ -134,6 +188,7 @@ class MiscArm(QtCore.QThread):
self.arm_absolute_control_publisher.publish(message)
self.wait_for_targets_reached(movement)
self.msleep(250)
def wait_for_targets_reached(self, movement):
base_set = movement[0]
@@ -176,6 +231,9 @@ class MiscArm(QtCore.QThread):
self.arm_controls_clear_faults_button.clicked.connect(self.on_clear_faults_button_pressed__slot)
self.arm_controls_reset_motor_drivers_button.clicked.connect(self.on_reset_drivers_button_pressed__slot)
self.arm_controls_approach_o2_button.clicked.connect(self.on_approach_o2_button_pressed__slot)
self.arm_controls_approach_beacon_button.clicked.connect(self.on_approach_beacon_button_pressed__slot)
self.gripper_home_button.clicked.connect(self.on_gripper_home_pressed)
self.gripper_toggle_light_button.clicked.connect(self.on_gripper_toggle_light_pressed)
@@ -203,9 +261,21 @@ class MiscArm(QtCore.QThread):
def on_unstow_arm_button_pressed__slot(self):
self.should_unstow_arm = True
def on_approach_o2_button_pressed__slot(self):
self.should_approach_o2 = True
def on_depart_o2_button_pressed__slot(self):
self.should_depart_o2 = True
def on_approach_beacon_button_pressed__slot(self):
self.should_approach_beacon = True
def on_depart_beacon_button_pressed__slot(self):
self.should_depart_beacon = True
def on_gripper_home_pressed(self):
message = GripperControlMessage()
message.gripper_mode = 1
message.gripper_mode = 2
message.gripper_position_absolute = -1
message.should_home = True
@@ -213,7 +283,7 @@ class MiscArm(QtCore.QThread):
def on_gripper_toggle_light_pressed(self):
message = GripperControlMessage()
message.gripper_mode = 1
message.gripper_mode = 2
message.toggle_light = True
message.gripper_position_absolute = -1

2
software/ros_packages/ground_station/src/Framework/MiscSystems/RDFCore.py
View File

@@ -98,7 +98,7 @@ class RDF(QtCore.QThread):
valid_range = []
for n in range(0, len(xf)):
if (xf[n] > 0.5) and (xf[n] < 5.0):
if (xf[n] > 0.5) and (xf[n] <= 5.0):
valid_range.append(n)
yf = numpy.take(yf, valid_range)

12
software/ros_packages/ground_station/src/Framework/NavigationSystems/SpeedAndHeadingIndication.py
View File

@@ -34,6 +34,9 @@ class SpeedAndHeadingIndication(QtCore.QThread):
heading_text_update_ready__signal = QtCore.pyqtSignal(str)
new_speed_update_ready__signal = QtCore.pyqtSignal(str)
pitch_update_ready__signal = QtCore.pyqtSignal(float)
roll_update_ready__signal = QtCore.pyqtSignal(float)
def __init__(self, shared_objects):
super(SpeedAndHeadingIndication, self).__init__()
@@ -46,6 +49,9 @@ class SpeedAndHeadingIndication(QtCore.QThread):
self.next_goal_label = self.right_screen.next_goal_label # type: QtWidgets.QLabel
self.current_speed_label = self.right_screen.current_speed_label # type: QtWidgets.QLabel
self.imu_pitch_lcd_number = self.right_screen.imu_pitch_lcd_number # type: QtWidgets.QLCDNumber
self.imu_roll_lcd_number = self.right_screen.imu_roll_lcd_number # type: QtWidgets.QLCDNumber
# ########## Get the settings instance ##########
self.settings = QtCore.QSettings()
@@ -110,6 +116,9 @@ class SpeedAndHeadingIndication(QtCore.QThread):
self.roll, self.pitch, self.yaw = transformations.euler_from_quaternion(quat)
self.current_heading = (self.euler_interpolator(self.yaw) + MappingSettings.DECLINATION_OFFSET) % 360
self.pitch_update_ready__signal.emit(self.pitch)
self.roll_update_ready__signal.emit(self.roll)
def rotate_compass_if_needed(self):
self.current_heading_shown_rotation_angle = int(self.current_heading)
@@ -161,6 +170,9 @@ class SpeedAndHeadingIndication(QtCore.QThread):
self.heading_compass_label.mousePressEvent = self.__on_heading_clicked__slot
self.pitch_update_ready__signal.connect(self.imu_pitch_lcd_number.display)
self.roll_update_ready__signal.connect(self.imu_roll_lcd_number.display)
def setup_signals(self, start_signal, signals_and_slots_signal, kill_signal):
start_signal.connect(self.start)
signals_and_slots_signal.connect(self.connect_signals_and_slots)

2
software/ros_packages/ground_station/src/Framework/VideoSystems/RoverVideoCoordinator.py
View File

@@ -381,7 +381,7 @@ class RoverVideoCoordinator(QtCore.QThread):
self.low_res_button_text_update_ready__signal.emit("DISABLED")
self.low_res_button_stylesheet_update_ready__signal.emit(COLOR_GREEN)
else:
self.in_low_res_mode = False
self.in_low_res_mode = True
self.set_max_resolutions_flag = True
self.low_res_button_text_update_ready__signal.emit("ENABLED")

2
software/ros_packages/ground_station/src/Resources/Settings/MappingSettings.py
View File

@@ -63,4 +63,4 @@ LAST_SELECTION = "Graf Hall"
LAST_ZOOM_LEVEL = MAPPING_LOCATIONS[LAST_SELECTION]["default_zoom"]
# ##### This is the offset from magnetic north to true north
DECLINATION_OFFSET = 15
DECLINATION_OFFSET = 0 # We set this to 0 so we can use a phone to calibrate

302
software/ros_packages/ground_station/src/Resources/Ui/left_screen.ui
View File

@@ -251,6 +251,48 @@
<property name="spacing">
<number>0</number>
</property>
<item row="10" column="1">
<widget class="QLabel" name="co2_levels_label">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>0</width>
<height>0</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>9999999</width>
<height>9999999</height>
</size>
</property>
<property name="font">
<font>
<pointsize>10</pointsize>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="styleSheet">
<string notr="true">background-color:darkgreen; border: 0.5px solid black;</string>
</property>
<property name="frameShape">
<enum>QFrame::NoFrame</enum>
</property>
<property name="text">
<string>CO2 Levels
--- ppm</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item row="5" column="3">
<widget class="QLabel" name="emmc">
<property name="font">
@@ -1322,48 +1364,6 @@ N/A</string>
</property>
</widget>
</item>
<item row="10" column="1">
<widget class="QLabel" name="co2_levels_label">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>0</width>
<height>0</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>9999999</width>
<height>9999999</height>
</size>
</property>
<property name="font">
<font>
<pointsize>10</pointsize>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="styleSheet">
<string notr="true">background-color:darkgreen; border: 0.5px solid black;</string>
</property>
<property name="frameShape">
<enum>QFrame::NoFrame</enum>
</property>
<property name="text">
<string>CO2 Levels
--- ppm</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
</layout>
</item>
</layout>
@@ -1390,8 +1390,145 @@ N/A</string>
</size>
</property>
<property name="currentIndex">
<number>1</number>
<number>0</number>
</property>
<widget class="QWidget" name="tab_6">
<attribute name="title">
<string>Science Cam</string>
</attribute>
<layout class="QGridLayout" name="gridLayout_20">
<item row="0" column="0">
<layout class="QVBoxLayout" name="verticalLayout_26">
<item>
<widget class="QLabel" name="label_72">
<property name="font">
<font>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="text">
<string>Video Output</string>
</property>
<property name="alignment">
<set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
</property>
</widget>
</item>
<item>
<layout class="QHBoxLayout" name="horizontalLayout_21">
<item>
<widget class="QPushButton" name="cam_lcd_output_button">
<property name="text">
<string>LCD</string>
</property>
</widget>
</item>
<item>
<widget class="QPushButton" name="cam_network_output_button">
<property name="text">
<string>Network</string>
</property>
</widget>
</item>
</layout>
</item>
</layout>
</item>
<item row="0" column="1">
<spacer name="horizontalSpacer_20">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>422</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item row="1" column="0">
<layout class="QVBoxLayout" name="verticalLayout_27">
<item>
<widget class="QLabel" name="label_73">
<property name="font">
<font>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="text">
<string>Controls</string>
</property>
<property name="alignment">
<set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
</property>
</widget>
</item>
<item>
<layout class="QFormLayout" name="formLayout">
<item row="0" column="0">
<widget class="QPushButton" name="cam_zoom_in_button">
<property name="text">
<string>Zoom In</string>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QPushButton" name="cam_zoom_out_button">
<property name="text">
<string>Zoom Out</string>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QPushButton" name="cam_full_zoom_in_button">
<property name="text">
<string>Full Zoom In</string>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QPushButton" name="cam_full_zoom_out_button">
<property name="text">
<string>Full Zoom Out</string>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QPushButton" name="cam_focus_button">
<property name="text">
<string>Focus</string>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QPushButton" name="cam_shoot_button">
<property name="text">
<string>Shoot</string>
</property>
</widget>
</item>
</layout>
</item>
</layout>
</item>
<item row="2" column="0">
<spacer name="verticalSpacer_8">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>331</height>
</size>
</property>
</spacer>
</item>
</layout>
</widget>
<widget class="QWidget" name="tab_5">
<attribute name="title">
<string>RDF</string>
@@ -1575,7 +1712,7 @@ N/A</string>
</font>
</property>
<property name="text">
<string>Preset Movements</string>
<string>Special Movements</string>
</property>
</widget>
</item>
@@ -1702,6 +1839,83 @@ N/A</string>
</item>
</layout>
</item>
<item>
<layout class="QVBoxLayout" name="verticalLayout_25">
<item>
<widget class="QLabel" name="label_69">
<property name="font">
<font>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="text">
<string>Task Movements</string>
</property>
</widget>
</item>
<item>
<layout class="QGridLayout" name="gridLayout_19">
<item row="0" column="0">
<widget class="QPushButton" name="arm_controls_approach_o2_button">
<property name="styleSheet">
<string notr="true"/>
</property>
<property name="text">
<string>Approach O2 Tank</string>
</property>
</widget>
</item>
<item row="5" column="0">
<spacer name="verticalSpacer_7">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeType">
<enum>QSizePolicy::MinimumExpanding</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>40</height>
</size>
</property>
</spacer>
</item>
<item row="1" column="0">
<widget class="QPushButton" name="arm_controls_depart_o2_button">
<property name="styleSheet">
<string notr="true"/>
</property>
<property name="text">
<string>Depart O2 Tank</string>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QPushButton" name="arm_controls_approach_beacon_button">
<property name="styleSheet">
<string notr="true"/>
</property>
<property name="text">
<string>Approach Beacon</string>
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QPushButton" name="arm_controls_depart_beacon_button">
<property name="styleSheet">
<string notr="true"/>
</property>
<property name="text">
<string>Depart Beacon</string>
</property>
</widget>
</item>
</layout>
</item>
</layout>
</item>
<item>
<spacer name="horizontalSpacer_15">
<property name="orientation">

98
software/ros_packages/ground_station/src/Resources/Ui/right_screen.ui
View File

@@ -1604,7 +1604,7 @@ Position</string>
</font>
</property>
<property name="styleSheet">
<string notr="true">background-color:darkgreen; border: 1px solid black;</string>
<string notr="true">border: 1px solid black;</string>
</property>
<property name="text">
<string>Normal</string>
@@ -1634,7 +1634,7 @@ Position</string>
</font>
</property>
<property name="styleSheet">
<string notr="true">border: 1px solid black;</string>
<string notr="true">border: 1px solid black; background-color:darkgreen;</string>
</property>
<property name="text">
<string>Pinch</string>
@@ -1810,6 +1810,49 @@ Position</string>
</property>
</spacer>
</item>
<item>
<layout class="QVBoxLayout" name="verticalLayout_6">
<property name="rightMargin">
<number>6</number>
</property>
<item>
<widget class="QLabel" name="label_102">
<property name="font">
<font>
<pointsize>10</pointsize>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="text">
<string>Reported Mode</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item>
<widget class="QLabel" name="gripper_reported_mode_label">
<property name="minimumSize">
<size>
<width>0</width>
<height>50</height>
</size>
</property>
<property name="styleSheet">
<string notr="true">border: 1px solid grey;</string>
</property>
<property name="text">
<string>-------</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
</layout>
</item>
<item>
<layout class="QVBoxLayout" name="verticalLayout_68">
<item>
@@ -1824,6 +1867,9 @@ Position</string>
<property name="text">
<string>Reported Setpoint</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item>
@@ -2511,16 +2557,6 @@ Position</string>
<property name="spacing">
<number>0</number>
</property>
<item>
<widget class="QDial" name="dial_3">
<property name="minimum">
<number>-100</number>
</property>
<property name="maximum">
<number>100</number>
</property>
</widget>
</item>
<item>
<widget class="QLabel" name="label">
<property name="font">
@@ -2537,6 +2573,16 @@ Position</string>
</property>
</widget>
</item>
<item>
<widget class="QLCDNumber" name="imu_pitch_lcd_number">
<property name="minimumSize">
<size>
<width>0</width>
<height>50</height>
</size>
</property>
</widget>
</item>
</layout>
</item>
<item>
@@ -2545,23 +2591,7 @@ Position</string>
<number>0</number>
</property>
<item>
<widget class="QDial" name="dial_4">
<property name="minimum">
<number>-100</number>
</property>
<property name="maximum">
<number>100</number>
</property>
<property name="invertedAppearance">
<bool>false</bool>
</property>
<property name="wrapping">
<bool>true</bool>
</property>
</widget>
</item>
<item>
<widget class="QLabel" name="label_15">
<widget class="QLabel" name="imu_pitch_lcd_number_2">
<property name="font">
<font>
<weight>75</weight>
@@ -2576,6 +2606,16 @@ Position</string>
</property>
</widget>
</item>
<item>
<widget class="QLCDNumber" name="imu_roll_lcd_number">
<property name="minimumSize">
<size>
<width>0</width>
<height>50</height>
</size>
</property>
</widget>
</item>
</layout>
</item>
</layout>

1
software/ros_packages/rover_control/CMakeLists.txt
View File

@@ -59,6 +59,7 @@ find_package(catkin REQUIRED COMPONENTS
MiningStatusMessage.msg
GripperControlMessage.msg
GripperStatusMessage.msg
CameraControlMessage.msg
)
## Generate services in the 'srv' folder

0
software/ros_packages/rover_control/__init__.py Normal file
View File

8
software/ros_packages/rover_control/msg/CameraControlMessage.msg Normal file
View File

@@ -0,0 +1,8 @@
uint8 camera_mode
bool zoom_in
bool zoom_out
bool full_zoom_in
bool full_zoom_out
bool focus
bool shoot

0
software/ros_packages/rover_control/src/__init__.py Normal file
View File

0
software/ros_packages/rover_control/src/effectors_control/__init__.py Normal file
View File

82
software/ros_packages/rover_control/src/effectors_control/effectors_control.py
View File

@@ -12,7 +12,7 @@ import minimalmodbus
# from std_msgs.msg import UInt8, UInt16
# Custom Imports
from rover_control.msg import MiningControlMessage, MiningStatusMessage, GripperControlMessage, GripperStatusMessage
from rover_control.msg import MiningControlMessage, MiningStatusMessage, GripperControlMessage, GripperStatusMessage, CameraControlMessage
#####################################
# Global Variables
@@ -45,6 +45,8 @@ GRIPPER_STATUS_PUBLISHER_TOPIC = "gripper/status"
MINING_CONTROL_SUBSCRIBER_TOPIC = "mining/control"
MINING_STATUS_PUBLISHER_TOPIC = "mining/status"
CAMERA_CONTROL_SUBSCRIBER_TOPIC = "camera/control"
# ##### Gripper Defines #####
GRIPPER_MODBUS_REGISTERS = {
"MODE": 0,
@@ -94,9 +96,17 @@ MINING_MODBUS_REGISTERS = {
"TARE": 7,
"CAL_FACTOR": 8,
"LIFT_POSITION": 9,
"TILT_POSITION": 10,
"MEASURED_WEIGHT": 11
"CHANGE_VIEW_MODE": 9,
"ZOOM_IN": 10,
"ZOOM_OUT": 11,
"FULL_ZOOM_IN": 12,
"FULL_ZOOM_OUT": 13,
"FOCUS": 14,
"SHOOT": 15,
"CURRENT_POSITION_LIFT": 16,
"CURRENT_POSITION_TILT": 17,
"MEASURED_WEIGHT": 18
}
@@ -139,6 +149,9 @@ class EffectorsControl(object):
self.mining_status_publisher_topic = rospy.get_param("~mining_status_publisher_topic",
MINING_STATUS_PUBLISHER_TOPIC)
self.camera_control_subscriber_topic = rospy.get_param("~camera_control_subscriber_topic",
CAMERA_CONTROL_SUBSCRIBER_TOPIC)
self.wait_time = 1.0 / rospy.get_param("~hertz", DEFAULT_HERTZ)
self.gripper_node = None # type:minimalmodbus.Instrument
@@ -153,14 +166,14 @@ class EffectorsControl(object):
# ##### Subscribers #####
self.gripper_control_subscriber = rospy.Subscriber(self.gripper_control_subscriber_topic, GripperControlMessage, self.gripper_control_message_received__callback)
self.mining_control_subscriber = rospy.Subscriber(self.mining_control_subscriber_topic, MiningControlMessage,
self.mining_control_message_received__callback)
self.mining_control_subscriber = rospy.Subscriber(self.mining_control_subscriber_topic, MiningControlMessage, self.mining_control_message_received__callback)
self.camera_control_subscriber = rospy.Subscriber(self.camera_control_subscriber_topic, CameraControlMessage, self.camera_control_message_received__callback)
# ##### Publishers #####
self.gripper_status_publisher = rospy.Publisher(self.gripper_status_publisher_topic, GripperStatusMessage, queue_size=1)
self.mining_status_publisher = rospy.Publisher(self.mining_status_publisher_topic, MiningStatusMessage,
queue_size=1)
self.mining_status_publisher = rospy.Publisher(self.mining_status_publisher_topic, MiningStatusMessage, queue_size=1)
# ##### Misc #####
self.modbus_nodes_seen_time = time()
@@ -175,6 +188,9 @@ class EffectorsControl(object):
self.gripper_control_message = None
self.new_gripper_control_message = False
self.camera_control_message = None # type: CameraControlMessage
self.new_camera_control_message = False
self.failed_gripper_modbus_count = 0
self.failed_mining_modbus_count = 0
@@ -210,7 +226,8 @@ class EffectorsControl(object):
try:
self.run_mining()
self.failed_mining_modbus_count = 0
except:
except Exception, e:
print e
self.failed_mining_modbus_count += 1
if self.failed_mining_modbus_count == FAILED_MINING_MODBUS_LIMIMT:
@@ -224,6 +241,7 @@ class EffectorsControl(object):
def run_mining(self):
self.process_mining_control_message()
self.send_mining_status_message()
self.process_camera_control_message()
def connect_to_nodes(self):
self.gripper_node = minimalmodbus.Instrument(self.port, int(self.gripper_node_id))
@@ -268,13 +286,39 @@ class EffectorsControl(object):
self.modbus_nodes_seen_time = time()
self.new_mining_control_message = False
def process_camera_control_message(self):
if self.new_camera_control_message:
print self.camera_control_message
self.mining_registers[MINING_MODBUS_REGISTERS["LIFT_SET_ABSOLUTE"]] = 1024
self.mining_registers[MINING_MODBUS_REGISTERS["LIFT_SET_POSITIVE"]] = 0
self.mining_registers[MINING_MODBUS_REGISTERS["LIFT_SET_NEGATIVE"]] = 0
self.mining_registers[MINING_MODBUS_REGISTERS["TILT_SET_ABSOLUTE"]] = 1024
self.mining_registers[MINING_MODBUS_REGISTERS["TILT_SET_POSITIVE"]] = 0
self.mining_registers[MINING_MODBUS_REGISTERS["TILT_SET_NEGATIVE"]] = 0
self.mining_registers[MINING_MODBUS_REGISTERS["MEASURE"]] = 0
self.mining_registers[MINING_MODBUS_REGISTERS["TARE"]] = 0
self.mining_registers[MINING_MODBUS_REGISTERS["CHANGE_VIEW_MODE"]] = self.camera_control_message.camera_mode
self.mining_registers[MINING_MODBUS_REGISTERS["ZOOM_IN"]] = 0
self.mining_registers[MINING_MODBUS_REGISTERS["ZOOM_OUT"]] = 0
self.mining_registers[MINING_MODBUS_REGISTERS["FULL_ZOOM_IN"]] = 0
self.mining_registers[MINING_MODBUS_REGISTERS["FULL_ZOOM_OUT"]] = 0
self.mining_registers[MINING_MODBUS_REGISTERS["FOCUS"]] = 0
self.mining_registers[MINING_MODBUS_REGISTERS["SHOOT"]] = 0
self.mining_node.write_registers(0, self.mining_registers)
self.modbus_nodes_seen_time = time()
self.new_camera_control_message = False
def send_mining_status_message(self):
if self.mining_node_present:
self.mining_registers = self.mining_node.read_registers(0, len(MINING_MODBUS_REGISTERS))
message = MiningStatusMessage()
message.lift_position = self.mining_registers[MINING_MODBUS_REGISTERS["LIFT_POSITION"]]
message.tilt_position = self.mining_registers[MINING_MODBUS_REGISTERS["TILT_POSITION"]]
message.lift_position = self.mining_registers[MINING_MODBUS_REGISTERS["CURRENT_POSITION_LIFT"]]
message.tilt_position = self.mining_registers[MINING_MODBUS_REGISTERS["CURRENT_POSITION_TILT"]]
message.measured_weight = self.mining_registers[MINING_MODBUS_REGISTERS["MEASURED_WEIGHT"]]
self.mining_status_publisher.publish(message)
@@ -296,16 +340,17 @@ class EffectorsControl(object):
homing_complete = False
while not homing_complete:
registers = self.gripper_node.read_registers(0, len(GRIPPER_MODBUS_REGISTERS))
# print registers
self.gripper_registers = self.gripper_node.read_registers(0, len(GRIPPER_MODBUS_REGISTERS))
self.send_gripper_status_message()
if registers[GRIPPER_MODBUS_REGISTERS["FINGER_POSITION_OUTPUT"]] == 0:
if self.gripper_registers[GRIPPER_MODBUS_REGISTERS["FINGER_POSITION_OUTPUT"]] == 0:
homing_complete = True
self.gripper_registers = None
else:
if self.gripper_control_message.toggle_light:
self.gripper_registers[GRIPPER_MODBUS_REGISTERS["LIGHT_STATE"]] = not self.gripper_registers[GRIPPER_MODBUS_REGISTERS["LIGHT_STATE"]]
self.gripper_registers[GRIPPER_MODBUS_REGISTERS["LIGHT_STATE"]] = 0 if self.gripper_registers[GRIPPER_MODBUS_REGISTERS["LIGHT_STATE"]] else 1
self.gripper_control_message.toggle_light = False
gripper_absolute = self.gripper_control_message.gripper_position_absolute
gripper_relative = self.gripper_control_message.gripper_position_relative
@@ -320,7 +365,8 @@ class EffectorsControl(object):
self.gripper_node.write_registers(0, DEFAULT_GRIPPER_REGISTERS)
self.new_gripper_control_message = False
self.gripper_control_message = None
self.new_gripper_control_message = False
def send_gripper_status_message(self):
registers = self.gripper_node.read_registers(0, len(GRIPPER_MODBUS_REGISTERS))
@@ -348,6 +394,10 @@ class EffectorsControl(object):
self.mining_control_message = control_message
self.new_mining_control_message = True
def camera_control_message_received__callback(self, control_message):
self.camera_control_message = control_message
self.new_camera_control_message = True
if __name__ == "__main__":
EffectorsControl()

12
software/ros_packages/rover_control/src/effectors_control/effectors_tester.py
View File

@@ -2,20 +2,18 @@
import rospy
import time
from rover_control.msg import GripperControlMessage
from rover_control.msg import CameraControlMessage
TOPIC = "/gripper/control"
TOPIC = "/rover_control/camera/control"
rospy.init_node("effectors_tester")
publisher = rospy.Publisher(TOPIC, GripperControlMessage, queue_size=1)
publisher = rospy.Publisher(TOPIC, CameraControlMessage, queue_size=1)
time.sleep(2)
message = GripperControlMessage()
message.gripper_mode = 1
message.gripper_position = 0
message.should_home = 0
message = CameraControlMessage()
message.camera_mode = 2
publisher.publish(message)

58
software/ros_packages/rover_control/src/effectors_control/emergency_scale.py Normal file
View File

@@ -0,0 +1,58 @@
#!/usr/bin/env python
#####################################
# Imports
#####################################
# Python native imports
import rospy
from time import time, sleep
import serial
from std_msgs.msg import Float64
#####################################
# Global Variables
#####################################
NODE_NAME = "effectors_control"
# ##### Communication Defines #####
DEFAULT_PORT = "/dev/rover/ttyScale"
# DEFAULT_PORT = "/dev/ttyUSB3"
DEFAULT_BAUD = 115200
DEFAULT_TOPIC = "scale/measurement"
CAL_FACTOR = 89500
#####################################
# DriveControl Class Definition
#####################################
class EmergencyScale(object):
def __init__(self):
rospy.init_node(NODE_NAME)
self.scale_serial = serial.Serial(port=DEFAULT_PORT, baudrate=DEFAULT_BAUD, timeout=200)
self.publisher = rospy.Publisher(DEFAULT_TOPIC, Float64, queue_size=1)
self.run()
def run(self):
sleep(.2)
self.scale_serial.write(str(CAL_FACTOR))
sleep(.2)
while not rospy.is_shutdown():
value = self.scale_serial.readline()
try:
message = Float64()
message.data = float(value)
self.publisher.publish(message)
print
except:
print "No data"
if __name__ == "__main__":
EmergencyScale()

3
software/ros_packages/rover_main/launch/ground_station/topic_transport_senders.launch
View File

@@ -24,7 +24,8 @@
{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_arm/control/absolute", compress: true, rate: 5.0}]
{name: "/rover_arm/control/absolute", compress: true, rate: 5.0},
{name: "/rover_control/camera/control", compress: true, rate: 5.0}]
</rosparam>
</node>
</group>

7
software/ros_packages/rover_main/launch/rover/control.launch
View File

@@ -35,8 +35,11 @@
<node name="chassis_pan_tilt" pkg="rover_control" type="chassis_pan_tilt_control.py" respawn="true" output="screen"/>
<!--<node name="effectors" pkg="rover_control" type="effectors_control.py" respawn="true" output="screen">-->
<node name="effectors" pkg="rover_control" type="effectors_control.py" respawn="true" output="screen">
<!--<param name="port" value="/dev/rover/ttyARM"/>-->
<!--</node>-->
</node>
<node name="scale" pkg="rover_control" type="emergency_scale.py" respawn="true" output="screen">
</node>
</group>
</launch>

2
software/ros_packages/rover_main/launch/rover/topic_transport_senders.launch
View File

@@ -158,7 +158,7 @@
{name: "/rover_control/mining/status", compress: false, rate: 5.0},
{name: "/rover_control/gripper/status", compress: false, rate: 5.0},
{name: "/rover_science/rdf/data", compress: false, rate: 50.0},
{name: "/rover_science/soil_probe/data", compress: false, rate: 50.0}
{name: "/rover_control/scale/measurement", compress: false, rate: 20.0}
]
</rosparam>
</node>

6
software/ros_packages/rover_status/src/system_statuses_node.py
View File

@@ -143,7 +143,11 @@ class SystemStatuses:
self.GPS_msg.num_satellites = int(self.Nmea_Message.num_sats)
self.GPS_msg.horizontal_dilution = float(self.Nmea_Message.horizontal_dil)
if self.Nmea_Message.sentence_type == 'VTG':
self.GPS_msg.kmph = float(self.Nmea_Message.spd_over_grnd_kmph)
try:
self.GPS_msg.kmph = float(self.Nmea_Message.spd_over_grnd_kmph)
except:
pass
if self.Nmea_Message.true_track is not None:
self.GPS_msg.gps_heading = float(self.Nmea_Message.true_track)
else: