caperren/osurc_mars_rover_2017_2018
1
0
Fork 0
mirror of https://github.com/OSURoboticsClub/Rover_2017_2018.git synced 2025-11-08 10:11:14 +00:00
Code Issues Packages Projects Releases Wiki Activity

Rover arm pretty much working! Still some minor changes needed probably, but very happy with it. Lots of misc changes.

Browse Source
This commit is contained in:
Corwin Perren
2018-08-05 05:10:39 -07:00
parent 5d7ac8f4dc
commit 44da64cb32
20 changed files with 1574 additions and 150 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
electrical/documents/ioni/base.drc
View File

@@ -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

34
electrical/documents/ioni/elbow.drc
View File

@@ -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

22
electrical/documents/ioni/roll.drc
View File

@@ -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

12
electrical/documents/ioni/shoulder.drc
View File

@@ -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

20
electrical/documents/ioni/wrist_pitch.drc
View File

@@ -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

16
electrical/documents/ioni/wrist_roll.drc
View File

@@ -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

153
software/firmware/rdf/rdf.ino Normal file
View File

@@ -0,0 +1,153 @@
////////// Includes //////////
#include <ModbusRtu.h>
////////// 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]);
}

84
software/firmware/temp/original_rdf.ino Normal file
View File

@@ -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()<t3+dtavg){
for(int i=0;i<3;i++){
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));
}
}
}

79
software/ros_packages/ground_station/src/Framework/ArmSystems/ArmIndication.py
View File

@@ -5,21 +5,26 @@
# Python native imports
from PyQt5 import QtCore, QtWidgets, QtGui
import logging
from time import time
import rospy
from rover_arm.msg import ArmStatusMessage
#####################################
# Global Variables
#####################################
THREAD_HERTZ = 2
ROTATION_SPEED_MODIFIER = 0.15
ARM_STATUS_TOPIC = "/rover_arm/status"
#####################################
# Controller Class Definition
#####################################
class ArmIndication(QtCore.QThread):
arm_joint_position_updated__signal = QtCore.pyqtSignal(int)
class ArmIndication(QtCore.QObject):
base_position_updated__signal = QtCore.pyqtSignal(int)
shoulder_position_updated__signal = QtCore.pyqtSignal(int)
elbow_position_updated__signal = QtCore.pyqtSignal(int)
roll_position_updated__signal = QtCore.pyqtSignal(int)
wrist_pitch_position_updated__signal = QtCore.pyqtSignal(int)
wrist_roll_position_updated__signal = QtCore.pyqtSignal(int)
def __init__(self, shared_objects):
super(ArmIndication, self).__init__()
@@ -41,56 +46,26 @@ class ArmIndication(QtCore.QThread):
# ########## 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.on_arm_status_update_received__callback)
self.current_position_delta = 0
self.shown_position = 0
def run(self):
while self.run_thread_flag:
start_time = time()
self.change_position_if_needed()
time_diff = time() - start_time
self.msleep(max(int(self.wait_time - time_diff), 0))
def change_position_if_needed(self):
self.shown_position += self.current_position_delta * ROTATION_SPEED_MODIFIER
self.shown_position %= 360
self.arm_joint_position_updated__signal.emit(self.shown_position)
def __on_position_change_requested__slot(self, event):
if event.button() == QtCore.Qt.LeftButton:
self.current_position_delta = 1
elif event.button() == QtCore.Qt.RightButton:
self.current_position_delta = 0
# ########## Connect Signals and Slots ##########
self.connect_signals_and_slots()
def connect_signals_and_slots(self):
self.arm_joint_position_updated__signal.connect(self.base_rotation_dial.setValue)
self.arm_joint_position_updated__signal.connect(self.shoulder_pitch_dial.setValue)
self.arm_joint_position_updated__signal.connect(self.elbow_pitch_dial.setValue)
self.arm_joint_position_updated__signal.connect(self.elbow_roll_dial.setValue)
self.arm_joint_position_updated__signal.connect(self.end_effector_pitch_dial.setValue)
self.arm_joint_position_updated__signal.connect(self.end_effector_rotation_dial.setValue)
self.base_position_updated__signal.connect(self.base_rotation_dial.setValue)
self.shoulder_position_updated__signal.connect(self.shoulder_pitch_dial.setValue)
self.elbow_position_updated__signal.connect(self.elbow_pitch_dial.setValue)
self.roll_position_updated__signal.connect(self.elbow_roll_dial.setValue)
self.wrist_pitch_position_updated__signal.connect(self.end_effector_pitch_dial.setValue)
self.wrist_roll_position_updated__signal.connect(self.end_effector_rotation_dial.setValue)
self.base_rotation_dial.mousePressEvent = self.__on_position_change_requested__slot
self.shoulder_pitch_dial.mousePressEvent = self.__on_position_change_requested__slot
self.elbow_pitch_dial.mousePressEvent = self.__on_position_change_requested__slot
self.elbow_roll_dial.mousePressEvent = self.__on_position_change_requested__slot
self.end_effector_pitch_dial.mousePressEvent = self.__on_position_change_requested__slot
self.end_effector_rotation_dial.mousePressEvent = self.__on_position_change_requested__slot
def on_arm_status_update_received__callback(self, data):
self.base_position_updated__signal.emit(int(data.base * 1000))
self.shoulder_position_updated__signal.emit(int(data.shoulder * 1000))
self.elbow_position_updated__signal.emit(int(data.elbow * 1000))
self.roll_position_updated__signal.emit(int(data.roll * 1000))
self.wrist_pitch_position_updated__signal.emit(int(data.wrist_pitch * 1000))
self.wrist_roll_position_updated__signal.emit(int(data.wrist_roll * 1000))
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

