diff --git a/electrical/documents/ioni/base.drc b/electrical/documents/ioni/base.drc index 4a3d058..24e6065 100644 --- a/electrical/documents/ioni/base.drc +++ b/electrical/documents/ioni/base.drc @@ -153,7 +153,7 @@ GranityVersionInt=11400 19\scaling=1 19\offset=0 19\readonly=false -19\value=-29203491 +19\value=-19123927 19\min=-536870912 19\max=536870911 20\name=FBR @@ -393,7 +393,7 @@ GranityVersionInt=11400 49\scaling=1 49\offset=0 49\readonly=false -49\value=3 +49\value=5 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=0 +50\value=10 50\min=-1000000 50\max=100 51\name=CRI 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 3b55fda..199d539 100644 --- a/electrical/documents/ioni/shoulder.drc +++ b/electrical/documents/ioni/shoulder.drc @@ -153,7 +153,7 @@ GranityVersionInt=11400 19\scaling=1 19\offset=0 19\readonly=false -19\value=340555 +19\value=28407 19\min=-536870912 19\max=536870911 20\name=FBR @@ -273,7 +273,7 @@ GranityVersionInt=11400 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=11400 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=11400 49\scaling=1 49\offset=0 49\readonly=false -49\value=1 +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=0 +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=870 +57\value=700 57\min=1 57\max=32767 58\name=CRV 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/firmware/rdf/rdf.ino b/software/firmware/rdf/rdf.ino new file mode 100644 index 0000000..6639da1 --- /dev/null +++ b/software/firmware/rdf/rdf.ino @@ -0,0 +1,153 @@ +////////// Includes ////////// +#include + +////////// Hardware / Data Enumerations ////////// +enum HARDWARE { + RS485_EN = 2, + RS485_RX = 7, + RS485_TX = 8, + + MOTOR_CURRENT_SENSE = A4, + MOTOR_DIRECTION = 19, + MOTOR_PWM = 20, + MOTOR_SLEEP = 21, + MOTOR_FAULT = 22, + + TEMP = A9, + + LED_RED = 1, + LED_GREEN = 32, + LED_BLUE = 6, + + LED_BLUE_EXTRA = 13 +}; + +enum MODBUS_REGISTERS { + DIRECTION = 0, // Input + SPEED = 1, // Input + SLEEP = 2, // Input + + CURRENT = 3, // Output + FAULT = 4, // Output + + TEMPERATURE = 5 // Output +}; + +////////// Global Variables ////////// +const uint8_t node_id = 2; +const uint8_t mobus_serial_port_number = 3; + +uint16_t modbus_data[] = {0, 0, 0, 0, 0, 0}; +uint8_t num_modbus_registers = 0; + +int8_t poll_state = 0; +bool communication_good = false; +uint8_t message_count = 0; + +uint16_t rampdown_step = 2000; + +////////// Class Instantiations ////////// +Modbus slave(node_id, mobus_serial_port_number, HARDWARE::RS485_EN); + +void setup() { + setup_hardware(); + + num_modbus_registers = sizeof(modbus_data) / sizeof(modbus_data[0]); + slave.begin(115200); // baud-rate at 19200 + slave.setTimeOut(150); + + Serial.begin(9600); +} + +void loop() { + poll_modbus(); + set_leds(); + set_motor(); + poll_sensors_and_motor_state(); +} + +void setup_hardware(){ + // Setup pins as inputs / outputs + pinMode(HARDWARE::RS485_EN, OUTPUT); + + pinMode(HARDWARE::MOTOR_CURRENT_SENSE, INPUT); + pinMode(HARDWARE::MOTOR_DIRECTION, OUTPUT); + pinMode(HARDWARE::MOTOR_PWM, OUTPUT); + pinMode(HARDWARE::MOTOR_SLEEP, OUTPUT); + pinMode(HARDWARE::MOTOR_FAULT, INPUT); + + pinMode(HARDWARE::TEMP, INPUT); + + pinMode(HARDWARE::LED_RED, OUTPUT); + pinMode(HARDWARE::LED_GREEN, OUTPUT); + pinMode(HARDWARE::LED_BLUE, OUTPUT); + + pinMode(HARDWARE::LED_BLUE_EXTRA, OUTPUT); + + // Set default pin states + digitalWrite(HARDWARE::MOTOR_SLEEP, HIGH); + + digitalWrite(HARDWARE::LED_RED, LOW); + digitalWrite(HARDWARE::LED_GREEN, HIGH); + digitalWrite(HARDWARE::LED_BLUE, HIGH); + + digitalWrite(HARDWARE::LED_BLUE_EXTRA, LOW); + + // Set the PWM resolution to 16-bits + analogWriteResolution(16); + + // Change motor PWM frequency so it's not in the audible range + analogWriteFrequency(HARDWARE::MOTOR_PWM, 25000); + + // Set teensy to increased analog resolution + analogReadResolution(13); +} + +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; + } + + digitalWrite(HARDWARE::LED_GREEN, LOW); + digitalWrite(HARDWARE::LED_RED, HIGH); + }else if(!communication_good){ + digitalWrite(HARDWARE::LED_BLUE_EXTRA, LOW); + digitalWrite(HARDWARE::LED_GREEN, HIGH); + digitalWrite(HARDWARE::LED_RED, LOW); + } +} + +void set_motor(){ + if(communication_good){ + digitalWrite(HARDWARE::MOTOR_DIRECTION, modbus_data[MODBUS_REGISTERS::DIRECTION]); + analogWrite(HARDWARE::MOTOR_PWM, modbus_data[MODBUS_REGISTERS::SPEED]); + digitalWrite(HARDWARE::MOTOR_SLEEP, modbus_data[MODBUS_REGISTERS::SLEEP]); + }else{ + while(modbus_data[MODBUS_REGISTERS::SPEED] != 0 && modbus_data[MODBUS_REGISTERS::SPEED] > rampdown_step){ + modbus_data[MODBUS_REGISTERS::SPEED] -= rampdown_step; + analogWrite(HARDWARE::MOTOR_PWM, modbus_data[MODBUS_REGISTERS::SPEED]); + delay(2); + } + + modbus_data[MODBUS_REGISTERS::SPEED] = 0; + analogWrite(HARDWARE::MOTOR_PWM, modbus_data[MODBUS_REGISTERS::SPEED]); + } + +} + +void poll_sensors_and_motor_state(){ + // Not the most elegant calculations, could clean up. + modbus_data[MODBUS_REGISTERS::CURRENT] = (uint16_t)(((((analogRead(HARDWARE::MOTOR_CURRENT_SENSE) / 8192.0) * 3.3) - 0.05) / 0.02) * 1000); + modbus_data[MODBUS_REGISTERS::FAULT] = !digitalRead(HARDWARE::MOTOR_FAULT); + modbus_data[MODBUS_REGISTERS::TEMPERATURE] = (uint16_t)(((((analogRead(HARDWARE::TEMP) / 8192.0) * 3.3) - 0.750) / 0.01) * 1000); + + Serial.println(modbus_data[MODBUS_REGISTERS::CURRENT]); +} 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() 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 / 32768.0) * BASE_SCALAR) * left_trigger_ratio + message.shoulder = (-(left_y_axis / 32768.0) * SHOULDER_SCALAR) * left_trigger_ratio + message.elbow = ((right_y_axis / 32768.0) * ELBOW_SCALAR) * left_trigger_ratio + message.roll = (-(right_x_axis / 32768.0) * ROLL_SCALAR) * left_trigger_ratio + + elif right_trigger > 25: + should_publish = True + message.wrist_roll = ((right_x_axis / 32768.0) * WRIST_ROLL_SCALAR) * right_trigger_ratio + message.wrist_pitch = ((left_y_axis / 32768.0) * WRIST_PITCH_SCALAR) * right_trigger_ratio + + if should_publish: + self.relative_arm_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) 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..105d57a --- /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.0] +] + +ARM_UNSTOW_PROCEDURE = [ + [0.0, -0.25, -0.5, -0.25, 0.25, 0.0], + [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/Resources/Ui/left_screen.ui b/software/ros_packages/ground_station/src/Resources/Ui/left_screen.ui index 71188a0..76a7b62 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,708 @@ N/A 0 + + + 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 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 5556771..85ae46b 100644 --- a/software/ros_packages/ground_station/src/ground_station.py +++ b/software/ros_packages/ground_station/src/ground_station.py @@ -26,6 +26,7 @@ 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 ##################################### # Global Variables @@ -105,6 +106,7 @@ class GroundStation(QtCore.QObject): # ##### 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)) @@ -112,13 +114,13 @@ class GroundStation(QtCore.QObject): self.__add_thread("Joystick Sender", JoystickControlSender.JoystickControlSender(self.shared_objects)) self.__add_thread("Controller Sender", ControllerControlSender.ControllerControlSender(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.connect_signals_and_slots_signal.emit() self.__connect_signals_to_slots() diff --git a/software/ros_packages/rover_arm/src/rover_arm.cpp b/software/ros_packages/rover_arm/src/rover_arm.cpp index 043a470..17ec2a9 100644 --- a/software/ros_packages/rover_arm/src/rover_arm.cpp +++ b/software/ros_packages/rover_arm/src/rover_arm.cpp @@ -21,7 +21,8 @@ int device_address = 1; ////////// 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"; @@ -31,33 +32,48 @@ const string default_arm_status_topic = "status"; // Base const smuint8 base_address = 1; const smint32 base_counts_per_rev = 5725807; -const double base_min = -0.5; +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; @@ -88,6 +104,21 @@ public: 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() { @@ -104,7 +135,6 @@ public: reset_controllers(); ros::spinOnce(); -// sleep(1); } @@ -114,8 +144,12 @@ public: void reset_controllers() { if (should_reset) { smSetParameter(arm_bus_handle, base_address, SMP_SYSTEM_CONTROL, SMP_SYSTEM_CONTROL_RESTART); - received_first_joint_position_update = false; - should_clear_faults = true; + 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; } } @@ -153,7 +187,6 @@ public: 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 @@ -161,23 +194,46 @@ public: 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_clear_faults = true; should_calibrate = false; } } @@ -292,6 +348,14 @@ public: } + 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; } @@ -305,10 +369,14 @@ public: 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; + 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); @@ -322,8 +390,6 @@ public: void relative_position_callback(const rover_arm::ArmControlMessage::ConstPtr &msg) { if (!received_first_joint_position_update) { return; } - ROS_INFO("Shoulder before: %ld", shoulder_set_position); - 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; @@ -334,9 +400,10 @@ public: 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_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); @@ -346,7 +413,6 @@ public: new_positions_received = true; } - ROS_INFO("Shoulder after: %ld", shoulder_set_position); } private: 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 18d4aad..78225ea 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 @@ -9,7 +9,8 @@ [{name: "/rover_control/command_control/ground_station_drive", compress: true, rate: 15.0}, {name: "/rover_control/tower/pan_tilt/control", compress: true, rate: 30.0}, {name: "/rover_control/chassis/pan_tilt/control", compress: true, rate: 30.0}, - {name: "/rover_control/mining/control", compress: true, rate: 15.0}] + {name: "/rover_control/mining/control", compress: true, rate: 15.0}, + {name: "/rover_arm/control/relative", compress: true, rate: 30.0}] @@ -21,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..c01a022 100644 --- a/software/ros_packages/rover_main/launch/rover.launch +++ b/software/ros_packages/rover_main/launch/rover.launch @@ -1,6 +1,7 @@ + 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..91e94c3 --- /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/topic_transport_senders.launch b/software/ros_packages/rover_main/launch/rover/topic_transport_senders.launch index 3ce6974..4b667b2 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,7 +155,17 @@ {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: "/rover_control/mining/status", compress: false, rate: 5.0} + ] + + + + + + + + [ + {name: "/rover_arm/status", compress: false, rate: 5.0} ]