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

Mining system completegit add .!

Browse Source
This commit is contained in:
Corwin Perren
2018-07-29 21:46:10 -07:00
parent b79693c855
commit 092318ec10
20 changed files with 1223 additions and 310 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

@@ -11,7 +11,7 @@
SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM2CZLZF", ENV{ID_USB_INTERFACE_NUM}=="00", SYMLINK+="rover/ttyREAR"
SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM2CZLZF", ENV{ID_USB_INTERFACE_NUM}=="01", SYMLINK+="rover/ttyLEFT"
SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM2CZLZF", ENV{ID_USB_INTERFACE_NUM}=="02", SYMLINK+="rover/ttyRIGHT"
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}=="NM2CZLZF", ENV{ID_USB_INTERFACE_NUM}=="03", SYMLINK+="rover/ttyEffectors"
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/ttyTowerAndPanTilt"

1
software/environment/rover/UDEV_rules/install_rules.sh
View File

@@ -1,3 +1,4 @@
#!/bin/bash
sudo cp 99-rover-cameras.rules /etc/udev/rules.d/.
sudo cp 99-rover-usb-serial.rules /etc/udev/rules.d/.
sudo reboot

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

@@ -1,289 +1,335 @@
////////// Includes //////////
#include "HX711.h"
////////// Hardware / Data Enumerations //////////
enum HARDWARE {
RS485_EN = 6,
RS485_RX = 9,
RS485_TX = 10,
MOTOR_LIFT_A = 27,
MOTOR_LIFT_B = 28,
MOTOR_LIFT_PWM = 25,
MOTOR_LIFT_CS = 31,
MOTOR_LIFT_EN = 24,
MOTOR_LIFT_FB = A10,
MOTOR_TILT_A = 30,
MOTOR_TILT_B = 29,
MOTOR_TILT_PWM = 32,
MOTOR_TILT_CS = 26,
MOTOR_TILT_EN = 33,
MOTOR_TILT_FB = A11,
LED_13 = 13,
LED_RED = 20,
LED_BLUE = 21,
LED_GREEN = 22,
SCALE_DOUT = 8,
SCALE_CLK = 7
};
enum MOTORS {
LIFT = 0,
TILT = 1,
};
enum MODBUS_REGISTERS {
// Inputs
SET_POSITION_LIFT = 0,
SET_POSITION_TILT = 1,
TARE = 2,
CALIBRATION_FACTOR = 3,
// Outputs
CURRENT_POSITION_LIFT = 4,
CURRENT_POSITION_TILT = 5,
MEASURED_WEIGHT = 6,
};
////////// Global Variables //////////
int set_position_lift = 0;
int set_position_tilt = 0;
int tolerance = 20; //tolerance for position
float calibration_factor = -120000; //for the scale
// modbus stuff
const uint8_t node_id = 2;
const uint8_t mobus_serial_port_number = 3;
uint16_t modbus_data[] = {0, 0, 0, 0, 0, 0, 0};
uint8_t num_modbus_registers = 0;
int8_t poll_state = 0;
bool communication_good = false;
uint8_t message_count = 0;
// nice human words for motor states
#define BRAKEVCC 0
#define CW 1
#define CCW 2
#define BRAKEGND 3
////////// Class Instantiations //////////
Modbus slave(node_id, mobus_serial_port_number, HARDWARE::RS485_EN);
void setup() {
Serial.begin(9600); // debug
setup_hardware();
num_modbus_registers = sizeof(modbus_data) / sizeof(modbus_data[0]);
slave.begin(115200); // baud-rate at 19200
slave.setTimeOut(150);
}
void loop() {
poll_modbus();
set_leds();
set_motors();
set_scale();
poll_scale();
}
void setup_hardware() {
pinMode(HARDWARE::RS485_EN, OUTPUT);
pinMode(HARDWARE::MOTOR_LIFT_A, OUTPUT);
pinMode(HARDWARE::MOTOR_LIFT_B, OUTPUT);
pinMode(HARDWARE::MOTOR_LIFT_PWM, OUTPUT);
pinMode(HARDWARE::MOTOR_LIFT_EN, OUTPUT);
pinMode(HARDWARE::MOTOR_LIFT_CS, INPUT);
pinMode(HARDWARE::MOTOR_LIFT_FB, INPUT);
pinMode(HARDWARE::MOTOR_TILT_A, OUTPUT);
pinMode(HARDWARE::MOTOR_TILT_B, OUTPUT);
pinMode(HARDWARE::MOTOR_TILT_PWM, OUTPUT);
pinMode(HARDWARE::MOTOR_TILT_EN, OUTPUT);
pinMode(HARDWARE::MOTOR_TILT_CS, INPUT);
pinMode(HARDWARE::MOTOR_TILT_FB, INPUT);
pinMode(HARDWARE::LED_13, OUTPUT);
pinMode(HARDWARE::LED_RED, OUTPUT);
pinMode(HARDWARE::LED_BLUE, OUTPUT);
pinMode(HARDWARE::LED_GREEN, OUTPUT);
// set defualt states
digitalWrite(HARDWARE::LED_RED, LOW);
digitalWrite(HARDWARE::LED_GREEN, HIGH);
digitalWrite(HARDWARE::LED_BLUE, HIGH);
digitalWrite(HARDWARE::LED_13, LOW);
digitalWrite(HARDWARE::MOTOR_LIFT_EN, HIGH);
digitalWrite(HARDWARE::MOTOR_TILT_EN, HIGH);
// Change motor PWM frequency so it's not in the audible range
analogWriteFrequency(HARDWARE::MOTOR_LIFT_PWM, 25000);
analogWriteFrequency(HARDWARE::MOTOR_TILT_PWM, 25000);
// set the current desired position to the current position
set_position_lift = analogRead(HARDWARE::MOTOR_LIFT_FB);
set_position_tilt = analogRead(HARDWARE::MOTOR_TILT_FB);
// setup scale
scale.set_scale();
scale.tare(); //Reset the scale to 0
scale.set_scale(calibration_factor);
}
void poll_modbus(){
poll_state = slave.poll(modbus_data, num_modbus_registers);
communication_good = !slave.getTimeOutState();
}
void set_leds(){
if(poll_state > 4){
message_count++;
if(message_count > 2){
digitalWrite(HARDWARE::LED_13, !digitalRead(HARDWARE::LED_13));
message_count = 0;
}
digitalWrite(HARDWARE::LED_GREEN, LOW);
digitalWrite(HARDWARE::LED_RED, HIGH);
}else if(!communication_good){
digitalWrite(HARDWARE::LED_13, LOW);
digitalWrite(HARDWARE::LED_GREEN, HIGH);
digitalWrite(HARDWARE::LED_RED, LOW);
}
}
void set_motors() {
set_position_lift = modbus_data[MODBUS_REGISTERS::SET_POSITION_LIFT];
set_position_tilt = modbus_data[MODBUS_REGISTERS::SET_POSITION_TILT];
current_position_lift = analogRead(HARDWARE::MOTOR_LIFT_FB);
current_position_tilt = analogRead(HARDWARE::MOTOR_TILT_FB);
if (abs(current_position_lift - set_position_lift) > tolerance) {
if (current_position_lift < set_position_lift) {
set_motor_output(MOTORS::LIFT, CCW, 255);
}
else {
set_motor_output(MOTORS::LIFT, CW, 255);
}
}
else {
motor_off(MOTORS::LIFT);
}
if (abs(current_position_tilt - set_position_tilt) > tolerance) {
if (current_position_tilt < set_position_tilt) {
set_motor_output(MOTORS::TILT, CCW, 255);
}
else {
set_motor_output(MOTORS::TILT, CW, 255);
}
}
else {
motor_off(MOTORS::TILT);
}
}
void set_scale(){
scale.set_scale(modbus_data[MODBUS_REGISTERS::CALIBRATION_FACTOR]);
}
void poll_scale(){
modbus_data[MODBUS_REGISTERS::MEASURED_WEIGHT] = scale.get_units()*-1
}
//---Set Motor Output---//
/*
Inputs: motor number, direction, pwm value
Returns: nothing
Will set a motor going in a specific direction the motor will continue
going in that direction, at that speed until told to do otherwise.
direct: Should be between 0 and 3, with the following result
0: Brake to VCC
1: Clockwise
2: CounterClockwise
3: Brake to GND
pwm: should be a value between 0 and 255, higher the number, the faster
it'll go
----------------
Control Logic:
----------------
A | B
Brake VCC: 1 1
CW: 1 0
CCW: 0 1
Brake GND: 0 0
----------------
*/
void set_motor_output(int motor, int direction, int pwm_input) {
int a;
int b;
int pwm;
if (motor == MOTORS::LIFT) {
a = HARDWARE::MOTOR_LIFT_A;
b = HARDWARE::MOTOR_LIFT_B;
pwm = HARDWARE::MOTOR_LIFT_PWM;
}
else if (motor == MOTORS::TILT) {
a = HARDWARE::MOTOR_TILT_A;
b = HARDWARE::MOTOR_TILT_B;
pwm = HARDWARE::MOTOR_TILT_PWM;
}
else {
return;
}
if (direction <= 4) {
// Set A
if (direction <= 1) {
digitalWrite(a, HIGH);
}
else {
digitalWrite(a, LOW);
}
// Set B
if ((direction == 0) || (direction == 2)) {
digitalWrite(b, HIGH);
}
else {
digitalWrite(b, LOW);
}
analogWrite(pwm, pwm_input);
}
}
void motor_off(int motor) {
int a;
int b;
int pwm;
if (motor == MOTORS::LIFT) {
a = HARDWARE::MOTOR_LIFT_A;
b = HARDWARE::MOTOR_LIFT_B;
pwm = HARDWARE::MOTOR_LIFT_PWM;
}
else if (motor == MOTORS::TILT) {
a = HARDWARE::MOTOR_TILT_A;
b = HARDWARE::MOTOR_TILT_B;
pwm = HARDWARE::MOTOR_TILT_PWM;
}
else {
return;
}
digitalWrite(a, LOW);
digitalWrite(b, LOW);
analogWrite(pwm, 0);
}
////////// Includes //////////
#include "HX711.h"
#include <ModbusRtu.h>
////////// Hardware / Data Enumerations //////////
enum HARDWARE {
RS485_EN = 6,
RS485_RX = 9,
RS485_TX = 10,
MOTOR_LIFT_A = 27,
MOTOR_LIFT_B = 28,
MOTOR_LIFT_PWM = 25,
MOTOR_LIFT_CS = 31,
MOTOR_LIFT_EN = 24,
MOTOR_LIFT_FB = A10,
MOTOR_TILT_A = 30,
MOTOR_TILT_B = 29,
MOTOR_TILT_PWM = 32,
MOTOR_TILT_CS = 26,
MOTOR_TILT_EN = 33,
MOTOR_TILT_FB = A11,
LED_13 = 13,
LED_RED = 20,
LED_BLUE = 21,
LED_GREEN = 22,
SCALE_DOUT = 8,
SCALE_CLK = 7
};
enum MOTORS {
LIFT = 0,
TILT = 1,
};
enum MODBUS_REGISTERS {
// Inputs
SET_POSITION_LIFT_POSITIVE = 0,
SET_POSITION_LIFT_NEGATIVE = 1,
SET_POSITION_TILT_POSITIVE = 2,
SET_POSITION_TILT_NEGATIVE = 3,
SET_POSITION_LIFT_ABSOLUTE = 4,
SET_POSITION_TILT_ABSOLUTE = 5,
MEASURE = 6,
TARE = 7,
CALIBRATION_FACTOR = 8,
// Outputs
CURRENT_POSITION_LIFT = 9,
CURRENT_POSITION_TILT = 10,
MEASURED_WEIGHT = 11
};
////////// Global Variables //////////
int set_position_lift = 1023;
int set_position_tilt = 350;
int current_position_lift = 0;
int current_position_tilt = 0;
int tolerance = 20; //tolerance for position
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};
uint8_t num_modbus_registers = 0;
int8_t poll_state = 0;
bool communication_good = false;
uint8_t message_count = 0;
// nice human words for motor states
#define BRAKEVCC 0
#define CW 1
#define CCW 2
#define BRAKEGND 3
////////// Class Instantiations //////////
HX711 scale(HARDWARE::SCALE_DOUT, HARDWARE::SCALE_CLK);
Modbus slave(node_id, mobus_serial_port_number, HARDWARE::RS485_EN);
void setup() {
Serial.begin(9600); // debug
// while(!Serial);
setup_hardware();
num_modbus_registers = sizeof(modbus_data) / sizeof(modbus_data[0]);
slave.begin(115200); // baud-rate at 19200
slave.setTimeOut(200);
}
void loop() {
poll_modbus();
set_leds();
set_motors();
set_scale();
poll_scale();
}
void setup_hardware() {
pinMode(HARDWARE::RS485_EN, OUTPUT);
pinMode(HARDWARE::MOTOR_LIFT_A, OUTPUT);
pinMode(HARDWARE::MOTOR_LIFT_B, OUTPUT);
pinMode(HARDWARE::MOTOR_LIFT_PWM, OUTPUT);
pinMode(HARDWARE::MOTOR_LIFT_EN, OUTPUT);
pinMode(HARDWARE::MOTOR_LIFT_CS, INPUT);
pinMode(HARDWARE::MOTOR_LIFT_FB, INPUT);
pinMode(HARDWARE::MOTOR_TILT_A, OUTPUT);
pinMode(HARDWARE::MOTOR_TILT_B, OUTPUT);
pinMode(HARDWARE::MOTOR_TILT_PWM, OUTPUT);
pinMode(HARDWARE::MOTOR_TILT_EN, OUTPUT);
pinMode(HARDWARE::MOTOR_TILT_CS, INPUT);
pinMode(HARDWARE::MOTOR_TILT_FB, INPUT);
pinMode(HARDWARE::LED_13, OUTPUT);
pinMode(HARDWARE::LED_RED, OUTPUT);
pinMode(HARDWARE::LED_BLUE, OUTPUT);
pinMode(HARDWARE::LED_GREEN, OUTPUT);
// set defualt states
digitalWrite(HARDWARE::LED_RED, LOW);
digitalWrite(HARDWARE::LED_GREEN, HIGH);
digitalWrite(HARDWARE::LED_BLUE, HIGH);
digitalWrite(HARDWARE::LED_13, LOW);
digitalWrite(HARDWARE::MOTOR_LIFT_EN, HIGH);
digitalWrite(HARDWARE::MOTOR_TILT_EN, HIGH);
// Change motor PWM frequency so it's not in the audible range
analogWriteFrequency(HARDWARE::MOTOR_LIFT_PWM, 25000);
analogWriteFrequency(HARDWARE::MOTOR_TILT_PWM, 25000);
// set the current desired position to the current position
// set_position_lift = analogRead(HARDWARE::MOTOR_LIFT_FB);
// set_position_tilt = analogRead(HARDWARE::MOTOR_TILT_FB);
current_position_lift = analogRead(HARDWARE::MOTOR_LIFT_FB);
current_position_tilt = analogRead(HARDWARE::MOTOR_TILT_FB);
// setup scale
scale.set_scale(last_calibration_factor);
scale.tare(); //Reset the scale to 0
}
void poll_modbus(){
poll_state = slave.poll(modbus_data, num_modbus_registers);
communication_good = !slave.getTimeOutState();
}
void set_leds(){
if(poll_state > 4){
message_count++;
if(message_count > 2){
digitalWrite(HARDWARE::LED_13, !digitalRead(HARDWARE::LED_13));
message_count = 0;
}
digitalWrite(HARDWARE::LED_GREEN, LOW);
digitalWrite(HARDWARE::LED_RED, HIGH);
}else if(!communication_good){
digitalWrite(HARDWARE::LED_13, LOW);
digitalWrite(HARDWARE::LED_GREEN, HIGH);
digitalWrite(HARDWARE::LED_RED, LOW);
}
}
void set_motors() {
if (modbus_data[MODBUS_REGISTERS::SET_POSITION_LIFT_ABSOLUTE] < 1024 ){
set_position_lift = modbus_data[MODBUS_REGISTERS::SET_POSITION_LIFT_ABSOLUTE];
modbus_data[MODBUS_REGISTERS::SET_POSITION_LIFT_ABSOLUTE] = 1024;
}else{
set_position_lift += modbus_data[MODBUS_REGISTERS::SET_POSITION_LIFT_POSITIVE] - modbus_data[MODBUS_REGISTERS::SET_POSITION_LIFT_NEGATIVE];
set_position_lift = constrain(set_position_lift, 0, 1023);
modbus_data[MODBUS_REGISTERS::SET_POSITION_LIFT_POSITIVE] = 0;
modbus_data[MODBUS_REGISTERS::SET_POSITION_LIFT_NEGATIVE] = 0;
}
if(modbus_data[MODBUS_REGISTERS::SET_POSITION_TILT_ABSOLUTE] < 1024){
set_position_tilt = modbus_data[MODBUS_REGISTERS::SET_POSITION_TILT_ABSOLUTE];
modbus_data[MODBUS_REGISTERS::SET_POSITION_TILT_ABSOLUTE] = 1024;
}else{
set_position_tilt += modbus_data[MODBUS_REGISTERS::SET_POSITION_TILT_POSITIVE] - modbus_data[MODBUS_REGISTERS::SET_POSITION_TILT_NEGATIVE] ;
set_position_tilt = constrain(set_position_tilt, 0, 1023);
modbus_data[MODBUS_REGISTERS::SET_POSITION_TILT_POSITIVE] = 0;
modbus_data[MODBUS_REGISTERS::SET_POSITION_TILT_NEGATIVE] = 0;
}
current_position_lift = analogRead(HARDWARE::MOTOR_LIFT_FB);
current_position_tilt = analogRead(HARDWARE::MOTOR_TILT_FB);
modbus_data[MODBUS_REGISTERS::CURRENT_POSITION_LIFT] = current_position_lift;
modbus_data[MODBUS_REGISTERS::CURRENT_POSITION_TILT] = current_position_tilt;
if (abs(current_position_lift - set_position_lift) > tolerance) {
if (current_position_lift < set_position_lift) {
set_motor_output(MOTORS::LIFT, CCW, 255);
}
else {
set_motor_output(MOTORS::LIFT, CW, 255);
}
}
else {
motor_off(MOTORS::LIFT);
}
if (abs(current_position_tilt - set_position_tilt) > tolerance) {
if (current_position_tilt < set_position_tilt) {
set_motor_output(MOTORS::TILT, CCW, 255);
}
else {
set_motor_output(MOTORS::TILT, CW, 255);
}
}
else {
motor_off(MOTORS::TILT);
}
}
void set_scale(){
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;
}
if (modbus_data[MODBUS_REGISTERS::TARE] == 1){
scale.tare();
modbus_data[MODBUS_REGISTERS::TARE] = 0;
}
}
void poll_scale(){
if(modbus_data[MODBUS_REGISTERS::MEASURE] == 1){
// Serial.println(scale.get_units()*-1000);
modbus_data[MODBUS_REGISTERS::MEASURED_WEIGHT] = scale.get_units()*-1000;
modbus_data[MODBUS_REGISTERS::MEASURE] = 0;
}
}
//---Set Motor Output---//
/*
Inputs: motor number, direction, pwm value
Returns: nothing
Will set a motor going in a specific direction the motor will continue
going in that direction, at that speed until told to do otherwise.
direct: Should be between 0 and 3, with the following result
0: Brake to VCC
1: Clockwise
2: CounterClockwise
3: Brake to GND
pwm: should be a value between 0 and 255, higher the number, the faster
it'll go
----------------
Control Logic:
----------------
A | B
Brake VCC: 1 1
CW: 1 0
CCW: 0 1
Brake GND: 0 0
----------------
*/
void set_motor_output(int motor, int direction, int pwm_input) {
int a;
int b;
int pwm;
if (motor == MOTORS::LIFT) {
a = HARDWARE::MOTOR_LIFT_A;
b = HARDWARE::MOTOR_LIFT_B;
pwm = HARDWARE::MOTOR_LIFT_PWM;
}
else if (motor == MOTORS::TILT) {
a = HARDWARE::MOTOR_TILT_A;
b = HARDWARE::MOTOR_TILT_B;
pwm = HARDWARE::MOTOR_TILT_PWM;
}
else {
return;
}
if (direction <= 4) {
// Set A
if (direction <= 1) {
digitalWrite(a, HIGH);
}
else {
digitalWrite(a, LOW);
}
// Set B
if ((direction == 0) || (direction == 2)) {
digitalWrite(b, HIGH);
}
else {
digitalWrite(b, LOW);
}
analogWrite(pwm, pwm_input);
}
}
void motor_off(int motor) {
int a;
int b;
int pwm;
if (motor == MOTORS::LIFT) {
a = HARDWARE::MOTOR_LIFT_A;
b = HARDWARE::MOTOR_LIFT_B;
pwm = HARDWARE::MOTOR_LIFT_PWM;
}
else if (motor == MOTORS::TILT) {
a = HARDWARE::MOTOR_TILT_A;
b = HARDWARE::MOTOR_TILT_B;
pwm = HARDWARE::MOTOR_TILT_PWM;
}
else {
return;
}
digitalWrite(a, LOW);
digitalWrite(b, LOW);
analogWrite(pwm, 0);
}

