mirror of
https://github.com/OSURoboticsClub/Rover_2017_2018.git
synced 2025-11-08 10:11:14 +00:00
Rover arm pretty much working! Still some minor changes needed probably, but very happy with it. Lots of misc changes.
This commit is contained in:
@@ -153,7 +153,7 @@ GranityVersionInt=11400
|
|||||||
19\scaling=1
|
19\scaling=1
|
||||||
19\offset=0
|
19\offset=0
|
||||||
19\readonly=false
|
19\readonly=false
|
||||||
19\value=-29203491
|
19\value=-19123927
|
||||||
19\min=-536870912
|
19\min=-536870912
|
||||||
19\max=536870911
|
19\max=536870911
|
||||||
20\name=FBR
|
20\name=FBR
|
||||||
@@ -393,7 +393,7 @@ GranityVersionInt=11400
|
|||||||
49\scaling=1
|
49\scaling=1
|
||||||
49\offset=0
|
49\offset=0
|
||||||
49\readonly=false
|
49\readonly=false
|
||||||
49\value=3
|
49\value=5
|
||||||
49\min=0
|
49\min=0
|
||||||
49\max=536870911
|
49\max=536870911
|
||||||
50\name=TBT
|
50\name=TBT
|
||||||
@@ -401,7 +401,7 @@ GranityVersionInt=11400
|
|||||||
50\scaling=1
|
50\scaling=1
|
||||||
50\offset=0
|
50\offset=0
|
||||||
50\readonly=false
|
50\readonly=false
|
||||||
50\value=0
|
50\value=10
|
||||||
50\min=-1000000
|
50\min=-1000000
|
||||||
50\max=100
|
50\max=100
|
||||||
51\name=CRI
|
51\name=CRI
|
||||||
|
|||||||
@@ -9,7 +9,7 @@ GranityVersionInt=11400
|
|||||||
1\scaling=1
|
1\scaling=1
|
||||||
1\offset=0
|
1\offset=0
|
||||||
1\readonly=true
|
1\readonly=true
|
||||||
1\value=10702
|
1\value=10707
|
||||||
1\min=2
|
1\min=2
|
||||||
1\max=1
|
1\max=1
|
||||||
2\name=HWTYPE
|
2\name=HWTYPE
|
||||||
@@ -25,7 +25,7 @@ GranityVersionInt=11400
|
|||||||
3\scaling=1
|
3\scaling=1
|
||||||
3\offset=0
|
3\offset=0
|
||||||
3\readonly=true
|
3\readonly=true
|
||||||
3\value=110028853
|
3\value=110028686
|
||||||
3\min=2
|
3\min=2
|
||||||
3\max=1
|
3\max=1
|
||||||
4\name=BUILDREVISION
|
4\name=BUILDREVISION
|
||||||
@@ -33,7 +33,7 @@ GranityVersionInt=11400
|
|||||||
4\scaling=1
|
4\scaling=1
|
||||||
4\offset=0
|
4\offset=0
|
||||||
4\readonly=true
|
4\readonly=true
|
||||||
4\value=163070257
|
4\value=157269493
|
||||||
4\min=2
|
4\min=2
|
||||||
4\max=1
|
4\max=1
|
||||||
5\name=CEI
|
5\name=CEI
|
||||||
@@ -49,7 +49,7 @@ GranityVersionInt=11400
|
|||||||
6\scaling=1
|
6\scaling=1
|
||||||
6\offset=0
|
6\offset=0
|
||||||
6\readonly=true
|
6\readonly=true
|
||||||
6\value=-314970192
|
6\value=-29713247
|
||||||
6\min=2
|
6\min=2
|
||||||
6\max=1
|
6\max=1
|
||||||
7\name=SMO
|
7\name=SMO
|
||||||
@@ -153,7 +153,7 @@ GranityVersionInt=11400
|
|||||||
19\scaling=1
|
19\scaling=1
|
||||||
19\offset=0
|
19\offset=0
|
||||||
19\readonly=false
|
19\readonly=false
|
||||||
19\value=0
|
19\value=3222680
|
||||||
19\min=-536870912
|
19\min=-536870912
|
||||||
19\max=536870911
|
19\max=536870911
|
||||||
20\name=FBR
|
20\name=FBR
|
||||||
@@ -193,7 +193,7 @@ GranityVersionInt=11400
|
|||||||
24\scaling=1
|
24\scaling=1
|
||||||
24\offset=0
|
24\offset=0
|
||||||
24\readonly=false
|
24\readonly=false
|
||||||
24\value=1000
|
24\value=600
|
||||||
24\min=0
|
24\min=0
|
||||||
24\max=300000
|
24\max=300000
|
||||||
25\name=KVI
|
25\name=KVI
|
||||||
@@ -201,7 +201,7 @@ GranityVersionInt=11400
|
|||||||
25\scaling=1
|
25\scaling=1
|
||||||
25\offset=0
|
25\offset=0
|
||||||
25\readonly=false
|
25\readonly=false
|
||||||
25\value=150
|
25\value=100
|
||||||
25\min=0
|
25\min=0
|
||||||
25\max=300000
|
25\max=300000
|
||||||
26\name=KPP
|
26\name=KPP
|
||||||
@@ -209,7 +209,7 @@ GranityVersionInt=11400
|
|||||||
26\scaling=1
|
26\scaling=1
|
||||||
26\offset=0
|
26\offset=0
|
||||||
26\readonly=false
|
26\readonly=false
|
||||||
26\value=50
|
26\value=40
|
||||||
26\min=0
|
26\min=0
|
||||||
26\max=300000
|
26\max=300000
|
||||||
27\name=VFF
|
27\name=VFF
|
||||||
@@ -291,7 +291,7 @@ GranityVersionInt=11400
|
|||||||
36\readonly=false
|
36\readonly=false
|
||||||
36\value=3
|
36\value=3
|
||||||
36\min=0
|
36\min=0
|
||||||
36\max=5.99999999999999
|
36\max=6
|
||||||
37\name=FOV
|
37\name=FOV
|
||||||
37\addr=569
|
37\addr=569
|
||||||
37\scaling=100
|
37\scaling=100
|
||||||
@@ -385,7 +385,7 @@ GranityVersionInt=11400
|
|||||||
48\scaling=1
|
48\scaling=1
|
||||||
48\offset=0
|
48\offset=0
|
||||||
48\readonly=false
|
48\readonly=false
|
||||||
48\value=96
|
48\value=128
|
||||||
48\min=0
|
48\min=0
|
||||||
48\max=-1
|
48\max=-1
|
||||||
49\name=TTR
|
49\name=TTR
|
||||||
@@ -393,7 +393,7 @@ GranityVersionInt=11400
|
|||||||
49\scaling=1
|
49\scaling=1
|
||||||
49\offset=0
|
49\offset=0
|
||||||
49\readonly=false
|
49\readonly=false
|
||||||
49\value=4
|
49\value=2
|
||||||
49\min=0
|
49\min=0
|
||||||
49\max=536870911
|
49\max=536870911
|
||||||
50\name=TBT
|
50\name=TBT
|
||||||
@@ -401,7 +401,7 @@ GranityVersionInt=11400
|
|||||||
50\scaling=1
|
50\scaling=1
|
||||||
50\offset=0
|
50\offset=0
|
||||||
50\readonly=false
|
50\readonly=false
|
||||||
50\value=10
|
50\value=90
|
||||||
50\min=-1000000
|
50\min=-1000000
|
||||||
50\max=100
|
50\max=100
|
||||||
51\name=CRI
|
51\name=CRI
|
||||||
@@ -457,7 +457,7 @@ GranityVersionInt=11400
|
|||||||
57\scaling=1
|
57\scaling=1
|
||||||
57\offset=0
|
57\offset=0
|
||||||
57\readonly=false
|
57\readonly=false
|
||||||
57\value=1630
|
57\value=1200
|
||||||
57\min=1
|
57\min=1
|
||||||
57\max=32767
|
57\max=32767
|
||||||
58\name=CRV
|
58\name=CRV
|
||||||
@@ -521,7 +521,7 @@ GranityVersionInt=11400
|
|||||||
65\scaling=1
|
65\scaling=1
|
||||||
65\offset=0
|
65\offset=0
|
||||||
65\readonly=false
|
65\readonly=false
|
||||||
65\value=0
|
65\value=416
|
||||||
65\min=0
|
65\min=0
|
||||||
65\max=-1
|
65\max=-1
|
||||||
66\name=HMV
|
66\name=HMV
|
||||||
@@ -561,7 +561,7 @@ GranityVersionInt=11400
|
|||||||
70\scaling=1
|
70\scaling=1
|
||||||
70\offset=0
|
70\offset=0
|
||||||
70\readonly=false
|
70\readonly=false
|
||||||
70\value=0
|
70\value=2457601
|
||||||
70\min=-536870912
|
70\min=-536870912
|
||||||
70\max=536870911
|
70\max=536870911
|
||||||
71\name=HLL
|
71\name=HLL
|
||||||
@@ -569,7 +569,7 @@ GranityVersionInt=11400
|
|||||||
71\scaling=1
|
71\scaling=1
|
||||||
71\offset=0
|
71\offset=0
|
||||||
71\readonly=false
|
71\readonly=false
|
||||||
71\value=0
|
71\value=-2457601
|
||||||
71\min=-536870912
|
71\min=-536870912
|
||||||
71\max=536870911
|
71\max=536870911
|
||||||
72\name=HMF
|
72\name=HMF
|
||||||
@@ -673,7 +673,7 @@ GranityVersionInt=11400
|
|||||||
84\scaling=1
|
84\scaling=1
|
||||||
84\offset=0
|
84\offset=0
|
||||||
84\readonly=true
|
84\readonly=true
|
||||||
84\value=4194047
|
84\value=62914303
|
||||||
84\min=0
|
84\min=0
|
||||||
84\max=0
|
84\max=0
|
||||||
85\name=CAPS2
|
85\name=CAPS2
|
||||||
|
|||||||
@@ -1,7 +1,7 @@
|
|||||||
[Header]
|
[Header]
|
||||||
DRCVersion=110
|
DRCVersion=110
|
||||||
GranityVersion=1.14.1
|
GranityVersion=1.14.0
|
||||||
GranityVersionInt=11401
|
GranityVersionInt=11400
|
||||||
|
|
||||||
[GDT3Params]
|
[GDT3Params]
|
||||||
1\name=GCFWVER
|
1\name=GCFWVER
|
||||||
@@ -153,7 +153,7 @@ GranityVersionInt=11401
|
|||||||
19\scaling=1
|
19\scaling=1
|
||||||
19\offset=0
|
19\offset=0
|
||||||
19\readonly=false
|
19\readonly=false
|
||||||
19\value=0
|
19\value=769700
|
||||||
19\min=-536870912
|
19\min=-536870912
|
||||||
19\max=536870911
|
19\max=536870911
|
||||||
20\name=FBR
|
20\name=FBR
|
||||||
@@ -193,7 +193,7 @@ GranityVersionInt=11401
|
|||||||
24\scaling=1
|
24\scaling=1
|
||||||
24\offset=0
|
24\offset=0
|
||||||
24\readonly=false
|
24\readonly=false
|
||||||
24\value=1100
|
24\value=500
|
||||||
24\min=0
|
24\min=0
|
||||||
24\max=300000
|
24\max=300000
|
||||||
25\name=KVI
|
25\name=KVI
|
||||||
@@ -201,7 +201,7 @@ GranityVersionInt=11401
|
|||||||
25\scaling=1
|
25\scaling=1
|
||||||
25\offset=0
|
25\offset=0
|
||||||
25\readonly=false
|
25\readonly=false
|
||||||
25\value=800
|
25\value=50
|
||||||
25\min=0
|
25\min=0
|
||||||
25\max=300000
|
25\max=300000
|
||||||
26\name=KPP
|
26\name=KPP
|
||||||
@@ -209,7 +209,7 @@ GranityVersionInt=11401
|
|||||||
26\scaling=1
|
26\scaling=1
|
||||||
26\offset=0
|
26\offset=0
|
||||||
26\readonly=false
|
26\readonly=false
|
||||||
26\value=1600
|
26\value=10
|
||||||
26\min=0
|
26\min=0
|
||||||
26\max=300000
|
26\max=300000
|
||||||
27\name=VFF
|
27\name=VFF
|
||||||
@@ -473,7 +473,7 @@ GranityVersionInt=11401
|
|||||||
59\scaling=1000
|
59\scaling=1000
|
||||||
59\offset=0
|
59\offset=0
|
||||||
59\readonly=false
|
59\readonly=false
|
||||||
59\value=3.855
|
59\value=4.086
|
||||||
59\min=0.001
|
59\min=0.001
|
||||||
59\max=200
|
59\max=200
|
||||||
60\name=ML
|
60\name=ML
|
||||||
@@ -481,7 +481,7 @@ GranityVersionInt=11401
|
|||||||
60\scaling=1000
|
60\scaling=1000
|
||||||
60\offset=0
|
60\offset=0
|
||||||
60\readonly=false
|
60\readonly=false
|
||||||
60\value=2.079
|
60\value=1.825
|
||||||
60\min=0.001
|
60\min=0.001
|
||||||
60\max=200
|
60\max=200
|
||||||
61\name=MTC
|
61\name=MTC
|
||||||
@@ -521,7 +521,7 @@ GranityVersionInt=11401
|
|||||||
65\scaling=1
|
65\scaling=1
|
||||||
65\offset=0
|
65\offset=0
|
||||||
65\readonly=false
|
65\readonly=false
|
||||||
65\value=0
|
65\value=416
|
||||||
65\min=0
|
65\min=0
|
||||||
65\max=-1
|
65\max=-1
|
||||||
66\name=HMV
|
66\name=HMV
|
||||||
@@ -561,7 +561,7 @@ GranityVersionInt=11401
|
|||||||
70\scaling=1
|
70\scaling=1
|
||||||
70\offset=0
|
70\offset=0
|
||||||
70\readonly=false
|
70\readonly=false
|
||||||
70\value=0
|
70\value=408781
|
||||||
70\min=-536870912
|
70\min=-536870912
|
||||||
70\max=536870911
|
70\max=536870911
|
||||||
71\name=HLL
|
71\name=HLL
|
||||||
@@ -569,7 +569,7 @@ GranityVersionInt=11401
|
|||||||
71\scaling=1
|
71\scaling=1
|
||||||
71\offset=0
|
71\offset=0
|
||||||
71\readonly=false
|
71\readonly=false
|
||||||
71\value=0
|
71\value=-408781
|
||||||
71\min=-536870912
|
71\min=-536870912
|
||||||
71\max=536870911
|
71\max=536870911
|
||||||
72\name=HMF
|
72\name=HMF
|
||||||
|
|||||||
@@ -153,7 +153,7 @@ GranityVersionInt=11400
|
|||||||
19\scaling=1
|
19\scaling=1
|
||||||
19\offset=0
|
19\offset=0
|
||||||
19\readonly=false
|
19\readonly=false
|
||||||
19\value=340555
|
19\value=28407
|
||||||
19\min=-536870912
|
19\min=-536870912
|
||||||
19\max=536870911
|
19\max=536870911
|
||||||
20\name=FBR
|
20\name=FBR
|
||||||
@@ -273,7 +273,7 @@ GranityVersionInt=11400
|
|||||||
34\scaling=1000
|
34\scaling=1000
|
||||||
34\offset=0
|
34\offset=0
|
||||||
34\readonly=false
|
34\readonly=false
|
||||||
34\value=5
|
34\value=10
|
||||||
34\min=0.001
|
34\min=0.001
|
||||||
34\max=23.478
|
34\max=23.478
|
||||||
35\name=MCC
|
35\name=MCC
|
||||||
@@ -385,7 +385,7 @@ GranityVersionInt=11400
|
|||||||
48\scaling=1
|
48\scaling=1
|
||||||
48\offset=0
|
48\offset=0
|
||||||
48\readonly=false
|
48\readonly=false
|
||||||
48\value=96
|
48\value=224
|
||||||
48\min=0
|
48\min=0
|
||||||
48\max=-1
|
48\max=-1
|
||||||
49\name=TTR
|
49\name=TTR
|
||||||
@@ -393,7 +393,7 @@ GranityVersionInt=11400
|
|||||||
49\scaling=1
|
49\scaling=1
|
||||||
49\offset=0
|
49\offset=0
|
||||||
49\readonly=false
|
49\readonly=false
|
||||||
49\value=1
|
49\value=2
|
||||||
49\min=0
|
49\min=0
|
||||||
49\max=536870911
|
49\max=536870911
|
||||||
50\name=TBT
|
50\name=TBT
|
||||||
@@ -401,7 +401,7 @@ GranityVersionInt=11400
|
|||||||
50\scaling=1
|
50\scaling=1
|
||||||
50\offset=0
|
50\offset=0
|
||||||
50\readonly=false
|
50\readonly=false
|
||||||
50\value=0
|
50\value=90
|
||||||
50\min=-1000000
|
50\min=-1000000
|
||||||
50\max=100
|
50\max=100
|
||||||
51\name=CRI
|
51\name=CRI
|
||||||
@@ -457,7 +457,7 @@ GranityVersionInt=11400
|
|||||||
57\scaling=1
|
57\scaling=1
|
||||||
57\offset=0
|
57\offset=0
|
||||||
57\readonly=false
|
57\readonly=false
|
||||||
57\value=870
|
57\value=700
|
||||||
57\min=1
|
57\min=1
|
||||||
57\max=32767
|
57\max=32767
|
||||||
58\name=CRV
|
58\name=CRV
|
||||||
|
|||||||
@@ -9,7 +9,7 @@ GranityVersionInt=11400
|
|||||||
1\scaling=1
|
1\scaling=1
|
||||||
1\offset=0
|
1\offset=0
|
||||||
1\readonly=true
|
1\readonly=true
|
||||||
1\value=10702
|
1\value=10707
|
||||||
1\min=2
|
1\min=2
|
||||||
1\max=1
|
1\max=1
|
||||||
2\name=HWTYPE
|
2\name=HWTYPE
|
||||||
@@ -25,7 +25,7 @@ GranityVersionInt=11400
|
|||||||
3\scaling=1
|
3\scaling=1
|
||||||
3\offset=0
|
3\offset=0
|
||||||
3\readonly=true
|
3\readonly=true
|
||||||
3\value=110028853
|
3\value=110028746
|
||||||
3\min=2
|
3\min=2
|
||||||
3\max=1
|
3\max=1
|
||||||
4\name=BUILDREVISION
|
4\name=BUILDREVISION
|
||||||
@@ -33,7 +33,7 @@ GranityVersionInt=11400
|
|||||||
4\scaling=1
|
4\scaling=1
|
||||||
4\offset=0
|
4\offset=0
|
||||||
4\readonly=true
|
4\readonly=true
|
||||||
4\value=163070257
|
4\value=157269493
|
||||||
4\min=2
|
4\min=2
|
||||||
4\max=1
|
4\max=1
|
||||||
5\name=CEI
|
5\name=CEI
|
||||||
@@ -49,7 +49,7 @@ GranityVersionInt=11400
|
|||||||
6\scaling=1
|
6\scaling=1
|
||||||
6\offset=0
|
6\offset=0
|
||||||
6\readonly=true
|
6\readonly=true
|
||||||
6\value=-314970192
|
6\value=-390223875
|
||||||
6\min=2
|
6\min=2
|
||||||
6\max=1
|
6\max=1
|
||||||
7\name=SMO
|
7\name=SMO
|
||||||
@@ -153,7 +153,7 @@ GranityVersionInt=11400
|
|||||||
19\scaling=1
|
19\scaling=1
|
||||||
19\offset=0
|
19\offset=0
|
||||||
19\readonly=false
|
19\readonly=false
|
||||||
19\value=0
|
19\value=-17238226
|
||||||
19\min=-536870912
|
19\min=-536870912
|
||||||
19\max=536870911
|
19\max=536870911
|
||||||
20\name=FBR
|
20\name=FBR
|
||||||
@@ -291,7 +291,7 @@ GranityVersionInt=11400
|
|||||||
36\readonly=false
|
36\readonly=false
|
||||||
36\value=3
|
36\value=3
|
||||||
36\min=0
|
36\min=0
|
||||||
36\max=5.99999999999999
|
36\max=6
|
||||||
37\name=FOV
|
37\name=FOV
|
||||||
37\addr=569
|
37\addr=569
|
||||||
37\scaling=100
|
37\scaling=100
|
||||||
@@ -521,7 +521,7 @@ GranityVersionInt=11400
|
|||||||
65\scaling=1
|
65\scaling=1
|
||||||
65\offset=0
|
65\offset=0
|
||||||
65\readonly=false
|
65\readonly=false
|
||||||
65\value=0
|
65\value=416
|
||||||
65\min=0
|
65\min=0
|
||||||
65\max=-1
|
65\max=-1
|
||||||
66\name=HMV
|
66\name=HMV
|
||||||
@@ -561,7 +561,7 @@ GranityVersionInt=11400
|
|||||||
70\scaling=1
|
70\scaling=1
|
||||||
70\offset=0
|
70\offset=0
|
||||||
70\readonly=false
|
70\readonly=false
|
||||||
70\value=0
|
70\value=948448
|
||||||
70\min=-536870912
|
70\min=-536870912
|
||||||
70\max=536870911
|
70\max=536870911
|
||||||
71\name=HLL
|
71\name=HLL
|
||||||
@@ -569,7 +569,7 @@ GranityVersionInt=11400
|
|||||||
71\scaling=1
|
71\scaling=1
|
||||||
71\offset=0
|
71\offset=0
|
||||||
71\readonly=false
|
71\readonly=false
|
||||||
71\value=0
|
71\value=-948448
|
||||||
71\min=-536870912
|
71\min=-536870912
|
||||||
71\max=536870911
|
71\max=536870911
|
||||||
72\name=HMF
|
72\name=HMF
|
||||||
@@ -673,7 +673,7 @@ GranityVersionInt=11400
|
|||||||
84\scaling=1
|
84\scaling=1
|
||||||
84\offset=0
|
84\offset=0
|
||||||
84\readonly=true
|
84\readonly=true
|
||||||
84\value=4194047
|
84\value=62914303
|
||||||
84\min=0
|
84\min=0
|
||||||
84\max=0
|
84\max=0
|
||||||
85\name=CAPS2
|
85\name=CAPS2
|
||||||
|
|||||||
@@ -9,7 +9,7 @@ GranityVersionInt=11400
|
|||||||
1\scaling=1
|
1\scaling=1
|
||||||
1\offset=0
|
1\offset=0
|
||||||
1\readonly=true
|
1\readonly=true
|
||||||
1\value=10702
|
1\value=10707
|
||||||
1\min=2
|
1\min=2
|
||||||
1\max=1
|
1\max=1
|
||||||
2\name=HWTYPE
|
2\name=HWTYPE
|
||||||
@@ -25,7 +25,7 @@ GranityVersionInt=11400
|
|||||||
3\scaling=1
|
3\scaling=1
|
||||||
3\offset=0
|
3\offset=0
|
||||||
3\readonly=true
|
3\readonly=true
|
||||||
3\value=110028853
|
3\value=110028918
|
||||||
3\min=2
|
3\min=2
|
||||||
3\max=1
|
3\max=1
|
||||||
4\name=BUILDREVISION
|
4\name=BUILDREVISION
|
||||||
@@ -33,7 +33,7 @@ GranityVersionInt=11400
|
|||||||
4\scaling=1
|
4\scaling=1
|
||||||
4\offset=0
|
4\offset=0
|
||||||
4\readonly=true
|
4\readonly=true
|
||||||
4\value=163070257
|
4\value=157269493
|
||||||
4\min=2
|
4\min=2
|
||||||
4\max=1
|
4\max=1
|
||||||
5\name=CEI
|
5\name=CEI
|
||||||
@@ -49,7 +49,7 @@ GranityVersionInt=11400
|
|||||||
6\scaling=1
|
6\scaling=1
|
||||||
6\offset=0
|
6\offset=0
|
||||||
6\readonly=true
|
6\readonly=true
|
||||||
6\value=-314970192
|
6\value=-69557674
|
||||||
6\min=2
|
6\min=2
|
||||||
6\max=1
|
6\max=1
|
||||||
7\name=SMO
|
7\name=SMO
|
||||||
@@ -153,7 +153,7 @@ GranityVersionInt=11400
|
|||||||
19\scaling=1
|
19\scaling=1
|
||||||
19\offset=0
|
19\offset=0
|
||||||
19\readonly=false
|
19\readonly=false
|
||||||
19\value=0
|
19\value=-21753107
|
||||||
19\min=-536870912
|
19\min=-536870912
|
||||||
19\max=536870911
|
19\max=536870911
|
||||||
20\name=FBR
|
20\name=FBR
|
||||||
@@ -291,7 +291,7 @@ GranityVersionInt=11400
|
|||||||
36\readonly=false
|
36\readonly=false
|
||||||
36\value=3
|
36\value=3
|
||||||
36\min=0
|
36\min=0
|
||||||
36\max=5.99999999999999
|
36\max=6
|
||||||
37\name=FOV
|
37\name=FOV
|
||||||
37\addr=569
|
37\addr=569
|
||||||
37\scaling=100
|
37\scaling=100
|
||||||
@@ -521,7 +521,7 @@ GranityVersionInt=11400
|
|||||||
65\scaling=1
|
65\scaling=1
|
||||||
65\offset=0
|
65\offset=0
|
||||||
65\readonly=false
|
65\readonly=false
|
||||||
65\value=0
|
65\value=416
|
||||||
65\min=0
|
65\min=0
|
||||||
65\max=-1
|
65\max=-1
|
||||||
66\name=HMV
|
66\name=HMV
|
||||||
@@ -673,7 +673,7 @@ GranityVersionInt=11400
|
|||||||
84\scaling=1
|
84\scaling=1
|
||||||
84\offset=0
|
84\offset=0
|
||||||
84\readonly=true
|
84\readonly=true
|
||||||
84\value=4194047
|
84\value=62914303
|
||||||
84\min=0
|
84\min=0
|
||||||
84\max=0
|
84\max=0
|
||||||
85\name=CAPS2
|
85\name=CAPS2
|
||||||
|
|||||||
153
software/firmware/rdf/rdf.ino
Normal file
153
software/firmware/rdf/rdf.ino
Normal 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
84
software/firmware/temp/original_rdf.ino
Normal 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));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -5,21 +5,26 @@
|
|||||||
# Python native imports
|
# Python native imports
|
||||||
from PyQt5 import QtCore, QtWidgets, QtGui
|
from PyQt5 import QtCore, QtWidgets, QtGui
|
||||||
import logging
|
import logging
|
||||||
from time import time
|
import rospy
|
||||||
|
|
||||||
|
from rover_arm.msg import ArmStatusMessage
|
||||||
|
|
||||||
#####################################
|
#####################################
|
||||||
# Global Variables
|
# Global Variables
|
||||||
#####################################
|
#####################################
|
||||||
THREAD_HERTZ = 2
|
ARM_STATUS_TOPIC = "/rover_arm/status"
|
||||||
|
|
||||||
ROTATION_SPEED_MODIFIER = 0.15
|
|
||||||
|
|
||||||
|
|
||||||
#####################################
|
#####################################
|
||||||
# Controller Class Definition
|
# Controller Class Definition
|
||||||
#####################################
|
#####################################
|
||||||
class ArmIndication(QtCore.QThread):
|
class ArmIndication(QtCore.QObject):
|
||||||
arm_joint_position_updated__signal = QtCore.pyqtSignal(int)
|
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):
|
def __init__(self, shared_objects):
|
||||||
super(ArmIndication, self).__init__()
|
super(ArmIndication, self).__init__()
|
||||||
@@ -41,56 +46,26 @@ class ArmIndication(QtCore.QThread):
|
|||||||
# ########## Get the Pick And Plate instance of the logger ##########
|
# ########## Get the Pick And Plate instance of the logger ##########
|
||||||
self.logger = logging.getLogger("groundstation")
|
self.logger = logging.getLogger("groundstation")
|
||||||
|
|
||||||
# ########## Thread Flags ##########
|
|
||||||
self.run_thread_flag = True
|
|
||||||
|
|
||||||
# ########## Class Variables ##########
|
# ########## 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
|
# ########## Connect Signals and Slots ##########
|
||||||
self.shown_position = 0
|
self.connect_signals_and_slots()
|
||||||
|
|
||||||
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
|
|
||||||
|
|
||||||
def connect_signals_and_slots(self):
|
def connect_signals_and_slots(self):
|
||||||
self.arm_joint_position_updated__signal.connect(self.base_rotation_dial.setValue)
|
self.base_position_updated__signal.connect(self.base_rotation_dial.setValue)
|
||||||
self.arm_joint_position_updated__signal.connect(self.shoulder_pitch_dial.setValue)
|
self.shoulder_position_updated__signal.connect(self.shoulder_pitch_dial.setValue)
|
||||||
self.arm_joint_position_updated__signal.connect(self.elbow_pitch_dial.setValue)
|
self.elbow_position_updated__signal.connect(self.elbow_pitch_dial.setValue)
|
||||||
self.arm_joint_position_updated__signal.connect(self.elbow_roll_dial.setValue)
|
self.roll_position_updated__signal.connect(self.elbow_roll_dial.setValue)
|
||||||
self.arm_joint_position_updated__signal.connect(self.end_effector_pitch_dial.setValue)
|
self.wrist_pitch_position_updated__signal.connect(self.end_effector_pitch_dial.setValue)
|
||||||
self.arm_joint_position_updated__signal.connect(self.end_effector_rotation_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
|
def on_arm_status_update_received__callback(self, data):
|
||||||
self.shoulder_pitch_dial.mousePressEvent = self.__on_position_change_requested__slot
|
self.base_position_updated__signal.emit(int(data.base * 1000))
|
||||||
self.elbow_pitch_dial.mousePressEvent = self.__on_position_change_requested__slot
|
self.shoulder_position_updated__signal.emit(int(data.shoulder * 1000))
|
||||||
self.elbow_roll_dial.mousePressEvent = self.__on_position_change_requested__slot
|
self.elbow_position_updated__signal.emit(int(data.elbow * 1000))
|
||||||
self.end_effector_pitch_dial.mousePressEvent = self.__on_position_change_requested__slot
|
self.roll_position_updated__signal.emit(int(data.roll * 1000))
|
||||||
self.end_effector_rotation_dial.mousePressEvent = self.__on_position_change_requested__slot
|
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
|
|
||||||
|
|||||||
@@ -8,7 +8,7 @@ from inputs import devices, GamePad
|
|||||||
from time import time
|
from time import time
|
||||||
|
|
||||||
import rospy
|
import rospy
|
||||||
from rover_control.msg import DriveCommandMessage, TowerPanTiltControlMessage
|
from rover_arm.msg import ArmControlMessage
|
||||||
|
|
||||||
#####################################
|
#####################################
|
||||||
# Global Variables
|
# Global Variables
|
||||||
@@ -17,6 +17,21 @@ GAME_CONTROLLER_NAME = "Afterglow Gamepad for Xbox 360"
|
|||||||
|
|
||||||
DRIVE_COMMAND_HERTZ = 20
|
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
|
# Controller Class Definition
|
||||||
@@ -156,13 +171,16 @@ class ControllerControlSender(QtCore.QThread):
|
|||||||
|
|
||||||
self.wait_time = 1.0 / DRIVE_COMMAND_HERTZ
|
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):
|
def run(self):
|
||||||
self.logger.debug("Starting Joystick Thread")
|
self.logger.debug("Starting Joystick Thread")
|
||||||
|
|
||||||
while self.run_thread_flag:
|
while self.run_thread_flag:
|
||||||
start_time = time()
|
start_time = time()
|
||||||
|
|
||||||
# print self.controller.controller_states
|
self.process_and_send_arm_control()
|
||||||
|
|
||||||
time_diff = time() - start_time
|
time_diff = time() - start_time
|
||||||
|
|
||||||
@@ -173,6 +191,43 @@ class ControllerControlSender(QtCore.QThread):
|
|||||||
def connect_signals_and_slots(self):
|
def connect_signals_and_slots(self):
|
||||||
pass
|
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):
|
def setup_signals(self, start_signal, signals_and_slots_signal, kill_signal):
|
||||||
start_signal.connect(self.start)
|
start_signal.connect(self.start)
|
||||||
signals_and_slots_signal.connect(self.connect_signals_and_slots)
|
signals_and_slots_signal.connect(self.connect_signals_and_slots)
|
||||||
|
|||||||
@@ -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
|
||||||
@@ -1392,6 +1392,708 @@ N/A</string>
|
|||||||
<property name="currentIndex">
|
<property name="currentIndex">
|
||||||
<number>0</number>
|
<number>0</number>
|
||||||
</property>
|
</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">
|
<widget class="QWidget" name="tab_3">
|
||||||
<attribute name="title">
|
<attribute name="title">
|
||||||
<string>Mining</string>
|
<string>Mining</string>
|
||||||
|
|||||||
@@ -151,7 +151,7 @@
|
|||||||
</font>
|
</font>
|
||||||
</property>
|
</property>
|
||||||
<property name="text">
|
<property name="text">
|
||||||
<string>Base Rotation</string>
|
<string>Base</string>
|
||||||
</property>
|
</property>
|
||||||
<property name="alignment">
|
<property name="alignment">
|
||||||
<set>Qt::AlignCenter</set>
|
<set>Qt::AlignCenter</set>
|
||||||
@@ -167,10 +167,10 @@
|
|||||||
</size>
|
</size>
|
||||||
</property>
|
</property>
|
||||||
<property name="minimum">
|
<property name="minimum">
|
||||||
<number>0</number>
|
<number>-500</number>
|
||||||
</property>
|
</property>
|
||||||
<property name="maximum">
|
<property name="maximum">
|
||||||
<number>360</number>
|
<number>500</number>
|
||||||
</property>
|
</property>
|
||||||
<property name="pageStep">
|
<property name="pageStep">
|
||||||
<number>10</number>
|
<number>10</number>
|
||||||
@@ -218,7 +218,7 @@
|
|||||||
</font>
|
</font>
|
||||||
</property>
|
</property>
|
||||||
<property name="text">
|
<property name="text">
|
||||||
<string>Shoulder Pitch</string>
|
<string>Shoulder</string>
|
||||||
</property>
|
</property>
|
||||||
<property name="alignment">
|
<property name="alignment">
|
||||||
<set>Qt::AlignCenter</set>
|
<set>Qt::AlignCenter</set>
|
||||||
@@ -234,10 +234,10 @@
|
|||||||
</size>
|
</size>
|
||||||
</property>
|
</property>
|
||||||
<property name="minimum">
|
<property name="minimum">
|
||||||
<number>0</number>
|
<number>-250</number>
|
||||||
</property>
|
</property>
|
||||||
<property name="maximum">
|
<property name="maximum">
|
||||||
<number>360</number>
|
<number>250</number>
|
||||||
</property>
|
</property>
|
||||||
<property name="pageStep">
|
<property name="pageStep">
|
||||||
<number>10</number>
|
<number>10</number>
|
||||||
@@ -267,7 +267,7 @@
|
|||||||
<double>90.000000000000000</double>
|
<double>90.000000000000000</double>
|
||||||
</property>
|
</property>
|
||||||
<property name="notchesVisible">
|
<property name="notchesVisible">
|
||||||
<bool>false</bool>
|
<bool>true</bool>
|
||||||
</property>
|
</property>
|
||||||
</widget>
|
</widget>
|
||||||
</item>
|
</item>
|
||||||
@@ -289,7 +289,7 @@
|
|||||||
</font>
|
</font>
|
||||||
</property>
|
</property>
|
||||||
<property name="text">
|
<property name="text">
|
||||||
<string>Elbow Pitch</string>
|
<string>Elbow</string>
|
||||||
</property>
|
</property>
|
||||||
<property name="alignment">
|
<property name="alignment">
|
||||||
<set>Qt::AlignCenter</set>
|
<set>Qt::AlignCenter</set>
|
||||||
@@ -305,10 +305,10 @@
|
|||||||
</size>
|
</size>
|
||||||
</property>
|
</property>
|
||||||
<property name="minimum">
|
<property name="minimum">
|
||||||
<number>0</number>
|
<number>-500</number>
|
||||||
</property>
|
</property>
|
||||||
<property name="maximum">
|
<property name="maximum">
|
||||||
<number>360</number>
|
<number>500</number>
|
||||||
</property>
|
</property>
|
||||||
<property name="pageStep">
|
<property name="pageStep">
|
||||||
<number>10</number>
|
<number>10</number>
|
||||||
@@ -338,7 +338,7 @@
|
|||||||
<double>90.000000000000000</double>
|
<double>90.000000000000000</double>
|
||||||
</property>
|
</property>
|
||||||
<property name="notchesVisible">
|
<property name="notchesVisible">
|
||||||
<bool>false</bool>
|
<bool>true</bool>
|
||||||
</property>
|
</property>
|
||||||
</widget>
|
</widget>
|
||||||
</item>
|
</item>
|
||||||
@@ -358,7 +358,7 @@
|
|||||||
</font>
|
</font>
|
||||||
</property>
|
</property>
|
||||||
<property name="text">
|
<property name="text">
|
||||||
<string>Elbow Roll</string>
|
<string>Roll</string>
|
||||||
</property>
|
</property>
|
||||||
<property name="alignment">
|
<property name="alignment">
|
||||||
<set>Qt::AlignCenter</set>
|
<set>Qt::AlignCenter</set>
|
||||||
@@ -374,10 +374,10 @@
|
|||||||
</size>
|
</size>
|
||||||
</property>
|
</property>
|
||||||
<property name="minimum">
|
<property name="minimum">
|
||||||
<number>0</number>
|
<number>-250</number>
|
||||||
</property>
|
</property>
|
||||||
<property name="maximum">
|
<property name="maximum">
|
||||||
<number>360</number>
|
<number>250</number>
|
||||||
</property>
|
</property>
|
||||||
<property name="pageStep">
|
<property name="pageStep">
|
||||||
<number>10</number>
|
<number>10</number>
|
||||||
@@ -407,7 +407,7 @@
|
|||||||
<double>90.000000000000000</double>
|
<double>90.000000000000000</double>
|
||||||
</property>
|
</property>
|
||||||
<property name="notchesVisible">
|
<property name="notchesVisible">
|
||||||
<bool>false</bool>
|
<bool>true</bool>
|
||||||
</property>
|
</property>
|
||||||
</widget>
|
</widget>
|
||||||
</item>
|
</item>
|
||||||
@@ -431,7 +431,7 @@
|
|||||||
</font>
|
</font>
|
||||||
</property>
|
</property>
|
||||||
<property name="text">
|
<property name="text">
|
||||||
<string>End Effector Pitch</string>
|
<string>Wrist Pitch</string>
|
||||||
</property>
|
</property>
|
||||||
<property name="alignment">
|
<property name="alignment">
|
||||||
<set>Qt::AlignCenter</set>
|
<set>Qt::AlignCenter</set>
|
||||||
@@ -447,10 +447,10 @@
|
|||||||
</size>
|
</size>
|
||||||
</property>
|
</property>
|
||||||
<property name="minimum">
|
<property name="minimum">
|
||||||
<number>0</number>
|
<number>-250</number>
|
||||||
</property>
|
</property>
|
||||||
<property name="maximum">
|
<property name="maximum">
|
||||||
<number>360</number>
|
<number>250</number>
|
||||||
</property>
|
</property>
|
||||||
<property name="pageStep">
|
<property name="pageStep">
|
||||||
<number>10</number>
|
<number>10</number>
|
||||||
@@ -480,7 +480,7 @@
|
|||||||
<double>90.000000000000000</double>
|
<double>90.000000000000000</double>
|
||||||
</property>
|
</property>
|
||||||
<property name="notchesVisible">
|
<property name="notchesVisible">
|
||||||
<bool>false</bool>
|
<bool>true</bool>
|
||||||
</property>
|
</property>
|
||||||
</widget>
|
</widget>
|
||||||
</item>
|
</item>
|
||||||
@@ -498,7 +498,7 @@
|
|||||||
</font>
|
</font>
|
||||||
</property>
|
</property>
|
||||||
<property name="text">
|
<property name="text">
|
||||||
<string>End Effector Rotation</string>
|
<string>Wrist Roll</string>
|
||||||
</property>
|
</property>
|
||||||
<property name="alignment">
|
<property name="alignment">
|
||||||
<set>Qt::AlignCenter</set>
|
<set>Qt::AlignCenter</set>
|
||||||
@@ -514,10 +514,10 @@
|
|||||||
</size>
|
</size>
|
||||||
</property>
|
</property>
|
||||||
<property name="minimum">
|
<property name="minimum">
|
||||||
<number>0</number>
|
<number>-1000</number>
|
||||||
</property>
|
</property>
|
||||||
<property name="maximum">
|
<property name="maximum">
|
||||||
<number>360</number>
|
<number>1000</number>
|
||||||
</property>
|
</property>
|
||||||
<property name="pageStep">
|
<property name="pageStep">
|
||||||
<number>10</number>
|
<number>10</number>
|
||||||
@@ -547,7 +547,7 @@
|
|||||||
<double>90.000000000000000</double>
|
<double>90.000000000000000</double>
|
||||||
</property>
|
</property>
|
||||||
<property name="notchesVisible">
|
<property name="notchesVisible">
|
||||||
<bool>false</bool>
|
<bool>true</bool>
|
||||||
</property>
|
</property>
|
||||||
</widget>
|
</widget>
|
||||||
</item>
|
</item>
|
||||||
|
|||||||
@@ -26,6 +26,7 @@ import Framework.SettingsSystems.UbiquitiRadioSettings as UbiquitiRadioSettings
|
|||||||
import Framework.InputSystems.SpaceNavControlSender as SpaceNavControlSender
|
import Framework.InputSystems.SpaceNavControlSender as SpaceNavControlSender
|
||||||
import Framework.MiscSystems.MiningCore as MiningCore
|
import Framework.MiscSystems.MiningCore as MiningCore
|
||||||
import Framework.MiscSystems.BashConsoleCore as BashConsoleCore
|
import Framework.MiscSystems.BashConsoleCore as BashConsoleCore
|
||||||
|
import Framework.MiscSystems.MiscArmCore as MiscArmCore
|
||||||
|
|
||||||
#####################################
|
#####################################
|
||||||
# Global Variables
|
# Global Variables
|
||||||
@@ -105,6 +106,7 @@ class GroundStation(QtCore.QObject):
|
|||||||
|
|
||||||
# ##### Instantiate Regular Classes ######
|
# ##### Instantiate Regular Classes ######
|
||||||
self.__add_non_thread("Mining System", MiningCore.Mining(self.shared_objects))
|
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 ######
|
# ##### Instantiate Threaded Classes ######
|
||||||
self.__add_thread("Video Coordinator", RoverVideoCoordinator.RoverVideoCoordinator(self.shared_objects))
|
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("Joystick Sender", JoystickControlSender.JoystickControlSender(self.shared_objects))
|
||||||
self.__add_thread("Controller Sender", ControllerControlSender.ControllerControlSender(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("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("Rover Status", StatusCore.SensorCore(self.shared_objects))
|
||||||
self.__add_thread("Ubiquiti Status", UbiquitiStatusCore.UbiquitiStatus(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("Ubiquiti Radio Settings", UbiquitiRadioSettings.UbiquitiRadioSettings(self.shared_objects))
|
||||||
self.__add_thread("Waypoints Coordinator", WaypointsCoordinator.WaypointsCoordinator(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("Spacenav Sender", SpaceNavControlSender.SpaceNavControlSender(self.shared_objects))
|
||||||
self.__add_thread("Bash Console", BashConsoleCore.BashConsole(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_and_slots_signal.emit()
|
||||||
self.__connect_signals_to_slots()
|
self.__connect_signals_to_slots()
|
||||||
|
|||||||
@@ -21,7 +21,8 @@ int device_address = 1;
|
|||||||
|
|
||||||
////////// ROS Variables and Defines /////
|
////////// ROS Variables and Defines /////
|
||||||
// ROS Parameter Defaults
|
// 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_absolute_position_control_topic = "control/absolute";
|
||||||
const string default_relative_position_control_topic = "control/relative";
|
const string default_relative_position_control_topic = "control/relative";
|
||||||
@@ -31,33 +32,48 @@ const string default_arm_status_topic = "status";
|
|||||||
// Base
|
// Base
|
||||||
const smuint8 base_address = 1;
|
const smuint8 base_address = 1;
|
||||||
const smint32 base_counts_per_rev = 5725807;
|
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;
|
const double base_max_rev = 0.5;
|
||||||
|
|
||||||
|
smint32 base_min_rev_counts = 0;
|
||||||
|
smint32 base_max_rev_counts = 0;
|
||||||
|
|
||||||
// Shoulder
|
// Shoulder
|
||||||
const smuint8 shoulder_address = 2;
|
const smuint8 shoulder_address = 2;
|
||||||
const smint32 shoulder_counts_per_rev = 2620130;
|
const smint32 shoulder_counts_per_rev = 2620130;
|
||||||
const double shoulder_min_rev = -0.25;
|
const double shoulder_min_rev = -0.25;
|
||||||
const double shoulder_max_rev = 0.25;
|
const double shoulder_max_rev = 0.25;
|
||||||
|
|
||||||
|
smint32 shoulder_min_rev_counts = 0;
|
||||||
|
smint32 shoulder_max_rev_counts = 0;
|
||||||
|
|
||||||
//Elbow
|
//Elbow
|
||||||
const smuint8 elbow_address = 3;
|
const smuint8 elbow_address = 3;
|
||||||
const smint32 elbow_counts_per_rev = 4917661;
|
const smint32 elbow_counts_per_rev = 4917661;
|
||||||
const double elbow_min_rev = -0.5;
|
const double elbow_min_rev = -0.5;
|
||||||
const double elbow_max_rev = 0.5;
|
const double elbow_max_rev = 0.5;
|
||||||
|
|
||||||
|
smint32 elbow_min_rev_counts = 0;
|
||||||
|
smint32 elbow_max_rev_counts = 0;
|
||||||
|
|
||||||
//Roll
|
//Roll
|
||||||
const smuint8 roll_address = 4;
|
const smuint8 roll_address = 4;
|
||||||
const smint32 roll_counts_per_rev = 1637581;
|
const smint32 roll_counts_per_rev = 1637581;
|
||||||
const double roll_min_rev = -0.25;
|
const double roll_min_rev = -0.25;
|
||||||
const double roll_max_rev = 0.25;
|
const double roll_max_rev = 0.25;
|
||||||
|
|
||||||
|
smint32 roll_min_rev_counts = 0;
|
||||||
|
smint32 roll_max_rev_counts = 0;
|
||||||
|
|
||||||
//Wrist Pitch
|
//Wrist Pitch
|
||||||
const smuint8 wrist_pitch_address = 5;
|
const smuint8 wrist_pitch_address = 5;
|
||||||
const smint32 wrist_pitch_counts_per_rev = 3799492;
|
const smint32 wrist_pitch_counts_per_rev = 3799492;
|
||||||
const double wrist_pitch_min_rev = -0.25;
|
const double wrist_pitch_min_rev = -0.25;
|
||||||
const double wrist_pitch_max_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
|
//Wrist Roll
|
||||||
const smuint8 wrist_roll_address = 6;
|
const smuint8 wrist_roll_address = 6;
|
||||||
const smint32 wrist_roll_counts_per_rev = 3799492;
|
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);
|
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() {
|
void run() {
|
||||||
@@ -104,7 +135,6 @@ public:
|
|||||||
reset_controllers();
|
reset_controllers();
|
||||||
|
|
||||||
ros::spinOnce();
|
ros::spinOnce();
|
||||||
// sleep(1);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
@@ -114,8 +144,12 @@ public:
|
|||||||
void reset_controllers() {
|
void reset_controllers() {
|
||||||
if (should_reset) {
|
if (should_reset) {
|
||||||
smSetParameter(arm_bus_handle, base_address, SMP_SYSTEM_CONTROL, SMP_SYSTEM_CONTROL_RESTART);
|
smSetParameter(arm_bus_handle, base_address, SMP_SYSTEM_CONTROL, SMP_SYSTEM_CONTROL_RESTART);
|
||||||
received_first_joint_position_update = false;
|
smSetParameter(arm_bus_handle, shoulder_address, SMP_SYSTEM_CONTROL, SMP_SYSTEM_CONTROL_RESTART);
|
||||||
should_clear_faults = true;
|
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;
|
should_reset = false;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -153,7 +187,6 @@ public:
|
|||||||
smint32 base_position_to_offset;
|
smint32 base_position_to_offset;
|
||||||
smRead2Parameters(arm_bus_handle, base_address, SMP_SERIAL_ENC_OFFSET, &base_current_offset,
|
smRead2Parameters(arm_bus_handle, base_address, SMP_SERIAL_ENC_OFFSET, &base_current_offset,
|
||||||
SMP_ACTUAL_POSITION_FB, &base_position_to_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);
|
smSetParameter(arm_bus_handle, base_address, SMP_SERIAL_ENC_OFFSET, base_current_offset - base_position_to_offset);
|
||||||
|
|
||||||
// Shoulder
|
// Shoulder
|
||||||
@@ -161,23 +194,46 @@ public:
|
|||||||
smint32 shoulder_position_to_offset;
|
smint32 shoulder_position_to_offset;
|
||||||
smRead2Parameters(arm_bus_handle, shoulder_address, SMP_SERIAL_ENC_OFFSET, &shoulder_current_offset,
|
smRead2Parameters(arm_bus_handle, shoulder_address, SMP_SERIAL_ENC_OFFSET, &shoulder_current_offset,
|
||||||
SMP_ACTUAL_POSITION_FB, &shoulder_position_to_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);
|
smSetParameter(arm_bus_handle, shoulder_address, SMP_SERIAL_ENC_OFFSET, shoulder_current_offset - shoulder_position_to_offset);
|
||||||
|
|
||||||
|
|
||||||
// Elbow
|
// 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
|
// 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
|
// 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
|
// 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
|
// 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, 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_reset = true;
|
||||||
should_clear_faults = true;
|
|
||||||
should_calibrate = false;
|
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) {
|
void absolute_position_callback(const rover_arm::ArmControlMessage::ConstPtr &msg) {
|
||||||
if (!received_first_joint_position_update) { return; }
|
if (!received_first_joint_position_update) { return; }
|
||||||
|
|
||||||
@@ -305,10 +369,14 @@ public:
|
|||||||
|
|
||||||
wrist_pitch_set_position = msg->wrist_pitch * wrist_pitch_counts_per_rev;
|
wrist_pitch_set_position = msg->wrist_pitch * wrist_pitch_counts_per_rev;
|
||||||
|
|
||||||
smint32 roll_pitch_position_difference = (wrist_pitch_set_position - wrist_pitch_last_set_position) / 2;
|
smint32 roll_pitch_position_difference = wrist_pitch_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;
|
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_clear_faults = (msg->clear_faults);
|
||||||
should_reset = (msg->reset_controllers);
|
should_reset = (msg->reset_controllers);
|
||||||
@@ -322,8 +390,6 @@ public:
|
|||||||
void relative_position_callback(const rover_arm::ArmControlMessage::ConstPtr &msg) {
|
void relative_position_callback(const rover_arm::ArmControlMessage::ConstPtr &msg) {
|
||||||
if (!received_first_joint_position_update) { return; }
|
if (!received_first_joint_position_update) { return; }
|
||||||
|
|
||||||
ROS_INFO("Shoulder before: %ld", shoulder_set_position);
|
|
||||||
|
|
||||||
base_set_position += msg->base * base_counts_per_rev;
|
base_set_position += msg->base * base_counts_per_rev;
|
||||||
shoulder_set_position += msg->shoulder * shoulder_counts_per_rev;
|
shoulder_set_position += msg->shoulder * shoulder_counts_per_rev;
|
||||||
elbow_set_position += msg->elbow * elbow_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;
|
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;
|
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;
|
wrist_pitch_last_set_position = wrist_pitch_set_position;
|
||||||
|
|
||||||
|
constrain_set_positions();
|
||||||
|
|
||||||
should_clear_faults = (msg->clear_faults);
|
should_clear_faults = (msg->clear_faults);
|
||||||
should_reset = (msg->reset_controllers);
|
should_reset = (msg->reset_controllers);
|
||||||
@@ -346,7 +413,6 @@ public:
|
|||||||
new_positions_received = true;
|
new_positions_received = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
ROS_INFO("Shoulder after: %ld", shoulder_set_position);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|||||||
@@ -79,5 +79,9 @@
|
|||||||
<param name="port" value="17019" />
|
<param name="port" value="17019" />
|
||||||
</node>
|
</node>
|
||||||
|
|
||||||
|
<node name="udp_arm_statuses" pkg="nimbro_topic_transport" type="udp_receiver" output="screen">
|
||||||
|
<param name="port" value="17020" />
|
||||||
|
</node>
|
||||||
|
|
||||||
</group>
|
</group>
|
||||||
</launch>
|
</launch>
|
||||||
|
|||||||
@@ -9,7 +9,8 @@
|
|||||||
[{name: "/rover_control/command_control/ground_station_drive", compress: true, rate: 15.0},
|
[{name: "/rover_control/command_control/ground_station_drive", compress: true, rate: 15.0},
|
||||||
{name: "/rover_control/tower/pan_tilt/control", compress: true, rate: 30.0},
|
{name: "/rover_control/tower/pan_tilt/control", compress: true, rate: 30.0},
|
||||||
{name: "/rover_control/chassis/pan_tilt/control", compress: true, rate: 30.0},
|
{name: "/rover_control/chassis/pan_tilt/control", compress: true, rate: 30.0},
|
||||||
{name: "/rover_control/mining/control", compress: true, rate: 15.0}]
|
{name: "/rover_control/mining/control", compress: true, rate: 15.0},
|
||||||
|
{name: "/rover_arm/control/relative", compress: true, rate: 30.0}]
|
||||||
</rosparam>
|
</rosparam>
|
||||||
</node>
|
</node>
|
||||||
|
|
||||||
@@ -21,7 +22,8 @@
|
|||||||
{name: "/cameras/undercarriage/camera_control", compress: false, rate: 5.0},
|
{name: "/cameras/undercarriage/camera_control", compress: false, rate: 5.0},
|
||||||
{name: "/cameras/main_navigation/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: "/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>
|
</rosparam>
|
||||||
</node>
|
</node>
|
||||||
</group>
|
</group>
|
||||||
|
|||||||
@@ -1,6 +1,7 @@
|
|||||||
<launch>
|
<launch>
|
||||||
<!-- ########## Start Rover Control Nodes ########## -->
|
<!-- ########## Start Rover Control Nodes ########## -->
|
||||||
<include file="$(find rover_main)/launch/rover/control.launch"/>
|
<include file="$(find rover_main)/launch/rover/control.launch"/>
|
||||||
|
<include file="$(find rover_main)/launch/rover/arm.launch"/>
|
||||||
|
|
||||||
<!-- ########## Start All Rover Camera Nodes ########## -->
|
<!-- ########## Start All Rover Camera Nodes ########## -->
|
||||||
<include file="$(find rover_main)/launch/rover/cameras.launch"/>
|
<include file="$(find rover_main)/launch/rover/cameras.launch"/>
|
||||||
|
|||||||
3
software/ros_packages/rover_main/launch/rover/arm.launch
Normal file
3
software/ros_packages/rover_main/launch/rover/arm.launch
Normal file
@@ -0,0 +1,3 @@
|
|||||||
|
<launch>
|
||||||
|
<node name="rover_arm_control" pkg="rover_arm" type="rover_arm" respawn="true" output="screen"/>
|
||||||
|
</launch>
|
||||||
@@ -155,7 +155,17 @@
|
|||||||
{name: "/rover_status/battery_status", compress: false, rate: 1.0},
|
{name: "/rover_status/battery_status", compress: false, rate: 1.0},
|
||||||
{name: "/rover_control/tower/status/co2", compress: false, rate: 1.0},
|
{name: "/rover_control/tower/status/co2", compress: false, rate: 1.0},
|
||||||
{name: "/rover_odometry/imu/data", compress: false, rate: 10.0},
|
{name: "/rover_odometry/imu/data", compress: false, rate: 10.0},
|
||||||
{name: "/rover_control/mining/status", compress: false, rate: 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>
|
</rosparam>
|
||||||
</node>
|
</node>
|
||||||
|
|||||||
Reference in New Issue
Block a user