diff --git a/software/environment/rover/UDEV_rules/99-rover-usb-serial.rules b/software/environment/rover/UDEV_rules/99-rover-usb-serial.rules index 2cc27db..418b174 100644 --- a/software/environment/rover/UDEV_rules/99-rover-usb-serial.rules +++ b/software/environment/rover/UDEV_rules/99-rover-usb-serial.rules @@ -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" diff --git a/software/environment/rover/UDEV_rules/install_rules.sh b/software/environment/rover/UDEV_rules/install_rules.sh index a292b6c..2b6a9d7 100755 --- a/software/environment/rover/UDEV_rules/install_rules.sh +++ b/software/environment/rover/UDEV_rules/install_rules.sh @@ -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 diff --git a/software/firmware/mining/mining.ino b/software/firmware/mining/mining.ino index 13ea106..291e4a9 100644 --- a/software/firmware/mining/mining.ino +++ b/software/firmware/mining/mining.ino @@ -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 + +////////// 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); +} diff --git a/software/ros_packages/ground_station/src/Framework/InputSystems/SpaceNavControlSender.py b/software/ros_packages/ground_station/src/Framework/InputSystems/SpaceNavControlSender.py index 933ddc1..43fd862 100644 --- a/software/ros_packages/ground_station/src/Framework/InputSystems/SpaceNavControlSender.py +++ b/software/ros_packages/ground_station/src/Framework/InputSystems/SpaceNavControlSender.py @@ -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 diff --git a/software/ros_packages/ground_station/src/Framework/MiscSystems/MiningCore.py b/software/ros_packages/ground_station/src/Framework/MiscSystems/MiningCore.py new file mode 100644 index 0000000..87f8aec --- /dev/null +++ b/software/ros_packages/ground_station/src/Framework/MiscSystems/MiningCore.py @@ -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) diff --git a/software/ros_packages/ground_station/src/Resources/Ui/left_screen.ui b/software/ros_packages/ground_station/src/Resources/Ui/left_screen.ui index 6360c49..71188a0 100644 --- a/software/ros_packages/ground_station/src/Resources/Ui/left_screen.ui +++ b/software/ros_packages/ground_station/src/Resources/Ui/left_screen.ui @@ -1392,6 +1392,334 @@ N/A 0 + + + Mining + + + + + + + + + 12 + 75 + true + + + + Lift Position + + + + + + + 1023 + + + 512 + + + %v + + + + + + + + + + + + 12 + 75 + true + + + + Tilt Position + + + + + + + 1023 + + + 512 + + + %v + + + + + + + + + + 20 + 75 + true + + + + Scoop Measurement + + + Qt::AlignCenter + + + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + Tare + + + + + + + Measure + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + 65535 + + + 114 + + + + + + + Set Cal Factor + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + 0 + 0 + + + + + 300 + 150 + + + + + 300 + 150 + + + + + 9 + + + + QFrame::NoFrame + + + 0.000000000000000 + + + 0 + + + + + + + + 19 + 75 + true + + + + g + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + Qt::Horizontal + + + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + Transport + + + + + + + Measure + + + + + + + Scoop + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + SSH Console diff --git a/software/ros_packages/ground_station/src/ground_station.py b/software/ros_packages/ground_station/src/ground_station.py index 3fe78f7..7be7e2d 100644 --- a/software/ros_packages/ground_station/src/ground_station.py +++ b/software/ros_packages/ground_station/src/ground_station.py @@ -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) diff --git a/software/ros_packages/rover_control/CMakeLists.txt b/software/ros_packages/rover_control/CMakeLists.txt index 2c9d483..d9dde07 100644 --- a/software/ros_packages/rover_control/CMakeLists.txt +++ b/software/ros_packages/rover_control/CMakeLists.txt @@ -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 diff --git a/software/ros_packages/rover_control/msg/MiningControlMessage.msg b/software/ros_packages/rover_control/msg/MiningControlMessage.msg new file mode 100644 index 0000000..3404fe2 --- /dev/null +++ b/software/ros_packages/rover_control/msg/MiningControlMessage.msg @@ -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 \ No newline at end of file diff --git a/software/ros_packages/rover_control/msg/MiningStatusMessage.msg b/software/ros_packages/rover_control/msg/MiningStatusMessage.msg new file mode 100644 index 0000000..56414fe --- /dev/null +++ b/software/ros_packages/rover_control/msg/MiningStatusMessage.msg @@ -0,0 +1,4 @@ +uint16 lift_position +uint16 tilt_position + +uint16 measured_weight \ No newline at end of file diff --git a/software/ros_packages/rover_control/src/chassis_pan_tilt_control/chassis_pan_tilt_control.py b/software/ros_packages/rover_control/src/chassis_pan_tilt_control/chassis_pan_tilt_control.py index 3d39a7c..80d5541 100755 --- a/software/ros_packages/rover_control/src/chassis_pan_tilt_control/chassis_pan_tilt_control.py +++ b/software/ros_packages/rover_control/src/chassis_pan_tilt_control/chassis_pan_tilt_control.py @@ -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() diff --git a/software/ros_packages/rover_control/src/effectors_control/effectors_control.py b/software/ros_packages/rover_control/src/effectors_control/effectors_control.py new file mode 100644 index 0000000..65b05a5 --- /dev/null +++ b/software/ros_packages/rover_control/src/effectors_control/effectors_control.py @@ -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() diff --git a/software/ros_packages/rover_control/src/effectors_control/effectors_tester.py b/software/ros_packages/rover_control/src/effectors_control/effectors_tester.py new file mode 100755 index 0000000..55dad53 --- /dev/null +++ b/software/ros_packages/rover_control/src/effectors_control/effectors_tester.py @@ -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) diff --git a/software/ros_packages/rover_control/src/tower_pan_tilt_control/tower_and_pan_tilt_control.py b/software/ros_packages/rover_control/src/tower_pan_tilt_control/tower_and_pan_tilt_control.py index 112083d..b8e5499 100755 --- a/software/ros_packages/rover_control/src/tower_pan_tilt_control/tower_and_pan_tilt_control.py +++ b/software/ros_packages/rover_control/src/tower_pan_tilt_control/tower_and_pan_tilt_control.py @@ -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: diff --git a/software/ros_packages/rover_main/launch/ground_station/topic_transport_senders.launch b/software/ros_packages/rover_main/launch/ground_station/topic_transport_senders.launch index 57369c3..18d4aad 100644 --- a/software/ros_packages/rover_main/launch/ground_station/topic_transport_senders.launch +++ b/software/ros_packages/rover_main/launch/ground_station/topic_transport_senders.launch @@ -8,7 +8,8 @@ [{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}] diff --git a/software/ros_packages/rover_main/launch/rover/control.launch b/software/ros_packages/rover_main/launch/rover/control.launch index 74761a2..b425982 100644 --- a/software/ros_packages/rover_main/launch/rover/control.launch +++ b/software/ros_packages/rover_main/launch/rover/control.launch @@ -34,5 +34,7 @@ + + diff --git a/software/ros_packages/rover_main/launch/rover/test_control.launch b/software/ros_packages/rover_main/launch/rover/test_control.launch new file mode 100644 index 0000000..db676d7 --- /dev/null +++ b/software/ros_packages/rover_main/launch/rover/test_control.launch @@ -0,0 +1,6 @@ + + + + + + diff --git a/software/ros_packages/rover_main/launch/rover/topic_transport_senders.launch b/software/ros_packages/rover_main/launch/rover/topic_transport_senders.launch index bba4c13..88e9805 100644 --- a/software/ros_packages/rover_main/launch/rover/topic_transport_senders.launch +++ b/software/ros_packages/rover_main/launch/rover/topic_transport_senders.launch @@ -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}, ] diff --git a/software/testing/modbus/mining/mining_tester.py b/software/testing/modbus/mining/mining_tester.py index 56f40ee..5149258 100644 --- a/software/testing/modbus/mining/mining_tester.py +++ b/software/testing/modbus/mining/mining_tester.py @@ -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) diff --git a/software/testing/udp_debug.py b/software/testing/udp_debug.py new file mode 100644 index 0000000..06ee85b --- /dev/null +++ b/software/testing/udp_debug.py @@ -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 \ No newline at end of file