59
software/ros_packages/ground_station/src/Framework/InputSystems/ControllerControlSender.py
View File

@@ -8,7 +8,7 @@ from inputs import devices, GamePad
from time import time
import rospy
from rover_control.msg import DriveCommandMessage, TowerPanTiltControlMessage
from rover_arm.msg import ArmControlMessage
#####################################
# Global Variables
@@ -17,6 +17,21 @@ GAME_CONTROLLER_NAME = "Afterglow Gamepad for Xbox 360"
DRIVE_COMMAND_HERTZ = 20
RELATIVE_ARM_CONTROL_TOPIC = "/rover_arm/control/relative"
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
#####################################
# Controller Class Definition
@@ -156,13 +171,16 @@ class ControllerControlSender(QtCore.QThread):
self.wait_time = 1.0 / DRIVE_COMMAND_HERTZ
self.relative_arm_control_publisher = rospy.Publisher(RELATIVE_ARM_CONTROL_TOPIC, ArmControlMessage,
queue_size=1)
def run(self):
self.logger.debug("Starting Joystick Thread")
while self.run_thread_flag:
start_time = time()
# print self.controller.controller_states
self.process_and_send_arm_control()
time_diff = time() - start_time
@@ -173,6 +191,43 @@ class ControllerControlSender(QtCore.QThread):
def connect_signals_and_slots(self):
pass
def process_and_send_arm_control(self):
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 / 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)

367
software/ros_packages/ground_station/src/Framework/MiscSystems/MiscArmCore.py Normal file
View File

@@ -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

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

