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}=="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}=="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}=="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}=="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" 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 #!/bin/bash
sudo cp 99-rover-cameras.rules /etc/udev/rules.d/. sudo cp 99-rover-cameras.rules /etc/udev/rules.d/.
sudo cp 99-rover-usb-serial.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 ////////// ////////// Includes //////////
#include "HX711.h" #include "HX711.h"
#include <ModbusRtu.h>
////////// Hardware / Data Enumerations //////////
enum HARDWARE { ////////// Hardware / Data Enumerations //////////
RS485_EN = 6, enum HARDWARE {
RS485_RX = 9, RS485_EN = 6,
RS485_TX = 10, RS485_RX = 9,
RS485_TX = 10,
MOTOR_LIFT_A = 27,
MOTOR_LIFT_B = 28, MOTOR_LIFT_A = 27,
MOTOR_LIFT_PWM = 25, MOTOR_LIFT_B = 28,
MOTOR_LIFT_CS = 31, MOTOR_LIFT_PWM = 25,
MOTOR_LIFT_EN = 24, MOTOR_LIFT_CS = 31,
MOTOR_LIFT_FB = A10, MOTOR_LIFT_EN = 24,
MOTOR_LIFT_FB = A10,
MOTOR_TILT_A = 30,
MOTOR_TILT_B = 29, MOTOR_TILT_A = 30,
MOTOR_TILT_PWM = 32, MOTOR_TILT_B = 29,
MOTOR_TILT_CS = 26, MOTOR_TILT_PWM = 32,
MOTOR_TILT_EN = 33, MOTOR_TILT_CS = 26,
MOTOR_TILT_FB = A11, MOTOR_TILT_EN = 33,
MOTOR_TILT_FB = A11,
LED_13 = 13,
LED_13 = 13,
LED_RED = 20,
LED_BLUE = 21, LED_RED = 20,
LED_GREEN = 22, LED_BLUE = 21,
LED_GREEN = 22,
SCALE_DOUT = 8,
SCALE_CLK = 7 SCALE_DOUT = 8,
SCALE_CLK = 7
};
};
enum MOTORS {
LIFT = 0, enum MOTORS {
TILT = 1, LIFT = 0,
}; TILT = 1,
};
enum MODBUS_REGISTERS {
// Inputs enum MODBUS_REGISTERS {
SET_POSITION_LIFT = 0, // Inputs
SET_POSITION_TILT = 1, SET_POSITION_LIFT_POSITIVE = 0,
TARE = 2, SET_POSITION_LIFT_NEGATIVE = 1,
CALIBRATION_FACTOR = 3, SET_POSITION_TILT_POSITIVE = 2,
SET_POSITION_TILT_NEGATIVE = 3,
// Outputs SET_POSITION_LIFT_ABSOLUTE = 4,
CURRENT_POSITION_LIFT = 4, SET_POSITION_TILT_ABSOLUTE = 5,
CURRENT_POSITION_TILT = 5, MEASURE = 6,
MEASURED_WEIGHT = 6, TARE = 7,
}; CALIBRATION_FACTOR = 8,
////////// Global Variables ////////// // Outputs
int set_position_lift = 0; CURRENT_POSITION_LIFT = 9,
int set_position_tilt = 0; CURRENT_POSITION_TILT = 10,
int tolerance = 20; //tolerance for position MEASURED_WEIGHT = 11
};
float calibration_factor = -120000; //for the scale
////////// Global Variables //////////
// modbus stuff int set_position_lift = 1023;
const uint8_t node_id = 2; int set_position_tilt = 350;
const uint8_t mobus_serial_port_number = 3; int current_position_lift = 0;
uint16_t modbus_data[] = {0, 0, 0, 0, 0, 0, 0}; int current_position_tilt = 0;
uint8_t num_modbus_registers = 0; int tolerance = 20; //tolerance for position
int8_t poll_state = 0;
bool communication_good = false; float last_calibration_factor = -120000; //for the scale
uint8_t message_count = 0;
// modbus stuff
// nice human words for motor states const uint8_t node_id = 2;
#define BRAKEVCC 0 const uint8_t mobus_serial_port_number = 2;
#define CW 1 uint16_t modbus_data[] = {0, 0, 0, 0, 9999, 9999, 0, 0, 972, 0, 0, 0};
#define CCW 2 uint8_t num_modbus_registers = 0;
#define BRAKEGND 3 int8_t poll_state = 0;
bool communication_good = false;
////////// Class Instantiations ////////// uint8_t message_count = 0;
Modbus slave(node_id, mobus_serial_port_number, HARDWARE::RS485_EN);
// nice human words for motor states
void setup() { #define BRAKEVCC 0
Serial.begin(9600); // debug #define CW 1
#define CCW 2
setup_hardware(); #define BRAKEGND 3
num_modbus_registers = sizeof(modbus_data) / sizeof(modbus_data[0]);
slave.begin(115200); // baud-rate at 19200 ////////// Class Instantiations //////////
slave.setTimeOut(150); HX711 scale(HARDWARE::SCALE_DOUT, HARDWARE::SCALE_CLK);
} Modbus slave(node_id, mobus_serial_port_number, HARDWARE::RS485_EN);
void loop() { void setup() {
poll_modbus(); Serial.begin(9600); // debug
set_leds(); // while(!Serial);
set_motors();
set_scale(); setup_hardware();
poll_scale(); num_modbus_registers = sizeof(modbus_data) / sizeof(modbus_data[0]);
} slave.begin(115200); // baud-rate at 19200
slave.setTimeOut(200);
void setup_hardware() { }
pinMode(HARDWARE::RS485_EN, OUTPUT);
void loop() {
pinMode(HARDWARE::MOTOR_LIFT_A, OUTPUT); poll_modbus();
pinMode(HARDWARE::MOTOR_LIFT_B, OUTPUT); set_leds();
pinMode(HARDWARE::MOTOR_LIFT_PWM, OUTPUT); set_motors();
pinMode(HARDWARE::MOTOR_LIFT_EN, OUTPUT); set_scale();
pinMode(HARDWARE::MOTOR_LIFT_CS, INPUT); poll_scale();
pinMode(HARDWARE::MOTOR_LIFT_FB, INPUT); }
pinMode(HARDWARE::MOTOR_TILT_A, OUTPUT); void setup_hardware() {
pinMode(HARDWARE::MOTOR_TILT_B, OUTPUT); pinMode(HARDWARE::RS485_EN, OUTPUT);
pinMode(HARDWARE::MOTOR_TILT_PWM, OUTPUT);
pinMode(HARDWARE::MOTOR_TILT_EN, OUTPUT); pinMode(HARDWARE::MOTOR_LIFT_A, OUTPUT);
pinMode(HARDWARE::MOTOR_TILT_CS, INPUT); pinMode(HARDWARE::MOTOR_LIFT_B, OUTPUT);
pinMode(HARDWARE::MOTOR_TILT_FB, INPUT); pinMode(HARDWARE::MOTOR_LIFT_PWM, OUTPUT);
pinMode(HARDWARE::MOTOR_LIFT_EN, OUTPUT);
pinMode(HARDWARE::LED_13, OUTPUT); pinMode(HARDWARE::MOTOR_LIFT_CS, INPUT);
pinMode(HARDWARE::LED_RED, OUTPUT); pinMode(HARDWARE::MOTOR_LIFT_FB, INPUT);
pinMode(HARDWARE::LED_BLUE, OUTPUT);
pinMode(HARDWARE::LED_GREEN, OUTPUT); pinMode(HARDWARE::MOTOR_TILT_A, OUTPUT);
pinMode(HARDWARE::MOTOR_TILT_B, OUTPUT);
// set defualt states pinMode(HARDWARE::MOTOR_TILT_PWM, OUTPUT);
digitalWrite(HARDWARE::LED_RED, LOW); pinMode(HARDWARE::MOTOR_TILT_EN, OUTPUT);
digitalWrite(HARDWARE::LED_GREEN, HIGH); pinMode(HARDWARE::MOTOR_TILT_CS, INPUT);
digitalWrite(HARDWARE::LED_BLUE, HIGH); pinMode(HARDWARE::MOTOR_TILT_FB, INPUT);
digitalWrite(HARDWARE::LED_13, LOW);
digitalWrite(HARDWARE::MOTOR_LIFT_EN, HIGH); pinMode(HARDWARE::LED_13, OUTPUT);
digitalWrite(HARDWARE::MOTOR_TILT_EN, HIGH); pinMode(HARDWARE::LED_RED, OUTPUT);
pinMode(HARDWARE::LED_BLUE, OUTPUT);
// Change motor PWM frequency so it's not in the audible range pinMode(HARDWARE::LED_GREEN, OUTPUT);
analogWriteFrequency(HARDWARE::MOTOR_LIFT_PWM, 25000);
analogWriteFrequency(HARDWARE::MOTOR_TILT_PWM, 25000); // set defualt states
digitalWrite(HARDWARE::LED_RED, LOW);
// set the current desired position to the current position digitalWrite(HARDWARE::LED_GREEN, HIGH);
set_position_lift = analogRead(HARDWARE::MOTOR_LIFT_FB); digitalWrite(HARDWARE::LED_BLUE, HIGH);
set_position_tilt = analogRead(HARDWARE::MOTOR_TILT_FB); digitalWrite(HARDWARE::LED_13, LOW);
digitalWrite(HARDWARE::MOTOR_LIFT_EN, HIGH);
// setup scale digitalWrite(HARDWARE::MOTOR_TILT_EN, HIGH);
scale.set_scale();
scale.tare(); //Reset the scale to 0 // Change motor PWM frequency so it's not in the audible range
scale.set_scale(calibration_factor); analogWriteFrequency(HARDWARE::MOTOR_LIFT_PWM, 25000);
} analogWriteFrequency(HARDWARE::MOTOR_TILT_PWM, 25000);
void poll_modbus(){ // set the current desired position to the current position
poll_state = slave.poll(modbus_data, num_modbus_registers); // set_position_lift = analogRead(HARDWARE::MOTOR_LIFT_FB);
communication_good = !slave.getTimeOutState(); // set_position_tilt = analogRead(HARDWARE::MOTOR_TILT_FB);
} current_position_lift = analogRead(HARDWARE::MOTOR_LIFT_FB);
current_position_tilt = analogRead(HARDWARE::MOTOR_TILT_FB);
void set_leds(){
if(poll_state > 4){ // setup scale
message_count++; scale.set_scale(last_calibration_factor);
if(message_count > 2){ scale.tare(); //Reset the scale to 0
digitalWrite(HARDWARE::LED_13, !digitalRead(HARDWARE::LED_13)); }
message_count = 0;
} void poll_modbus(){
poll_state = slave.poll(modbus_data, num_modbus_registers);
digitalWrite(HARDWARE::LED_GREEN, LOW); communication_good = !slave.getTimeOutState();
digitalWrite(HARDWARE::LED_RED, HIGH); }
}else if(!communication_good){
digitalWrite(HARDWARE::LED_13, LOW); void set_leds(){
digitalWrite(HARDWARE::LED_GREEN, HIGH); if(poll_state > 4){
digitalWrite(HARDWARE::LED_RED, LOW); message_count++;
} if(message_count > 2){
} digitalWrite(HARDWARE::LED_13, !digitalRead(HARDWARE::LED_13));
message_count = 0;
void set_motors() { }
set_position_lift = modbus_data[MODBUS_REGISTERS::SET_POSITION_LIFT];
set_position_tilt = modbus_data[MODBUS_REGISTERS::SET_POSITION_TILT]; digitalWrite(HARDWARE::LED_GREEN, LOW);
digitalWrite(HARDWARE::LED_RED, HIGH);
current_position_lift = analogRead(HARDWARE::MOTOR_LIFT_FB); }else if(!communication_good){
current_position_tilt = analogRead(HARDWARE::MOTOR_TILT_FB); digitalWrite(HARDWARE::LED_13, LOW);
digitalWrite(HARDWARE::LED_GREEN, HIGH);
if (abs(current_position_lift - set_position_lift) > tolerance) { digitalWrite(HARDWARE::LED_RED, LOW);
if (current_position_lift < set_position_lift) { }
set_motor_output(MOTORS::LIFT, CCW, 255); }
}
else { void set_motors() {
set_motor_output(MOTORS::LIFT, CW, 255); 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 { }else{
motor_off(MOTORS::LIFT); 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);
if (abs(current_position_tilt - set_position_tilt) > tolerance) { modbus_data[MODBUS_REGISTERS::SET_POSITION_LIFT_POSITIVE] = 0;
if (current_position_tilt < set_position_tilt) { modbus_data[MODBUS_REGISTERS::SET_POSITION_LIFT_NEGATIVE] = 0;
set_motor_output(MOTORS::TILT, CCW, 255); }
}
else { if(modbus_data[MODBUS_REGISTERS::SET_POSITION_TILT_ABSOLUTE] < 1024){
set_motor_output(MOTORS::TILT, CW, 255); set_position_tilt = modbus_data[MODBUS_REGISTERS::SET_POSITION_TILT_ABSOLUTE];
} modbus_data[MODBUS_REGISTERS::SET_POSITION_TILT_ABSOLUTE] = 1024;
} }else{
else { set_position_tilt += modbus_data[MODBUS_REGISTERS::SET_POSITION_TILT_POSITIVE] - modbus_data[MODBUS_REGISTERS::SET_POSITION_TILT_NEGATIVE] ;
motor_off(MOTORS::TILT); 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;
void set_scale(){ }
scale.set_scale(modbus_data[MODBUS_REGISTERS::CALIBRATION_FACTOR]);
} current_position_lift = analogRead(HARDWARE::MOTOR_LIFT_FB);
current_position_tilt = analogRead(HARDWARE::MOTOR_TILT_FB);
void poll_scale(){
modbus_data[MODBUS_REGISTERS::MEASURED_WEIGHT] = scale.get_units()*-1 modbus_data[MODBUS_REGISTERS::CURRENT_POSITION_LIFT] = current_position_lift;
} modbus_data[MODBUS_REGISTERS::CURRENT_POSITION_TILT] = current_position_tilt;
//---Set Motor Output---// if (abs(current_position_lift - set_position_lift) > tolerance) {
/* if (current_position_lift < set_position_lift) {
Inputs: motor number, direction, pwm value set_motor_output(MOTORS::LIFT, CCW, 255);
Returns: nothing }
else {
Will set a motor going in a specific direction the motor will continue set_motor_output(MOTORS::LIFT, CW, 255);
going in that direction, at that speed until told to do otherwise. }
}
direct: Should be between 0 and 3, with the following result else {
0: Brake to VCC motor_off(MOTORS::LIFT);
1: Clockwise }
2: CounterClockwise
3: Brake to GND if (abs(current_position_tilt - set_position_tilt) > tolerance) {
if (current_position_tilt < set_position_tilt) {
pwm: should be a value between 0 and 255, higher the number, the faster set_motor_output(MOTORS::TILT, CCW, 255);
it'll go }
---------------- else {
Control Logic: set_motor_output(MOTORS::TILT, CW, 255);
---------------- }
A | B }
Brake VCC: 1 1 else {
CW: 1 0 motor_off(MOTORS::TILT);
CCW: 0 1 }
Brake GND: 0 0 }
----------------
*/ void set_scale(){
void set_motor_output(int motor, int direction, int pwm_input) { float cal_factor = modbus_data[MODBUS_REGISTERS::CALIBRATION_FACTOR] * -100;
if(cal_factor != last_calibration_factor){
int a; scale.set_scale(cal_factor);
int b; last_calibration_factor = cal_factor;
int pwm; }
if (motor == MOTORS::LIFT) { if (modbus_data[MODBUS_REGISTERS::TARE] == 1){
a = HARDWARE::MOTOR_LIFT_A; scale.tare();
b = HARDWARE::MOTOR_LIFT_B; modbus_data[MODBUS_REGISTERS::TARE] = 0;
pwm = HARDWARE::MOTOR_LIFT_PWM; }
} }
else if (motor == MOTORS::TILT) {
a = HARDWARE::MOTOR_TILT_A; void poll_scale(){
b = HARDWARE::MOTOR_TILT_B; if(modbus_data[MODBUS_REGISTERS::MEASURE] == 1){
pwm = HARDWARE::MOTOR_TILT_PWM; // Serial.println(scale.get_units()*-1000);
} modbus_data[MODBUS_REGISTERS::MEASURED_WEIGHT] = scale.get_units()*-1000;
else { modbus_data[MODBUS_REGISTERS::MEASURE] = 0;
return; }
} }
if (direction <= 4) { //---Set Motor Output---//
// Set A /*
if (direction <= 1) { Inputs: motor number, direction, pwm value
digitalWrite(a, HIGH); Returns: nothing
}
else { Will set a motor going in a specific direction the motor will continue
digitalWrite(a, LOW); going in that direction, at that speed until told to do otherwise.
}
direct: Should be between 0 and 3, with the following result
// Set B 0: Brake to VCC
if ((direction == 0) || (direction == 2)) { 1: Clockwise
digitalWrite(b, HIGH); 2: CounterClockwise
} 3: Brake to GND
else {
digitalWrite(b, LOW); pwm: should be a value between 0 and 255, higher the number, the faster
} it'll go
analogWrite(pwm, pwm_input); ----------------
} Control Logic:
} ----------------
A | B
void motor_off(int motor) { Brake VCC: 1 1
int a; CW: 1 0
int b; CCW: 0 1
int pwm; Brake GND: 0 0
----------------
if (motor == MOTORS::LIFT) { */
a = HARDWARE::MOTOR_LIFT_A; void set_motor_output(int motor, int direction, int pwm_input) {
b = HARDWARE::MOTOR_LIFT_B;
pwm = HARDWARE::MOTOR_LIFT_PWM; int a;
} int b;
else if (motor == MOTORS::TILT) { int pwm;
a = HARDWARE::MOTOR_TILT_A;
b = HARDWARE::MOTOR_TILT_B; if (motor == MOTORS::LIFT) {
pwm = HARDWARE::MOTOR_TILT_PWM; a = HARDWARE::MOTOR_LIFT_A;
} b = HARDWARE::MOTOR_LIFT_B;
else { pwm = HARDWARE::MOTOR_LIFT_PWM;
return; }
} else if (motor == MOTORS::TILT) {
a = HARDWARE::MOTOR_TILT_A;
digitalWrite(a, LOW); b = HARDWARE::MOTOR_TILT_B;
digitalWrite(b, LOW); pwm = HARDWARE::MOTOR_TILT_PWM;
analogWrite(pwm, 0); }
} 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 import rospy
from rover_control.msg import MiningControlMessage
##################################### #####################################
# Global Variables # 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): class SpaceNavControlSender(QtCore.QThread):
spacenav_state_update__signal = QtCore.pyqtSignal(object) spacenav_state_update__signal = QtCore.pyqtSignal(object)
GUI_MODE = 0 MINING_MODE = 0
ARM_MODE = 1 ARM_MODE = 1
def __init__(self, shared_objects): def __init__(self, shared_objects):
@@ -95,7 +105,10 @@ class SpaceNavControlSender(QtCore.QThread):
5: "f_pressed" 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): def run(self):
spnav.spnav_open() spnav.spnav_open()
@@ -110,7 +123,7 @@ class SpaceNavControlSender(QtCore.QThread):
time_diff = time() - start_time 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): def process_spnav_events(self):
event = spnav.spnav_poll_event() event = spnav.spnav_poll_event()
@@ -132,16 +145,33 @@ class SpaceNavControlSender(QtCore.QThread):
def check_control_mode_change(self): def check_control_mode_change(self):
if self.spnav_states["1_pressed"]: 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"]: elif self.spnav_states["2_pressed"]:
self.current_control_mode = self.ARM_MODE self.current_control_mode = self.ARM_MODE
def broadcast_control_state(self): def broadcast_control_state(self):
if self.current_control_mode == self.GUI_MODE: if self.current_control_mode == self.MINING_MODE:
self.spacenav_state_update__signal.emit(self.spnav_states) self.send_mining_commands()
# self.spacenav_state_update__signal.emit(self.spnav_states)
elif self.current_control_mode == self.ARM_MODE: elif self.current_control_mode == self.ARM_MODE:
pass 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): def connect_signals_and_slots(self):
pass 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"> <property name="currentIndex">
<number>0</number> <number>0</number>
</property> </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"> <widget class="QWidget" name="tab">
<attribute name="title"> <attribute name="title">
<string>SSH Console</string> <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.StatusSystems.UbiquitiStatusCore as UbiquitiStatusCore
import Framework.SettingsSystems.UbiquitiRadioSettings as UbiquitiRadioSettings import Framework.SettingsSystems.UbiquitiRadioSettings as UbiquitiRadioSettings
import Framework.InputSystems.SpaceNavControlSender as SpaceNavControlSender import Framework.InputSystems.SpaceNavControlSender as SpaceNavControlSender
import Framework.MiscSystems.MiningCore as MiningCore
##################################### #####################################
# Global Variables # Global Variables
@@ -101,6 +102,7 @@ class GroundStation(QtCore.QObject):
rospy.init_node("ground_station") rospy.init_node("ground_station")
# ##### Instantiate Regular Classes ###### # ##### Instantiate Regular Classes ######
self.__add_non_thread("Mining System", MiningCore.Mining(self.shared_objects))
# ##### Instantiate Threaded Classes ###### # ##### Instantiate Threaded Classes ######
self.__add_thread("Video Coordinator", RoverVideoCoordinator.RoverVideoCoordinator(self.shared_objects)) 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, instance.setup_signals(self.start_threads_signal, self.connect_signals_and_slots_signal,
self.kill_threads_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): 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"]["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) 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 DriveStatusMessage.msg
IrisStatusMessage.msg IrisStatusMessage.msg
TowerPanTiltControlMessage.msg TowerPanTiltControlMessage.msg
MiningControlMessage.msg
MiningStatusMessage.msg
) )
## Generate services in the 'srv' folder ## 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) delay_before_tx=TX_DELAY)
def run(self): def run(self):
self.send_startup_centering_command() # self.send_startup_centering_command()
while not rospy.is_shutdown(): while not rospy.is_shutdown():
start_time = time() 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): def send_startup_centering_and_lights_off_command(self):
try: try:
registers = list(PAN_TILT_CONTROL_DEFAULT_MESSAGE) # registers = list(PAN_TILT_CONTROL_DEFAULT_MESSAGE)
registers[PAN_TILT_MODBUS_REGISTERS["CENTER_ALL"]] = 1 # registers[PAN_TILT_MODBUS_REGISTERS["CENTER_ALL"]] = 1
self.pan_tilt_node.write_registers(0, registers) # self.pan_tilt_node.write_registers(0, registers)
self.tower_node.write_register(0, TOWER_LIGHT_STATES["LIGHT_OFF"]) self.tower_node.write_register(0, TOWER_LIGHT_STATES["LIGHT_OFF"])
except: except:

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

@@ -8,7 +8,8 @@
<rosparam param="topics"> <rosparam param="topics">
[{name: "/rover_control/command_control/ground_station_drive", compress: true, rate: 15.0}, [{name: "/rover_control/command_control/ground_station_drive", compress: true, rate: 15.0},
{name: "/rover_control/tower/pan_tilt/control", compress: true, rate: 30.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> </rosparam>
</node> </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="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="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> </group>
</launch> </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_status/battery_status", compress: false, rate: 1.0},
{name: "/rover_control/tower/status/co2", 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_odometry/imu/data", compress: false, rate: 10.0},
{name: "/rover_control/mining/status", compress: false, rate: 2.0},
] ]
</rosparam> </rosparam>
</node> </node>

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

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