44
software/ros_packages/ground_station/src/Framework/InputSystems/SpaceNavControlSender.py
View File

@@ -9,10 +9,20 @@ import spnav
import rospy
from rover_control.msg import MiningControlMessage
#####################################
# Global Variables
#####################################
THREAD_HERTZ = 15
THREAD_HERTZ = 100
MINING_CONTROL_TOPIC = "/rover_control/mining/control"
Y_ANGULAR_DEADBAND = 0.05
Z_LINEAR_DEADBAND = 0.15
MINING_LIFT_SCALAR = 5
MINING_TILT_SCALAR = 5
#####################################
@@ -21,7 +31,7 @@ THREAD_HERTZ = 15
class SpaceNavControlSender(QtCore.QThread):
spacenav_state_update__signal = QtCore.pyqtSignal(object)
GUI_MODE = 0
MINING_MODE = 0
ARM_MODE = 1
def __init__(self, shared_objects):
@@ -95,7 +105,10 @@ class SpaceNavControlSender(QtCore.QThread):
5: "f_pressed"
}
self.current_control_mode = self.GUI_MODE
# ##### Mining Control #####
self.mining_control_publisher = rospy.Publisher(MINING_CONTROL_TOPIC, MiningControlMessage, queue_size=1)
self.current_control_mode = self.MINING_MODE
def run(self):
spnav.spnav_open()
@@ -110,7 +123,7 @@ class SpaceNavControlSender(QtCore.QThread):
time_diff = time() - start_time
self.msleep(max(int((self.wait_time - time_diff) * 1000), 0))
# self.msleep(max(int((self.wait_time - time_diff) * 1000), 0))
def process_spnav_events(self):
event = spnav.spnav_poll_event()
@@ -132,16 +145,33 @@ class SpaceNavControlSender(QtCore.QThread):
def check_control_mode_change(self):
if self.spnav_states["1_pressed"]:
self.current_control_mode = self.GUI_MODE
self.current_control_mode = self.MINING_MODE
elif self.spnav_states["2_pressed"]:
self.current_control_mode = self.ARM_MODE
def broadcast_control_state(self):
if self.current_control_mode == self.GUI_MODE:
self.spacenav_state_update__signal.emit(self.spnav_states)
if self.current_control_mode == self.MINING_MODE:
self.send_mining_commands()
# self.spacenav_state_update__signal.emit(self.spnav_states)
elif self.current_control_mode == self.ARM_MODE:
pass
def send_mining_commands(self):
linear_z = self.spnav_states["linear_z"]
angular_y = self.spnav_states["angular_y"]
message = MiningControlMessage()
message.lift_set_absolute = 1024
message.tilt_set_absolute = 1024
message.lift_set_relative = linear_z * MINING_LIFT_SCALAR if abs(linear_z) > Z_LINEAR_DEADBAND else 0
message.tilt_set_relative = angular_y * MINING_TILT_SCALAR if abs(angular_y) > Y_ANGULAR_DEADBAND else 0
message.cal_factor = -1
self.mining_control_publisher.publish(message)
# print self.spnav_states["linear_z"], self.spnav_states["angular_y"]
def connect_signals_and_slots(self):
pass