@@ -1392,6 +1392,708 @@ N/A</string>
<property name="currentIndex">
<number>0</number>
</property>
<widget class="QWidget" name="tab_4">
<attribute name="title">
<string>Arm</string>
</attribute>
<layout class="QGridLayout" name="gridLayout_15">
<item row="0" column="0">
<layout class="QVBoxLayout" name="verticalLayout_14">
<item>
<widget class="QLabel" name="label_67">
<property name="font">
<font>
<pointsize>18</pointsize>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="text">
<string>Arm Controls</string>
</property>
</widget>
</item>
<item>
<layout class="QHBoxLayout" name="horizontalLayout_15">
<item>
<layout class="QVBoxLayout" name="verticalLayout_15">
<item>
<widget class="QLabel" name="label_68">
<property name="font">
<font>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="text">
<string>Preset Movements</string>
</property>
</widget>
</item>
<item>
<layout class="QGridLayout" name="gridLayout_13">
<item row="3" column="0">
<widget class="QPushButton" name="arm_control_upright_zeroed_button">
<property name="text">
<string>Upright Zeroed</string>
</property>
</widget>
</item>
<item row="2" column="0">
<spacer name="verticalSpacer_4">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>40</height>
</size>
</property>
</spacer>
</item>
<item row="0" column="0">
<widget class="QPushButton" name="arm_controls_stow_arm_button">
<property name="text">
<string>Stow Arm</string>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QPushButton" name="arm_controls_unstow_arm_button">
<property name="text">
<string>Unstow Arm</string>
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QLabel" name="label_71">
<property name="styleSheet">
<string notr="true">color:red;</string>
</property>
<property name="text">
<string>^ CAREFUL! ^</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
</layout>
</item>
</layout>
</item>
<item>
<layout class="QVBoxLayout" name="verticalLayout_17">
<item>
<widget class="QLabel" name="label_70">
<property name="font">
<font>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="text">
<string>Special Functions</string>
</property>
</widget>
</item>
<item>
<layout class="QGridLayout" name="gridLayout_14">
<item row="0" column="0">
<widget class="QPushButton" name="arm_controls_calibration_button">
<property name="text">
<string>Set Calibration</string>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QPushButton" name="arm_controls_clear_faults_button">
<property name="text">
<string>Clear Faults</string>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QPushButton" name="arm_controls_reset_motor_drivers_button">
<property name="text">
<string>Reset Motor Drivers</string>
</property>
</widget>
</item>
</layout>
</item>
<item>
<spacer name="verticalSpacer_5">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>40</height>
</size>
</property>
</spacer>
</item>
</layout>
</item>
</layout>
</item>
</layout>
</item>
<item row="3" column="0">
<spacer name="verticalSpacer_3">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>40</height>
</size>
</property>
</spacer>
</item>
<item row="2" column="0">
<layout class="QHBoxLayout" name="horizontalLayout_14">
<item>
<layout class="QVBoxLayout" name="verticalLayout_7">
<item>
<widget class="QLabel" name="label_19">
<property name="font">
<font>
<pointsize>18</pointsize>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="text">
<string>Motor Driver Statuses</string>
</property>
</widget>
</item>
<item>
<layout class="QGridLayout" name="gridLayout_9">
<item row="0" column="0">
<layout class="QVBoxLayout" name="verticalLayout_6">
<item>
<widget class="QLabel" name="label_20">
<property name="font">
<font>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="text">
<string>Base</string>
</property>
</widget>
</item>
<item>
<layout class="QFormLayout" name="formLayout">
<item row="0" column="0">
<widget class="QLabel" name="label_21">
<property name="text">
<string>Comms:</string>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="arm_controls_base_comms_label">
<property name="font">
<font>
<pointsize>6</pointsize>
<weight>50</weight>
<bold>false</bold>
</font>
</property>
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QLabel" name="label_23">
<property name="text">
<string>Status:</string>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QLabel" name="label_27">
<property name="text">
<string>Faults:</string>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="arm_controls_base_status_label">
<property name="font">
<font>
<pointsize>6</pointsize>
<weight>50</weight>
<bold>false</bold>
</font>
</property>
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="arm_controls_base_faults_label">
<property name="font">
<font>
<pointsize>6</pointsize>
<weight>50</weight>
<bold>false</bold>
</font>
</property>
<property name="text">
<string/>
</property>
</widget>
</item>
</layout>
</item>
</layout>
</item>
<item row="0" column="1">
<layout class="QVBoxLayout" name="verticalLayout_9">
<item>
<widget class="QLabel" name="label_32">
<property name="font">
<font>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="text">
<string>Shoulder</string>
</property>
</widget>
</item>
<item>
<layout class="QFormLayout" name="formLayout_2">
<item row="0" column="0">
<widget class="QLabel" name="label_33">
<property name="text">
<string>Comms:</string>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="arm_controls_shoulder_comms_label">
<property name="font">
<font>
<pointsize>6</pointsize>
<weight>50</weight>
<bold>false</bold>
</font>
</property>
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QLabel" name="label_35">
<property name="text">
<string>Status:</string>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QLabel" name="label_36">
<property name="text">
<string>Faults:</string>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="arm_controls_shoulder_status_label">
<property name="font">
<font>
<pointsize>6</pointsize>
<weight>50</weight>
<bold>false</bold>
</font>
</property>
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="arm_controls_shoulder_faults_label">
<property name="font">
<font>
<pointsize>6</pointsize>
<weight>50</weight>
<bold>false</bold>
</font>
</property>
<property name="text">
<string/>
</property>
</widget>
</item>
</layout>
</item>
</layout>
</item>
<item row="0" column="2">
<layout class="QVBoxLayout" name="verticalLayout_10">
<item>
<widget class="QLabel" name="label_39">
<property name="font">
<font>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="text">
<string>Elbow</string>
</property>
</widget>
</item>
<item>
<layout class="QFormLayout" name="formLayout_3">
<item row="0" column="0">
<widget class="QLabel" name="label_40">
<property name="text">
<string>Comms:</string>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="arm_controls_elbow_comms_label">
<property name="font">
<font>
<pointsize>6</pointsize>
<weight>50</weight>
<bold>false</bold>
</font>
</property>
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QLabel" name="label_42">
<property name="text">
<string>Status:</string>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QLabel" name="label_43">
<property name="text">
<string>Faults:</string>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="arm_controls_elbow_status_label">
<property name="font">
<font>
<pointsize>6</pointsize>
<weight>50</weight>
<bold>false</bold>
</font>
</property>
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="arm_controls_elbow_faults_label">
<property name="font">
<font>
<pointsize>6</pointsize>
<weight>50</weight>
<bold>false</bold>
</font>
</property>
<property name="text">
<string/>
</property>
</widget>
</item>
</layout>
</item>
</layout>
</item>
<item row="1" column="0">
<layout class="QVBoxLayout" name="verticalLayout_11">
<item>
<widget class="QLabel" name="label_46">
<property name="font">
<font>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="text">
<string>Roll</string>
</property>
</widget>
</item>
<item>
<layout class="QFormLayout" name="formLayout_4">
<item row="0" column="0">
<widget class="QLabel" name="label_47">
<property name="text">
<string>Comms:</string>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="arm_controls_roll_comms_label">
<property name="font">
<font>
<pointsize>6</pointsize>
<weight>50</weight>
<bold>false</bold>
</font>
</property>
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QLabel" name="label_49">
<property name="text">
<string>Status:</string>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QLabel" name="label_50">
<property name="text">
<string>Faults:</string>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="arm_controls_roll_status_label">
<property name="font">
<font>
<pointsize>6</pointsize>
<weight>50</weight>
<bold>false</bold>
</font>
</property>
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="arm_controls_roll_faults_label">
<property name="font">
<font>
<pointsize>6</pointsize>
<weight>50</weight>
<bold>false</bold>
</font>
</property>
<property name="text">
<string/>
</property>
</widget>
</item>
</layout>
</item>
</layout>
</item>
<item row="1" column="1">
<layout class="QVBoxLayout" name="verticalLayout_12">
<item>
<widget class="QLabel" name="label_53">
<property name="font">
<font>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="text">
<string>Wrist Pitch</string>
</property>
</widget>
</item>
<item>
<layout class="QFormLayout" name="formLayout_5">
<item row="0" column="0">
<widget class="QLabel" name="label_54">
<property name="text">
<string>Comms:</string>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="arm_controls_wrist_pitch_comms_label">
<property name="font">
<font>
<pointsize>6</pointsize>
<weight>50</weight>
<bold>false</bold>
</font>
</property>
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QLabel" name="label_56">
<property name="text">
<string>Status:</string>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QLabel" name="label_57">
<property name="text">
<string>Faults:</string>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="arm_controls_wrist_pitch_status_label">
<property name="font">
<font>
<pointsize>6</pointsize>
<weight>50</weight>
<bold>false</bold>
</font>
</property>
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="arm_controls_wrist_pitch_faults_label">
<property name="font">
<font>
<pointsize>6</pointsize>
<weight>50</weight>
<bold>false</bold>
</font>
</property>
<property name="text">
<string/>
</property>
</widget>
</item>
</layout>
</item>
</layout>
</item>
<item row="1" column="2">
<layout class="QVBoxLayout" name="verticalLayout_13">
<item>
<widget class="QLabel" name="label_60">
<property name="font">
<font>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="text">
<string>Wrist Roll</string>
</property>
</widget>
</item>
<item>
<layout class="QFormLayout" name="formLayout_6">
<item row="0" column="0">
<widget class="QLabel" name="label_61">
<property name="text">
<string>Comms:</string>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="arm_controls_wrist_roll_comms_label">
<property name="font">
<font>
<pointsize>6</pointsize>
<weight>50</weight>
<bold>false</bold>
</font>
</property>
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QLabel" name="label_63">
<property name="text">
<string>Status:</string>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QLabel" name="label_64">
<property name="text">
<string>Faults:</string>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="arm_controls_wrist_roll_status_label">
<property name="font">
<font>
<pointsize>6</pointsize>
<weight>50</weight>
<bold>false</bold>
</font>
</property>
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="arm_controls_wrist_roll_faults_label">
<property name="font">
<font>
<pointsize>6</pointsize>
<weight>50</weight>
<bold>false</bold>
</font>
</property>
<property name="text">
<string/>
</property>
</widget>
</item>
</layout>
</item>
</layout>
</item>
</layout>
</item>
</layout>
</item>
</layout>
</item>
<item row="1" column="0">
<widget class="Line" name="line_8">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
</widget>
</item>
</layout>
</widget>
<widget class="QWidget" name="tab_3">
<attribute name="title">
<string>Mining</string>

