diff --git a/electrical/documents/ioni/base.drc b/electrical/documents/ioni/base.drc index b800893..24e6065 100644 --- a/electrical/documents/ioni/base.drc +++ b/electrical/documents/ioni/base.drc @@ -9,7 +9,7 @@ GranityVersionInt=11400 1\scaling=1 1\offset=0 1\readonly=true -1\value=10702 +1\value=10707 1\min=2 1\max=1 2\name=HWTYPE @@ -25,7 +25,7 @@ GranityVersionInt=11400 3\scaling=1 3\offset=0 3\readonly=true -3\value=110028853 +3\value=110028573 3\min=2 3\max=1 4\name=BUILDREVISION @@ -33,7 +33,7 @@ GranityVersionInt=11400 4\scaling=1 4\offset=0 4\readonly=true -4\value=163070257 +4\value=157269493 4\min=2 4\max=1 5\name=CEI @@ -49,7 +49,7 @@ GranityVersionInt=11400 6\scaling=1 6\offset=0 6\readonly=true -6\value=-314970192 +6\value=526566403 6\min=2 6\max=1 7\name=SMO @@ -153,7 +153,7 @@ GranityVersionInt=11400 19\scaling=1 19\offset=0 19\readonly=false -19\value=0 +19\value=-19123927 19\min=-536870912 19\max=536870911 20\name=FBR @@ -291,7 +291,7 @@ GranityVersionInt=11400 36\readonly=false 36\value=3 36\min=0 -36\max=5.99999999999999 +36\max=6 37\name=FOV 37\addr=569 37\scaling=100 @@ -313,7 +313,7 @@ GranityVersionInt=11400 39\scaling=1 39\offset=0 39\readonly=false -39\value=100000 +39\value=175000 39\min=0 39\max=1000000 40\name=FVT @@ -377,7 +377,7 @@ GranityVersionInt=11400 47\scaling=1 47\offset=0 47\readonly=false -47\value=7 +47\value=39 47\min=0 47\max=32767 48\name=TCH @@ -385,7 +385,7 @@ GranityVersionInt=11400 48\scaling=1 48\offset=0 48\readonly=false -48\value=96 +48\value=128 48\min=0 48\max=-1 49\name=TTR @@ -393,7 +393,7 @@ GranityVersionInt=11400 49\scaling=1 49\offset=0 49\readonly=false -49\value=4 +49\value=5 49\min=0 49\max=536870911 50\name=TBT @@ -521,7 +521,7 @@ GranityVersionInt=11400 65\scaling=1 65\offset=0 65\readonly=false -65\value=0 +65\value=416 65\min=0 65\max=-1 66\name=HMV @@ -537,7 +537,7 @@ GranityVersionInt=11400 67\scaling=1 67\offset=0 67\readonly=false -67\value=10 +67\value=1 67\min=1 67\max=32767 68\name=HMH @@ -561,7 +561,7 @@ GranityVersionInt=11400 70\scaling=1 70\offset=0 70\readonly=false -70\value=0 +70\value=2861472 70\min=-536870912 70\max=536870911 71\name=HLL @@ -569,7 +569,7 @@ GranityVersionInt=11400 71\scaling=1 71\offset=0 71\readonly=false -71\value=0 +71\value=-2861472 71\min=-536870912 71\max=536870911 72\name=HMF @@ -673,7 +673,7 @@ GranityVersionInt=11400 84\scaling=1 84\offset=0 84\readonly=true -84\value=4194047 +84\value=62914303 84\min=0 84\max=0 85\name=CAPS2 diff --git a/electrical/documents/ioni/elbow.drc b/electrical/documents/ioni/elbow.drc index 3ddcd92..ff5b8cf 100644 --- a/electrical/documents/ioni/elbow.drc +++ b/electrical/documents/ioni/elbow.drc @@ -9,7 +9,7 @@ GranityVersionInt=11400 1\scaling=1 1\offset=0 1\readonly=true -1\value=10702 +1\value=10707 1\min=2 1\max=1 2\name=HWTYPE @@ -25,7 +25,7 @@ GranityVersionInt=11400 3\scaling=1 3\offset=0 3\readonly=true -3\value=110028853 +3\value=110028686 3\min=2 3\max=1 4\name=BUILDREVISION @@ -33,7 +33,7 @@ GranityVersionInt=11400 4\scaling=1 4\offset=0 4\readonly=true -4\value=163070257 +4\value=157269493 4\min=2 4\max=1 5\name=CEI @@ -49,7 +49,7 @@ GranityVersionInt=11400 6\scaling=1 6\offset=0 6\readonly=true -6\value=-314970192 +6\value=-29713247 6\min=2 6\max=1 7\name=SMO @@ -153,7 +153,7 @@ GranityVersionInt=11400 19\scaling=1 19\offset=0 19\readonly=false -19\value=0 +19\value=3222680 19\min=-536870912 19\max=536870911 20\name=FBR @@ -193,7 +193,7 @@ GranityVersionInt=11400 24\scaling=1 24\offset=0 24\readonly=false -24\value=1000 +24\value=600 24\min=0 24\max=300000 25\name=KVI @@ -201,7 +201,7 @@ GranityVersionInt=11400 25\scaling=1 25\offset=0 25\readonly=false -25\value=150 +25\value=100 25\min=0 25\max=300000 26\name=KPP @@ -209,7 +209,7 @@ GranityVersionInt=11400 26\scaling=1 26\offset=0 26\readonly=false -26\value=50 +26\value=40 26\min=0 26\max=300000 27\name=VFF @@ -291,7 +291,7 @@ GranityVersionInt=11400 36\readonly=false 36\value=3 36\min=0 -36\max=5.99999999999999 +36\max=6 37\name=FOV 37\addr=569 37\scaling=100 @@ -385,7 +385,7 @@ GranityVersionInt=11400 48\scaling=1 48\offset=0 48\readonly=false -48\value=96 +48\value=128 48\min=0 48\max=-1 49\name=TTR @@ -393,7 +393,7 @@ GranityVersionInt=11400 49\scaling=1 49\offset=0 49\readonly=false -49\value=4 +49\value=2 49\min=0 49\max=536870911 50\name=TBT @@ -401,7 +401,7 @@ GranityVersionInt=11400 50\scaling=1 50\offset=0 50\readonly=false -50\value=10 +50\value=90 50\min=-1000000 50\max=100 51\name=CRI @@ -457,7 +457,7 @@ GranityVersionInt=11400 57\scaling=1 57\offset=0 57\readonly=false -57\value=1630 +57\value=1200 57\min=1 57\max=32767 58\name=CRV @@ -521,7 +521,7 @@ GranityVersionInt=11400 65\scaling=1 65\offset=0 65\readonly=false -65\value=0 +65\value=416 65\min=0 65\max=-1 66\name=HMV @@ -561,7 +561,7 @@ GranityVersionInt=11400 70\scaling=1 70\offset=0 70\readonly=false -70\value=0 +70\value=2457601 70\min=-536870912 70\max=536870911 71\name=HLL @@ -569,7 +569,7 @@ GranityVersionInt=11400 71\scaling=1 71\offset=0 71\readonly=false -71\value=0 +71\value=-2457601 71\min=-536870912 71\max=536870911 72\name=HMF @@ -673,7 +673,7 @@ GranityVersionInt=11400 84\scaling=1 84\offset=0 84\readonly=true -84\value=4194047 +84\value=62914303 84\min=0 84\max=0 85\name=CAPS2 diff --git a/electrical/documents/ioni/roll.drc b/electrical/documents/ioni/roll.drc index 5a5427b..3cb01e3 100644 --- a/electrical/documents/ioni/roll.drc +++ b/electrical/documents/ioni/roll.drc @@ -1,7 +1,7 @@ [Header] DRCVersion=110 -GranityVersion=1.14.1 -GranityVersionInt=11401 +GranityVersion=1.14.0 +GranityVersionInt=11400 [GDT3Params] 1\name=GCFWVER @@ -153,7 +153,7 @@ GranityVersionInt=11401 19\scaling=1 19\offset=0 19\readonly=false -19\value=0 +19\value=769700 19\min=-536870912 19\max=536870911 20\name=FBR @@ -193,7 +193,7 @@ GranityVersionInt=11401 24\scaling=1 24\offset=0 24\readonly=false -24\value=1100 +24\value=500 24\min=0 24\max=300000 25\name=KVI @@ -201,7 +201,7 @@ GranityVersionInt=11401 25\scaling=1 25\offset=0 25\readonly=false -25\value=800 +25\value=50 25\min=0 25\max=300000 26\name=KPP @@ -209,7 +209,7 @@ GranityVersionInt=11401 26\scaling=1 26\offset=0 26\readonly=false -26\value=1600 +26\value=10 26\min=0 26\max=300000 27\name=VFF @@ -473,7 +473,7 @@ GranityVersionInt=11401 59\scaling=1000 59\offset=0 59\readonly=false -59\value=3.855 +59\value=4.086 59\min=0.001 59\max=200 60\name=ML @@ -481,7 +481,7 @@ GranityVersionInt=11401 60\scaling=1000 60\offset=0 60\readonly=false -60\value=2.079 +60\value=1.825 60\min=0.001 60\max=200 61\name=MTC @@ -521,7 +521,7 @@ GranityVersionInt=11401 65\scaling=1 65\offset=0 65\readonly=false -65\value=0 +65\value=416 65\min=0 65\max=-1 66\name=HMV @@ -561,7 +561,7 @@ GranityVersionInt=11401 70\scaling=1 70\offset=0 70\readonly=false -70\value=0 +70\value=408781 70\min=-536870912 70\max=536870911 71\name=HLL @@ -569,7 +569,7 @@ GranityVersionInt=11401 71\scaling=1 71\offset=0 71\readonly=false -71\value=0 +71\value=-408781 71\min=-536870912 71\max=536870911 72\name=HMF diff --git a/electrical/documents/ioni/shoulder.drc b/electrical/documents/ioni/shoulder.drc index 24fd809..199d539 100644 --- a/electrical/documents/ioni/shoulder.drc +++ b/electrical/documents/ioni/shoulder.drc @@ -1,7 +1,7 @@ [Header] DRCVersion=110 -GranityVersion=1.14.1 -GranityVersionInt=11401 +GranityVersion=1.14.0 +GranityVersionInt=11400 [GDT3Params] 1\name=GCFWVER @@ -153,7 +153,7 @@ GranityVersionInt=11401 19\scaling=1 19\offset=0 19\readonly=false -19\value=0 +19\value=28407 19\min=-536870912 19\max=536870911 20\name=FBR @@ -273,7 +273,7 @@ GranityVersionInt=11401 34\scaling=1000 34\offset=0 34\readonly=false -34\value=5 +34\value=10 34\min=0.001 34\max=23.478 35\name=MCC @@ -385,7 +385,7 @@ GranityVersionInt=11401 48\scaling=1 48\offset=0 48\readonly=false -48\value=96 +48\value=224 48\min=0 48\max=-1 49\name=TTR @@ -393,7 +393,7 @@ GranityVersionInt=11401 49\scaling=1 49\offset=0 49\readonly=false -49\value=4 +49\value=2 49\min=0 49\max=536870911 50\name=TBT @@ -401,7 +401,7 @@ GranityVersionInt=11401 50\scaling=1 50\offset=0 50\readonly=false -50\value=10 +50\value=90 50\min=-1000000 50\max=100 51\name=CRI @@ -457,7 +457,7 @@ GranityVersionInt=11401 57\scaling=1 57\offset=0 57\readonly=false -57\value=870 +57\value=700 57\min=1 57\max=32767 58\name=CRV @@ -521,7 +521,7 @@ GranityVersionInt=11401 65\scaling=1 65\offset=0 65\readonly=false -65\value=0 +65\value=416 65\min=0 65\max=-1 66\name=HMV @@ -537,7 +537,7 @@ GranityVersionInt=11401 67\scaling=1 67\offset=0 67\readonly=false -67\value=10 +67\value=1 67\min=1 67\max=32767 68\name=HMH @@ -561,7 +561,7 @@ GranityVersionInt=11401 70\scaling=1 70\offset=0 70\readonly=false -70\value=0 +70\value=654050 70\min=-536870912 70\max=536870911 71\name=HLL @@ -569,7 +569,7 @@ GranityVersionInt=11401 71\scaling=1 71\offset=0 71\readonly=false -71\value=0 +71\value=-654050 71\min=-536870912 71\max=536870911 72\name=HMF diff --git a/electrical/documents/ioni/wrist_pitch.drc b/electrical/documents/ioni/wrist_pitch.drc index 9da3448..ca27267 100644 --- a/electrical/documents/ioni/wrist_pitch.drc +++ b/electrical/documents/ioni/wrist_pitch.drc @@ -9,7 +9,7 @@ GranityVersionInt=11400 1\scaling=1 1\offset=0 1\readonly=true -1\value=10702 +1\value=10707 1\min=2 1\max=1 2\name=HWTYPE @@ -25,7 +25,7 @@ GranityVersionInt=11400 3\scaling=1 3\offset=0 3\readonly=true -3\value=110028853 +3\value=110028746 3\min=2 3\max=1 4\name=BUILDREVISION @@ -33,7 +33,7 @@ GranityVersionInt=11400 4\scaling=1 4\offset=0 4\readonly=true -4\value=163070257 +4\value=157269493 4\min=2 4\max=1 5\name=CEI @@ -49,7 +49,7 @@ GranityVersionInt=11400 6\scaling=1 6\offset=0 6\readonly=true -6\value=-314970192 +6\value=-390223875 6\min=2 6\max=1 7\name=SMO @@ -153,7 +153,7 @@ GranityVersionInt=11400 19\scaling=1 19\offset=0 19\readonly=false -19\value=0 +19\value=-17238226 19\min=-536870912 19\max=536870911 20\name=FBR @@ -291,7 +291,7 @@ GranityVersionInt=11400 36\readonly=false 36\value=3 36\min=0 -36\max=5.99999999999999 +36\max=6 37\name=FOV 37\addr=569 37\scaling=100 @@ -521,7 +521,7 @@ GranityVersionInt=11400 65\scaling=1 65\offset=0 65\readonly=false -65\value=0 +65\value=416 65\min=0 65\max=-1 66\name=HMV @@ -561,7 +561,7 @@ GranityVersionInt=11400 70\scaling=1 70\offset=0 70\readonly=false -70\value=0 +70\value=948448 70\min=-536870912 70\max=536870911 71\name=HLL @@ -569,7 +569,7 @@ GranityVersionInt=11400 71\scaling=1 71\offset=0 71\readonly=false -71\value=0 +71\value=-948448 71\min=-536870912 71\max=536870911 72\name=HMF @@ -673,7 +673,7 @@ GranityVersionInt=11400 84\scaling=1 84\offset=0 84\readonly=true -84\value=4194047 +84\value=62914303 84\min=0 84\max=0 85\name=CAPS2 diff --git a/electrical/documents/ioni/wrist_roll.drc b/electrical/documents/ioni/wrist_roll.drc index 9501335..62215e4 100644 --- a/electrical/documents/ioni/wrist_roll.drc +++ b/electrical/documents/ioni/wrist_roll.drc @@ -9,7 +9,7 @@ GranityVersionInt=11400 1\scaling=1 1\offset=0 1\readonly=true -1\value=10702 +1\value=10707 1\min=2 1\max=1 2\name=HWTYPE @@ -25,7 +25,7 @@ GranityVersionInt=11400 3\scaling=1 3\offset=0 3\readonly=true -3\value=110028853 +3\value=110028918 3\min=2 3\max=1 4\name=BUILDREVISION @@ -33,7 +33,7 @@ GranityVersionInt=11400 4\scaling=1 4\offset=0 4\readonly=true -4\value=163070257 +4\value=157269493 4\min=2 4\max=1 5\name=CEI @@ -49,7 +49,7 @@ GranityVersionInt=11400 6\scaling=1 6\offset=0 6\readonly=true -6\value=-314970192 +6\value=-69557674 6\min=2 6\max=1 7\name=SMO @@ -153,7 +153,7 @@ GranityVersionInt=11400 19\scaling=1 19\offset=0 19\readonly=false -19\value=0 +19\value=-21753107 19\min=-536870912 19\max=536870911 20\name=FBR @@ -291,7 +291,7 @@ GranityVersionInt=11400 36\readonly=false 36\value=3 36\min=0 -36\max=5.99999999999999 +36\max=6 37\name=FOV 37\addr=569 37\scaling=100 @@ -521,7 +521,7 @@ GranityVersionInt=11400 65\scaling=1 65\offset=0 65\readonly=false -65\value=0 +65\value=416 65\min=0 65\max=-1 66\name=HMV @@ -673,7 +673,7 @@ GranityVersionInt=11400 84\scaling=1 84\offset=0 84\readonly=true -84\value=4194047 +84\value=62914303 84\min=0 84\max=0 85\name=CAPS2 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..901ff80 100644 --- a/software/environment/rover/UDEV_rules/99-rover-usb-serial.rules +++ b/software/environment/rover/UDEV_rules/99-rover-usb-serial.rules @@ -7,11 +7,13 @@ ######################### +##### NOTE!!!! The IONIs cannot be on IRIS. The throughput is too high for it to handle with only a 12Mbit usb hub onboard ##### + # IRIS Board Mappings 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" @@ -23,6 +25,8 @@ SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{seria SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM34WE0R", ENV{ID_USB_INTERFACE_NUM}=="02", SYMLINK+="rover/ttyIRIS" SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM34WE0R", ENV{ID_USB_INTERFACE_NUM}=="03", SYMLINK+="rover/ttyExtraUART" +# External 485 adapter for the ARM +SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6001", ATTRS{serial}=="AH05K2Y8", SYMLINK+="rover/ttyARM" ##### Mappings from OMSI / EXPO driving with individual adapters # SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6001", ATTRS{serial}=="AH05K2Y8", SYMLINK+="rover/ttyLEFT" 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 987e664..291e4a9 100644 --- a/software/firmware/mining/mining.ino +++ b/software/firmware/mining/mining.ino @@ -1,302 +1,335 @@ -////////// 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 = 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 current_position_lift = 0; -int current_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 = 2; -uint16_t modbus_data[] = {1023, 350, 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 ////////// -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 - - setup_hardware(); - num_modbus_registers = sizeof(modbus_data) / sizeof(modbus_data[0]); - slave.begin(115200); // baud-rate at 19200 - slave.setTimeOut(500); -} - -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(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() { - 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); - - 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(){ - scale.set_scale(modbus_data[MODBUS_REGISTERS::CALIBRATION_FACTOR]*-1000); - - if (modbus_data[MODBUS_REGISTERS::TARE] == 1){ - scale.tare(); - modbus_data[MODBUS_REGISTERS::TARE] = 0; - } -} - -void poll_scale(){ - modbus_data[MODBUS_REGISTERS::MEASURED_WEIGHT] = scale.get_units()*-1000; -} - -//---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/firmware/rdf/rdf.ino b/software/firmware/rdf/rdf.ino new file mode 100644 index 0000000..2168922 --- /dev/null +++ b/software/firmware/rdf/rdf.ino @@ -0,0 +1,88 @@ +////////// Includes ////////// +#include + +////////// Hardware / Data Enumerations ////////// +enum HARDWARE { + COMMS_RS485_EN = 3, + COMMS_RS485_RX = 9, + COMMS_RS485_TX = 10, + + // COMMS_RS485_EN = 2, + // COMMS_RS485_RX = 0, + // COMMS_RS485_TX = 1, + + RDF_INPUT = A7, + + LED_BLUE_EXTRA = 13 +}; + +enum MODBUS_REGISTERS { + RAW_DATA = 0 +}; + + +////////// Global Variables ////////// +///// Modbus +const uint8_t node_id = 1; +const uint8_t modbus_serial_port_number = 2; + +uint16_t modbus_data[] = {0}; +uint8_t num_modbus_registers = 0; + +int8_t poll_state = 0; +bool communication_good = false; +uint8_t message_count = 0; + +////////// Class Instantiations ////////// +Modbus slave(node_id, modbus_serial_port_number, HARDWARE::COMMS_RS485_EN); + +void setup() { + // Debugging + // Serial.begin(9600); + // while (!Serial); + + // Raw pin setup + setup_hardware(); + + // Setup modbus serial communication + num_modbus_registers = sizeof(modbus_data) / sizeof(modbus_data[0]); + slave.begin(115200); // baud-rate at 19200 + slave.setTimeOut(100); + +} + +void loop() { + // Do normal polling + poll_modbus(); + set_leds(); + +} + + +void setup_hardware() { + // Setup pins as inputs / outputs + pinMode(HARDWARE::RDF_INPUT, INPUT); + pinMode(HARDWARE::LED_BLUE_EXTRA, OUTPUT); + + analogReadResolution(13); + +} + +void poll_modbus() { + poll_state = slave.poll(modbus_data, num_modbus_registers); + communication_good = !slave.getTimeOutState(); + + modbus_data[MODBUS_REGISTERS::RAW_DATA] = analogRead(HARDWARE::RDF_INPUT); +} + +void set_leds() { + if (poll_state > 4) { + message_count++; + if (message_count > 2) { + digitalWrite(HARDWARE::LED_BLUE_EXTRA, !digitalRead(HARDWARE::LED_BLUE_EXTRA)); + message_count = 0; + } + } else if (!communication_good) { + digitalWrite(HARDWARE::LED_BLUE_EXTRA, LOW); + } +} diff --git a/software/firmware/temp/new_rdf.ino b/software/firmware/temp/new_rdf.ino new file mode 100644 index 0000000..ee8627f --- /dev/null +++ b/software/firmware/temp/new_rdf.ino @@ -0,0 +1,172 @@ +////////// Includes ////////// +#include + +////////// Hardware / Data Enumerations ////////// +enum HARDWARE { + COMMS_RS485_EN = 3, + COMMS_RS485_RX = 9, + COMMS_RS485_TX = 10, + + // COMMS_RS485_EN = 2, + // COMMS_RS485_RX = 0, + // COMMS_RS485_TX = 1, + + RDF_INPUT = A7, + + LED_BLUE_EXTRA = 13 +}; + +enum MODBUS_REGISTERS { + SENSITIVITY = 0, + RAW_DATA = 1, // Input + CLEAN_DATA_POSITIVE = 2, + CLEAN_DATA_NEGATIVE = 3, + FREQUENCY = 4, +}; + + +////////// Global Variables ////////// +///// Modbus +const uint8_t node_id = 1; +const uint8_t modbus_serial_port_number = 2; + +uint16_t modbus_data[] = {50, 0, 0, 0, 0}; +uint8_t num_modbus_registers = 0; + +int8_t poll_state = 0; +bool communication_good = false; +uint8_t message_count = 0; + + +//////////////// Anothony's stuff ///////////// +int state; +float freq; +float ambientNoise; +unsigned long totalDataPoints; +int dataBuff[3]; +int data[3]; +unsigned long t1,t2,t3; +int dt1,dt2 =0; +float dtavg; +int tcnt =2; +bool upstate = false; + + +////////// Class Instantiations ////////// +Modbus slave(node_id, modbus_serial_port_number, HARDWARE::COMMS_RS485_EN); + +void setup() { + // Debugging + Serial.begin(9600); + while (!Serial); + + // Raw pin setup + setup_hardware(); + + // Setup modbus serial communication + num_modbus_registers = sizeof(modbus_data) / sizeof(modbus_data[0]); + slave.begin(115200); // baud-rate at 19200 + slave.setTimeOut(150); +} + +void loop() { + anthonys_rdf_code(); + // Do normal polling + poll_modbus(); + set_leds(); + +} + +void anthonys_rdf_code(){ + modbus_data[MODBUS_REGISTERS::RAW_DATA] = analogRead(HARDWARE::RDF_INPUT); + + for(int i=0;i<3;i++){ + dataBuff[i] = data[i]; + } + dataSet(); + int change = changeCheck(); + if(change){ + state = change; + if(change == 1){ + t1 = millis(); + dt1 = t1-t2; + }else{ + t2 = millis(); + dt2 = t2-t1; + } + if(dt2>dt1-100&&dt1>dt2-100){ + dtavg = (dtavg*float(tcnt-2)/float(tcnt))+(((dt1+dt2)/2.0)*float(2.0/tcnt)); + tcnt += 2; + freq = 500.0/dtavg; + } + } + if(change ==1 || millis() > dtavg*2+t3){ + t3 = millis(); + upstate = true; + } + if(millis()=0){ + modbus_data[MODBUS_REGISTERS::CLEAN_DATA_NEGATIVE] = 0; + modbus_data[MODBUS_REGISTERS::CLEAN_DATA_POSITIVE] = out_data; + }else{ + modbus_data[MODBUS_REGISTERS::CLEAN_DATA_POSITIVE] = 0; + modbus_data[MODBUS_REGISTERS::CLEAN_DATA_NEGATIVE] = out_data; + } + modbus_data[MODBUS_REGISTERS::FREQUENCY] = freq * 100; + // Serial.print(data[i]-ambientNoise); + // Serial.print(", "); + Serial.println(freq); + } + }else{ + float avgdat,dtot =0; + for(int i=0;i<3;i++) + dtot+=data[i]; + avgdat = dtot/3.0; + ambientNoise = ambientNoise*float(totalDataPoints)/float(totalDataPoints+3)+avgdat*(3.0/float(totalDataPoints+3)); + } +} + +void dataSet(){ + for(int i=0;i<3;i++){ + data[i]=analogRead(HARDWARE::RDF_INPUT); + delay(1); + } +} + +int changeCheck(){ + uint16_t sensitivity = modbus_data[MODBUS_REGISTERS::SENSITIVITY]; + int newSignalState = 0; // 0= no change 1= signal start 2= signal stop + if((data[0])>(dataBuff[2]+sensitivity)||data[2] > data[0]+sensitivity) + newSignalState = 1; + if((data[2] < data[0]-sensitivity)||(data[0]<(dataBuff[2]-sensitivity))) + newSignalState = 2; + return newSignalState; + +} + +void setup_hardware() { + // Setup pins as inputs / outputs + pinMode(HARDWARE::RDF_INPUT, INPUT); + pinMode(HARDWARE::LED_BLUE_EXTRA, OUTPUT); + +} + +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_BLUE_EXTRA, !digitalRead(HARDWARE::LED_BLUE_EXTRA)); + message_count = 0; + } + } else if (!communication_good) { + digitalWrite(HARDWARE::LED_BLUE_EXTRA, LOW); + } +} diff --git a/software/firmware/temp/original_rdf.ino b/software/firmware/temp/original_rdf.ino new file mode 100644 index 0000000..6c7be81 --- /dev/null +++ b/software/firmware/temp/original_rdf.ino @@ -0,0 +1,84 @@ +#define Rpin A5 //Reciever pin is A1 +int sensitivity = 50; //the lower this number, the more sensitive. +int state; //=1 if can hear ping =2 if cannot hear ping +float freq; +float ambientNoise; +unsigned long totalDataPoints; +int dataBuff[3]; +int data[3]; +unsigned long t1,t2,t3; +int dt1,dt2 =0; +float dtavg; +int tcnt =2; +bool upstate = false; +bool mode = false; //switches between two modes (t/f) +void setup() { + pinMode(Rpin, INPUT); + Serial.begin(9600); + totalDataPoints = 0; + state = 2; +} +void dataSet(){ + for(int i=0;i<3;i++){ + data[i]=analogRead(Rpin); + delay(1); + } +} +int changeCheck(){ + int newSignalState = 0; // 0= no change 1= signal start 2= signal stop + if((data[0])>(dataBuff[2]+sensitivity)||data[2] > data[0]+sensitivity) + newSignalState = 1; + if((data[2] < data[0]-sensitivity)||(data[0]<(dataBuff[2]-sensitivity))) + newSignalState = 2; + return newSignalState; + +} +void loop() { + + if(Serial.available()){ + Serial.read(); + mode = !mode; + } + if(mode == true){ //mode 1 is raw output from the antena. + Serial.println(analogRead(Rpin)); + delay(1); + } + else{ + for(int i=0;i<3;i++) + dataBuff[i] = data[i]; + dataSet(); + int change = changeCheck(); + if(change){ + state = change; + if(change == 1){ + t1 = millis(); + dt1 = t1-t2; + }else{ + t2 = millis(); + dt2 = t2-t1; + } + if(dt2>dt1-100&&dt1>dt2-100){ + dtavg = (dtavg*float(tcnt-2)/float(tcnt))+(((dt1+dt2)/2.0)*float(2.0/tcnt)); + tcnt += 2; + freq = 500.0/dtavg; + } + } + if(change ==1 || millis() > dtavg*2+t3){ + t3 = millis(); + upstate = true; + } + if(millis() PAUSE_STATE_CHANGE_TIME: self.drive_paused = not self.drive_paused self.show_changed_pause_state() @@ -223,7 +236,8 @@ class JoystickControlSender(QtCore.QThread): self.publish_pan_tilt_control_commands() def publish_drive_command(self): - throttle_axis = max((255 - self.joystick.controller_states["throttle_axis"]) / 255.0, THROTTLE_MIN) + throttle_axis = 1 + # throttle_axis = max((255 - self.joystick.controller_states["throttle_axis"]) / 255.0, THROTTLE_MIN) if self.drive_paused: drive_message = DriveCommandMessage() @@ -240,11 +254,11 @@ class JoystickControlSender(QtCore.QThread): self.drive_command_publisher.publish(drive_message) def publish_camera_control_commands(self): - trigger_pressed = self.joystick.controller_states["trigger_pressed"] - three_pressed = self.joystick.controller_states["three_pressed"] - four_pressed = self.joystick.controller_states["four_pressed"] - five_pressed = self.joystick.controller_states["five_pressed"] - six_pressed = self.joystick.controller_states["six_pressed"] + trigger_pressed = self.joystick.controller_states["y"] + three_pressed = self.joystick.controller_states["left_bumper"] + four_pressed = self.joystick.controller_states["right_bumper"] + five_pressed = self.joystick.controller_states["left_trigger"] + six_pressed = self.joystick.controller_states["right_trigger"] if (five_pressed or six_pressed) and (time() - self.last_camera_change_time) > CAMERA_CHANGE_TIME: change = -1 if five_pressed else 1 @@ -261,9 +275,9 @@ class JoystickControlSender(QtCore.QThread): self.last_camera_toggle_time = time() def publish_pan_tilt_control_commands(self): - button_eight = self.joystick.controller_states["seven_pressed"] - hat_x = self.joystick.controller_states["hat_x_axis"] - hat_y = self.joystick.controller_states["hat_y_axis"] + button_eight = self.joystick.controller_states["a"] + hat_x = self.joystick.controller_states["d_pad_x"] + hat_y = self.joystick.controller_states["d_pad_y"] if (hat_x == 0 and not self.last_hat_x_was_movement) and ( hat_y == 0 and not self.last_hat_y_was_movement) and not button_eight: @@ -290,20 +304,14 @@ class JoystickControlSender(QtCore.QThread): def get_drive_message(self, throttle_axis): drive_message = DriveCommandMessage() - y_axis = throttle_axis * (-(self.joystick.controller_states["y_axis"] - 512) / 512.0) - z_axis = throttle_axis * (-(self.joystick.controller_states["z_axis"] - 128) / 128.0) - x_axis = throttle_axis * (-(self.joystick.controller_states["x_axis"] - 512) / 512.0) + left_y_axis = self.joystick.controller_states["left_y_axis"] if abs(self.joystick.controller_states["left_y_axis"]) > STICK_DEADBAND else 0 + right_y_axis = self.joystick.controller_states["right_y_axis"] if abs(self.joystick.controller_states["right_y_axis"]) > STICK_DEADBAND else 0 - if abs(y_axis) < Y_AXIS_DEADBAND: - y_axis = 0 + left_y_axis = throttle_axis * (-(left_y_axis - STICK_OFFSET) / STICK_MAX) + right_y_axis = throttle_axis * (-(right_y_axis - STICK_OFFSET) / STICK_MAX) - if abs(x_axis) < X_AXIS_DEADBAND and y_axis == 0: - twist = z_axis - else: - twist = x_axis if y_axis >= 0 else -x_axis - - drive_message.drive_twist.linear.x = y_axis - drive_message.drive_twist.angular.z = twist + drive_message.drive_twist.linear.x = (left_y_axis + right_y_axis) / 2.0 + drive_message.drive_twist.angular.z = (right_y_axis - left_y_axis) / 2.0 return drive_message 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..099b818 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,9 +105,13 @@ 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): + self.logger.debug("Starting SpaceNav Mouse Thread") spnav.spnav_open() while self.run_thread_flag: @@ -110,12 +124,15 @@ 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)) + + self.logger.debug("Stopping SpaceNav Mouse Thread") def process_spnav_events(self): event = spnav.spnav_poll_event() if event: + # print event if event.ev_type == spnav.SPNAV_EVENT_MOTION: self.spnav_states["linear_x"] = event.translation[0] / 350.0 self.spnav_states["linear_y"] = event.translation[2] / 350.0 @@ -132,16 +149,20 @@ 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 + + # 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/InputSystems/XBOXControllerControlSender.py b/software/ros_packages/ground_station/src/Framework/InputSystems/XBOXControllerControlSender.py new file mode 100644 index 0000000..0412966 --- /dev/null +++ b/software/ros_packages/ground_station/src/Framework/InputSystems/XBOXControllerControlSender.py @@ -0,0 +1,309 @@ +##################################### +# Imports +##################################### +# Python native imports +from PyQt5 import QtCore, QtWidgets +import logging +from inputs import devices, GamePad +from time import time + +import rospy +from rover_arm.msg import ArmControlMessage +from rover_control.msg import MiningControlMessage + +##################################### +# Global Variables +##################################### +GAME_CONTROLLER_NAME = "Afterglow Gamepad for Xbox 360" + +DRIVE_COMMAND_HERTZ = 20 + +RELATIVE_ARM_CONTROL_TOPIC = "/rover_arm/control/relative" +MINING_CONTROL_TOPIC = "/rover_control/mining/control" + +BASE_SCALAR = 0.003 +SHOULDER_SCALAR = 0.002 +ELBOW_SCALAR = 0.002 +ROLL_SCALAR = 0.003 +WRIST_PITCH_SCALAR = 0.003 +WRIST_ROLL_SCALAR = 0.006 + +LEFT_X_AXIS_DEADZONE = 1500 +LEFT_Y_AXIS_DEADZONE = 1500 + +RIGHT_X_AXIS_DEADZONE = 1500 +RIGHT_Y_AXIS_DEADZONE = 1500 + +THUMB_STICK_MAX = 32768.0 + +MINING_LIFT_SCALAR = 5 +MINING_TILT_SCALAR = 5 + + +COLOR_GREEN = "background-color:darkgreen; border: 1px solid black;" +COLOR_NONE = "border: 1px solid black;" + + +##################################### +# Controller Class Definition +##################################### +class XBOXController(QtCore.QThread): + def __init__(self): + super(XBOXController, self).__init__() + + # ########## Thread Flags ########## + self.run_thread_flag = True + self.setup_controller_flag = True + self.data_acquisition_and_broadcast_flag = True + self.controller_acquired = False + + # ########## Class Variables ########## + self.gamepad = None # type: GamePad + + self.controller_states = { + "left_x_axis": 0, + "left_y_axis": 0, + "left_stick_button": 0, + + "right_x_axis": 0, + "right_y_axis": 0, + "right_stick_button": 0, + + "left_trigger": 0, + "left_bumper": 0, + + "right_trigger": 0, + "right_bumper": 0, + + "hat_x_axis": 0, + "hat_y_axis": 0, + + "back_button": 0, + "start_button": 0, + "xbox_button": 0, + + "x_button": 0, + "a_button": 0, + "b_button": 0, + "y_button": 0 + } + + self.raw_mapping_to_class_mapping = { + "ABS_X": "left_x_axis", + "ABS_Y": "left_y_axis", + "BTN_THUMBL": "left_stick_button", + + "ABS_RX": "right_x_axis", + "ABS_RY": "right_y_axis", + "BTN_THUMBR": "right_stick_button", + + "ABS_Z": "left_trigger", + "BTN_TL": "left_bumper", + + "ABS_RZ": "right_trigger", + "BTN_TR": "right_bumper", + + "ABS_HAT0X": "hat_x_axis", + "ABS_HAT0Y": "hat_y_axis", + + "BTN_SELECT": "back_button", + "BTN_START": "start_button", + "BTN_MODE": "xbox_button", + + "BTN_NORTH": "x_button", + "BTN_SOUTH": "a_button", + "BTN_EAST": "b_button", + "BTN_WEST": "y_button" + } + + self.ready = False + + self.start() + + def run(self): + + while self.run_thread_flag: + if self.setup_controller_flag: + self.controller_acquired = self.__setup_controller() + self.setup_controller_flag = False + if self.data_acquisition_and_broadcast_flag: + self.__get_controller_data() + + def __setup_controller(self): + for device in devices.gamepads: + # print device + if device.name == GAME_CONTROLLER_NAME: + self.gamepad = device + + return True + return False + + def __get_controller_data(self): + if self.controller_acquired: + events = self.gamepad.read() + + for event in events: + # For seeing codes you haven't added yet... + # if event.code not in self.raw_mapping_to_class_mapping and event.code != "SYN_REPORT": + # print event.code, ":", event.state + + if event.code in self.raw_mapping_to_class_mapping: + # print event.code, ":", event.state + self.controller_states[self.raw_mapping_to_class_mapping[event.code]] = event.state + + self.ready = True + + +##################################### +# Controller Class Definition +##################################### +class XBOXControllerControlSender(QtCore.QThread): + STATES = [ + "ARM", + "MINING" + ] + + xbox_control_arm_stylesheet_update_ready__signal = QtCore.pyqtSignal(str) + xbox_control_mining_stylesheet_update_ready__signal = QtCore.pyqtSignal(str) + + def __init__(self, shared_objects): + super(XBOXControllerControlSender, self).__init__() + + # ########## Reference to class init variables ########## + self.shared_objects = shared_objects + self.left_screen = self.shared_objects["screens"]["left_screen"] + self.xbox_mode_arm_label = self.left_screen.xbox_mode_arm_label # type: QtWidgets.QLabel + self.xbox_mode_mining_label = self.left_screen.xbox_mode_mining_label # type: QtWidgets.QLabel + + # ########## 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 + + self.controller = XBOXController() + + # ########## Class Variables ########## + + self.wait_time = 1.0 / DRIVE_COMMAND_HERTZ + + self.relative_arm_control_publisher = rospy.Publisher(RELATIVE_ARM_CONTROL_TOPIC, ArmControlMessage, + queue_size=1) + self.mining_control_publisher = rospy.Publisher(MINING_CONTROL_TOPIC, MiningControlMessage, queue_size=1) + + self.current_state = self.STATES.index("ARM") + self.state_just_changed = False + + self.last_xbox_button_state = 0 + + def run(self): + self.logger.debug("Starting Joystick Thread") + + while self.run_thread_flag: + start_time = time() + + self.change_control_state_if_needed() + + if self.current_state == self.STATES.index("ARM"): + self.process_and_send_arm_control() + elif self.current_state == self.STATES.index("MINING"): + self.send_mining_commands() + + time_diff = time() - start_time + + self.msleep(max(int(self.wait_time - time_diff), 0)) + + self.logger.debug("Stopping Joystick Thread") + + def connect_signals_and_slots(self): + self.xbox_control_arm_stylesheet_update_ready__signal.connect(self.xbox_mode_arm_label.setStyleSheet) + self.xbox_control_mining_stylesheet_update_ready__signal.connect(self.xbox_mode_mining_label.setStyleSheet) + + def change_control_state_if_needed(self): + if self.state_just_changed: + self.xbox_control_arm_stylesheet_update_ready__signal.emit(COLOR_NONE) + self.xbox_control_mining_stylesheet_update_ready__signal.emit(COLOR_GREEN) + self.state_just_changed = False + + xbox_state = self.controller.controller_states["xbox_button"] + + if self.last_xbox_button_state == 0 and xbox_state == 1: + self.current_state += 1 + self.current_state = self.current_state % len(self.STATES) + self.state_just_changed = True + self.last_xbox_button_state = 1 + + elif self.last_xbox_button_state == 1 and xbox_state == 0: + self.last_xbox_button_state = 0 + + def process_and_send_arm_control(self): + if self.state_just_changed: + self.xbox_control_arm_stylesheet_update_ready__signal.emit(COLOR_GREEN) + self.xbox_control_mining_stylesheet_update_ready__signal.emit(COLOR_NONE) + self.state_just_changed = False + + message = ArmControlMessage() + + should_publish = False + + left_trigger = self.controller.controller_states["left_trigger"] + right_trigger = self.controller.controller_states["right_trigger"] + + left_x_axis = self.controller.controller_states["left_x_axis"] if abs(self.controller.controller_states[ + "left_x_axis"]) > LEFT_X_AXIS_DEADZONE else 0 + left_y_axis = self.controller.controller_states["left_y_axis"] if abs(self.controller.controller_states[ + "left_y_axis"]) > LEFT_Y_AXIS_DEADZONE else 0 + right_y_axis = self.controller.controller_states["right_y_axis"] if abs(self.controller.controller_states[ + "right_y_axis"]) > RIGHT_Y_AXIS_DEADZONE else 0 + right_x_axis = self.controller.controller_states["right_x_axis"] if abs(self.controller.controller_states[ + "right_x_axis"]) > RIGHT_X_AXIS_DEADZONE else 0 + + # print left_x_axis, ":", left_y_axis, ":", right_x_axis, ":", right_y_axis + + left_trigger_ratio = left_trigger / 255.0 + right_trigger_ratio = right_trigger / 255.0 + + if left_trigger > 25: + should_publish = True + message.base = ((left_x_axis / THUMB_STICK_MAX) * BASE_SCALAR) * left_trigger_ratio + message.shoulder = ((left_y_axis / THUMB_STICK_MAX) * SHOULDER_SCALAR) * left_trigger_ratio + message.elbow = (-(right_y_axis / THUMB_STICK_MAX) * ELBOW_SCALAR) * left_trigger_ratio + message.roll = (-(right_x_axis / THUMB_STICK_MAX) * ROLL_SCALAR) * left_trigger_ratio + + elif right_trigger > 25: + should_publish = True + message.wrist_roll = ((right_x_axis / THUMB_STICK_MAX) * WRIST_ROLL_SCALAR) * right_trigger_ratio + message.wrist_pitch = (-(left_y_axis / THUMB_STICK_MAX) * WRIST_PITCH_SCALAR) * right_trigger_ratio + + if should_publish: + self.relative_arm_control_publisher.publish(message) + + def send_mining_commands(self): + + left_y_axis = self.controller.controller_states["left_y_axis"] if abs(self.controller.controller_states["left_y_axis"]) > LEFT_Y_AXIS_DEADZONE else 0 + right_y_axis = self.controller.controller_states["right_y_axis"] if abs(self.controller.controller_states[ + "right_y_axis"]) > RIGHT_Y_AXIS_DEADZONE else 0 + + if left_y_axis or right_y_axis: + + message = MiningControlMessage() + + message.lift_set_absolute = 1024 + message.tilt_set_absolute = 1024 + + message.lift_set_relative = (-(left_y_axis / THUMB_STICK_MAX) * MINING_LIFT_SCALAR) + message.tilt_set_relative = ((right_y_axis / THUMB_STICK_MAX) * MINING_TILT_SCALAR) + message.cal_factor = -1 + + self.mining_control_publisher.publish(message) + + def setup_signals(self, start_signal, signals_and_slots_signal, kill_signal): + start_signal.connect(self.start) + signals_and_slots_signal.connect(self.connect_signals_and_slots) + kill_signal.connect(self.on_kill_threads_requested__slot) + + def on_kill_threads_requested__slot(self): + self.run_thread_flag = False diff --git a/software/ros_packages/ground_station/src/Framework/MiscSystems/SSHConsoleCore.py b/software/ros_packages/ground_station/src/Framework/MiscSystems/BashConsoleCore.py similarity index 57% rename from software/ros_packages/ground_station/src/Framework/MiscSystems/SSHConsoleCore.py rename to software/ros_packages/ground_station/src/Framework/MiscSystems/BashConsoleCore.py index 952f715..45974f9 100644 --- a/software/ros_packages/ground_station/src/Framework/MiscSystems/SSHConsoleCore.py +++ b/software/ros_packages/ground_station/src/Framework/MiscSystems/BashConsoleCore.py @@ -6,29 +6,26 @@ from PyQt5 import QtCore, QtWidgets import logging from time import time -import paramiko + ##################################### # Global Variables ##################################### -ACCESS_POINT_IP = "192.168.1.20" # The channel only has to be set on the access point. The staion will adjust. -ACCESS_POINT_USER = "ubnt" -ACCESS_POINT_PASSWORD = "rover4lyfe^" # We don't care about this password, don't freak out. Wifi is open anyways... +THREAD_HERTZ = 5 ##################################### # UbiquitiRadioSettings Class Definition ##################################### -class SSHConsole(QtCore.QThread): +class BashConsole(QtCore.QThread): def __init__(self, shared_objects): - super(SSHConsole, self).__init__() + super(BashConsole, self).__init__() # ########## Reference to class init variables ########## self.shared_objects = shared_objects self.left_screen = self.shared_objects["screens"]["left_screen"] - self.ubiquiti_channel_spin_box = self.left_screen.ssh_console_widget # type: QtWidgets.QSpinBox - self.ubiquiti_channel_apply_button = self.left_screen.ubiquiti_channel_apply_button # type: QtWidgets.QPushButton + self.ssh_widget = self.left_screen.ssh_console_widget # type: QtWidgets.QSpinBox # ########## Get the settings instance ########## self.settings = QtCore.QSettings() @@ -40,8 +37,23 @@ class SSHConsole(QtCore.QThread): self.run_thread_flag = True # ########## Class Variables ########## + self.wait_time = 1.0 / THREAD_HERTZ - self.connect_signals_and_slots() + def run(self): + while self.run_thread_flag: + start_time = time() + + time_diff = time() - start_time + + self.msleep(max(int(self.wait_time - time_diff), 0)) def connect_signals_and_slots(self): - pass \ No newline at end of file + pass + + def setup_signals(self, start_signal, signals_and_slots_signal, kill_signal): + start_signal.connect(self.start) + signals_and_slots_signal.connect(self.connect_signals_and_slots) + kill_signal.connect(self.on_kill_threads_requested__slot) + + def on_kill_threads_requested__slot(self): + self.run_thread_flag = False 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..2967871 --- /dev/null +++ b/software/ros_packages/ground_station/src/Framework/MiscSystems/MiningCore.py @@ -0,0 +1,143 @@ +# 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) + + weight_measurement_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) + + self.weight_measurement_update_ready__signal.connect(self.mining_qlcdnumber.display) + + 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.weight_measurement_update_ready__signal.emit(status.measured_weight) diff --git a/software/ros_packages/ground_station/src/Framework/MiscSystems/MiscArmCore.py b/software/ros_packages/ground_station/src/Framework/MiscSystems/MiscArmCore.py new file mode 100644 index 0000000..2755e98 --- /dev/null +++ b/software/ros_packages/ground_station/src/Framework/MiscSystems/MiscArmCore.py @@ -0,0 +1,367 @@ +# coding=utf-8 +##################################### +# Imports +##################################### +# Python native imports +from PyQt5 import QtCore, QtWidgets +import logging +import rospy +from time import time + +from rover_arm.msg import ArmControlMessage, ArmStatusMessage + +##################################### +# Global Variables +##################################### +ARM_RELATIVE_CONTROL_TOPIC = "/rover_arm/control/relative" +ARM_ABSOLUTE_CONTROL_TOPIC = "/rover_arm/control/absolute" +ARM_STATUS_TOPIC = "/rover_arm/status" + +THREAD_HERTZ = 5 + +COMMS_TO_STRING = { + 0: "NO STATUS", + 1: "COMMS OK", + 2: "NO DEVICE", + 4: "BUS ERROR", + 8: "GEN COMM ERROR", + 16: "PARAMETER ERROR", + 32: "LENGTH ERROR" +} + +TARGET_REACHED_BIT_POSITION = 1 + +STATUS_TO_STRING = { + 1: "TARGET REACHED", + 2: "ERROR RECOVERY", + 3: "RUN", + 4: "ENABLED", + 5: "FAULT STOP", + 6: "WARNING", + 7: "STO ACTIVE", + 8: "SERVO READY", + 10: "BRAKING", + 11: "HOMING", + 12: "INITIALIZED", + 13: "VOLT OK", + 15: "PERMANENT STOP" +} + +FAULT_TO_STRING = { + 1: "TRACKING ERROR", + 2: "OVER CURRENT", + # 3: "COMMUNICATION ERROR", # Was showing even though things were working??? + 4: "ENCODER FAILURE", + 5: "OVER TEMP", + 6: "UNDER VOLTAGE", + 7: "OVER VOLTAGE", + 8: "PROG OR MEM ERROR", + 9: "HARDWARE ERROR", + 10: "OVER VELOCITY", + 11: "INIT ERROR", + 12: "MOTION ERROR", + 13: "RANGE ERROR", + 14: "POWER STAGE FORCED OFF", + 15: "HOST COMM ERROR" +} + +POSITIONAL_TOLERANCE = 0.02 + +# Order is [base, shoulder, elbow, roll, wrist_pitch, wrist_roll] +ARM_STOW_PROCEDURE = [ + [0.0, -0.035, -0.28, 0.0, 0.0, 0.0], + [0.0, -0.035, -0.28, -0.25, 0.25, 0.0], + [0.0, -0.035, -0.5, -0.25, 0.25, 0.0], + [0.0, -0.25, -0.5, -0.25, 0.25, -0.25] +] + +ARM_UNSTOW_PROCEDURE = [ + [0.0, -0.25, -0.5, -0.25, 0.25, -0.25], + [0.0, -0.035, -0.5, -0.25, 0.25, 0.0], + [0.0, -0.035, -0.28, -0.25, 0.25, 0.0], + [0.0, -0.035, -0.28, 0.0, 0.0, 0.0] +] + + +##################################### +# UbiquitiRadioSettings Class Definition +##################################### +class MiscArm(QtCore.QThread): + base_comms_state_update_ready__signal = QtCore.pyqtSignal(str) + shoulder_comms_state_update_ready__signal = QtCore.pyqtSignal(str) + elbow_comms_state_update_ready__signal = QtCore.pyqtSignal(str) + roll_comms_state_update_ready__signal = QtCore.pyqtSignal(str) + wrist_pitch_comms_state_update_ready__signal = QtCore.pyqtSignal(str) + wrist_roll_comms_state_update_ready__signal = QtCore.pyqtSignal(str) + + base_status_update_ready__signal = QtCore.pyqtSignal(str) + shoulder_status_update_ready__signal = QtCore.pyqtSignal(str) + elbow_status_update_ready__signal = QtCore.pyqtSignal(str) + roll_status_update_ready__signal = QtCore.pyqtSignal(str) + wrist_pitch_status_update_ready__signal = QtCore.pyqtSignal(str) + wrist_roll_status_update_ready__signal = QtCore.pyqtSignal(str) + + base_faults_update_ready__signal = QtCore.pyqtSignal(str) + shoulder_faults_update_ready__signal = QtCore.pyqtSignal(str) + elbow_faults_update_ready__signal = QtCore.pyqtSignal(str) + roll_faults_update_ready__signal = QtCore.pyqtSignal(str) + wrist_pitch_faults_update_ready__signal = QtCore.pyqtSignal(str) + wrist_roll_faults_update_ready__signal = QtCore.pyqtSignal(str) + + def __init__(self, shared_objects): + super(MiscArm, self).__init__() + + # ########## Reference to class init variables ########## + self.shared_objects = shared_objects + self.left_screen = self.shared_objects["screens"]["left_screen"] + + self.arm_control_upright_zeroed_button = self.left_screen.arm_control_upright_zeroed_button # type:QtWidgets.QPushButton + self.arm_controls_stow_arm_button = self.left_screen.arm_controls_stow_arm_button # type:QtWidgets.QPushButton + self.arm_controls_unstow_arm_button = self.left_screen.arm_controls_unstow_arm_button # type:QtWidgets.QPushButton + + self.arm_controls_calibration_button = self.left_screen.arm_controls_calibration_button # type:QtWidgets.QPushButton + self.arm_controls_clear_faults_button = self.left_screen.arm_controls_clear_faults_button # type:QtWidgets.QPushButton + self.arm_controls_reset_motor_drivers_button = self.left_screen.arm_controls_reset_motor_drivers_button # type:QtWidgets.QPushButton + + self.arm_controls_base_comms_label = self.left_screen.arm_controls_base_comms_label # type:QtWidgets.QLabel + self.arm_controls_base_status_label = self.left_screen.arm_controls_base_status_label # type:QtWidgets.QLabel + self.arm_controls_base_faults_label = self.left_screen.arm_controls_base_faults_label # type:QtWidgets.QLabel + + self.arm_controls_shoulder_comms_label = self.left_screen.arm_controls_shoulder_comms_label # type:QtWidgets.QLabel + self.arm_controls_shoulder_status_label = self.left_screen.arm_controls_shoulder_status_label # type:QtWidgets.QLabel + self.arm_controls_shoulder_faults_label = self.left_screen.arm_controls_shoulder_faults_label # type:QtWidgets.QLabel + self.arm_controls_elbow_comms_label = self.left_screen.arm_controls_elbow_comms_label # type:QtWidgets.QLabel + self.arm_controls_elbow_status_label = self.left_screen.arm_controls_elbow_status_label # type:QtWidgets.QLabel + self.arm_controls_elbow_faults_label = self.left_screen.arm_controls_elbow_faults_label # type:QtWidgets.QLabel + + self.arm_controls_roll_comms_label = self.left_screen.arm_controls_roll_comms_label # type:QtWidgets.QLabel + self.arm_controls_roll_status_label = self.left_screen.arm_controls_roll_status_label # type:QtWidgets.QLabel + self.arm_controls_roll_faults_label = self.left_screen.arm_controls_roll_faults_label # type:QtWidgets.QLabel + + self.arm_controls_wrist_pitch_comms_label = self.left_screen.arm_controls_wrist_pitch_comms_label # type:QtWidgets.QLabel + self.arm_controls_wrist_pitch_status_label = self.left_screen.arm_controls_wrist_pitch_status_label # type:QtWidgets.QLabel + self.arm_controls_wrist_pitch_faults_label = self.left_screen.arm_controls_wrist_pitch_faults_label # type:QtWidgets.QLabel + + self.arm_controls_wrist_roll_comms_label = self.left_screen.arm_controls_wrist_roll_comms_label # type:QtWidgets.QLabel + self.arm_controls_wrist_roll_status_label = self.left_screen.arm_controls_wrist_roll_status_label # type:QtWidgets.QLabel + self.arm_controls_wrist_roll_faults_label = self.left_screen.arm_controls_wrist_roll_faults_label # type:QtWidgets.QLabel + + # ########## 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.wait_time = 1.0 / THREAD_HERTZ + + self.arm_status_subscriber = rospy.Subscriber(ARM_STATUS_TOPIC, ArmStatusMessage, + self.new_arm_status_message_received__callback) + + self.arm_relative_control_publisher = rospy.Publisher(ARM_RELATIVE_CONTROL_TOPIC, ArmControlMessage, + queue_size=1) + self.arm_absolute_control_publisher = rospy.Publisher(ARM_ABSOLUTE_CONTROL_TOPIC, ArmControlMessage, + queue_size=1) + + self.base_position = 0 + self.shoulder_position = 0 + self.elbow_position = 0 + self.roll_position = 0 + self.wrist_pitch_position = 0 + self.wrist_roll_position = 0 + + self.should_stow_arm = False + self.should_unstow_arm = False + + def run(self): + self.logger.debug("Starting MiscArm Thread") + + while self.run_thread_flag: + start_time = time() + + if self.should_stow_arm: + self.stow_rover_arm() + self.should_stow_arm = False + elif self.should_unstow_arm: + self.unstow_rover_arm() + self.should_unstow_arm = False + + time_diff = time() - start_time + + self.msleep(max(int(self.wait_time - time_diff), 0)) + + self.logger.debug("Stopping MiscArm Thread") + + def stow_rover_arm(self): + for movement in ARM_STOW_PROCEDURE: + self.process_absolute_move_command(movement) + + def unstow_rover_arm(self): + for movement in ARM_UNSTOW_PROCEDURE: + self.process_absolute_move_command(movement) + + def process_absolute_move_command(self, movement): + message = ArmControlMessage() + + message.base = movement[0] + message.shoulder = movement[1] + message.elbow = movement[2] + message.roll = movement[3] + message.wrist_pitch = movement[4] + message.wrist_roll = movement[5] + + print message + self.arm_absolute_control_publisher.publish(message) + + self.wait_for_targets_reached(movement) + + def wait_for_targets_reached(self, movement): + base_set = movement[0] + shoulder_set = movement[1] + elbow_set = movement[2] + roll_set = movement[3] + wrist_pitch_set = movement[4] + wrist_roll_set = movement[5] - (wrist_pitch_set / 2.0) + + while abs(self.base_position - base_set) > POSITIONAL_TOLERANCE: + self.logger.debug("Waiting for base| %f\t%f" % (self.base_position, base_set)) + self.msleep(10) + + while abs(self.shoulder_position - shoulder_set) > POSITIONAL_TOLERANCE: + self.logger.debug("Waiting for shoulder| %f\t%f" % (self.shoulder_position, shoulder_set)) + self.msleep(10) + + while abs(self.elbow_position - elbow_set) > POSITIONAL_TOLERANCE: + self.logger.debug("Waiting for elbow| %f\t%f" % (self.elbow_position, elbow_set)) + self.msleep(10) + + while abs(self.roll_position - roll_set) > POSITIONAL_TOLERANCE: + self.logger.debug("Waiting for roll| %f\t%f" % (self.roll_position, roll_set)) + self.msleep(10) + + while abs(self.wrist_pitch_position - wrist_pitch_set) > POSITIONAL_TOLERANCE: + self.logger.debug("Waiting for wrist_pitch| %f\t%f" % (self.wrist_pitch_position, wrist_pitch_set)) + self.msleep(10) + + while abs(self.wrist_roll_position - wrist_roll_set) > POSITIONAL_TOLERANCE: + self.logger.debug("Waiting for wrist_roll| %f\t%f" % (self.wrist_roll_position, wrist_roll_set)) + self.msleep(10) + + def connect_signals_and_slots(self): + self.arm_controls_stow_arm_button.clicked.connect(self.on_stow_arm_button_pressed__slot) + self.arm_controls_unstow_arm_button.clicked.connect(self.on_unstow_arm_button_pressed__slot) + self.arm_control_upright_zeroed_button.clicked.connect(self.on_upright_zeroed_button_pressed__slot) + + self.arm_controls_calibration_button.clicked.connect(self.on_set_calibration_button_pressed__slot) + self.arm_controls_clear_faults_button.clicked.connect(self.on_clear_faults_button_pressed__slot) + self.arm_controls_reset_motor_drivers_button.clicked.connect(self.on_reset_drivers_button_pressed__slot) + + self.base_comms_state_update_ready__signal.connect(self.arm_controls_base_comms_label.setText) + self.shoulder_comms_state_update_ready__signal.connect(self.arm_controls_shoulder_comms_label.setText) + self.elbow_comms_state_update_ready__signal.connect(self.arm_controls_elbow_comms_label.setText) + self.roll_comms_state_update_ready__signal.connect(self.arm_controls_roll_comms_label.setText) + self.wrist_pitch_comms_state_update_ready__signal.connect(self.arm_controls_wrist_pitch_comms_label.setText) + self.wrist_roll_comms_state_update_ready__signal.connect(self.arm_controls_wrist_roll_comms_label.setText) + + self.base_status_update_ready__signal.connect(self.arm_controls_base_status_label.setText) + self.shoulder_status_update_ready__signal.connect(self.arm_controls_shoulder_status_label.setText) + self.elbow_status_update_ready__signal.connect(self.arm_controls_elbow_status_label.setText) + self.roll_status_update_ready__signal.connect(self.arm_controls_roll_status_label.setText) + self.wrist_pitch_status_update_ready__signal.connect(self.arm_controls_wrist_pitch_status_label.setText) + self.wrist_roll_status_update_ready__signal.connect(self.arm_controls_wrist_roll_status_label.setText) + + self.base_faults_update_ready__signal.connect(self.arm_controls_base_faults_label.setText) + self.shoulder_faults_update_ready__signal.connect(self.arm_controls_shoulder_faults_label.setText) + self.elbow_faults_update_ready__signal.connect(self.arm_controls_elbow_faults_label.setText) + self.roll_faults_update_ready__signal.connect(self.arm_controls_roll_faults_label.setText) + self.wrist_pitch_faults_update_ready__signal.connect(self.arm_controls_wrist_pitch_faults_label.setText) + self.wrist_roll_faults_update_ready__signal.connect(self.arm_controls_wrist_roll_faults_label.setText) + + def on_upright_zeroed_button_pressed__slot(self): + self.process_absolute_move_command([0 for _ in range(6)]) + + def on_set_calibration_button_pressed__slot(self): + message = ArmControlMessage() + message.calibrate = True + self.arm_relative_control_publisher.publish(message) + + def on_clear_faults_button_pressed__slot(self): + message = ArmControlMessage() + message.clear_faults = True + self.arm_relative_control_publisher.publish(message) + + def on_reset_drivers_button_pressed__slot(self): + message = ArmControlMessage() + message.reset_controllers = True + self.arm_relative_control_publisher.publish(message) + + def on_stow_arm_button_pressed__slot(self): + self.should_stow_arm = True + + def on_unstow_arm_button_pressed__slot(self): + self.should_unstow_arm = True + + def new_arm_status_message_received__callback(self, data): + self.base_comms_state_update_ready__signal.emit(self.process_comms_to_string(data.base_comm_status)) + self.shoulder_comms_state_update_ready__signal.emit(self.process_comms_to_string(data.shoulder_comm_status)) + self.elbow_comms_state_update_ready__signal.emit(self.process_comms_to_string(data.elbow_comm_status)) + self.roll_comms_state_update_ready__signal.emit(self.process_comms_to_string(data.roll_comm_status)) + self.wrist_pitch_comms_state_update_ready__signal.emit( + self.process_comms_to_string(data.wrist_pitch_comm_status)) + self.wrist_roll_comms_state_update_ready__signal.emit(self.process_comms_to_string(data.wrist_roll_comm_status)) + + self.base_status_update_ready__signal.emit(self.process_statuses_to_string(data.base_status)) + self.shoulder_status_update_ready__signal.emit(self.process_statuses_to_string(data.shoulder_status)) + self.elbow_status_update_ready__signal.emit(self.process_statuses_to_string(data.elbow_status)) + self.roll_status_update_ready__signal.emit(self.process_statuses_to_string(data.roll_status)) + self.wrist_pitch_status_update_ready__signal.emit(self.process_statuses_to_string(data.wrist_pitch_status)) + self.wrist_roll_status_update_ready__signal.emit(self.process_statuses_to_string(data.wrist_roll_status)) + + self.base_faults_update_ready__signal.emit(self.process_faults_to_string(data.base_faults)) + self.shoulder_faults_update_ready__signal.emit(self.process_faults_to_string(data.shoulder_faults)) + self.elbow_faults_update_ready__signal.emit(self.process_faults_to_string(data.elbow_faults)) + self.roll_faults_update_ready__signal.emit(self.process_faults_to_string(data.roll_faults)) + self.wrist_pitch_faults_update_ready__signal.emit(self.process_faults_to_string(data.wrist_pitch_faults)) + self.wrist_roll_faults_update_ready__signal.emit(self.process_faults_to_string(data.wrist_roll_faults)) + + self.base_position = data.base + self.shoulder_position = data.shoulder + self.elbow_position = data.elbow + self.roll_position = data.roll + self.wrist_pitch_position = data.wrist_pitch + self.wrist_roll_position = data.wrist_roll + + @staticmethod + def process_faults_to_string(faults): + fault_output = "" + + for bit_position in FAULT_TO_STRING: + if (1 << bit_position) & faults: + fault_output += FAULT_TO_STRING[bit_position] + "\n" + + return fault_output[:-1] + + @staticmethod + def process_statuses_to_string(statuses): + status_output = "" + + for bit_position in STATUS_TO_STRING: + if (1 << bit_position) & statuses: + status_output += STATUS_TO_STRING[bit_position] + "\n" + + return status_output[:-1] + + @staticmethod + def process_comms_to_string(comms): + return COMMS_TO_STRING[comms] if comms in COMMS_TO_STRING else "UNKNOWN" + + def setup_signals(self, start_signal, signals_and_slots_signal, kill_signal): + start_signal.connect(self.start) + signals_and_slots_signal.connect(self.connect_signals_and_slots) + kill_signal.connect(self.on_kill_threads_requested__slot) + + def on_kill_threads_requested__slot(self): + self.run_thread_flag = False diff --git a/software/ros_packages/ground_station/src/Framework/MiscSystems/RDFCore.py b/software/ros_packages/ground_station/src/Framework/MiscSystems/RDFCore.py new file mode 100644 index 0000000..011b6d6 --- /dev/null +++ b/software/ros_packages/ground_station/src/Framework/MiscSystems/RDFCore.py @@ -0,0 +1,91 @@ +# coding=utf-8 +##################################### +# Imports +##################################### +# Python native imports +from PyQt5 import QtCore, QtWidgets +import logging +import rospy +from time import time + +from std_msgs.msg import Float64MultiArray + +##################################### +# Global Variables +##################################### +RDF_DATA_TOPIC = "/rdf/data" + +THREAD_HERTZ = 5 + + +##################################### +# UbiquitiRadioSettings Class Definition +##################################### +class RDF(QtCore.QThread): + + lcd_number_update_ready__signal = QtCore.pyqtSignal(int) + + def __init__(self, shared_objects): + super(RDF, self).__init__() + + # ########## Reference to class init variables ########## + self.shared_objects = shared_objects + self.left_screen = self.shared_objects["screens"]["left_screen"] + + self.rssi_lcdnumber = self.left_screen.rssi_lcdnumber # type:QtWidgets.QLCDNumber + + # ########## 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.wait_time = 1.0 / THREAD_HERTZ + + self.rdf_subscriber = rospy.Subscriber(RDF_DATA_TOPIC, Float64MultiArray, self.new_rdf_message_received__callback) + + self.data = [] + self.data_limit = 100 + + + def run(self): + self.logger.debug("Starting RDF Thread") + + while self.run_thread_flag: + start_time = time() + + temp = list(self.data) + if temp: + average = sum(temp) / len(temp) + self.lcd_number_update_ready__signal.emit(average) + + time_diff = time() - start_time + + self.msleep(max(int(self.wait_time - time_diff), 0)) + + self.logger.debug("Stopping RDF Thread") + + def new_rdf_message_received__callback(self, data): + if len(self.data) >= self.data_limit: + del self.data[0] + + # if len(self.times) >= self.data_limit: + # del self.times[0] + + self.data.append(data.data[0]) + # self.times.append(data.data[1]) + + def connect_signals_and_slots(self): + self.lcd_number_update_ready__signal.connect(self.rssi_lcdnumber.display) + + def setup_signals(self, start_signal, signals_and_slots_signal, kill_signal): + start_signal.connect(self.start) + signals_and_slots_signal.connect(self.connect_signals_and_slots) + kill_signal.connect(self.on_kill_threads_requested__slot) + + def on_kill_threads_requested__slot(self): + self.run_thread_flag = False 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..627e6d2 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,1046 @@ N/A 0 + + + RDF + + + + + + + + + + Arm + + + + + + + + + 18 + 75 + true + + + + Arm Controls + + + + + + + + + + + + 75 + true + + + + Preset Movements + + + + + + + + + Upright Zeroed + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + Stow Arm + + + + + + + Unstow Arm + + + + + + + color:red; + + + ^ CAREFUL! ^ + + + Qt::AlignCenter + + + + + + + + + + + + + + 75 + true + + + + Special Functions + + + + + + + + + Set Calibration + + + + + + + Clear Faults + + + + + + + Reset Motor Drivers + + + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + + + + + 18 + 75 + true + + + + Motor Driver Statuses + + + + + + + + + + + + 75 + true + + + + Base + + + + + + + + + Comms: + + + + + + + + 6 + 50 + false + + + + + + + + + + + Status: + + + + + + + Faults: + + + + + + + + 6 + 50 + false + + + + + + + + + + + + 6 + 50 + false + + + + + + + + + + + + + + + + + + 75 + true + + + + Shoulder + + + + + + + + + Comms: + + + + + + + + 6 + 50 + false + + + + + + + + + + + Status: + + + + + + + Faults: + + + + + + + + 6 + 50 + false + + + + + + + + + + + + 6 + 50 + false + + + + + + + + + + + + + + + + + + 75 + true + + + + Elbow + + + + + + + + + Comms: + + + + + + + + 6 + 50 + false + + + + + + + + + + + Status: + + + + + + + Faults: + + + + + + + + 6 + 50 + false + + + + + + + + + + + + 6 + 50 + false + + + + + + + + + + + + + + + + + + 75 + true + + + + Roll + + + + + + + + + Comms: + + + + + + + + 6 + 50 + false + + + + + + + + + + + Status: + + + + + + + Faults: + + + + + + + + 6 + 50 + false + + + + + + + + + + + + 6 + 50 + false + + + + + + + + + + + + + + + + + + 75 + true + + + + Wrist Pitch + + + + + + + + + Comms: + + + + + + + + 6 + 50 + false + + + + + + + + + + + Status: + + + + + + + Faults: + + + + + + + + 6 + 50 + false + + + + + + + + + + + + 6 + 50 + false + + + + + + + + + + + + + + + + + + 75 + true + + + + Wrist Roll + + + + + + + + + Comms: + + + + + + + + 6 + 50 + false + + + + + + + + + + + Status: + + + + + + + Faults: + + + + + + + + 6 + 50 + false + + + + + + + + + + + + 6 + 50 + false + + + + + + + + + + + + + + + + + + + + + Qt::Horizontal + + + + + + + + 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 @@ -1534,13 +2574,13 @@ N/A - 640 + 470 360 - 640 + 470 360 @@ -2130,6 +3170,170 @@ N/A + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + Qt::Vertical + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + 6 + + + + + + 12 + 75 + true + + + + Xbox Mode + + + + + + + 0 + + + + + + 120 + 120 + + + + + 120 + 120 + + + + background-color:darkgreen; border: 1px solid black; + + + Arm + + + Qt::AlignCenter + + + + + + + + + 0 + + + + + + 120 + 120 + + + + + 120 + 120 + + + + border: 1px solid black; + + + Mining + + + Qt::AlignCenter + + + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + Qt::Vertical + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + diff --git a/software/ros_packages/ground_station/src/Resources/Ui/right_screen.ui b/software/ros_packages/ground_station/src/Resources/Ui/right_screen.ui index a0b604d..03e6c01 100644 --- a/software/ros_packages/ground_station/src/Resources/Ui/right_screen.ui +++ b/software/ros_packages/ground_station/src/Resources/Ui/right_screen.ui @@ -151,7 +151,7 @@ - Base Rotation + Base Qt::AlignCenter @@ -167,10 +167,10 @@ - 0 + -500 - 360 + 500 10 @@ -218,7 +218,7 @@ - Shoulder Pitch + Shoulder Qt::AlignCenter @@ -234,10 +234,10 @@ - 0 + -250 - 360 + 250 10 @@ -267,7 +267,7 @@ 90.000000000000000 - false + true @@ -289,7 +289,7 @@ - Elbow Pitch + Elbow Qt::AlignCenter @@ -305,10 +305,10 @@ - 0 + -500 - 360 + 500 10 @@ -338,7 +338,7 @@ 90.000000000000000 - false + true @@ -358,7 +358,7 @@ - Elbow Roll + Roll Qt::AlignCenter @@ -374,10 +374,10 @@ - 0 + -250 - 360 + 250 10 @@ -407,7 +407,7 @@ 90.000000000000000 - false + true @@ -431,7 +431,7 @@ - End Effector Pitch + Wrist Pitch Qt::AlignCenter @@ -447,10 +447,10 @@ - 0 + -250 - 360 + 250 10 @@ -480,7 +480,7 @@ 90.000000000000000 - false + true @@ -498,7 +498,7 @@ - End Effector Rotation + Wrist Roll Qt::AlignCenter @@ -514,10 +514,10 @@ - 0 + -1000 - 360 + 1000 10 @@ -547,7 +547,7 @@ 90.000000000000000 - false + true diff --git a/software/ros_packages/ground_station/src/ground_station.py b/software/ros_packages/ground_station/src/ground_station.py index 3fe78f7..a155de6 100644 --- a/software/ros_packages/ground_station/src/ground_station.py +++ b/software/ros_packages/ground_station/src/ground_station.py @@ -15,14 +15,18 @@ import Framework.StartupSystems.ROSMasterChecker as ROSMasterChecker import Framework.LoggingSystems.Logger as Logger import Framework.VideoSystems.RoverVideoCoordinator as RoverVideoCoordinator import Framework.MapSystems.RoverMapCoordinator as RoverMapCoordinator -import Framework.InputSystems.JoystickControlSender as JoystickControlSender +import Framework.InputSystems.LogitechControllerControlSender as JoystickControlSender +import Framework.InputSystems.XBOXControllerControlSender as ControllerControlSender import Framework.NavigationSystems.SpeedAndHeadingIndication as SpeedAndHeading import Framework.NavigationSystems.WaypointsCoordinator as WaypointsCoordinator import Framework.ArmSystems.ArmIndication as ArmIndication 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 +import Framework.MiscSystems.BashConsoleCore as BashConsoleCore +import Framework.MiscSystems.MiscArmCore as MiscArmCore +import Framework.MiscSystems.RDFCore as RDFCore ##################################### # Global Variables @@ -101,18 +105,22 @@ class GroundStation(QtCore.QObject): rospy.init_node("ground_station") # ##### Instantiate Regular Classes ###### + self.__add_non_thread("Mining System", MiningCore.Mining(self.shared_objects)) + self.__add_non_thread("Arm Indication", ArmIndication.ArmIndication(self.shared_objects)) # ##### Instantiate Threaded Classes ###### self.__add_thread("Video Coordinator", RoverVideoCoordinator.RoverVideoCoordinator(self.shared_objects)) self.__add_thread("Map Coordinator", RoverMapCoordinator.RoverMapCoordinator(self.shared_objects)) - self.__add_thread("Joystick Sender", JoystickControlSender.JoystickControlSender(self.shared_objects)) + self.__add_thread("Joystick Sender", JoystickControlSender.LogitechControllerControlSender(self.shared_objects)) + self.__add_thread("Controller Sender", ControllerControlSender.XBOXControllerControlSender(self.shared_objects)) self.__add_thread("Speed and Heading", SpeedAndHeading.SpeedAndHeadingIndication(self.shared_objects)) - self.__add_thread("Arm Indication", ArmIndication.ArmIndication(self.shared_objects)) self.__add_thread("Rover Status", StatusCore.SensorCore(self.shared_objects)) self.__add_thread("Ubiquiti Status", UbiquitiStatusCore.UbiquitiStatus(self.shared_objects)) self.__add_thread("Ubiquiti Radio Settings", UbiquitiRadioSettings.UbiquitiRadioSettings(self.shared_objects)) self.__add_thread("Waypoints Coordinator", WaypointsCoordinator.WaypointsCoordinator(self.shared_objects)) - self.__add_thread("Spacenav Sender", SpaceNavControlSender.SpaceNavControlSender(self.shared_objects)) + self.__add_thread("Bash Console", BashConsoleCore.BashConsole(self.shared_objects)) + self.__add_thread("Misc Arm", MiscArmCore.MiscArm(self.shared_objects)) + self.__add_thread("RDF", RDFCore.RDF(self.shared_objects)) self.connect_signals_and_slots_signal.emit() self.__connect_signals_to_slots() @@ -132,6 +140,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/mr1718-arm-urdf_export/CMakeLists.txt b/software/ros_packages/mr1718-arm-urdf_export/CMakeLists.txt new file mode 100644 index 0000000..90865c0 --- /dev/null +++ b/software/ros_packages/mr1718-arm-urdf_export/CMakeLists.txt @@ -0,0 +1,14 @@ +cmake_minimum_required(VERSION 2.8.3) + +project(mr1718-arm-urdf_export) + +find_package(catkin REQUIRED) + +catkin_package() + +find_package(roslaunch) + +foreach(dir config launch meshes urdf) + install(DIRECTORY ${dir}/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir}) +endforeach(dir) diff --git a/software/ros_packages/mr1718-arm-urdf_export/config/joint_names_mr1718-arm-urdf_export.yaml b/software/ros_packages/mr1718-arm-urdf_export/config/joint_names_mr1718-arm-urdf_export.yaml new file mode 100644 index 0000000..8e49505 --- /dev/null +++ b/software/ros_packages/mr1718-arm-urdf_export/config/joint_names_mr1718-arm-urdf_export.yaml @@ -0,0 +1 @@ +controller_joint_names: ['shoulder', 'elbow', 'roll', 'wrist_base', 'wrist_pitch', 'wrist_roll', ] diff --git a/software/ros_packages/mr1718-arm-urdf_export/launch/display.launch b/software/ros_packages/mr1718-arm-urdf_export/launch/display.launch new file mode 100644 index 0000000..7d30f63 --- /dev/null +++ b/software/ros_packages/mr1718-arm-urdf_export/launch/display.launch @@ -0,0 +1,26 @@ + + + + + + + + + \ No newline at end of file diff --git a/software/ros_packages/mr1718-arm-urdf_export/launch/gazebo.launch b/software/ros_packages/mr1718-arm-urdf_export/launch/gazebo.launch new file mode 100644 index 0000000..3120fdf --- /dev/null +++ b/software/ros_packages/mr1718-arm-urdf_export/launch/gazebo.launch @@ -0,0 +1,20 @@ + + + + + + \ No newline at end of file diff --git a/software/ros_packages/mr1718-arm-urdf_export/meshes/base_link.STL b/software/ros_packages/mr1718-arm-urdf_export/meshes/base_link.STL new file mode 100644 index 0000000..cf25e06 Binary files /dev/null and b/software/ros_packages/mr1718-arm-urdf_export/meshes/base_link.STL differ diff --git a/software/ros_packages/mr1718-arm-urdf_export/meshes/elbow_link.STL b/software/ros_packages/mr1718-arm-urdf_export/meshes/elbow_link.STL new file mode 100644 index 0000000..aa95455 Binary files /dev/null and b/software/ros_packages/mr1718-arm-urdf_export/meshes/elbow_link.STL differ diff --git a/software/ros_packages/mr1718-arm-urdf_export/meshes/roll_link.STL b/software/ros_packages/mr1718-arm-urdf_export/meshes/roll_link.STL new file mode 100644 index 0000000..4ed92c8 Binary files /dev/null and b/software/ros_packages/mr1718-arm-urdf_export/meshes/roll_link.STL differ diff --git a/software/ros_packages/mr1718-arm-urdf_export/meshes/shoulder_link.STL b/software/ros_packages/mr1718-arm-urdf_export/meshes/shoulder_link.STL new file mode 100644 index 0000000..8a97f5c Binary files /dev/null and b/software/ros_packages/mr1718-arm-urdf_export/meshes/shoulder_link.STL differ diff --git a/software/ros_packages/mr1718-arm-urdf_export/meshes/wrist_link.STL b/software/ros_packages/mr1718-arm-urdf_export/meshes/wrist_link.STL new file mode 100644 index 0000000..fc3e117 Binary files /dev/null and b/software/ros_packages/mr1718-arm-urdf_export/meshes/wrist_link.STL differ diff --git a/software/ros_packages/mr1718-arm-urdf_export/meshes/wrist_pitch_link.STL b/software/ros_packages/mr1718-arm-urdf_export/meshes/wrist_pitch_link.STL new file mode 100644 index 0000000..c7d3ee5 Binary files /dev/null and b/software/ros_packages/mr1718-arm-urdf_export/meshes/wrist_pitch_link.STL differ diff --git a/software/ros_packages/mr1718-arm-urdf_export/meshes/wrist_roll_link.STL b/software/ros_packages/mr1718-arm-urdf_export/meshes/wrist_roll_link.STL new file mode 100644 index 0000000..33f5bad Binary files /dev/null and b/software/ros_packages/mr1718-arm-urdf_export/meshes/wrist_roll_link.STL differ diff --git a/software/ros_packages/mr1718-arm-urdf_export/package.xml b/software/ros_packages/mr1718-arm-urdf_export/package.xml new file mode 100644 index 0000000..cf4513e --- /dev/null +++ b/software/ros_packages/mr1718-arm-urdf_export/package.xml @@ -0,0 +1,21 @@ + + mr1718-arm-urdf_export + 1.0.0 + +