139
software/ros_packages/ground_station/src/Framework/MiscSystems/MiningCore.py Normal file
View File

@@ -0,0 +1,139 @@
# coding=utf-8
#####################################
# Imports
#####################################
# Python native imports
from PyQt5 import QtCore, QtWidgets
import logging
import rospy
from rover_control.msg import MiningStatusMessage, MiningControlMessage
#####################################
# Global Variables
#####################################
MINING_STATUS_TOPIC = "/rover_control/mining/status"
MINING_CONTROL_TOPIC = "/rover_control/mining/control"
TRAVEL_POSITION_LIFT = 110
TRAVEL_POSITION_TILT = 1023
MEASURE_POSITION_LIFT = 350
MEASURE_POSITION_TILT = 1023
SCOOP_POSITION_LIFT = 228
SCOOP_POSITION_TILT = 215
#####################################
# UbiquitiRadioSettings Class Definition
#####################################
class Mining(QtCore.QObject):
lift_position_update_ready__signal = QtCore.pyqtSignal(int)
tilt_position_update_ready__signal = QtCore.pyqtSignal(int)
def __init__(self, shared_objects):
super(Mining, self).__init__()
# ########## Reference to class init variables ##########
self.shared_objects = shared_objects
self.left_screen = self.shared_objects["screens"]["left_screen"]
self.mining_qlcdnumber = self.left_screen.mining_qlcdnumber # type:QtWidgets.QLCDNumber
self.mining_tare_button = self.left_screen.mining_tare_button # type:QtWidgets.QPushButton
self.mining_measure_button = self.left_screen.mining_measure_button # type:QtWidgets.QPushButton
self.mining_cal_factor_spinbox = self.left_screen.mining_cal_factor_spinbox # type:QtWidgets.QSpinBox
self.mining_set_cal_factor_button = self.left_screen.mining_set_cal_factor_button # type:QtWidgets.QPushButton
self.lift_position_progress_bar = self.left_screen.lift_position_progress_bar # type:QtWidgets.QProgressBar
self.tilt_position_progress_bar = self.left_screen.tilt_position_progress_bar # type:QtWidgets.QProgressBar
self.mining_measure_move_button = self.left_screen.mining_measure_move_button # type:QtWidgets.QPushButton
self.mining_transport_move_button = self.left_screen.mining_transport_move_button # type:QtWidgets.QPushButton
self.mining_scoop_move_button = self.left_screen.mining_scoop_move_button # type:QtWidgets.QPushButton
# ########## Get the settings instance ##########
self.settings = QtCore.QSettings()
# ########## Get the Pick And Plate instance of the logger ##########
self.logger = logging.getLogger("groundstation")
# ########## Thread Flags ##########
self.run_thread_flag = True
# ########## Class Variables ##########
self.mining_status_subscriber = rospy.Subscriber(MINING_STATUS_TOPIC, MiningStatusMessage,
self.mining_status_message_received__callback)
self.mining_control_publisher = rospy.Publisher(MINING_CONTROL_TOPIC, MiningControlMessage, queue_size=1)
self.connect_signals_and_slots()
def connect_signals_and_slots(self):
self.mining_set_cal_factor_button.clicked.connect(self.on_mining_set_cal_factor_clicked__slot)
self.mining_tare_button.clicked.connect(self.on_mining_tare_clicked__slot)
self.mining_measure_button.clicked.connect(self.on_mining_measure_clicked__slot)
self.mining_measure_move_button.clicked.connect(self.on_mining_move_measure_clicked__slot)
self.mining_transport_move_button.clicked.connect(self.on_mining_move_transport_clicked__slot)
self.mining_scoop_move_button.clicked.connect(self.on_mining_move_scoop_clicked__slot)
self.tilt_position_update_ready__signal.connect(self.tilt_position_progress_bar.setValue)
self.lift_position_update_ready__signal.connect(self.lift_position_progress_bar.setValue)
def on_mining_set_cal_factor_clicked__slot(self):
message = MiningControlMessage()
message.tilt_set_absolute = 1024
message.lift_set_absolute = 1024
message.cal_factor = self.mining_cal_factor_spinbox.value()
self.mining_control_publisher.publish(message)
def on_mining_tare_clicked__slot(self):
message = MiningControlMessage()
message.tilt_set_absolute = 1024
message.lift_set_absolute = 1024
message.cal_factor = -1
message.tare = 1
self.mining_control_publisher.publish(message)
def on_mining_measure_clicked__slot(self):
message = MiningControlMessage()
message.tilt_set_absolute = 1024
message.lift_set_absolute = 1024
message.cal_factor = -1
message.measure = True
self.mining_control_publisher.publish(message)
def on_mining_move_transport_clicked__slot(self):
message = MiningControlMessage()
message.tilt_set_absolute = TRAVEL_POSITION_TILT
message.lift_set_absolute = TRAVEL_POSITION_LIFT
message.cal_factor = -1
self.mining_control_publisher.publish(message)
def on_mining_move_measure_clicked__slot(self):
message = MiningControlMessage()
message.tilt_set_absolute = MEASURE_POSITION_TILT
message.lift_set_absolute = MEASURE_POSITION_LIFT
message.cal_factor = -1
self.mining_control_publisher.publish(message)
def on_mining_move_scoop_clicked__slot(self):
message = MiningControlMessage()
message.tilt_set_absolute = SCOOP_POSITION_TILT
message.lift_set_absolute = SCOOP_POSITION_LIFT
message.cal_factor = -1
self.mining_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.mining_qlcdnumber.display(status.measured_weight)

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

