Description: sends movement commands to ESC's and updates telemetry data node
'''
import rospy, subprocess, sys
from multijoy.msg import MultiJoy
from mobility.msg import Status
from sensor_msgs.msg import Joy
# To import packages from different Directories
rootDir = subprocess.check_output('locate TitanRover2019 | head -1',
shell=True).strip().decode('utf-8')
sys.path.insert(0, rootDir + '/build/resources/python-packages')
from pysaber import DriveEsc
import pyarm
# Instantiating The Class Object For PySabertooth
wheels = DriveEsc(128, "mixed")
armMix = DriveEsc(129, "notMixed")
IDLE_TIMEOUT = 15 #seconds
#use actual button numbers instead of 0-indexed array
#comms source for reference (variables not used)
ERROR = -1
LOCAL = 0
GHZ = 1
MHZ = 2
#modes
PAUSE = -1 #LS + B3
IDLE = 0
MOBILITY = 1 #LS + B2
# Given a single GPS coordinate, the Rover will drive to this point (within a
# predetermined threshold). Driving occurs in a linear fashion.
# Given a heading and distance (cm), GPS coordinate will be generated and
# the Rover will drive to this point.
# Give a heading, the Rover will rotate in place to face this direction.
######################################################################################
import time
import sys
import math
import numpy as np # remove if no longer needed
from decimal import Decimal
import rospy
from pysaber import DriveEsc
from gnss.msg import gps
from finalimu.msg import fimu
wheels = DriveEsc(128, "mixed")
MINFORWARDSPEED = 20
MAXFORWARDSPEED = 30
TARGETTHRESHOLD = 40 # In cm
CORRECTIONTHRESHOLD = 10 # In degrees
HEADINGTHRESHOLD = 15 # In degrees
class Driver:
def __init__(self):
'''
# Tailored to Runt Rover
self.__angleX = [5, 15, 25]
self.__rotateY = [40, 50, 90]
import subprocess
import sys
# To import packages from different Directories
rootDir = subprocess.check_output('locate TitanRover2019 | head -1', shell=True).strip().decode('utf-8')
sys.path.insert(0, rootDir + '/build/resources/python-packages')
from pysaber import DriveEsc
motors = DriveEsc(129, "mixed")
while True:
motors.driveBoth(127, 127)
import rospy
from multijoy.msg import MultiJoy
from sensor_msgs.msg import Joy
import subprocess, sys
global drill
drill = 0.5
# To import packages from different Directories
rootDir = subprocess.check_output('locate TitanRover2019 | head -1', shell=True).strip().decode('utf-8')
sys.path.insert(0, rootDir + '/build/resources/python-packages')
from pysaber import DriveEsc
linear = DriveEsc(129, "notMixed")
motors = DriveEsc(128, "notMixed")
def main(data):
global drill
if data.joys[0].buttons[1] is 1 and drill >= 0.1:
drill -= 0.05
if data.joys[0].buttons[3] is 1 and drill <= 1.0:
drill += 0.05
#print(drill)
motors.driveBoth(40*int(data.joys[0].axes[2]), 40*int(data.joys[0].axes[2]))
#print(40*int(data.joys[0].axes[2]), 40*int(data.joys[0].axes[2]))
linear.driveBoth(127*drill*int(data.joys[0].axes[5]), 127*drill*int(data.joys[0].axes[4]))
#print(127*drill*int(data.joys[0].axes[5]), 127*drill*int(data.joys[0].axes[4]))
if __name__ == '__main__':