URDF Description package for mr1718-arm-urdf_export

+

This package contains configuration data, 3D models and launch files +for mr1718-arm-urdf_export robot

+ + me + + BSD + catkin + roslaunch + robot_state_publisher + rviz + joint_state_publisher + gazebo + + + + diff --git a/software/ros_packages/mr1718-arm-urdf_export/robots/mr1718-arm-urdf_export.urdf b/software/ros_packages/mr1718-arm-urdf_export/robots/mr1718-arm-urdf_export.urdf new file mode 100644 index 0000000..7ef89fa --- /dev/null +++ b/software/ros_packages/mr1718-arm-urdf_export/robots/mr1718-arm-urdf_export.urdf @@ -0,0 +1,361 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/software/ros_packages/rover_arm/CMakeLists.txt b/software/ros_packages/rover_arm/CMakeLists.txt index 506f96b..8ed758a 100644 --- a/software/ros_packages/rover_arm/CMakeLists.txt +++ b/software/ros_packages/rover_arm/CMakeLists.txt @@ -9,8 +9,9 @@ add_compile_options(-std=c++11) ## is used, also find other catkin packages find_package(catkin REQUIRED COMPONENTS roscpp - #message_generation std_msgs + std_msgs + message_generation ) ## System dependencies are found with CMake's conventions @@ -47,9 +48,11 @@ find_package(catkin REQUIRED COMPONENTS ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) ## Generate messages in the 'msg' folder -#add_message_files( -# FILES -#) +add_message_files( + FILES + ArmControlMessage.msg + ArmStatusMessage.msg +) ## Generate services in the 'srv' folder # add_service_files( @@ -66,10 +69,10 @@ find_package(catkin REQUIRED COMPONENTS # ) ## Generate added messages and services with any dependencies listed here -#generate_messages( -# DEPENDENCIES -# std_msgs -#) +generate_messages( + DEPENDENCIES + std_msgs +) ################################################ ## Declare ROS dynamic reconfigure parameters ## @@ -103,7 +106,7 @@ find_package(catkin REQUIRED COMPONENTS catkin_package( # INCLUDE_DIRS include # LIBRARIES rover_arm - CATKIN_DEPENDS roscpp std_msgs + CATKIN_DEPENDS roscpp std_msgs message_generation # DEPENDS system_lib ) @@ -150,7 +153,7 @@ add_executable(${PROJECT_NAME} src/rover_arm.cpp ) target_link_libraries( ${PROJECT_NAME} ${catkin_LIBRARIES} - simplemotion + simplemotion2 ) ############# diff --git a/software/ros_packages/rover_arm/msg/ArmControlMessage.msg b/software/ros_packages/rover_arm/msg/ArmControlMessage.msg new file mode 100644 index 0000000..38ae114 --- /dev/null +++ b/software/ros_packages/rover_arm/msg/ArmControlMessage.msg @@ -0,0 +1,10 @@ +float64 base +float64 shoulder +float64 elbow +float64 roll +float64 wrist_pitch +float64 wrist_roll + +bool clear_faults +bool reset_controllers +bool calibrate \ No newline at end of file diff --git a/software/ros_packages/rover_arm/msg/ArmStatusMessage.msg b/software/ros_packages/rover_arm/msg/ArmStatusMessage.msg new file mode 100644 index 0000000..c52ff73 --- /dev/null +++ b/software/ros_packages/rover_arm/msg/ArmStatusMessage.msg @@ -0,0 +1,27 @@ +float64 base +float64 shoulder +float64 elbow +float64 roll +float64 wrist_pitch +float64 wrist_roll + +int32 base_comm_status +int32 shoulder_comm_status +int32 elbow_comm_status +int32 roll_comm_status +int32 wrist_pitch_comm_status +int32 wrist_roll_comm_status + +int32 base_status +int32 shoulder_status +int32 elbow_status +int32 roll_status +int32 wrist_pitch_status +int32 wrist_roll_status + +int32 base_faults +int32 shoulder_faults +int32 elbow_faults +int32 roll_faults +int32 wrist_pitch_faults +int32 wrist_roll_faults \ No newline at end of file diff --git a/software/ros_packages/rover_arm/package.xml b/software/ros_packages/rover_arm/package.xml index d6fd2cd..78fecd3 100644 --- a/software/ros_packages/rover_arm/package.xml +++ b/software/ros_packages/rover_arm/package.xml @@ -51,10 +51,16 @@ catkin roscpp std_msgs + ros_msgs + message_generation roscpp std_msgs + ros_msgs roscpp std_msgs + ros_msgs + message_generation + message_runtime diff --git a/software/ros_packages/rover_arm/src/rover_arm.cpp b/software/ros_packages/rover_arm/src/rover_arm.cpp index cf134c2..17ec2a9 100644 --- a/software/ros_packages/rover_arm/src/rover_arm.cpp +++ b/software/ros_packages/rover_arm/src/rover_arm.cpp @@ -1,10 +1,14 @@ #include #include +#include +#include #include #include #include "simplemotion/simplemotion.h" +#include +#include using namespace std; @@ -12,83 +16,468 @@ using namespace std; int device_address = 1; ////////// Global Variables ////////// +////////// Simplemotion variables and defines ///// +#define SMP_SERIAL_ENC_OFFSET 575 //This was missing from simplemotion_defs.h + +////////// ROS Variables and Defines ///// // ROS Parameter Defaults -const string default_port = "/dev/ttyUSB0"; +//const string default_port = "/dev/ttyUSB0"; +const string default_port = "/dev/rover/ttyARM"; + +const string default_absolute_position_control_topic = "control/absolute"; +const string default_relative_position_control_topic = "control/relative"; +const string default_arm_status_topic = "status"; + +////////// Axis limits and conversions ////////// +// Base +const smuint8 base_address = 1; +const smint32 base_counts_per_rev = 5725807; +const double base_min_rev = -0.5; +const double base_max_rev = 0.5; + +smint32 base_min_rev_counts = 0; +smint32 base_max_rev_counts = 0; + +// Shoulder +const smuint8 shoulder_address = 2; +const smint32 shoulder_counts_per_rev = 2620130; +const double shoulder_min_rev = -0.25; +const double shoulder_max_rev = 0.25; + +smint32 shoulder_min_rev_counts = 0; +smint32 shoulder_max_rev_counts = 0; + +//Elbow +const smuint8 elbow_address = 3; +const smint32 elbow_counts_per_rev = 4917661; +const double elbow_min_rev = -0.5; +const double elbow_max_rev = 0.5; + +smint32 elbow_min_rev_counts = 0; +smint32 elbow_max_rev_counts = 0; + +//Roll +const smuint8 roll_address = 4; +const smint32 roll_counts_per_rev = 1637581; +const double roll_min_rev = -0.25; +const double roll_max_rev = 0.25; + +smint32 roll_min_rev_counts = 0; +smint32 roll_max_rev_counts = 0; + +//Wrist Pitch +const smuint8 wrist_pitch_address = 5; +const smint32 wrist_pitch_counts_per_rev = 3799492; +const double wrist_pitch_min_rev = -0.25; +const double wrist_pitch_max_rev = 0.25; + +smint32 wrist_pitch_min_rev_counts = 0; +smint32 wrist_pitch_max_rev_counts = 0; + +//Wrist Roll +const smuint8 wrist_roll_address = 6; +const smint32 wrist_roll_counts_per_rev = 3799492; -// Axis Defaults -const smint32 axis1_max_count = 100000; -float axis1_max_degrees = 180.0; - -const smint32 axis2_max_count = 100000; - -const smint32 axis3_max_count = 100000; - -const smint32 axis4_max_count = 100000; - -const smint32 axis5_max_count = 320000; - -const smint32 axis6_max_count = 100000; - - -class RoverArm{ +class RoverArm { public: - RoverArm(int argc, char** argv){ + RoverArm(int argc, char **argv) { ros::init(argc, argv, "rover_arm"); node_handle = new ros::NodeHandle("~"); node_handle->param("port", arm_port, default_port); - - // Connect to arm, exit if failed arm_bus_handle = smOpenBus(arm_port.c_str()); - if(arm_bus_handle < 0){ + if (arm_bus_handle < 0) { ROS_ERROR("Could not connect to arm"); + return; + } else { + arm_successfully_connected = true; } + + absolute_position_control_subscriber = node_handle->subscribe(default_absolute_position_control_topic, 1, + &RoverArm::absolute_position_callback, this); + relative_position_control_subscriber = node_handle->subscribe(default_relative_position_control_topic, 1, + &RoverArm::relative_position_callback, this); + + status_publisher = node_handle->advertise(default_arm_status_topic, 1); + + base_min_rev_counts = smint32(base_min_rev * base_counts_per_rev); + base_max_rev_counts = smint32(base_max_rev * base_counts_per_rev); + + shoulder_min_rev_counts = smint32(shoulder_min_rev * shoulder_counts_per_rev); + shoulder_max_rev_counts = smint32(shoulder_max_rev * shoulder_counts_per_rev); + + elbow_min_rev_counts = smint32(elbow_min_rev * elbow_counts_per_rev); + elbow_max_rev_counts = smint32(elbow_max_rev * elbow_counts_per_rev); + + roll_min_rev_counts = smint32(roll_min_rev * roll_counts_per_rev); + roll_max_rev_counts = smint32(roll_max_rev * roll_counts_per_rev); + + wrist_pitch_min_rev_counts = smint32(wrist_pitch_min_rev * wrist_pitch_counts_per_rev); + wrist_pitch_max_rev_counts = smint32(wrist_pitch_max_rev * wrist_pitch_counts_per_rev); + } - void run(){ + void run() { char dir = 0; - printf("OK?: %d", ros::ok()); - while(ros::ok()){ - smSetParameter(arm_bus_handle, device_address, SMP_FAULTS, 0); - smSetParameter(arm_bus_handle, device_address, SMP_CONTROL_BITS1, SMP_CB1_ENABLE); + while (ros::ok()) { + if (!arm_successfully_connected) { return; } - smint32 status = 0; - while(!(status & STAT_TARGET_REACHED)){ - smRead1Parameter(arm_bus_handle, device_address, SMP_STATUS, &status); - } - - dir = !dir; - - if(dir){ - smSetParameter(arm_bus_handle, device_address, SMP_ABSOLUTE_SETPOINT, 0); - }else{ - smSetParameter(arm_bus_handle, device_address, SMP_ABSOLUTE_SETPOINT, 330000); - } + clear_faults(); + set_calibration(); + get_joint_statuses(); + set_joint_positions(); + reset_controllers(); ros::spinOnce(); } + ROS_ERROR("Shutting down."); } + + void reset_controllers() { + if (should_reset) { + smSetParameter(arm_bus_handle, base_address, SMP_SYSTEM_CONTROL, SMP_SYSTEM_CONTROL_RESTART); + smSetParameter(arm_bus_handle, shoulder_address, SMP_SYSTEM_CONTROL, SMP_SYSTEM_CONTROL_RESTART); + smSetParameter(arm_bus_handle, elbow_address, SMP_SYSTEM_CONTROL, SMP_SYSTEM_CONTROL_RESTART); + smSetParameter(arm_bus_handle, roll_address, SMP_SYSTEM_CONTROL, SMP_SYSTEM_CONTROL_RESTART); + smSetParameter(arm_bus_handle, wrist_pitch_address, SMP_SYSTEM_CONTROL, SMP_SYSTEM_CONTROL_RESTART); + smSetParameter(arm_bus_handle, wrist_roll_address, SMP_SYSTEM_CONTROL, SMP_SYSTEM_CONTROL_RESTART); +// arm_successfully_connected = false; + should_reset = false; + } + } + + void clear_faults() { + if (should_clear_faults) { + smSetParameter(arm_bus_handle, base_address, SMP_FAULTS, 0); + smSetParameter(arm_bus_handle, base_address, SMP_CONTROL_BITS1, SMP_CB1_ENABLE); + + smSetParameter(arm_bus_handle, shoulder_address, SMP_FAULTS, 0); + smSetParameter(arm_bus_handle, shoulder_address, SMP_CONTROL_BITS1, SMP_CB1_ENABLE); + + smSetParameter(arm_bus_handle, elbow_address, SMP_FAULTS, 0); + smSetParameter(arm_bus_handle, elbow_address, SMP_CONTROL_BITS1, SMP_CB1_ENABLE); + + smSetParameter(arm_bus_handle, roll_address, SMP_FAULTS, 0); + smSetParameter(arm_bus_handle, roll_address, SMP_CONTROL_BITS1, SMP_CB1_ENABLE); + + smSetParameter(arm_bus_handle, wrist_pitch_address, SMP_FAULTS, 0); + smSetParameter(arm_bus_handle, wrist_pitch_address, SMP_CONTROL_BITS1, SMP_CB1_ENABLE); + + smSetParameter(arm_bus_handle, wrist_roll_address, SMP_FAULTS, 0); + smSetParameter(arm_bus_handle, wrist_roll_address, SMP_CONTROL_BITS1, SMP_CB1_ENABLE); + + should_clear_faults = false; + } + + } + + void set_calibration() { + if (should_calibrate) { + + // Base + smint32 base_current_offset; + smint32 base_position_to_offset; + smRead2Parameters(arm_bus_handle, base_address, SMP_SERIAL_ENC_OFFSET, &base_current_offset, + SMP_ACTUAL_POSITION_FB, &base_position_to_offset); + smSetParameter(arm_bus_handle, base_address, SMP_SERIAL_ENC_OFFSET, base_current_offset - base_position_to_offset); + + // Shoulder + smint32 shoulder_current_offset; + smint32 shoulder_position_to_offset; + smRead2Parameters(arm_bus_handle, shoulder_address, SMP_SERIAL_ENC_OFFSET, &shoulder_current_offset, + SMP_ACTUAL_POSITION_FB, &shoulder_position_to_offset); + smSetParameter(arm_bus_handle, shoulder_address, SMP_SERIAL_ENC_OFFSET, shoulder_current_offset - shoulder_position_to_offset); + + + // Elbow + smint32 elbow_current_offset; + smint32 elbow_position_to_offset; + smRead2Parameters(arm_bus_handle, elbow_address, SMP_SERIAL_ENC_OFFSET, &elbow_current_offset, + SMP_ACTUAL_POSITION_FB, &elbow_position_to_offset); + smSetParameter(arm_bus_handle, elbow_address, SMP_SERIAL_ENC_OFFSET, elbow_current_offset - elbow_position_to_offset); + + // Roll + smint32 roll_current_offset; + smint32 roll_position_to_offset; + smRead2Parameters(arm_bus_handle, roll_address, SMP_SERIAL_ENC_OFFSET, &roll_current_offset, + SMP_ACTUAL_POSITION_FB, &roll_position_to_offset); + smSetParameter(arm_bus_handle, roll_address, SMP_SERIAL_ENC_OFFSET, roll_current_offset - roll_position_to_offset); + + // Wrist Pitch + smint32 wrist_pitch_current_offset; + smint32 wrist_pitch_position_to_offset; + smRead2Parameters(arm_bus_handle, wrist_pitch_address, SMP_SERIAL_ENC_OFFSET, &wrist_pitch_current_offset, + SMP_ACTUAL_POSITION_FB, &wrist_pitch_position_to_offset); + smSetParameter(arm_bus_handle, wrist_pitch_address, SMP_SERIAL_ENC_OFFSET, wrist_pitch_current_offset - wrist_pitch_position_to_offset); + + // Wrist Roll + smint32 wrist_roll_current_offset; + smint32 wrist_roll_position_to_offset; + smRead2Parameters(arm_bus_handle, wrist_roll_address, SMP_SERIAL_ENC_OFFSET, &wrist_roll_current_offset, + SMP_ACTUAL_POSITION_FB, &wrist_roll_position_to_offset); + smSetParameter(arm_bus_handle, wrist_roll_address, SMP_SERIAL_ENC_OFFSET, wrist_roll_current_offset - wrist_roll_position_to_offset); + + // Save config through reboots + smSetParameter(arm_bus_handle, base_address, SMP_SYSTEM_CONTROL, SMP_SYSTEM_CONTROL_SAVECFG); + smSetParameter(arm_bus_handle, shoulder_address, SMP_SYSTEM_CONTROL, SMP_SYSTEM_CONTROL_SAVECFG); + smSetParameter(arm_bus_handle, elbow_address, SMP_SYSTEM_CONTROL, SMP_SYSTEM_CONTROL_SAVECFG); + smSetParameter(arm_bus_handle, roll_address, SMP_SYSTEM_CONTROL, SMP_SYSTEM_CONTROL_SAVECFG); + smSetParameter(arm_bus_handle, wrist_pitch_address, SMP_SYSTEM_CONTROL, SMP_SYSTEM_CONTROL_SAVECFG); + smSetParameter(arm_bus_handle, wrist_roll_address, SMP_SYSTEM_CONTROL, SMP_SYSTEM_CONTROL_SAVECFG); + + should_reset = true; + should_calibrate = false; + } + } + + void get_joint_statuses() { + + base_comm_state = smRead3Parameters(arm_bus_handle, base_address, SMP_STATUS, &base_status, SMP_FAULTS, + &base_faults, SMP_ACTUAL_POSITION_FB, &base_current_position); + + + shoulder_comm_state = smRead3Parameters(arm_bus_handle, shoulder_address, SMP_STATUS, &shoulder_status, + SMP_FAULTS, + &shoulder_faults, SMP_ACTUAL_POSITION_FB, &shoulder_current_position); + + + elbow_comm_state = smRead3Parameters(arm_bus_handle, elbow_address, SMP_STATUS, &elbow_status, SMP_FAULTS, + &elbow_faults, SMP_ACTUAL_POSITION_FB, &elbow_current_position); + + + roll_comm_state = smRead3Parameters(arm_bus_handle, roll_address, SMP_STATUS, &roll_status, SMP_FAULTS, + &roll_faults, SMP_ACTUAL_POSITION_FB, &roll_current_position); + + + wrist_pitch_comm_state = smRead3Parameters(arm_bus_handle, wrist_pitch_address, SMP_STATUS, &wrist_pitch_status, + SMP_FAULTS, + &wrist_pitch_faults, SMP_ACTUAL_POSITION_FB, + &wrist_pitch_current_position); + + + wrist_roll_comm_state = smRead3Parameters(arm_bus_handle, wrist_roll_address, SMP_STATUS, &wrist_roll_status, + SMP_FAULTS, + &wrist_roll_faults, SMP_ACTUAL_POSITION_FB, + &wrist_roll_current_position); + + if (!received_first_joint_position_update) { + base_set_position = base_current_position; + shoulder_set_position = shoulder_current_position; + elbow_set_position = elbow_current_position; + roll_set_position = roll_current_position; + wrist_pitch_last_set_position = wrist_pitch_current_position; + wrist_pitch_set_position = wrist_pitch_current_position; + wrist_roll_set_position = wrist_roll_current_position; + + received_first_joint_position_update = true; + } + + status_message.base = base_current_position / double(base_counts_per_rev); + status_message.shoulder = shoulder_current_position / double(shoulder_counts_per_rev); + status_message.elbow = elbow_current_position / double(elbow_counts_per_rev); + status_message.roll = roll_set_position / double(roll_counts_per_rev); + status_message.wrist_pitch = wrist_pitch_set_position / double(wrist_pitch_counts_per_rev); + status_message.wrist_roll = wrist_roll_set_position / double(wrist_roll_counts_per_rev); + + status_message.base_comm_status = base_comm_state; + status_message.shoulder_comm_status = shoulder_comm_state; + status_message.elbow_comm_status = elbow_comm_state; + status_message.roll_comm_status = roll_comm_state; + status_message.wrist_pitch_comm_status = wrist_pitch_comm_state; + status_message.wrist_roll_comm_status = wrist_roll_comm_state; + + status_message.base_status = base_status; + status_message.shoulder_status = shoulder_status; + status_message.elbow_status = elbow_status; + status_message.roll_status = roll_status; + status_message.wrist_pitch_status = wrist_pitch_status; + status_message.wrist_roll_status = wrist_roll_status; + + status_message.base_faults = base_faults; + status_message.shoulder_faults = shoulder_faults; + status_message.elbow_faults = elbow_faults; + status_message.roll_faults = roll_faults; + status_message.wrist_pitch_faults = wrist_pitch_faults; + status_message.wrist_roll_faults = wrist_roll_faults; + + status_publisher.publish(status_message); + + } + + void set_joint_positions() { + if (new_positions_received) { + set_base_position(); + set_shoulder_position(); + set_elbow_position(); + set_roll_position(); + set_wrist_positions(); + + new_positions_received = false; + } + } + + void set_base_position() { + smSetParameter(arm_bus_handle, base_address, SMP_ABSOLUTE_SETPOINT, base_set_position); + } + + void set_shoulder_position() { + smSetParameter(arm_bus_handle, shoulder_address, SMP_ABSOLUTE_SETPOINT, shoulder_set_position); + + } + + void set_elbow_position() { + smSetParameter(arm_bus_handle, elbow_address, SMP_ABSOLUTE_SETPOINT, elbow_set_position); + + } + + void set_roll_position() { + smSetParameter(arm_bus_handle, roll_address, SMP_ABSOLUTE_SETPOINT, roll_set_position); + } + + void set_wrist_positions() { + smSetParameter(arm_bus_handle, wrist_roll_address, SMP_ABSOLUTE_SETPOINT, wrist_roll_set_position); + smSetParameter(arm_bus_handle, wrist_pitch_address, SMP_ABSOLUTE_SETPOINT, wrist_pitch_set_position); + + } + + void constrain_set_positions(){ + base_set_position = min(max(base_set_position, base_min_rev_counts), base_max_rev_counts); + shoulder_set_position = min(max(shoulder_set_position, shoulder_min_rev_counts), shoulder_max_rev_counts); + elbow_set_position = min(max(elbow_set_position, elbow_min_rev_counts), elbow_max_rev_counts); + roll_set_position = min(max(roll_set_position, roll_min_rev_counts), roll_max_rev_counts); + wrist_pitch_set_position = min(max(wrist_pitch_set_position, wrist_pitch_min_rev_counts), wrist_pitch_max_rev_counts); + } + + void absolute_position_callback(const rover_arm::ArmControlMessage::ConstPtr &msg) { + if (!received_first_joint_position_update) { return; } + + + base_set_position = msg->base * base_counts_per_rev; + shoulder_set_position = msg->shoulder * shoulder_counts_per_rev; + elbow_set_position = msg->elbow * elbow_counts_per_rev; + roll_set_position = msg->roll * roll_counts_per_rev; + + wrist_roll_set_position = msg->wrist_roll * wrist_roll_counts_per_rev; + + wrist_pitch_set_position = msg->wrist_pitch * wrist_pitch_counts_per_rev; + + smint32 roll_pitch_position_difference = wrist_pitch_set_position / 2; + wrist_roll_set_position -= roll_pitch_position_difference; + wrist_pitch_last_set_position = wrist_pitch_set_position; + + ROS_INFO("BASE: %ld\tSHOULDER: %ld\tELBOW: %ld\tROLL: %ld\tWRIST_PITCH: %ld\tWRIST_ROLL: %ld\t", base_set_position, shoulder_set_position, elbow_set_position, roll_set_position, wrist_pitch_set_position, wrist_roll_set_position); + + constrain_set_positions(); + ROS_INFO("BASE: %ld\tSHOULDER: %ld\tELBOW: %ld\tROLL: %ld\tWRIST_PITCH: %ld\tWRIST_ROLL: %ld\t", base_set_position, shoulder_set_position, elbow_set_position, roll_set_position, wrist_pitch_set_position, wrist_roll_set_position); + + should_clear_faults = (msg->clear_faults); + should_reset = (msg->reset_controllers); + should_calibrate = msg->calibrate; + + if(!should_calibrate && !should_reset){ + new_positions_received = true; + } + } + + void relative_position_callback(const rover_arm::ArmControlMessage::ConstPtr &msg) { + if (!received_first_joint_position_update) { return; } + + base_set_position += msg->base * base_counts_per_rev; + shoulder_set_position += msg->shoulder * shoulder_counts_per_rev; + elbow_set_position += msg->elbow * elbow_counts_per_rev; + roll_set_position += msg->roll * roll_counts_per_rev; + + wrist_roll_set_position += msg->wrist_roll * wrist_roll_counts_per_rev; + + wrist_pitch_set_position += msg->wrist_pitch * wrist_pitch_counts_per_rev; + + smint32 roll_pitch_position_difference = (wrist_pitch_set_position - wrist_pitch_last_set_position) / 2; + wrist_roll_set_position -= roll_pitch_position_difference; + wrist_pitch_last_set_position = wrist_pitch_set_position; + + constrain_set_positions(); + + should_clear_faults = (msg->clear_faults); + should_reset = (msg->reset_controllers); + should_calibrate = msg->calibrate; + + if(!should_calibrate && !should_reset) { + new_positions_received = true; + } + + } + private: ros::NodeHandle *node_handle; string arm_port; smbus arm_bus_handle; + bool arm_successfully_connected = false; + ros::Publisher status_publisher; + rover_arm::ArmStatusMessage status_message; + + ros::Subscriber absolute_position_control_subscriber; + ros::Subscriber relative_position_control_subscriber; + + bool received_first_joint_position_update = false; + + bool new_positions_received = false; + + smint32 base_set_position = 0; + smint32 base_current_position = 0; + smint32 base_comm_state = 0; + smint32 base_status = 0; + smint32 base_faults = 0; + + smint32 shoulder_set_position = 0; + smint32 shoulder_current_position = 0; + smint32 shoulder_comm_state = 0; + smint32 shoulder_status = 0; + smint32 shoulder_faults = 0; + + smint32 elbow_set_position = 0; + smint32 elbow_current_position = 0; + smint32 elbow_comm_state = 0; + smint32 elbow_status = 0; + smint32 elbow_faults = 0; + + smint32 roll_set_position = 0; + smint32 roll_current_position = 0; + smint32 roll_comm_state = 0; + smint32 roll_status = 0; + smint32 roll_faults = 0; + + smint32 wrist_pitch_last_set_position = 0; + smint32 wrist_pitch_set_position = 0; + smint32 wrist_pitch_current_position = 0; + smint32 wrist_pitch_comm_state = 0; + smint32 wrist_pitch_status = 0; + smint32 wrist_pitch_faults = 0; + + smint32 wrist_roll_set_position = 0; + smint32 wrist_roll_current_position = 0; + smint32 wrist_roll_comm_state = 0; + smint32 wrist_roll_status = 0; + smint32 wrist_roll_faults = 0; + + bool should_calibrate = false; + bool should_clear_faults = true; + bool should_reset = false; }; - -int main(int argc, char** argv){ +int main(int argc, char **argv) { RoverArm rover_arm(argc, argv); rover_arm.run(); } \ No newline at end of file diff --git a/software/ros_packages/rover_arm/src/simplemotion/CMakeLists.txt b/software/ros_packages/rover_arm/src/simplemotion/CMakeLists.txt index 56b849a..c3a25f1 100644 --- a/software/ros_packages/rover_arm/src/simplemotion/CMakeLists.txt +++ b/software/ros_packages/rover_arm/src/simplemotion/CMakeLists.txt @@ -1,3 +1,3 @@ -add_library(simplemotion rs232.c simplemotion.c busdevice.c sm_consts.c) +add_library(simplemotion2 rs232.c simplemotion.c busdevice.c sm_consts.c) -target_include_directories (simplemotion PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) \ No newline at end of file +target_include_directories (simplemotion2 PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) \ No newline at end of file diff --git a/software/ros_packages/rover_arm/src/simplemotion/busdevice.c b/software/ros_packages/rover_arm/src/simplemotion/busdevice.c index 9bf82c8..5b43ddc 100644 --- a/software/ros_packages/rover_arm/src/simplemotion/busdevice.c +++ b/software/ros_packages/rover_arm/src/simplemotion/busdevice.c @@ -63,7 +63,7 @@ smbusdevicehandle smBDOpen( const char *devicename ) //all handles in use if(handle>=SM_MAX_BUSES) return -1; - if(strncmp(devicename,"COM",3) == 0 || strncmp(devicename,"/dev/tty",8) == 0) //use rs232 lib + if(strncmp(devicename,"COM",3) == 0 || strncmp(devicename,"/dev/tty",8) == 0 || strncmp(devicename,"/dev/rover/tty",8) == 0) { BusDevice[handle].comPort=OpenComport( devicename, SM_BAUDRATE ); if( BusDevice[handle].comPort == -1 ) diff --git a/software/ros_packages/rover_arm_moveit_config/.setup_assistant b/software/ros_packages/rover_arm_moveit_config/.setup_assistant new file mode 100644 index 0000000..ec235b8 --- /dev/null +++ b/software/ros_packages/rover_arm_moveit_config/.setup_assistant @@ -0,0 +1,11 @@ +moveit_setup_assistant_config: + URDF: + package: mr1718-arm-urdf_export + relative_path: robots/mr1718-arm-urdf_export.urdf + xacro_args: "--inorder " + SRDF: + relative_path: config/mr1718-arm-urdf_export.srdf + CONFIG: + author_name: Corwin Perren + author_email: caperren@gmail.com + generated_timestamp: 1533095742 \ No newline at end of file diff --git a/software/ros_packages/rover_arm_moveit_config/CMakeLists.txt b/software/ros_packages/rover_arm_moveit_config/CMakeLists.txt new file mode 100644 index 0000000..1a7fe67 --- /dev/null +++ b/software/ros_packages/rover_arm_moveit_config/CMakeLists.txt @@ -0,0 +1,10 @@ +cmake_minimum_required(VERSION 2.8.3) +project(rover_arm_moveit_config) + +find_package(catkin REQUIRED) + +catkin_package() + +install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + PATTERN "setup_assistant.launch" EXCLUDE) +install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/software/ros_packages/rover_arm_moveit_config/config/fake_controllers.yaml b/software/ros_packages/rover_arm_moveit_config/config/fake_controllers.yaml new file mode 100644 index 0000000..44edbbc --- /dev/null +++ b/software/ros_packages/rover_arm_moveit_config/config/fake_controllers.yaml @@ -0,0 +1,9 @@ +controller_list: + - name: fake_manipulator_controller + joints: + - shoulder + - elbow + - roll + - wrist_base + - wrist_pitch + - wrist_roll \ No newline at end of file diff --git a/software/ros_packages/rover_arm_moveit_config/config/joint_limits.yaml b/software/ros_packages/rover_arm_moveit_config/config/joint_limits.yaml new file mode 100644 index 0000000..0ca9c75 --- /dev/null +++ b/software/ros_packages/rover_arm_moveit_config/config/joint_limits.yaml @@ -0,0 +1,34 @@ +# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed +# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] +# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] +joint_limits: + elbow: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + roll: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + shoulder: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + wrist_base: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + wrist_pitch: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + wrist_roll: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 \ No newline at end of file diff --git a/software/ros_packages/rover_arm_moveit_config/config/kinematics.yaml b/software/ros_packages/rover_arm_moveit_config/config/kinematics.yaml new file mode 100644 index 0000000..969ba14 --- /dev/null +++ b/software/ros_packages/rover_arm_moveit_config/config/kinematics.yaml @@ -0,0 +1,5 @@ +manipulator: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 + kinematics_solver_attempts: 3 \ No newline at end of file diff --git a/software/ros_packages/rover_arm_moveit_config/config/mr1718-arm-urdf_export.srdf b/software/ros_packages/rover_arm_moveit_config/config/mr1718-arm-urdf_export.srdf new file mode 100644 index 0000000..0cbf779 --- /dev/null +++ b/software/ros_packages/rover_arm_moveit_config/config/mr1718-arm-urdf_export.srdf @@ -0,0 +1,48 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/software/ros_packages/rover_arm_moveit_config/config/ompl_planning.yaml b/software/ros_packages/rover_arm_moveit_config/config/ompl_planning.yaml new file mode 100644 index 0000000..3d19aab --- /dev/null +++ b/software/ros_packages/rover_arm_moveit_config/config/ompl_planning.yaml @@ -0,0 +1,150 @@ +planner_configs: + SBL: + type: geometric::SBL + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + EST: + type: geometric::EST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LBKPIECE: + type: geometric::LBKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + BKPIECE: + type: geometric::BKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + KPIECE: + type: geometric::KPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.] + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + RRT: + type: geometric::RRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + RRTConnect: + type: geometric::RRTConnect + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + RRTstar: + type: geometric::RRTstar + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1 + TRRT: + type: geometric::TRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + max_states_failed: 10 # when to start increasing temp. default: 10 + temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0 + min_temperature: 10e-10 # lower limit of temp change. default: 10e-10 + init_temperature: 10e-6 # initial temperature. default: 10e-6 + frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + k_constant: 0.0 # value used to normalize expresssion. default: 0.0 set in setup() + PRM: + type: geometric::PRM + max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 + PRMstar: + type: geometric::PRMstar + FMT: + type: geometric::FMT + num_samples: 1000 # number of states that the planner should sample. default: 1000 + radius_multiplier: 1.1 # multiplier used for the nearest neighbors search radius. default: 1.1 + nearest_k: 1 # use Knearest strategy. default: 1 + cache_cc: 1 # use collision checking cache. default: 1 + heuristics: 0 # activate cost to go heuristics. default: 0 + extended_fmt: 1 # activate the extended FMT*: adding new samples if planner does not finish successfully. default: 1 + BFMT: + type: geometric::BFMT + num_samples: 1000 # number of states that the planner should sample. default: 1000 + radius_multiplier: 1.0 # multiplier used for the nearest neighbors search radius. default: 1.0 + nearest_k: 1 # use the Knearest strategy. default: 1 + balanced: 0 # exploration strategy: balanced true expands one tree every iteration. False will select the tree with lowest maximum cost to go. default: 1 + optimality: 1 # termination strategy: optimality true finishes when the best possible path is found. Otherwise, the algorithm will finish when the first feasible path is found. default: 1 + heuristics: 1 # activates cost to go heuristics. default: 1 + cache_cc: 1 # use the collision checking cache. default: 1 + extended_fmt: 1 # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1 + PDST: + type: geometric::PDST + STRIDE: + type: geometric::STRIDE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + use_projected_distance: 0 # whether nearest neighbors are computed based on distances in a projection of the state rather distances in the state space itself. default: 0 + degree: 16 # desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). default: 16 + max_degree: 18 # max degree of a node in the GNAT. default: 12 + min_degree: 12 # min degree of a node in the GNAT. default: 12 + max_pts_per_leaf: 6 # max points per leaf in the GNAT. default: 6 + estimated_dimension: 0.0 # estimated dimension of the free space. default: 0.0 + min_valid_path_fraction: 0.2 # Accept partially valid moves above fraction. default: 0.2 + BiTRRT: + type: geometric::BiTRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + temp_change_factor: 0.1 # how much to increase or decrease temp. default: 0.1 + init_temperature: 100 # initial temperature. default: 100 + frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frountier_node_ratio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + cost_threshold: 1e300 # the cost threshold. Any motion cost that is not better will not be expanded. default: inf + LBTRRT: + type: geometric::LBTRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + epsilon: 0.4 # optimality approximation factor. default: 0.4 + BiEST: + type: geometric::BiEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + ProjEST: + type: geometric::ProjEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LazyPRM: + type: geometric::LazyPRM + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + LazyPRMstar: + type: geometric::LazyPRMstar + SPARS: + type: geometric::SPARS + stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 + sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 + dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 + max_failures: 1000 # maximum consecutive failure limit. default: 1000 + SPARStwo: + type: geometric::SPARStwo + stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 + sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 + dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 + max_failures: 5000 # maximum consecutive failure limit. default: 5000 +manipulator: + default_planner_config: NonekConfigDefault + planner_configs: + - SBLkConfigDefault + - ESTkConfigDefault + - LBKPIECEkConfigDefault + - BKPIECEkConfigDefault + - KPIECEkConfigDefault + - RRTkConfigDefault + - RRTConnectkConfigDefault + - RRTstarkConfigDefault + - TRRTkConfigDefault + - PRMkConfigDefault + - PRMstarkConfigDefault + - FMTkConfigDefault + - BFMTkConfigDefault + - PDSTkConfigDefault + - STRIDEkConfigDefault + - BiTRRTkConfigDefault + - LBTRRTkConfigDefault + - BiESTkConfigDefault + - ProjESTkConfigDefault + - LazyPRMkConfigDefault + - LazyPRMstarkConfigDefault + - SPARSkConfigDefault + - SPARStwokConfigDefault + projection_evaluator: joints(shoulder,elbow) + longest_valid_segment_fraction: 0.005 \ No newline at end of file diff --git a/software/ros_packages/rover_arm_moveit_config/launch/default_warehouse_db.launch b/software/ros_packages/rover_arm_moveit_config/launch/default_warehouse_db.launch new file mode 100644 index 0000000..f13f03b --- /dev/null +++ b/software/ros_packages/rover_arm_moveit_config/launch/default_warehouse_db.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/software/ros_packages/rover_arm_moveit_config/launch/demo.launch b/software/ros_packages/rover_arm_moveit_config/launch/demo.launch new file mode 100644 index 0000000..d7f52fa --- /dev/null +++ b/software/ros_packages/rover_arm_moveit_config/launch/demo.launch @@ -0,0 +1,57 @@ + + + + + + + + + + + + + + + + + + + + + + + + + [move_group/fake_controller_joint_states] + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/software/ros_packages/rover_arm_moveit_config/launch/fake_moveit_controller_manager.launch.xml b/software/ros_packages/rover_arm_moveit_config/launch/fake_moveit_controller_manager.launch.xml new file mode 100644 index 0000000..eaf1080 --- /dev/null +++ b/software/ros_packages/rover_arm_moveit_config/launch/fake_moveit_controller_manager.launch.xml @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/software/ros_packages/rover_arm_moveit_config/launch/joystick_control.launch b/software/ros_packages/rover_arm_moveit_config/launch/joystick_control.launch new file mode 100644 index 0000000..9411f6e --- /dev/null +++ b/software/ros_packages/rover_arm_moveit_config/launch/joystick_control.launch @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/software/ros_packages/rover_arm_moveit_config/launch/move_group.launch b/software/ros_packages/rover_arm_moveit_config/launch/move_group.launch new file mode 100644 index 0000000..d92707d --- /dev/null +++ b/software/ros_packages/rover_arm_moveit_config/launch/move_group.launch @@ -0,0 +1,72 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/software/ros_packages/rover_arm_moveit_config/launch/moveit.rviz b/software/ros_packages/rover_arm_moveit_config/launch/moveit.rviz new file mode 100644 index 0000000..0877e1e --- /dev/null +++ b/software/ros_packages/rover_arm_moveit_config/launch/moveit.rviz @@ -0,0 +1,689 @@ +Panels: + - Class: rviz/Displays + Help Height: 84 + Name: Displays + Property Tree Widget: + Expanded: + - /MotionPlanning1 + Splitter Ratio: 0.74256 + Tree Height: 664 + - Class: rviz/Help + Name: Help + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + MoveIt_Goal_Tolerance: 0 + MoveIt_Planning_Time: 5 + MoveIt_Use_Constraint_Aware_IK: true + MoveIt_Warehouse_Host: 127.0.0.1 + MoveIt_Warehouse_Port: 33829 + Name: MotionPlanning + Planned Path: + Links: + base_bellow_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + double_stereo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_kinect_ir_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_kinect_rgb_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_prosilica_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_plate_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_tilt_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_elbow_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_forearm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_l_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_l_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_motor_accelerometer_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_palm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_r_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_r_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_shoulder_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_shoulder_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_upper_arm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_wrist_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_wrist_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + laser_tilt_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_elbow_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_forearm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_l_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_l_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_motor_accelerometer_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_palm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_r_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_r_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_shoulder_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_shoulder_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_upper_arm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_wrist_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_wrist_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sensor_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + torso_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Loop Animation: true + Robot Alpha: 0.5 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 0.05 s + Trajectory Topic: move_group/display_planned_path + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0 + Joint Violation Color: 255; 0; 255 + Planning Group: left_arm + Query Goal State: true + Query Start State: false + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 + Planning Scene Topic: move_group/monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 1 + Scene Color: 50; 230; 50 + Scene Display Time: 0.2 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + base_bellow_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + double_stereo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_kinect_ir_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_kinect_rgb_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_prosilica_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_plate_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_tilt_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_elbow_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_forearm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_l_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_l_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_motor_accelerometer_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_palm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_r_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_r_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_shoulder_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_shoulder_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_upper_arm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_wrist_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_wrist_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + laser_tilt_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_elbow_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_forearm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_l_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_l_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_motor_accelerometer_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_palm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_r_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_r_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_shoulder_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_shoulder_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_upper_arm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_wrist_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_wrist_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sensor_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + torso_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Robot Alpha: 0.5 + Show Scene Robot: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: /base_link + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + Value: true + Views: + Current: + Class: rviz/XYOrbit + Distance: 2.9965 + Focal Point: + X: 0.113567 + Y: 0.10592 + Z: 2.23518e-07 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 0.509797 + Target Frame: /base_link + Value: XYOrbit (rviz) + Yaw: 5.65995 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1337 + Help: + collapsed: false + Hide Left Dock: false + Hide Right Dock: false + Motion Planning: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000100000000000002a2000004bcfc0200000005fb000000100044006900730070006c00610079007301000000410000032d000000dd00fffffffb0000000800480065006c00700000000342000000bb0000007600fffffffb0000000a0056006900650077007300000003b0000000b0000000b000fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000017400ffffff0000047a000004bc00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Views: + collapsed: false + Width: 1828 + X: 459 + Y: -243 diff --git a/software/ros_packages/rover_arm_moveit_config/launch/moveit_rviz.launch b/software/ros_packages/rover_arm_moveit_config/launch/moveit_rviz.launch new file mode 100644 index 0000000..94d0ec8 --- /dev/null +++ b/software/ros_packages/rover_arm_moveit_config/launch/moveit_rviz.launch @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + diff --git a/software/ros_packages/rover_arm_moveit_config/launch/mr1718-arm-urdf_export_moveit_controller_manager.launch.xml b/software/ros_packages/rover_arm_moveit_config/launch/mr1718-arm-urdf_export_moveit_controller_manager.launch.xml new file mode 100644 index 0000000..5d02698 --- /dev/null +++ b/software/ros_packages/rover_arm_moveit_config/launch/mr1718-arm-urdf_export_moveit_controller_manager.launch.xml @@ -0,0 +1,3 @@ + + + diff --git a/software/ros_packages/rover_arm_moveit_config/launch/mr1718-arm-urdf_export_moveit_sensor_manager.launch.xml b/software/ros_packages/rover_arm_moveit_config/launch/mr1718-arm-urdf_export_moveit_sensor_manager.launch.xml new file mode 100644 index 0000000..5d02698 --- /dev/null +++ b/software/ros_packages/rover_arm_moveit_config/launch/mr1718-arm-urdf_export_moveit_sensor_manager.launch.xml @@ -0,0 +1,3 @@ + + + diff --git a/software/ros_packages/rover_arm_moveit_config/launch/ompl_planning_pipeline.launch.xml b/software/ros_packages/rover_arm_moveit_config/launch/ompl_planning_pipeline.launch.xml new file mode 100644 index 0000000..5e9b6b8 --- /dev/null +++ b/software/ros_packages/rover_arm_moveit_config/launch/ompl_planning_pipeline.launch.xml @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + diff --git a/software/ros_packages/rover_arm_moveit_config/launch/planning_context.launch b/software/ros_packages/rover_arm_moveit_config/launch/planning_context.launch new file mode 100644 index 0000000..e4de20a --- /dev/null +++ b/software/ros_packages/rover_arm_moveit_config/launch/planning_context.launch @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/software/ros_packages/rover_arm_moveit_config/launch/planning_pipeline.launch.xml b/software/ros_packages/rover_arm_moveit_config/launch/planning_pipeline.launch.xml new file mode 100644 index 0000000..ba55f5d --- /dev/null +++ b/software/ros_packages/rover_arm_moveit_config/launch/planning_pipeline.launch.xml @@ -0,0 +1,10 @@ + + + + + + + + + diff --git a/software/ros_packages/rover_arm_moveit_config/launch/run_benchmark_ompl.launch b/software/ros_packages/rover_arm_moveit_config/launch/run_benchmark_ompl.launch new file mode 100644 index 0000000..3fb26ce --- /dev/null +++ b/software/ros_packages/rover_arm_moveit_config/launch/run_benchmark_ompl.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/software/ros_packages/rover_arm_moveit_config/launch/sensor_manager.launch.xml b/software/ros_packages/rover_arm_moveit_config/launch/sensor_manager.launch.xml new file mode 100644 index 0000000..302e764 --- /dev/null +++ b/software/ros_packages/rover_arm_moveit_config/launch/sensor_manager.launch.xml @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/software/ros_packages/rover_arm_moveit_config/launch/setup_assistant.launch b/software/ros_packages/rover_arm_moveit_config/launch/setup_assistant.launch new file mode 100644 index 0000000..71995a5 --- /dev/null +++ b/software/ros_packages/rover_arm_moveit_config/launch/setup_assistant.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + diff --git a/software/ros_packages/rover_arm_moveit_config/launch/trajectory_execution.launch.xml b/software/ros_packages/rover_arm_moveit_config/launch/trajectory_execution.launch.xml new file mode 100644 index 0000000..c940ca7 --- /dev/null +++ b/software/ros_packages/rover_arm_moveit_config/launch/trajectory_execution.launch.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/software/ros_packages/rover_arm_moveit_config/launch/warehouse.launch b/software/ros_packages/rover_arm_moveit_config/launch/warehouse.launch new file mode 100644 index 0000000..4017269 --- /dev/null +++ b/software/ros_packages/rover_arm_moveit_config/launch/warehouse.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/software/ros_packages/rover_arm_moveit_config/launch/warehouse_settings.launch.xml b/software/ros_packages/rover_arm_moveit_config/launch/warehouse_settings.launch.xml new file mode 100644 index 0000000..e473b08 --- /dev/null +++ b/software/ros_packages/rover_arm_moveit_config/launch/warehouse_settings.launch.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/software/ros_packages/rover_arm_moveit_config/package.xml b/software/ros_packages/rover_arm_moveit_config/package.xml new file mode 100644 index 0000000..5d63569 --- /dev/null +++ b/software/ros_packages/rover_arm_moveit_config/package.xml @@ -0,0 +1,33 @@ + + + rover_arm_moveit_config + 0.3.0 + + An automatically generated package with all the configuration and launch files for using the mr1718-arm-urdf_export with the MoveIt! Motion Planning Framework + + Corwin Perren + Corwin Perren + + BSD + + http://moveit.ros.org/ + https://github.com/ros-planning/moveit/issues + https://github.com/ros-planning/moveit + + catkin + + moveit_ros_move_group + moveit_fake_controller_manager + moveit_kinematics + moveit_planners_ompl + moveit_ros_visualization + joint_state_publisher + robot_state_publisher + xacro + + + mr1718-arm-urdf_export + mr1718-arm-urdf_export + + + 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 100755 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_receivers.launch b/software/ros_packages/rover_main/launch/ground_station/topic_transport_receivers.launch index 9dee88a..c9fcb96 100644 --- a/software/ros_packages/rover_main/launch/ground_station/topic_transport_receivers.launch +++ b/software/ros_packages/rover_main/launch/ground_station/topic_transport_receivers.launch @@ -79,5 +79,9 @@ + + + + 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..ff6ba20 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,9 @@ [{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: 30.0}, + {name: "/rover_arm/control/relative", compress: true, rate: 30.0}] @@ -20,7 +22,8 @@ {name: "/cameras/undercarriage/camera_control", compress: false, rate: 5.0}, {name: "/cameras/main_navigation/camera_control", compress: false, rate: 5.0}, {name: "/cameras/end_effector/camera_control", compress: false, rate: 5.0}, - {name: "/rover_status/update_requested", compress: false, rate: 5.0},] + {name: "/rover_status/update_requested", compress: false, rate: 5.0}, + {name: "/rover_arm/control/absolute", compress: true, rate: 5.0}] diff --git a/software/ros_packages/rover_main/launch/rover.launch b/software/ros_packages/rover_main/launch/rover.launch index 25274f9..e86b82b 100644 --- a/software/ros_packages/rover_main/launch/rover.launch +++ b/software/ros_packages/rover_main/launch/rover.launch @@ -1,6 +1,8 @@ + + diff --git a/software/ros_packages/rover_main/launch/rover/arm.launch b/software/ros_packages/rover_main/launch/rover/arm.launch new file mode 100644 index 0000000..6f81c28 --- /dev/null +++ b/software/ros_packages/rover_main/launch/rover/arm.launch @@ -0,0 +1,3 @@ + + + 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/science.launch b/software/ros_packages/rover_main/launch/rover/science.launch new file mode 100644 index 0000000..9e88337 --- /dev/null +++ b/software/ros_packages/rover_main/launch/rover/science.launch @@ -0,0 +1,3 @@ + + + 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..3cdb67e 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,18 @@ {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: 5.0}, + {name: "/rdf/data", compress: false, rate: 50.0} + ] + + + + + + + + [ + {name: "/rover_arm/status", compress: false, rate: 5.0} ] diff --git a/software/ros_packages/rover_science/CMakeLists.txt b/software/ros_packages/rover_science/CMakeLists.txt new file mode 100644 index 0000000..f0a18b3 --- /dev/null +++ b/software/ros_packages/rover_science/CMakeLists.txt @@ -0,0 +1,204 @@ +cmake_minimum_required(VERSION 2.8.3) +project(rover_science) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + roscpp + rospy + std_msgs + message_generation +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a run_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +add_message_files( + FILES +) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +generate_messages( + DEPENDENCIES + std_msgs +) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a run_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES system_statuses +# CATKIN_DEPENDS roscpp rospy std_msgs +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/system_statuses.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +# add_executable(${PROJECT_NAME}_node src/system_statuses_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_system_statuses.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/software/ros_packages/rover_science/msg/RDFStatusMessage.msg b/software/ros_packages/rover_science/msg/RDFStatusMessage.msg new file mode 100644 index 0000000..1b39a87 --- /dev/null +++ b/software/ros_packages/rover_science/msg/RDFStatusMessage.msg @@ -0,0 +1,2 @@ +uint16 raw_value +double time \ No newline at end of file diff --git a/software/ros_packages/rover_science/package.xml b/software/ros_packages/rover_science/package.xml new file mode 100644 index 0000000..e4bbd2a --- /dev/null +++ b/software/ros_packages/rover_science/package.xml @@ -0,0 +1,68 @@ + + + rover_science + 0.0.0 + The rover_status package + + + + + matcurtay + + + + + + BSD + + + + + + + + + + + + + + + + + + + + + + + + message_generation + + + + + + message_runtime + + + + + catkin + roscpp + rospy + std_msgs + roscpp + rospy + std_msgs + roscpp + rospy + std_msgs + + + + + + + + diff --git a/software/ros_packages/rover_science/src/freq_estimates.py b/software/ros_packages/rover_science/src/freq_estimates.py new file mode 100644 index 0000000..8bc1604 --- /dev/null +++ b/software/ros_packages/rover_science/src/freq_estimates.py @@ -0,0 +1,128 @@ +from __future__ import division +from numpy.fft import rfft +from numpy import argmax, mean, diff, log +from matplotlib.mlab import find +from scipy.signal import blackmanharris, fftconvolve +from numpy import polyfit, arange + + +def freq_from_crossings(sig, fs): + """ + Estimate frequency by counting zero crossings + """ + # Find all indices right before a rising-edge zero crossing + indices = find((sig[1:] >= 0) & (sig[:-1] < 0)) + + # Naive (Measures 1000.185 Hz for 1000 Hz, for instance) + # crossings = indices + + # More accurate, using linear interpolation to find intersample + # zero-crossings (Measures 1000.000129 Hz for 1000 Hz, for instance) + crossings = [i - sig[i] / (sig[i+1] - sig[i]) for i in indices] + + # Some other interpolation based on neighboring points might be better. + # Spline, cubic, whatever + + return fs / mean(diff(crossings)) + + +def freq_from_fft(sig, fs): + """ + Estimate frequency from peak of FFT + """ + # Compute Fourier transform of windowed signal + windowed = sig * blackmanharris(len(sig)) + f = rfft(windowed) + + # Find the peak and interpolate to get a more accurate peak + i = argmax(abs(f)) # Just use this for less-accurate, naive version + true_i = parabolic(log(abs(f)), i)[0] + + # Convert to equivalent frequency + return fs * true_i / len(windowed) + + +def freq_from_autocorr(sig, fs): + """ + Estimate frequency using autocorrelation + """ + # Calculate autocorrelation (same thing as convolution, but with + # one input reversed in time), and throw away the negative lags + corr = fftconvolve(sig, sig[::-1], mode='full') + corr = corr[len(corr)//2:] + + # Find the first low point + d = diff(corr) + start = find(d > 0)[0] + + # Find the next peak after the low point (other than 0 lag). This bit is + # not reliable for long signals, due to the desired peak occurring between + # samples, and other peaks appearing higher. + # Should use a weighting function to de-emphasize the peaks at longer lags. + peak = argmax(corr[start:]) + start + px, py = parabolic(corr, peak) + + return fs / px + + +def freq_from_HPS(sig, fs): + """ + Estimate frequency using harmonic product spectrum (HPS) + """ + windowed = sig * blackmanharris(len(sig)) + + from pylab import subplot, plot, log, copy, show + + # harmonic product spectrum: + c = abs(rfft(windowed)) + maxharms = 8 + subplot(maxharms, 1, 1) + plot(log(c)) + for x in range(2, maxharms): + a = copy(c[::x]) # Should average or maximum instead of decimating + # max(c[::x],c[1::x],c[2::x],...) + c = c[:len(a)] + i = argmax(abs(c)) + true_i = parabolic(abs(c), i)[0] + print 'Pass %d: %f Hz' % (x, fs * true_i / len(windowed)) + c *= a + subplot(maxharms, 1, x) + plot(log(c)) + show() + + +def parabolic(f, x): + """Quadratic interpolation for estimating the true position of an + inter-sample maximum when nearby samples are known. + + f is a vector and x is an index for that vector. + + Returns (vx, vy), the coordinates of the vertex of a parabola that goes + through point x and its two neighbors. + + Example: + Defining a vector f with a local maximum at index 3 (= 6), find local + maximum if points 2, 3, and 4 actually defined a parabola. + + In [3]: f = [2, 3, 1, 6, 4, 2, 3, 1] + + In [4]: parabolic(f, argmax(f)) + Out[4]: (3.2142857142857144, 6.1607142857142856) + + """ + xv = 1 / 2. * (f[x - 1] - f[x + 1]) / (f[x - 1] - 2 * f[x] + f[x + 1]) + x + yv = f[x] - 1 / 4. * (f[x - 1] - f[x + 1]) * (xv - x) + return (xv, yv) + + +def parabolic_polyfit(f, x, n): + """Use the built-in polyfit() function to find the peak of a parabola + + f is a vector and x is an index for that vector. + + n is the number of samples of the curve used to fit the parabola. + """ + a, b, c = polyfit(arange(x - n // 2, x + n // 2 + 1), f[x - n // 2:x + n // 2 + 1], 2) + xv = -0.5 * b / a + yv = a * xv ** 2 + b * xv + c + return (xv, yv) diff --git a/software/ros_packages/rover_science/src/rover_science.py b/software/ros_packages/rover_science/src/rover_science.py new file mode 100755 index 0000000..c2562b4 --- /dev/null +++ b/software/ros_packages/rover_science/src/rover_science.py @@ -0,0 +1,197 @@ +#!/usr/bin/env python +##################################### +# Imports +##################################### +# Python native imports +import rospy + +from time import time, sleep + +import serial.rs485 +import minimalmodbus +import numpy + +# Custom Imports +from rover_control.msg import TowerPanTiltControlMessage +from std_msgs.msg import Float64MultiArray + +##################################### +# Global Variables +##################################### +NODE_NAME = "science_node" + +DEFAULT_PORT = "/dev/rover/ttyIRIS_2_0" +DEFAULT_BAUD = 115200 + +DEFAULT_INVERT = False + +DEFAULT_RDF_PUBLISHER_TOPIC = "rdf/data" + +PAN_TILT_NODE_ID = 1 + +COMMUNICATIONS_TIMEOUT = 0.005 # Seconds + +RX_DELAY = 0.01 +TX_DELAY = 0.01 + +DEFAULT_HERTZ = 20 + +PAN_TILT_MODBUS_REGISTERS = { + "CENTER_ALL": 0, + + "PAN_ADJUST_POSITIVE": 1, + "PAN_ADJUST_NEGATIVE": 2, + "TILT_ADJUST_POSITIVE": 3, + "TILT_ADJUST_NEGATIVE": 4 +} + +NODE_LAST_SEEN_TIMEOUT = 2 # seconds + + +##################################### +# DriveControl Class Definition +##################################### +class RoverScience(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.pan_tilt_node_id = rospy.get_param("~pan_tilt_node_id", PAN_TILT_NODE_ID) + + self.wait_time = 1.0 / rospy.get_param("~hertz", DEFAULT_HERTZ) + + self.pan_tilt_node = None + self.tower_node = None + + self.connect_to_pan_tilt_and_tower() + + self.rdf_publisher = rospy.Publisher(DEFAULT_RDF_PUBLISHER_TOPIC, Float64MultiArray, queue_size=1) + + self.pan_tilt_control_message = None + self.new_pan_tilt_control_message = False + + self.modbus_nodes_seen_time = time() + + self.data = numpy.array([]) + self.times = numpy.array([]) + self.max_data_len = 200 + + self.count = 0 + self.start_time = time() + + self.run() + + def __setup_minimalmodbus_for_485(self): + self.pan_tilt_node.serial = serial.rs485.RS485(self.port, baudrate=self.baud, timeout=COMMUNICATIONS_TIMEOUT) + self.pan_tilt_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.send_startup_centering_command() + while not rospy.is_shutdown(): + start_time = time() + try: + registers = self.pan_tilt_node.read_registers(0, 1) + self.rdf_publisher.publish(Float64MultiArray(data=[registers[0], time()])) + self.modbus_nodes_seen_time = time() + except Exception, Error: + print Error + + if (time() - self.modbus_nodes_seen_time) > NODE_LAST_SEEN_TIMEOUT: + print "Science not seen for", NODE_LAST_SEEN_TIMEOUT, "seconds. Exiting." + return # Exit so respawn can take over + + time_diff = time() - start_time + + # def run(self): + # # self.send_startup_centering_command() + # while not rospy.is_shutdown(): + # registers = self.pan_tilt_node.read_registers(0, 1) + # self.data = numpy.append(self.data, registers[0]) + # self.times = numpy.append(self.times, time()) + # + # if len(self.data) > self.max_data_len: + # numpy.delete(self.data, 0) + # numpy.delete(self.times, 0) + # + # # for item in self.smoothListTriangle(self.data): + # # print item + # + # print fq.freq_from_fft(self.data, 1/40.0) + # # self.data = self.smoothListGaussian(self.data) + # # + # # w = numpy.fft.fft(self.data) + # # + # # # for item in w: + # # # print abs(item) + # # # + # # # print + # # # print + # # numpy.delete(w, 0) + # # freqs = numpy.fft.fftfreq(len(w), 1/40.0) + # # # # print len(freqs), len(w) + # # # + # # # # print freqs + # # # # print(freqs.min(), freqs.max()) + # # # # # (-0.5, 0.499975) + # # # # + # # # # # Find the peak in the coefficients + # # idx = numpy.argmax(numpy.abs(w)) + # # freq = freqs[idx] + # # freq_in_hertz = abs(freq * 40) + # # print(freq_in_hertz) + + + def smoothListTriangle(self, list, strippedXs=False, degree=5): + + weight = [] + + window = degree * 2 - 1 + + smoothed = [0.0] * (len(list) - window) + + for x in range(1, 2 * degree): weight.append(degree - abs(degree - x)) + + w = numpy.array(weight) + + for i in range(len(smoothed)): + smoothed[i] = sum(numpy.array(list[i:i + window]) * w) / float(sum(w)) + + return smoothed + + def smoothListGaussian(self, list, strippedXs=False, degree=5): + + window = degree * 2 - 1 + + weight = numpy.array([1.0] * window) + + weightGauss = [] + + for i in range(window): + i = i - degree + 1 + + frac = i / float(window) + + gauss = 1 / (numpy.exp((4 * (frac)) ** 2)) + + weightGauss.append(gauss) + + weight = numpy.array(weightGauss) * weight + + smoothed = [0.0] * (len(list) - window) + + for i in range(len(smoothed)): + smoothed[i] = sum(numpy.array(list[i:i + window]) * weight) / sum(weight) + + return smoothed + + def connect_to_pan_tilt_and_tower(self): + self.pan_tilt_node = minimalmodbus.Instrument(self.port, int(self.pan_tilt_node_id)) + self.__setup_minimalmodbus_for_485() + + +if __name__ == "__main__": + RoverScience() diff --git a/software/rover_setup.sh b/software/rover_setup.sh index 2209c80..0845072 100755 --- a/software/rover_setup.sh +++ b/software/rover_setup.sh @@ -15,6 +15,7 @@ folders_to_link=( nimbro_topic_transport rover_main rover_odometry + rover_science ) # Print heading 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