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)
# generate useful diagnostic printouts
micropython.alloc_emergency_exception_buf(100)
# =========================================================================== #
# ======================== Run the Turret Code ============================== #
# =========================================================================== #
if __name__ == "__main__":
# Create share and queue variables
#Pan Coordinates Queue is used to deliver target pan encoder value to
#Pan Motor Task from Turret Hub Task
pan_coords = task_share.Queue('f',
2,
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,
"""
while True:
# If there's a character in the queue, print it
if print_queue.any():
# print (chr (print_queue.get ()), end = '')
print(print_queue.get(), end='')
# If there's another character, tell this task to run again ASAP
if print_queue.any():
print_task.go()
yield (0)
## This queue holds characters to be printed when the print task gets around
# to it.
global print_queue
print_queue = task_share.Queue('B',
BUF_SIZE,
name="Print_Queue",
thread_protect=THREAD_PROTECT,
overwrite=False)
## This is the task which schedules printing.
global print_task
print_task = cotask.Task(run, name='Printing', priority=0, profile=PROFILE)
# This line tells the task scheduler to add this task to the system task list
cotask.task_list.append(print_task)
# Share for entering IMU mode
imu_mode = task_share.Share('i', thread_protect=False, name="IMU_Mode")
# Share for the interface to tell the calculator that it is time to run a calibration
calibrate_calc = task_share.Share('i',
thread_protect=False,
name="Calibrate_Calculation")
# Share for the interface to tell the motors to run a calibration procedure.
run_calibration = task_share.Share('i',
thread_protect=False,
name="Run_Calibration")
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")
# Queue for storing calibration measurements used by the calculator.
calibration_data = task_share.Queue('f',
6,
thread_protect=False,
overwrite=False,
name="Calibration_Queue")
# Create the tasks. If trace is enabled for any task, memory will be
#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)
if __name__ == "__main__":
print('\033[2JTesting scheduler in cotask.py\n')
import controller
import encoder
import motor
import time
import utime
import filter1
import acc
import sensor
# Allocate memory so that exceptions raised in interrupt service routines can
# generate useful diagnostic printouts
micropython.alloc_emergency_exception_buf(100)
pinC0 = pyb.Pin(pyb.Pin.board.PC0, pyb.Pin.ANALOG)
queue = task_share.Queue('f', 1000)
adc = pyb.ADC(pinC0)
pinC1 = pyb.Pin(pyb.Pin.board.PC1, pyb.Pin.OUT_PP)
# Task constants
BALANCE = 0
FORWARD = 1
BACKWARD = 2
TURNLEFT = 3
TURNRIGHT = 4
THETA_OFFSET = 0.5
# Global shared variable
FRONT_SENSOR = 0
BACK_SENSOR = 0
"""
@file ir.py
@author Josh Anderson
@author Ethan Czuppa
This file contains the logic for handling the interrupts created by an IR remote sensor.
"""
import pyb
import utime
import task_share
IR_TMR_CH = None
IR_TMR_FREQ = 1000000
IR_QUEUE = task_share.Queue('I', 68, overwrite = False)
IR_QUEUE_EMPTY_TIME = 0
IR_START_CMD = 48
IR_STARTED = False
def init():
global IR_TMR_CH
ir_tmr = pyb.Timer(2, prescaler=79, period=65535)
IR_TMR_CH = ir_tmr.channel(3, pyb.Timer.IC, pin=pyb.Pin.board.PA2, polarity = pyb.Timer.BOTH)
IR_TMR_CH.callback(irq)
def irq(a):
"""Takes data from IR reciever and puts in queue"""
IR_QUEUE.put(IR_TMR_CH.capture(), in_ISR = True)
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
task1 = cotask.Task(task1_fun,
name='Task_1',
priority=1,
# Set up pin C0 as an input pin
pinC0 = pyb.Pin(pyb.Pin.board.PC0, pyb.Pin.IN)
# Set up pin C1 as an output pin
pinC1 = pyb.Pin(pyb.Pin.board.PC1, pyb.Pin.OUT_PP)
# Set up Timer 1 to create interrupts at 1 KHz
timer = pyb.Timer(1, freq=1000)
# Set up ADC to read pin C0
adcPC0 = pyb.ADC(pyb.Pin.board.PC0)
# Set up time quene
time = task_share.Queue('I',
1000,
thread_protect=True,
overwrite=False,
name='Time')
# Set up values quene
vals = task_share.Queue('I',
1000,
thread_protect=True,
overwrite=False,
name='Values')
# Define a function that reads ADC on pin C0
def read_adcPC0(timer):
time.put(utime.ticks_ms())
vals.put(adcPC0.read())
imu_mode = task_share.Share('i', thread_protect=False, name="IMU_Mode")
# Share for the interface to tell the calculator that it is time to run a calibration
calibrate_calc = task_share.Share('i',
thread_protect=False,
name="Calibrate_Calculation")
# Share for the interface to tell the motors to run a calibration procedure.
run_calibration = task_share.Share('i',
thread_protect=False,
name="Run_Calibration")
# Queue for storing calibration measurements used by the calculator.
calibration_data = task_share.Queue('f',
6,
thread_protect=False,
overwrite=False,
name="Calibration_Queue")
# 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 (task1_fun, name = 'Task_1', priority = 1,
#period = 1000, profile = True, trace = False)
#task2 = cotask.Task (task2_fun, name = 'Task_2', priority = 2,
#period = 100, profile = True, trace = False)
span_motor_task = cotask.Task(task_span_motor_fun,
name='Span_Motor_Task',
priority=6,
power_level2 = controller2.control_loop(enc2_position)
motor2.set_duty_cycle(power_level2)
if qflag == True:
qt2.put(time2, in_ISR=False)
qp2.put(enc2_position, in_ISR=False)
if qt2.full():
qflag = False
yield (0)
if __name__ == "__main__":
print('\033[2JTesting scheduler in cotask.py\n')
share0 = task_share.Share('i', thread_protect=True, name="Share_0")
qt1 = task_share.Queue('f', 500, thread_protect=True, overwrite=False)
qp1 = task_share.Queue('f', 500, thread_protect=True, overwrite=False)
qt2 = task_share.Queue('f', 500, thread_protect=True, overwrite=False)
qp2 = task_share.Queue('f', 500, thread_protect=True, overwrite=False)
#used to test different periods and plot the results
#per = eval(input())
task1 = cotask.Task(motor1_fun,
name='Motor_1',
priority=1,
period=25,
profile=True,
trace=False)
task2 = cotask.Task(motor2_fun,
name='Motor_2',
priority=2,
period=25,
# Allocate memory so that exceptions raised in interrupt service routines can
# generate useful diagnostic printouts
micropython.alloc_emergency_exception_buf (100)
# =========================================================================== #
# =============================== Run the Code ============================== #
# =========================================================================== #
if __name__ == "__main__":
####################################################################
############################ VARIABLES #############################
####################################################################
# 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,
# =========================================================================== #
# =============================== Run the Code ============================== #
# =========================================================================== #
if __name__ == "__main__":
####################################################################
############################ VARIABLES #############################
####################################################################
# print_q = task_share.Queue ('B', 100, name = "Print_Queue",
# thread_protect = True, overwrite = False)
# Motor Parameters
x_params = task_share.Queue('f',
10,
thread_protect=False,
overwrite=False,
name="x_params")
# z_params = task_share.Queue ('f', 10, thread_protect = False,
# overwrite = False, name = "z_params")
# y_params = task_share.Queue ('f', 10, thread_protect = False,
# overwrite = False, name = "y_params")
# 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,