@@ -1392,6 +1392,334 @@ N/A</string>
<property name="currentIndex">
<number>0</number>
</property>
<widget class="QWidget" name="tab_3">
<attribute name="title">
<string>Mining</string>
</attribute>
<layout class="QGridLayout" name="gridLayout_8">
<item row="5" column="0">
<layout class="QHBoxLayout" name="horizontalLayout_12">
<item>
<widget class="QLabel" name="label_15">
<property name="font">
<font>
<pointsize>12</pointsize>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="text">
<string>Lift Position</string>
</property>
</widget>
</item>
<item>
<widget class="QProgressBar" name="lift_position_progress_bar">
<property name="maximum">
<number>1023</number>
</property>
<property name="value">
<number>512</number>
</property>
<property name="format">
<string>%v</string>
</property>
</widget>
</item>
</layout>
</item>
<item row="6" column="0">
<layout class="QHBoxLayout" name="horizontalLayout_11">
<item>
<widget class="QLabel" name="label_14">
<property name="font">
<font>
<pointsize>12</pointsize>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="text">
<string>Tilt Position</string>
</property>
</widget>
</item>
<item>
<widget class="QProgressBar" name="tilt_position_progress_bar">
<property name="maximum">
<number>1023</number>
</property>
<property name="value">
<number>512</number>
</property>
<property name="format">
<string>%v</string>
</property>
</widget>
</item>
</layout>
</item>
<item row="0" column="0">
<widget class="QLabel" name="label_13">
<property name="font">
<font>
<pointsize>20</pointsize>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="text">
<string>Scoop Measurement</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item row="2" column="0">
<layout class="QHBoxLayout" name="horizontalLayout_5">
<item>
<spacer name="horizontalSpacer_3">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item>
<widget class="QPushButton" name="mining_tare_button">
<property name="text">
<string>Tare</string>
</property>
</widget>
</item>
<item>
<widget class="QPushButton" name="mining_measure_button">
<property name="text">
<string>Measure</string>
</property>
</widget>
</item>
<item>
<spacer name="horizontalSpacer_5">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
</layout>
</item>
<item row="3" column="0">
<layout class="QHBoxLayout" name="horizontalLayout_6">
<item>
<spacer name="horizontalSpacer_10">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item>
<widget class="QSpinBox" name="mining_cal_factor_spinbox">
<property name="maximum">
<number>65535</number>
</property>
<property name="value">
<number>114</number>
</property>
</widget>
</item>
<item>
<widget class="QPushButton" name="mining_set_cal_factor_button">
<property name="text">
<string>Set Cal Factor</string>
</property>
</widget>
</item>
<item>
<spacer name="horizontalSpacer_6">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
</layout>
</item>
<item row="1" column="0">
<layout class="QHBoxLayout" name="horizontalLayout_10">
<item>
<spacer name="horizontalSpacer_12">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item>
<widget class="QLCDNumber" name="mining_qlcdnumber">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>300</width>
<height>150</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>300</width>
<height>150</height>
</size>
</property>
<property name="font">
<font>
<pointsize>9</pointsize>
</font>
</property>
<property name="frameShape">
<enum>QFrame::NoFrame</enum>
</property>
<property name="value" stdset="0">
<double>0.000000000000000</double>
</property>
<property name="intValue" stdset="0">
<number>0</number>
</property>
</widget>
</item>
<item>
<widget class="QLabel" name="label_5">
<property name="font">
<font>
<pointsize>19</pointsize>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="text">
<string>g</string>
</property>
</widget>
</item>
<item>
<spacer name="horizontalSpacer_11">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
</layout>
</item>
<item row="8" column="0">
<spacer name="verticalSpacer">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>40</height>
</size>
</property>
</spacer>
</item>
<item row="4" column="0">
<widget class="Line" name="line_6">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
</widget>
</item>
<item row="7" column="0">
<layout class="QHBoxLayout" name="horizontalLayout_13">
<item>
<spacer name="horizontalSpacer_13">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item>
<widget class="QPushButton" name="mining_transport_move_button">
<property name="text">
<string>Transport</string>
</property>
</widget>
</item>
<item>
<widget class="QPushButton" name="mining_measure_move_button">
<property name="text">
<string>Measure</string>
</property>
</widget>
</item>
<item>
<widget class="QPushButton" name="mining_scoop_move_button">
<property name="text">
<string>Scoop</string>
</property>
</widget>
</item>
<item>
<spacer name="horizontalSpacer_14">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
</layout>
</item>
</layout>
</widget>
<widget class="QWidget" name="tab">
<attribute name="title">
<string>SSH Console</string>

