def __init__(self, pin, timer, channel):
''' Initializes the infared receiver
@param pin: pin is a pyb.Pin.board object
for the interrupt pin that will detect interrupts
from the IR reciever
@param timer: timer is the timer the interrupt pin will use
@param channel: channel is the channel the timer will use
'''
#----------------------------------------------------------------------#
# Allocate memory so that exceptions raised in interrupt service
# routines can generate useful diagnostic printouts
# comment this line out after testing
alloc_emergency_exception_buf(100)
#----------------------------------------------------------------------#
# assign the pin as an input pin
intPin = pyb.Pin(pin, pyb.Pin.IN)
# channel is the channel the timer will use, specified by the function
# parameter. Has to be brought to the Class scope because it is used in
# the irISR callback function
self.channel = channel
# Assign the timer a 16-bit period and a prescaler of 79 to collect
# accurate timestamps. Prescaler of 79 to account for 80Mhz clock speed
# to output counts as microseconds
tim = pyb.Timer(timer, prescaler=79, period=0xFFFF)
# set up the timer object to detect rising and fallingedges
tim.channel(channel,
pyb.Timer.IC,
polarity=pyb.Timer.BOTH,
pin=intPin,
callback=self.irISR)
# A queue to be used as a buffer for interrupt timestamp data. Buffer
# size is greater than full pulse count to account for repeat codes
# that disrupt a full pulse set.
self.ir_data = task_share.Queue('I',
200,
thread_protect=False,
overwrite=False,
name="ir_data")
# Data is a class scoped list that is filled with timestamps
# from the ir_data queue
self.data = []
self.address = task_share.Share('I',
thread_protect=False,
name="address")
self.command = task_share.Share('I',
thread_protect=False,
name="address")
self.command.put(0)
self.address.put(0)
self.task = cotask.Task(self.readInfaredSensorTask,
name='Infared Reading Task',
priority=5,
profile=True,
trace=False)
thread_protect=False,
overwrite=False,
name="Pan_Coords")
#Tilt Coordinates Queue is used to deliver target tilt IMU value to
#Tilt Motor Task from Turret Hub Task
tilt_coords = task_share.Queue('f',
2,
thread_protect=False,
overwrite=False,
name="Tilt_Coords")
#Pan Position Share is used to deliver current encoder value to
#to the Turret Hub and Pan Motor tasks from the Encoder Task
pan_position = task_share.Share('f',
thread_protect=False,
name="Pan_Position")
#Tilt Angle Share is used to deliver current IMU pitch value to
#to the Turret Hub and Tilt Motor tasks from the Encoder Task
tilt_angle = task_share.Share('f',
thread_protect=False,
name="Tilt_Position")
#Share Sent from Turret Hub Task to Nerf Gun Task to Start Feeding Bullets
FEED_BULLETS = task_share.Share('i',
thread_protect=False,
name="Feed_Bullets")
#Share sent from Turret Hub Task to Nerf Gun Task to
WINDUP_GUN = task_share.Share('i', thread_protect=False, name="Windup_Gun")
state = 0
yield (state)
# =============================================================================
if __name__ == "__main__":
vcp.write('Welcome to the STM32 Real-Time Operating System.\r\n'.encode())
import print_task
# Create a share and some queues to test diagnostic printouts
share0 = task_share.Share('i', thread_protect=False, name="Share_0")
# Shares for the interface to tell each motor where its destination is
span_setpoint = task_share.Share('f',
thread_protect=True,
name="Span_Motor_Setpoint")
tilt_setpoint = task_share.Share('f',
thread_protect=True,
name="Tilt_Motor_Setpoint")
# Shares for directly operating both motors from the interface.
span_mot_op = task_share.Share('i',
thread_protect=True,
name="Span_Motor_Operator")
tilt_mot_op = task_share.Share('i',
thread_protect=True,
elif state == 2:
pass
yield (state)
# =============================================================================
# Create the tasks. If trace is enabled for any task, memory will be allocated
# for state transition tracing, and the application will run out of memory
# after a while and quit. Therefore, use tracing only for debugging and set
# trace to False when it's not needed.
# Create inter-task communication variables
pos_ctrl1 = task_share.Share('H',
thread_protect=False,
name='Position Control 1')
setpoint1 = task_share.Share('I', thread_protect=False, name='Setpoint 1')
step_rsp1 = task_share.Share('H', thread_protect=False, name='Step Response 1')
get_data1 = task_share.Share('H', thread_protect=False, name='Get Data 1')
pos_ctrl2 = task_share.Share('H',
thread_protect=False,
name='Position Control 2')
setpoint2 = task_share.Share('I', thread_protect=False, name='Setpoint 2')
step_rsp2 = task_share.Share('H', thread_protect=False, name='Step Response 2')
get_data2 = task_share.Share('H', thread_protect=False, name='Get Data 2')
# Create tasks
motor1_task = cotask.Task(motor1_fun,
name='Motor 1',
priority=2,
#Address = (int(listToStr1,2))
Command = (int(listToStr3, 2))
IRShare.put(int(Command))
print('IRShare Command ' + str(IRShare.get()))
#print('----------New Packet----------\nRaw: 0b'+str(listToStr)+'\n\n ADDR: 0b'+str(listToStr1)+'\nnADDR: 0b'+str(listToStr2)+'\n CMD: 0b'+str(listToStr3)+'\n nCMD: 0b'+str(listToStr4)+'\n\nAddress (Decimal): '+str(Address)+'\nCommand (Decimal): '+str(Command)+'\n\n')
del IRdata[:]
del pulseWidth[:]
del parseData[:]
fullFlag = 0
yield None
yield None
'''Initialization of i2c, shares, queues, and objects used across multiple tasks'''
i2c = I2C(1, freq=200000) #Uses bus 3 because of the pins it is connected to
TOF1Share = task_share.Share('l')
TOF2Share = task_share.Share('l')
q0 = task_share.Queue('I',
68,
thread_protect=False,
overwrite=False,
name="Queue_0")
IRShare = task_share.Share('i')
shareIMU = task_share.Share('f')
shareLine = task_share.Share('i')
encoderR = EncoderDriver('PB6', 'PB7', 4, direction='clockwise')
encoderL = EncoderDriver('PC6', 'PC7', 8, direction='counterclockwise')
controllerR = ClosedLoopDriver(0, 0, .2, 100)
motorR = MotorDriver()
controllerL = ClosedLoopDriver(0, 0, .2, 20)
motorL = MotorDriver('PC1', 'PA0', 'PA1', 5, 100)
while (1):
## The shared variable q8 holds the current position of the pitch motor
q8.put(enc1.read())
## The shared variable q9 holds the current position of the roll motor
q9.put(enc2.read())
yield (0)
# =============================================================================
if __name__ == "__main__":
# Create a share and some queues to test diagnostic printouts
## Hold pitch values
q2 = task_share.Share('f', thread_protect=False, name="Queue_2")
## Hold roll values
q3 = task_share.Share('f', thread_protect=False, name="Queue_3")
## Hold pitch motor values
q4 = task_share.Share('f', thread_protect=False, name="Queue_4")
## Hold roll motor values
q5 = task_share.Share('f', thread_protect=False, name="Queue_5")
## Hold pitch motor error_sum
q6 = task_share.Share('f', thread_protect=False, name="Queue_6")
## Hold roll motor error_sum
q7 = task_share.Share('f', thread_protect=False, name="Queue_7")
yield (state)
# reinitialize the time and position array
# https://stackoverflow.com/questions/850795/different-ways-of-clearing-lists
times_1[:] = []
positions_1[:] = []
state = 1
yield (state)
# =============================================================================
if __name__ == "__main__":
# Create a share and some queues to test diagnostic printouts
enc_1_position = task_share.Share('i',
thread_protect=False,
name="Share_0_enc_1_position")
enc_2_position = task_share.Share('i',
thread_protect=False,
name="Share_0_enc_2_position")
Run = task_share.Share('i',
thread_protect=False,
name="Run_Intertask_Comm_Variable")
# Create the tasks. If trace is enabled for any task, memory will be
# allocated for state transition tracing, and the application will run out
# of memory after a while and quit. Therefore, use tracing only for
# debugging and set trace to False when it's not needed
task1 = cotask.Task(Encoder1_fun,
name='Task_1',
for time in times:
times[i] = time - starttime
i += 1
print(positions)
print(times)
# =============================================================================
if __name__ == "__main__":
print('\033[2JTesting scheduler in cotask.py\n')
# Create a share and some queues to test diagnostic printouts
share0 = task_share.Share('i', thread_protect=False, name="Share_0")
q0 = task_share.Queue('B',
6,
thread_protect=False,
overwrite=False,
name="Queue_0")
q1 = task_share.Queue('B',
8,
thread_protect=False,
overwrite=False,
name="Queue_1")
# Create the tasks. If trace is enabled for any task, memory will be
# allocated for state transition tracing, and the application will run out
# of memory after a while and quit. Therefore, use tracing only for
# debugging and set trace to False when it's not needed
#Done flag for get more paint state
SAUCED = 0
#NEW Value flag
NEWVAL1 = 0
NEWVAL2 = 0
NEWVAL3 = 0
#Motor done flags
mdone1 = 0
mdone2 = 0
mdone3 = 0
#flag to say file has been laoded
loaded = 0
print('\033[2JTesting scheduler in cotask.py\n')
# Create a share and some queues to test diagnostic printouts
sm1 = task_share.Share('f', thread_protect=False, name="Share_1")
sm2 = task_share.Share('f', thread_protect=False, name="Share_2")
sm3 = task_share.Share('f', thread_protect=False, name="Share_3")
# md1 = task_share.Share ('B', thread_protect = False, name = "mdone1")
# md2 = task_share.Share ('B', thread_protect = False, name = "mdone2")
# md_3 = task_share.Share ('B', thread_protect = False, name = "mdone3")
# nv = task_share.Share ('B', thread_protect = False, name = "new value")
# Create the tasks. If trace is enabled for any task, memory will be
# allocated for state transition tracing, and the application will run out
# of memory after a while and quit. Therefore, use tracing only for
# debugging and set trace to False when it's not needed
task1 = cotask.Task(Control1,
name='Control1',
priority=4,
period=10,
profile=True,
print(" creating interrupt for emergency stop")
# The same as the Go pin, but greating a short function call for the emergency
# stop button. This pin is defined last as to prevent memory issues found
# importing task_share.
Stop_Pin = pyb.Pin(pyb.Pin.cpu.C5, mode=pyb.Pin.IN, pull=pyb.Pin.PULL_DOWN)
'''External Interrupt Pin'''
import micropython
micropython.alloc_emergency_exception_buf(100)
extint = pyb.ExtInt(Stop_Pin, pyb.ExtInt.IRQ_FALLING, pyb.Pin.PULL_DOWN,
callback)
import task_share
# Variable Buffer Creation
ErrInit = task_share.Share('i',
thread_protect=False,
name="Initilization Error Flag")
ErrSleep = task_share.Share('i', thread_protect=False, name="Sleep Error Flag")
ErrCalibration = task_share.Share('i',
thread_protect=False,
name="Sleep Error Flag")
ErrLeveling = task_share.Share('i',
thread_protect=False,
name="X-Beam Leveling Error Flag")
ErrAssembly = task_share.Share('i',
thread_protect=False,
name="X-Beam Error Flag")
ErrBoltCsv = task_share.Share('i',
thread_protect=False,
name="BoltPattern.csv Error Flag")
ErrCalCsv = task_share.Share('i',
# M2 = M.MotorDriver(P.M2DIR, P.M2PWM, C.MOT_PWM_TIMER,
# C.MOT2_PWM_CH, C.MOT_FREQ, False)
M1 = M.MotorDriver(P.M1DIR, P.M1PWM, C.MOT1_PWM_TIMER, C.MOT1_PWM_CH,
C.MOT_FREQ, True)
M2 = M.MotorDriver(P.M2DIR, P.M2PWM, C.MOT2_PWM_TIMER, C.MOT2_PWM_CH,
C.MOT_FREQ, False)
MC1 = M.MotorController()
MC2 = M.MotorController()
ENC1 = M.Encoder(P.ENC1A, C.ENC1A_CH, P.ENC1B, C.ENC1B_CH, C.ENC1_TIMER, True)
ENC2 = M.Encoder(P.ENC2A, C.ENC2A_CH, P.ENC2B, C.ENC2B_CH, C.ENC2_TIMER)
IR = infared.Infared(P.IR, C.IR_TIMER, C.IR_CH)
DRIVE = M.Drive(M1, M2, ENC1, ENC2, MC1, MC2)
run = task_share.Share('I', thread_protect=False, name="run")
turn = task_share.Share('I', thread_protect=False, name="turn")
turn.put(0)
turning = task_share.Share('I', thread_protect=False, name="turning")
turning.put(0)
us_front = task_share.Share('I', thread_protect=False, name="front approach")
us_front.put(0)
us_rear = task_share.Share('I', thread_protect=False, name="back approach")
us_rear.put(0)
us_left = task_share.Share('I', thread_protect=False, name="left approach")
us_left.put(0)
us_right = task_share.Share('I', thread_protect=False, name="right approach")
us_right.put(0)
us_last = task_share.Share('I', thread_protect=False, name="last approach")
# set up interrupts and timer for ir sensor
timer_pin = pyb.Pin (pyb.Pin.board.PA8, pyb.Pin.IN)
timer_1 = pyb.Timer(1, period = 0xFFFF, prescaler = 79)
channel_1 = timer_1.channel(1, mode = pyb.Timer.IC, polarity = pyb.Timer.BOTH,
pin=timer_pin)
channel_1.callback(interrupt)
ir_time_queue = Queue("I", 68)
ir_full_flag = Share("i")
ir_full_flag.put(0, in_ISR = True)
#setup master controller share
master_go = Share("i")
master_go.put(0, in_ISR = False)
#Robot States
attack = task_share.Share ('i', thread_protect = False, name = "attack")
attack.put(0)
scan = task_share.Share ('i', thread_protect = False, name = "scan")
scan.put(0)
retreat = task_share.Share ('i', thread_protect = False, name = "retreat")
retreat.put(0)
retreating = task_share.Share ('i', thread_protect = False, name = "retreat")
retreating.put(0)
#setup motion control shares
ready_left = task_share.Share ('i', thread_protect = False, name = "right_flag")
ready_left.put(1)
i2c = pyb.I2C(1, pyb.I2C.MASTER)
imu = mybno055.BNO055(i2c)
imu.calibrate(
) # Resets the heading axis of the IMU on startup so that its angle is 0
heading_motor = ProjectClasses.MotorDriver(3, pyb.Pin.board.PA10,
pyb.Pin.board.PB4,
pyb.Pin.board.PB5)
heading_motor.set_duty_cycle(
20) # Slightly adjust the turret angle to a low positive value
time.sleep(0.1)
heading_motor.set_duty_cycle(0)
# Creating all of the intertask communications variables
heading_share = task_share.Share('f',
thread_protect=True,
name="heading_share")
pitch_share = task_share.Share('f',
thread_protect=True,
name="pitch_share")
heading_setpoint_share = task_share.Share('f',
thread_protect=True,
name="heading_setpoint")
pitch_setpoint_share = task_share.Share('f',
thread_protect=True,
name="pitch_setpoint")
heading_OK = task_share.Share('f', thread_protect=True, name="heading_OK")
pitch_OK = task_share.Share('f', thread_protect=True, name="pitch_OK")
trigger_ready_share = task_share.Share('f',
thread_protect=True,
name="trigger_ready_share")
# params: [status: zero or position; target: 60 steps; speed: 14 steps/sec; accel: 20 steps/sec^2]
x_params = task_share.Queue ('b', 5, thread_protect = False,
overwrite = False, name = "x_params")
z_params = task_share.Queue ('b', 5, thread_protect = False,
overwrite = False, name = "z_params")
y_params = task_share.Queue ('b', 5, thread_protect = False,
overwrite = False, name = "y_params")
p_params = task_share.Queue ('b', 5, thread_protect = False,
overwrite = False, name = "p_params")
# HIGO = task_share.Queue ('b', 50, thread_protect = False,
# overwrite = False, name = "HIGO")
# HOGI = task_share.Queue ('b', 50, thread_protect = False,
# overwrite = False, name = "HOGI")
x_encoder = task_share.Share ('f', thread_protect = False,
name = "x_encoder")
z_encoder = task_share.Share ('f', thread_protect = False,
name = "z_encoder")
y_encoder = task_share.Share ('f', thread_protect = False,
name = "y_encoder")
p_encoder = task_share.Share ('f', thread_protect = False,
name = "p_encoder")
x_limit = task_share.Share ('i', thread_protect = False,
name = "x_limit")
z_limit = task_share.Share ('i', thread_protect = False,
name = "z_limit")
y_limit = task_share.Share ('i', thread_protect = False,
name = "y_limit")
p_limit = task_share.Share ('i', thread_protect = False,
name = "p_limit")
# p_params = task_share.Queue ('f', 10, thread_protect = False,
# overwrite = False, name = "p_params")
# Motor Parameters
x_steps = task_share.Queue('f',
5,
thread_protect=False,
overwrite=False,
name="x_steps")
# z_steps = task_share.Queue ('f', 5, thread_protect = False,
# overwrite = False, name = "z_steps")
# y_steps = task_share.Queue ('f', 5, thread_protect = False,
# overwrite = False, name = "y_steps")
# p_steps = task_share.Queue ('f', 5, thread_protect = False,
# overwrite = False, name = "p_steps")
# Motor Positioning Status
x_status = task_share.Share('f', thread_protect=False, name="x_status")
# z_status = task_share.Share ('f', thread_protect = False,
# name = "z_status")
# y_status = task_share.Share ('f', thread_protect = False,
# name = "y_status")
# p_status = task_share.Share ('f', thread_protect = False,
# name = "p_status")
# Motor Enable
x_enable = task_share.Share('i', thread_protect=True, name="x_enable")
# z_enable = task_share.Share ('i', thread_protect = True,
# name = "z_enable")
# y_enable = task_share.Share ('i', thread_protect = True,
# name = "y_enable")
# p_enable = task_share.Share ('i', thread_protect = True,
# name = "p_enable")
# Encoder Value
