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