5
software/ros_packages/ground_station/src/ground_station.py
View File

@@ -23,6 +23,7 @@ import Framework.StatusSystems.StatusCore as StatusCore
import Framework.StatusSystems.UbiquitiStatusCore as UbiquitiStatusCore
import Framework.SettingsSystems.UbiquitiRadioSettings as UbiquitiRadioSettings
import Framework.InputSystems.SpaceNavControlSender as SpaceNavControlSender
import Framework.MiscSystems.MiningCore as MiningCore
#####################################
# Global Variables
@@ -101,6 +102,7 @@ class GroundStation(QtCore.QObject):
rospy.init_node("ground_station")
# ##### Instantiate Regular Classes ######
self.__add_non_thread("Mining System", MiningCore.Mining(self.shared_objects))
# ##### Instantiate Threaded Classes ######
self.__add_thread("Video Coordinator", RoverVideoCoordinator.RoverVideoCoordinator(self.shared_objects))
@@ -132,6 +134,9 @@ class GroundStation(QtCore.QObject):
instance.setup_signals(self.start_threads_signal, self.connect_signals_and_slots_signal,
self.kill_threads_signal)
def __add_non_thread(self, name, instance):
self.shared_objects["regular_classes"][name] = instance
def __connect_signals_to_slots(self):
self.shared_objects["screens"]["left_screen"].exit_requested_signal.connect(self.on_exit_requested__slot)
self.shared_objects["screens"]["right_screen"].exit_requested_signal.connect(self.on_exit_requested__slot)

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

