def scanStop(ser, u_prev, t_pan):
print('This is scanStop: ' + str(u_prev))
# u_prev is speed e (-250, 250)
t_begin = GS_timing.millis()
try:
#print("Writing: ", str.encode(str(convert_degS_code(speed,error)) + '\n'))
ser.write(
str.encode(
str(convert_degS_code2(np.copysign(60, u_prev), 9000)) + '\n'))
# time.sleep(T_sample)
except (OSError, serial.SerialException):
#print("Serial Exception Raised")
ser.close()
ser = initialize_serial(ser)
except (OSError, serial.SerialTimeoutException):
#print("Serial Timeour Exception Raised")
ser.close()
ser = initialize_serial(ser)
# wait until camera has rotated through large enough angle.
while (GS_timing.millis() - t_begin < t_pan * 1000):
pass # do nothing
try:
ser.write(str.encode(str(convert_degS_code2(0, 9000)) + '\n'))
except (OSError, serial.SerialException):
ser.close()
ser = initialize_serial(ser)
except (OSError, serial.SerialTimeoutException):
ser.close()
ser = initialize_serial(ser)
return ser, u_prev
def renderPosTWO_D(self):
startTime = accurateClock.millis()
self.lineTWO_D.set_data((self.posXTWO_D, self.posYTWO_D))
self.axTWO_D.draw_artist(self.lineTWO_D)
endTime = accurateClock.millis()
def reverseDirection(ser, u_prev, t_switch):
print('This is reverseDirection: ' + str(u_prev))
u_new = u_prev * (-1)
t_begin = GS_timing.millis()
try:
#print("Writing: ", str.encode(str(convert_degS_code(speed,error)) + '\n'))
ser.write(str.encode(str(convert_degS_code2(u_new, 9000)) + '\n'))
# time.sleep(T_sample)
except (OSError, serial.SerialException):
#print("Serial Exception Raised")
ser.close()
ser = initialize_serial(ser)
except (OSError, serial.SerialTimeoutException):
#print("Serial Timeour Exception Raised")
ser.close()
ser = initialize_serial(ser)
while (GS_timing.millis() - t_begin < t_switch * 1000):
pass # do nothing
try:
ser.write(str.encode(str(convert_degS_code2(0, 9000)) + '\n'))
except (OSError, serial.SerialException):
ser.close()
ser = initialize_serial(ser)
except (OSError, serial.SerialTimeoutException):
ser.close()
ser = initialize_serial(ser)
return ser, u_new
def renderPos(self):
startTime = accurateClock.millis()
self.line.set_data((self.posX, self.posY))
self.line.set_3d_properties(self.posZ)
self.ax.draw_artist(self.line)
endTime = accurateClock.millis()
def cmd_publisher():
pub = rospy.Publisher('cmd_angle_topic', cmd_angles, queue_size=10)
rospy.init_node('cmd_publisher', anonymous=False)
rate = rospy.Rate(100) # 1 Hz
starttime = timing.micros()
while not rospy.is_shutdown():
timenow = timing.micros()
timenow = starttime - timenow
t1 = timenow / 1000000
angles = cmd_angles()
angles.angle1 = amplitude * sin(frequency * (t1) + 1 * np.pi / 4)
angles.angle2 = amplitude * sin(frequency * (t1) + 2 * np.pi / 4)
angles.angle3 = amplitude * sin(frequency * (t1) + 3 * np.pi / 4)
angles.angle4 = amplitude * sin(frequency * (t1) + 4 * np.pi / 4)
angles.angle5 = amplitude * sin(frequency * (t1) + 5 * np.pi / 4)
pub.publish(angles)
rate.sleep()
def cmd_publisher():
pub = rospy.Publisher('cmd_angle_topic', cmd_angles, queue_size=10)
rospy.init_node('cmd_publisher', anonymous=False)
rate = rospy.Rate(100) # 1 Hz
starttime = timing.micros()
while not rospy.is_shutdown():
timenow = timing.micros()
timenow = starttime - timenow
t1 = timenow / 1000000
angles = cmd_angles()
angles.angle1 = amplitude * sin(
1 * (t1) +
np.pi / 2) #amplitude*sin(frequency*(time.time()-1*phase_offset))
angles.angle2 = amplitude * sin(
1 * (t1) +
np.pi) #amplitude*sin(frequency*(time.time()-2*phase_offset))
angles.angle3 = amplitude * sin(
1 * (t1) + 3 * np.pi / 2
) #30.*sin(0.3*(t1)+np.pi/8)#amplitude*sin(frequency*(time.time()-3*phase_offset))
angles.angle4 = 0.0 #28.*sin(0.3*(t1)+np.pi/16)#30.*sin(1.*(t1))
angles.angle5 = 0.0 #amplitude*sin(frequency*(time.time()-5*phase_offset))
pub.publish(angles)
rate.sleep()
def readFromPort(self):
connected = True
retval = None
# while self.serialPort.inWaiting():
while True:
#reading = serial_port.read().decode('ascii')
reading = self.serialPort.read()
# if(reading!=b''):
# print(reading)
if(reading == b'<'):
self.buff=bytearray()
self.startTime = gt.millis()
elif(reading==b'>'):
retval = moveStreamToStruct(self.buff)
self.buff=bytearray()
# if(self.startTime != None):
# print(gt.millis()-self.startTime)
return retval
else:
self.buff+=reading
return retval
"""
test.py
By Gabriel Staples
http://www.ElectricRCAircraftGuy.com
-click "Contact me" at the top of my website to find my email address
13 Aug. 2016
"""
import GS_timing as timing
print(timing.micros())
print(timing.millis())
print('version = ' + timing.VERSION)
print(timing._constrain(10, 11, 15)) #11
def millis():
start_time=GS_timing.millis()
while True:
yield GS_timing.millis()-start_time
def delayMicroseconds(n):
time.sleep(n / 1000000.)
def delayMillisecond(n):
time.sleep(n / 1000.)
t_start = 0
t_end = 0
#using time.sleep
print('using time.sleep')
print('delayMicroseconds(1)')
for x in range(10):
t_start = timing.micros() #us
delayMicroseconds(1)
t_end = timing.micros() #us
print('dt (us) = ' + str(t_end - t_start))
print('delayMicroseconds(2000)')
for x in range(10):
t_start = timing.micros() #us
delayMicroseconds(2000)
t_end = timing.micros() #us
print('dt (us) = ' + str(t_end - t_start))
#using GS_timing
print('\nusing GS_timing')
print('timing.delayMicroseconds(1)')
for x in range(10):
t_start = timing.micros() #us
def scan_camera(ser, t_pan):
global u_prev, t_continue, accrued_pan
#if scanning mode, rotate slowly to new position.
#accrued_pan is a variable that accumulates and...
#...lets us know when we should reverse the pan direction.
#desired speed changes when accrued pan reaches +/- 90deg.
#global variables: u_prev, accrued_pan, t_continue.
#start in the direction with greatest margin.
desired_speed = 0
if accrued_pan < 0:
desired_speed = 120
else:
desired_speed = 70
accrued_pan += u_prev * ((GS_timing.millis() - t_continue) / 1000)
t_scan = GS_timing.millis()
t_start_inc = GS_timing.millis()
#write these regardless of overshoot.
increments = smooth_u(desired_speed, u_prev)
for speed in increments:
try:
#print("Writing: ", str.encode(str(convert_degS_code(speed,error)) + '\n'))
ser.write(str.encode(str(convert_degS_code(speed, 1.5)) + '\n'))
#time.sleep(T_sample)
except (OSError, serial.SerialException):
#print("Serial Exception Raised")
ser.close()
ser = initialize_serial(ser)
except (OSError, serial.SerialTimeoutException):
#print("Serial Timeour Exception Raised")
ser.close()
ser = initialize_serial(ser)
accrued_pan += u_prev * ((GS_timing.millis() - t_scan) / 1000)
t_scan = GS_timing.millis()
u_prev = speed
while (GS_timing.millis() - t_start_inc < T_sample_inc * 1000):
accrued_pan += speed * ((GS_timing.millis() - t_scan) / 1000)
t_scan = GS_timing.millis()
pass #do nothing
while (GS_timing.millis() - t_start_inc < t_pan * 1000):
accrued_pan += u_prev * ((GS_timing.millis() - t_scan) / 1000)
t_scan = GS_timing.millis()
if abs(accrued_pan) >= 90:
print("LINE 77")
desired_speed = flip_direction(desired_speed)
u_prev = desired_speed
try:
#argument of 1.5 should keep LED off.
ser.write(
str.encode(str(convert_degS_code(desired_speed, 1.5)) + '\n'))
#time.sleep(T_sample)
except (OSError, serial.SerialException):
#print("Serial Exception Raised")
ser.close()
ser = initialize_serial(ser)
except (OSError, serial.SerialTimeoutException):
#print("Serial Timeour Exception Raised")
ser.close()
ser = initialize_serial(ser)
#set final speed to zero and return.
try:
ser.write(str.encode(str(convert_degS_code(0, 1.5)) + '\n'))
#time.sleep(T_sample)
except (OSError, serial.SerialException):
#print("Serial Exception Raised")
ser.close()
ser = initialize_serial(ser)
except (OSError, serial.SerialTimeoutException):
#print("Serial Timeour Exception Raised")
ser.close()
ser = initialize_serial(ser)
accrued_pan += u_prev * ((GS_timing.millis() - t_scan) / 1000)
u_prev = 0
t_continue = GS_timing.millis()
return ser
#represents rotation of motor in degrees. Update rule:
#accrued_pan += angular_speed(deg/s)*time(s)*1.05
#needs to stay within +/- 90 for scanning mode.
#include tolerance for error(i.e. *1.05)
#assume under normal operation this variable will remain within +/- 90
accrued_pan = 0
coord = 0
while True:
if scanning == 1:
#this will rotate the camera at slowest speed for given amt. of time.
#once the camera stops, then the object recognition algo. should execute.
ser = scan_camera(ser, 0.1)
t_start = GS_timing.millis()
# grab the current frame, then handle if we are using a
# VideoStream or VideoCapture object
frame = vs.read()
frame = frame[1] if args.get("video", False) else frame
# check to see if we have reached the end of the stream
if frame is None:
print('end of stream')
break
# resize the frame (so we can process it faster) and grab the
# frame dimensions
frame = imutils.rotate_bound(frame, 90)
frame = imutils.resize(frame, height=500)
vs = VideoStream(src=0).start()
time.sleep(1.0)
# otherwise, grab a reference to the video file
else:
vs = cv2.VideoCapture(args["video"])
# initialize the FPS throughput estimator
fps = None
#store previous value of u.
u_prev = 0
# loop over frames from the video stream
i = 0
while True:
t_start = GS_timing.millis()
# grab the current frame, then handle if we are using a
# VideoStream or VideoCapture object
frame = vs.read()
frame = frame[1] if args.get("video", False) else frame
# check to see if we have reached the end of the stream
if frame is None:
break
# resize the frame (so we can process it faster) and grab the
# frame dimensions
frame = imutils.resize(frame, width=500)
(H, W) = frame.shape[:2]
coord = 249.5
import GS_timing as timing
def delayMicroseconds(n):
time.sleep(n / 1000000.)
def delayMillisecond(n):
time.sleep(n / 1000.)
t_start = 0
t_end = 0
#using time.sleep
print('using time.sleep')
print('delayMicroseconds(1)')
for x in range(10):
t_start = timing.micros() #us
delayMicroseconds(1)
t_end = timing.micros() #us
print('dt (us) = ' + str(t_end - t_start))
print('delayMicroseconds(2000)')
for x in range(10):
t_start = timing.micros() #us
delayMicroseconds(2000)
t_end = timing.micros() #us
print('dt (us) = ' + str(t_end - t_start))
#using GS_timing
print('\nusing GS_timing')
print('timing.delayMicroseconds(1)')
for x in range(10):
t_start = timing.micros() #us
if __name__ == '__main__':
lowpriority()
guiCaller = gui.DataGui()
dataHandler = DataHandlerClass("/dev/ttyACM0", 115200)
dataHandler.flush()
eskfProcessor = EskfGlue()
# dataGetter.init()
# guiCaller.callLoop()
i = 0
try:
while (True):
i += 1
i %= 1000
# time.sleep(1)
startTime = gt.millis()
meas = dataHandler.readFromPort()
if (meas == None):
print("MEAS RETURN NONE")
continue
#Process ESKF
# print("PROC ESKF")
eskfProcessor.processESKF(meas)
# print(gt.millis()-startTime)
#Update GUI
pos = eskfProcessor.getPosition()
quat = eskfProcessor.getQuaternion()
render = False
if (USE_GUI and i % 20 == 0):
print("GUI START")
render = True
"""
test.py
By Gabriel Staples
http://www.ElectricRCAircraftGuy.com
-click "Contact me" at the top of my website to find my email address
13 Aug. 2016
"""
import GS_timing as timing
print(timing.micros())
print(timing.millis())
print('version = ' + timing.VERSION)
print(timing._constrain(10,11,15)) #11