46
software/ros_packages/ground_station/src/Resources/Ui/right_screen.ui
View File

@@ -151,7 +151,7 @@
</font>
</property>
<property name="text">
<string>Base Rotation</string>
<string>Base</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
@@ -167,10 +167,10 @@
</size>
</property>
<property name="minimum">
<number>0</number>
<number>-500</number>
</property>
<property name="maximum">
<number>360</number>
<number>500</number>
</property>
<property name="pageStep">
<number>10</number>
@@ -218,7 +218,7 @@
</font>
</property>
<property name="text">
<string>Shoulder Pitch</string>
<string>Shoulder</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
@@ -234,10 +234,10 @@
</size>
</property>
<property name="minimum">
<number>0</number>
<number>-250</number>
</property>
<property name="maximum">
<number>360</number>
<number>250</number>
</property>
<property name="pageStep">
<number>10</number>
@@ -267,7 +267,7 @@
<double>90.000000000000000</double>
</property>
<property name="notchesVisible">
<bool>false</bool>
<bool>true</bool>
</property>
</widget>
</item>
@@ -289,7 +289,7 @@
</font>
</property>
<property name="text">
<string>Elbow Pitch</string>
<string>Elbow</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
@@ -305,10 +305,10 @@
</size>
</property>
<property name="minimum">
<number>0</number>
<number>-500</number>
</property>
<property name="maximum">
<number>360</number>
<number>500</number>
</property>
<property name="pageStep">
<number>10</number>
@@ -338,7 +338,7 @@
<double>90.000000000000000</double>
</property>
<property name="notchesVisible">
<bool>false</bool>
<bool>true</bool>
</property>
</widget>
</item>
@@ -358,7 +358,7 @@
</font>
</property>
<property name="text">
<string>Elbow Roll</string>
<string>Roll</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
@@ -374,10 +374,10 @@
</size>
</property>
<property name="minimum">
<number>0</number>
<number>-250</number>
</property>
<property name="maximum">
<number>360</number>
<number>250</number>
</property>
<property name="pageStep">
<number>10</number>
@@ -407,7 +407,7 @@
<double>90.000000000000000</double>
</property>
<property name="notchesVisible">
<bool>false</bool>
<bool>true</bool>
</property>
</widget>
</item>
@@ -431,7 +431,7 @@
</font>
</property>
<property name="text">
<string>End Effector Pitch</string>
<string>Wrist Pitch</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
@@ -447,10 +447,10 @@
</size>
</property>
<property name="minimum">
<number>0</number>
<number>-250</number>
</property>
<property name="maximum">
<number>360</number>
<number>250</number>
</property>
<property name="pageStep">
<number>10</number>
@@ -480,7 +480,7 @@
<double>90.000000000000000</double>
</property>
<property name="notchesVisible">
<bool>false</bool>
<bool>true</bool>
</property>
</widget>
</item>
@@ -498,7 +498,7 @@
</font>
</property>
<property name="text">
<string>End Effector Rotation</string>
<string>Wrist Roll</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
@@ -514,10 +514,10 @@
</size>
</property>
<property name="minimum">
<number>0</number>
<number>-1000</number>
</property>
<property name="maximum">
<number>360</number>
<number>1000</number>
</property>
<property name="pageStep">
<number>10</number>
@@ -547,7 +547,7 @@
<double>90.000000000000000</double>
</property>
<property name="notchesVisible">
<bool>false</bool>
<bool>true</bool>
</property>
</widget>
</item>

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