@@ -55,6 +55,8 @@ find_package(catkin REQUIRED COMPONENTS
DriveStatusMessage.msg
IrisStatusMessage.msg
TowerPanTiltControlMessage.msg
MiningControlMessage.msg
MiningStatusMessage.msg
)
## Generate services in the 'srv' folder

10
software/ros_packages/rover_control/msg/MiningControlMessage.msg Normal file
View File

@@ -0,0 +1,10 @@
int32 lift_set_relative
int32 tilt_set_relative
uint16 lift_set_absolute
uint16 tilt_set_absolute
bool measure
bool tare
int16 cal_factor

4
software/ros_packages/rover_control/msg/MiningStatusMessage.msg Normal file
View File

@@ -0,0 +1,4 @@
uint16 lift_position
uint16 tilt_position
uint16 measured_weight

2
software/ros_packages/rover_control/src/chassis_pan_tilt_control/chassis_pan_tilt_control.py
View File

@@ -96,7 +96,7 @@ class ChassisPanTiltControl(object):
delay_before_tx=TX_DELAY)
def run(self):
self.send_startup_centering_command()
# self.send_startup_centering_command()
while not rospy.is_shutdown():
start_time = time()

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

@@ -0,0 +1,280 @@
#!/usr/bin/env python
#####################################
# Imports
#####################################
# Python native imports
import rospy
from time import time, sleep
import serial.rs485
import minimalmodbus
# from std_msgs.msg import UInt8, UInt16
# Custom Imports
from rover_control.msg import MiningControlMessage, MiningStatusMessage
#####################################
# Global Variables
#####################################
NODE_NAME = "effectors_control"
# ##### Communication Defines #####
DEFAULT_PORT = "/dev/rover/ttyEffectors"
DEFAULT_BAUD = 115200
GRIPPER_NODE_ID = 1
MINING_NODE_ID = 2
SCIENCE_NODE_ID = 3
GRIPPER_TIMEOUT = 0.15
MINING_TIMEOUT = 0.3
SCIENCE_TIMEOUT = 0.15
RX_DELAY = 0.01
TX_DELAY = 0.01
DEFAULT_HERTZ = 40
GRIPPER_CONTROL_SUBSCRIBER_TOPIC = "gripper/control"
MINING_CONTROL_SUBSCRIBER_TOPIC = "mining/control"
MINING_STATUS_PUBLISHER_TOPIC = "mining/status"
SCIENCE_CONTROL_SUBSCRIBER_TOPIC = "science/control"
# ##### Arm Defines #####
# ##### Mining Defines #####
MINING_MODBUS_REGISTERS = {
"LIFT_SET_POSITIVE": 0,
"LIFT_SET_NEGATIVE": 1,
"TILT_SET_POSITIVE": 2,
"TILT_SET_NEGATIVE": 3,
"TILT_SET_ABSOLUTE": 4,
"LIFT_SET_ABSOLUTE": 5,
"MEASURE": 6,
"TARE": 7,
"CAL_FACTOR": 8,
"LIFT_POSITION": 9,
"TILT_POSITION": 10,
"MEASURED_WEIGHT": 11
}
MINING_POSITIONAL_THRESHOLD = 20
# ##### Science Defines #####
# ##### Misc Defines #####
NODE_LAST_SEEN_TIMEOUT = 2 # seconds
UINT16_MAX = 65535
#####################################
# DriveControl Class Definition
#####################################
class EffectorsControl(object):
def __init__(self):
rospy.init_node(NODE_NAME)
self.port = rospy.get_param("~port", DEFAULT_PORT)
self.baud = rospy.get_param("~baud", DEFAULT_BAUD)
self.gripper_node_id = rospy.get_param("~gripper_node_id", GRIPPER_NODE_ID)
self.mining_node_id = rospy.get_param("~mining_node_id", MINING_NODE_ID)
self.science_node_id = rospy.get_param("~science_node_id", SCIENCE_NODE_ID)
self.gripper_control_subscriber_topic = rospy.get_param("~gripper_control_subscriber_topic",
GRIPPER_CONTROL_SUBSCRIBER_TOPIC)
self.mining_control_subscriber_topic = rospy.get_param("~mining_control_subscriber_topic",
MINING_CONTROL_SUBSCRIBER_TOPIC)
self.mining_status_publisher_topic = rospy.get_param("~mining_status_publisher_topic",
MINING_STATUS_PUBLISHER_TOPIC)
self.science_control_subscriber_topic = rospy.get_param("~science_control_subscriber_topic",
SCIENCE_CONTROL_SUBSCRIBER_TOPIC)
self.wait_time = 1.0 / rospy.get_param("~hertz", DEFAULT_HERTZ)
self.gripper_node = None # type:minimalmodbus.Instrument
self.mining_node = None # type:minimalmodbus.Instrument
self.science_node = None # type:minimalmodbus.Instrument
self.gripper_node_present = False
self.mining_node_present = True
self.science_node_present = False
self.connect_to_nodes()
# self.check_which_nodes_present()
# ##### Subscribers #####
self.mining_control_subscriber = rospy.Subscriber(self.mining_control_subscriber_topic, MiningControlMessage,
self.mining_control_message_received__callback)
# ##### Publishers #####
self.mining_status_publisher = rospy.Publisher(self.mining_status_publisher_topic, MiningStatusMessage, queue_size=1)
# ##### Misc #####
self.modbus_nodes_seen_time = time()
# ##### Mining Variables #####
self.mining_registers = [0 for _ in MINING_MODBUS_REGISTERS]
self.mining_control_message = None # type:MiningControlMessage
self.new_mining_control_message = False
self.run()
def __setup_minimalmodbus_for_485(self):
self.gripper_node.serial = serial.rs485.RS485(self.port, baudrate=self.baud, timeout=GRIPPER_TIMEOUT)
self.gripper_node.serial.rs485_mode = serial.rs485.RS485Settings(rts_level_for_rx=1, rts_level_for_tx=0,
delay_before_rx=RX_DELAY,
delay_before_tx=TX_DELAY)
self.mining_node.serial = serial.rs485.RS485(self.port, baudrate=self.baud, timeout=MINING_TIMEOUT)
self.mining_node.serial.rs485_mode = serial.rs485.RS485Settings(rts_level_for_rx=1, rts_level_for_tx=0,
delay_before_rx=RX_DELAY,
delay_before_tx=TX_DELAY)
self.science_node.serial = serial.rs485.RS485(self.port, baudrate=self.baud, timeout=SCIENCE_TIMEOUT)
self.science_node.serial.rs485_mode = serial.rs485.RS485Settings(rts_level_for_rx=1, rts_level_for_tx=0,
delay_before_rx=RX_DELAY,
delay_before_tx=TX_DELAY)
def run(self):
# self.initialize_mining_system()
while not rospy.is_shutdown():
try:
self.process_mining_control_message()
except IOError, e:
print e
if (time() - self.modbus_nodes_seen_time) > NODE_LAST_SEEN_TIMEOUT:
print "Lost connection to mining system. Exiting for reconnect."
return
except Exception, e:
print e
try:
self.send_mining_status_message()
except IOError, e:
print e
if (time() - self.modbus_nodes_seen_time) > NODE_LAST_SEEN_TIMEOUT:
print "Lost connection to mining system. Exiting for reconnect."
return
except Exception, e:
print e
def connect_to_nodes(self):
self.gripper_node = minimalmodbus.Instrument(self.port, int(self.gripper_node_id))
self.mining_node = minimalmodbus.Instrument(self.port, int(self.mining_node_id))
self.science_node = minimalmodbus.Instrument(self.port, int(self.science_node_id))
self.__setup_minimalmodbus_for_485()
def check_which_nodes_present(self):
try:
self.gripper_node.read_register(0)
self.gripper_node_present = True
except:
self.gripper_node_present = False
try:
self.mining_node.read_register(0)
self.mining_node_present = True
except:
self.mining_node_present = False
try:
self.science_node.read_register(0)
self.science_node_present = True
except:
self.science_node_present = False
def initialize_mining_system(self):
if self.mining_node_present:
self.mining_registers[MINING_MODBUS_REGISTERS["LIFT_SET"]] = 1023
self.mining_registers[MINING_MODBUS_REGISTERS["TILT_SET"]] = 350
self.mining_registers[MINING_MODBUS_REGISTERS["CAL_FACTOR"]] = 114
while abs(self.mining_registers[MINING_MODBUS_REGISTERS["LIFT_POSITION"]] - self.mining_registers[
MINING_MODBUS_REGISTERS["LIFT_SET"]]) > MINING_POSITIONAL_THRESHOLD or \
abs(self.mining_registers[MINING_MODBUS_REGISTERS["TILT_POSITION"]] - self.mining_registers[
MINING_MODBUS_REGISTERS["TILT_SET"]]) > MINING_POSITIONAL_THRESHOLD:
try:
self.mining_node.write_registers(0, self.mining_registers)
self.mining_registers = self.mining_node.read_registers(0, 7)
except Exception, e:
print "Had trouble communicating:", e
try:
self.mining_registers[MINING_MODBUS_REGISTERS["TARE"]] = 1
self.mining_node.write_registers(0, self.mining_registers)
self.mining_registers[MINING_MODBUS_REGISTERS["TARE"]] = 0
except:
print "Had trouble communicating: no tare: ", e
def process_mining_control_message(self):
if self.new_mining_control_message and self.mining_node_present:
lift_set_relative = self.mining_control_message.lift_set_relative
tilt_set_relative = self.mining_control_message.tilt_set_relative
lift_set_absolute = self.mining_control_message.lift_set_absolute
tilt_set_absolute = self.mining_control_message.tilt_set_absolute
cal_factor = min(self.mining_control_message.cal_factor, UINT16_MAX)
measure = self.mining_control_message.measure
tare = self.mining_control_message.tare
if lift_set_absolute < 1024:
self.mining_registers[MINING_MODBUS_REGISTERS["LIFT_SET_ABSOLUTE"]] = lift_set_absolute
else:
if lift_set_relative >= 0:
self.mining_registers[MINING_MODBUS_REGISTERS["LIFT_SET_POSITIVE"]] = lift_set_relative
else:
self.mining_registers[MINING_MODBUS_REGISTERS["LIFT_SET_NEGATIVE"]] = -lift_set_relative
if tilt_set_absolute < 1024:
self.mining_registers[MINING_MODBUS_REGISTERS["TILT_SET_ABSOLUTE"]] = tilt_set_absolute
else:
if tilt_set_relative >= 0:
self.mining_registers[MINING_MODBUS_REGISTERS["TILT_SET_POSITIVE"]] = tilt_set_relative
else:
self.mining_registers[MINING_MODBUS_REGISTERS["TILT_SET_NEGATIVE"]] = -tilt_set_relative
if cal_factor > -1:
self.mining_registers[MINING_MODBUS_REGISTERS["CAL_FACTOR"]] = cal_factor
self.mining_registers[MINING_MODBUS_REGISTERS["MEASURE"]] = int(measure)
self.mining_registers[MINING_MODBUS_REGISTERS["TARE"]] = int(tare)
self.mining_node.write_registers(0, self.mining_registers)
self.modbus_nodes_seen_time = time()
self.new_mining_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.measured_weight = self.mining_registers[MINING_MODBUS_REGISTERS["MEASURED_WEIGHT"]]
self.mining_status_publisher.publish(message)
self.modbus_nodes_seen_time = time()
def mining_control_message_received__callback(self, control_message):
self.mining_control_message = control_message
self.new_mining_control_message = True
if __name__ == "__main__":
EffectorsControl()

31
software/ros_packages/rover_control/src/effectors_control/effectors_tester.py Executable file
View File

@@ -0,0 +1,31 @@
#!/usr/bin/env python
import rospy
import time
from rover_control.msg import MiningControlMessage
DEFAULT_TOWER_PAN_TILT_CONTROL_TOPIC = "/rover_control/mining/control"
rospy.init_node("effectors_tester")
publisher = rospy.Publisher(DEFAULT_TOWER_PAN_TILT_CONTROL_TOPIC, MiningControlMessage, queue_size=1)
time.sleep(2)
message = MiningControlMessage()
message.lift_set = 200
message.tilt_set = 1023
message.cal_factor = -1
publisher.publish(message)
time.sleep(5)
# message = MiningControlMessage()
# message.lift_set = -200
# message.tilt_set = -100
# message.cal_factor = -1
#
# publisher.publish(message)
#
# time.sleep(2)

6
software/ros_packages/rover_control/src/tower_pan_tilt_control/tower_and_pan_tilt_control.py
View File

@@ -172,9 +172,9 @@ class TowerPanTiltControl(object):
def send_startup_centering_and_lights_off_command(self):
try:
registers = list(PAN_TILT_CONTROL_DEFAULT_MESSAGE)
registers[PAN_TILT_MODBUS_REGISTERS["CENTER_ALL"]] = 1
self.pan_tilt_node.write_registers(0, registers)
# registers = list(PAN_TILT_CONTROL_DEFAULT_MESSAGE)
# registers[PAN_TILT_MODBUS_REGISTERS["CENTER_ALL"]] = 1
# self.pan_tilt_node.write_registers(0, registers)
self.tower_node.write_register(0, TOWER_LIGHT_STATES["LIGHT_OFF"])
except:

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