@@ -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()

94
software/ros_packages/rover_arm/src/rover_arm.cpp
View File

@@ -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<rover_arm::ArmStatusMessage>(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:

4
software/ros_packages/rover_main/launch/ground_station/topic_transport_receivers.launch
View File

@@ -79,5 +79,9 @@
<param name="port" value="17019" />
</node>
<node name="udp_arm_statuses" pkg="nimbro_topic_transport" type="udp_receiver" output="screen">
<param name="port" value="17020" />
</node>
</group>
</launch>

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

@@ -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}]
</rosparam>
</node>
@@ -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}]
</rosparam>
</node>
</group>

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

@@ -1,6 +1,7 @@
<launch>
<!-- ########## Start Rover Control Nodes ########## -->
<include file="$(find rover_main)/launch/rover/control.launch"/>
<include file="$(find rover_main)/launch/rover/arm.launch"/>
<!-- ########## Start All Rover Camera Nodes ########## -->
<include file="$(find rover_main)/launch/rover/cameras.launch"/>

3
software/ros_packages/rover_main/launch/rover/arm.launch Normal file
View File

@@ -0,0 +1,3 @@
<launch>
<node name="rover_arm_control" pkg="rover_arm" type="rover_arm" respawn="true" output="screen"/>
</launch>

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

@@ -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}
]
</rosparam>
</node>
<node name="arm_status_udp_sender" pkg="nimbro_topic_transport" type="udp_sender">
<param name="destination_addr" value="$(arg target)" />
<param name="destination_port" value="17020" />
<rosparam param="topics">
[
{name: "/rover_arm/status", compress: false, rate: 5.0}
]
</rosparam>
</node>