@@ -8,7 +8,8 @@
<rosparam param="topics">
[{name: "/rover_control/command_control/ground_station_drive", compress: true, rate: 15.0},
{name: "/rover_control/tower/pan_tilt/control", compress: true, rate: 30.0},
{name: "/rover_control/chassis/pan_tilt/control", compress: true, rate: 30.0}]
{name: "/rover_control/chassis/pan_tilt/control", compress: true, rate: 30.0},
{name: "/rover_control/mining/control", compress: true, rate: 15.0}]
</rosparam>
</node>

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

@@ -34,5 +34,7 @@
<node name="tower_and_pan_tilt" pkg="rover_control" type="tower_and_pan_tilt_control.py" respawn="true" output="screen"/>
<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"/>
</group>
</launch>

6
software/ros_packages/rover_main/launch/rover/test_control.launch Normal file
View File

@@ -0,0 +1,6 @@
<launch>
<group ns="rover_control">
<node name="effectors" pkg="rover_control" type="effectors_control.py" respawn="true" output="screen">
</node>
</group>
</launch>

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

@@ -155,6 +155,7 @@
{name: "/rover_status/battery_status", compress: false, rate: 1.0},
{name: "/rover_control/tower/status/co2", compress: false, rate: 1.0},
{name: "/rover_odometry/imu/data", compress: false, rate: 10.0},
{name: "/rover_control/mining/status", compress: false, rate: 2.0},
]
</rosparam>
</node>

15
software/testing/modbus/mining/mining_tester.py
View File

@@ -19,7 +19,7 @@ import minimalmodbus
#####################################
NODE_NAME = "chassis_pan_tilt_control"
DEFAULT_PORT = "/dev/rover/ttyChassisPanTilt"
DEFAULT_PORT = "/dev/rover/ttyEffectors"
DEFAULT_BAUD = 115200
DEFAULT_INVERT = False
@@ -57,7 +57,7 @@ NODE_LAST_SEEN_TIMEOUT = 2 # seconds
#####################################
class MiningControl(object):
def __init__(self):
self.port = "/dev/ttyUSB0"
self.port = DEFAULT_PORT
self.baud = 115200
self.mining_node = None
@@ -93,8 +93,8 @@ class MiningControl(object):
while True:
try:
print self.mining_node.read_registers(0, 7)
# self.mining_registers[MINING_MODBUS_REGISTERS["CAL_FACTOR"]] = int(input("Enter new cal value:"))
# self.mining_node.write_registers(0, self.mining_registers)
self.mining_registers[MINING_MODBUS_REGISTERS["TILT_SET"]] = int(input("Enter new tilt value:"))
self.mining_node.write_registers(0, self.mining_registers)
except Exception, e:
print e
@@ -106,12 +106,11 @@ class MiningControl(object):
def initialize_mining_system(self):
self.mining_registers[MINING_MODBUS_REGISTERS["LIFT_SET"]] = 1023
self.mining_registers[MINING_MODBUS_REGISTERS["TILT_SET"]] = 350
self.mining_registers[MINING_MODBUS_REGISTERS["CAL_FACTOR"]] = 114
lift_current = 0
tilt_current = 0
while abs(self.mining_registers[MINING_MODBUS_REGISTERS["LIFT_POSITION"]] - self.mining_registers[MINING_MODBUS_REGISTERS["LIFT_SET"]]) > POSITIONAL_THRESHOLD or abs(self.mining_registers[MINING_MODBUS_REGISTERS["TILT_POSITION"]] - self.mining_registers[MINING_MODBUS_REGISTERS["TILT_SET"]]) > POSITIONAL_THRESHOLD:
while abs(self.mining_registers[MINING_MODBUS_REGISTERS["LIFT_POSITION"]] - self.mining_registers[MINING_MODBUS_REGISTERS["LIFT_SET"]]) > POSITIONAL_THRESHOLD or \
abs(self.mining_registers[MINING_MODBUS_REGISTERS["TILT_POSITION"]] - self.mining_registers[MINING_MODBUS_REGISTERS["TILT_SET"]]) > POSITIONAL_THRESHOLD:
try:
self.mining_node.write_registers(0, self.mining_registers)
self.mining_registers = self.mining_node.read_registers(0, 7)

28
software/testing/udp_debug.py Normal file
View File

@@ -0,0 +1,28 @@
import socket
import sys
# Create a UDP socket
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
server_address = ('task.cstag.ca', 4547)
messages = ["HELP", "LOGIN MTECH GITRDONE", "STATUS", "START", "STOP", "LOGOUT"]
print "Connected.... Enter commands now..."
# for message in messages:
while True:
try:
# Send data
message = raw_input()
# print type(message)
if message not in messages:
print "Invalid command. Please try again."
continue
sent = sock.sendto(message, server_address)
# Receive response
# print 'waiting to receive'
data, server = sock.recvfrom(4096)
print data
except:
pass