ME 405 Romi
Loading...
Searching...
No Matches
main.py
Go to the documentation of this file.
1
13
14#---------------------------------------------------------------------------------
15# Import All Program Files #
16#---------------------------------------------------------------------------------
17import task_share
18import cotask
19import os
20from motor import Motor
21from encoder import Encoder
22from pyb import Timer, Pin, UART, ExtInt, I2C
23from data_collector import data_collector
24from ui import ui
25from motor_encoder_left_class import motor_encoder_left_class
26from motor_encoder_right_class import motor_encoder_right_class
27from controller import Controller
28from control_task import control_task
29from line_sensor import Line_Sensor
30from ir_sensor import IR_Sensor
31from line_task import line_task
32from imu import IMU
33from imu_task import imu_task
34from observer import observer
35from pathing_plan_task import pathing_plan
36from bump_task import Bump_Task
37from bump__sensor import Bump_Sensor
38import machine
39
40
41# def RC_Input(line):
42# machine.soft_reset()
43
44# User_Button = ExtInt(Pin.cpu.C13, ExtInt.IRQ_FALLING, Pin.PULL_NONE, RC_Input)
45
46# Controller Saturation Defines
47ERR_SAT_LEFT=50.0
48EFF_SAT_LEFT=85.0
49
50ERR_SAT_RIGHT=50.0
51EFF_SAT_RIGHT=85.0
52
53ERR_SAT_LINE=1
54EFF_SAT_LINE=40
55
56ERR_SAT_YAW=.8
57EFF_SAT_YAW=100
58#---------------------------------------------------------------------------------
59# Create All Shares #
60#---------------------------------------------------------------------------------
61
62m_state_l = task_share.Share('b', thread_protect = False, name = "m_state_l") # 1=waiting but enabled
63m_state_r = task_share.Share('b', thread_protect = False, name = "m_state_r") # 1=waiting but enabled
64
65PWM_l = task_share.Share('f', thread_protect = False, name = "PWM_l")
66PWM_r = task_share.Share('f', thread_protect = False, name = "PWM_r")
67
68position_r = task_share.Share('f', thread_protect = False, name = "position_r")
69velocity_r = task_share.Share('f', thread_protect = False, name = "velocity_r")
70
71position_l = task_share.Share('f', thread_protect = False, name = "position_l")
72velocity_l = task_share.Share('f', thread_protect = False, name = "velocity_l")
73
74testing_flg = task_share.Share('b', thread_protect = False, name = "testing_flg")
75delay = task_share.Share('b', thread_protect = False, name = "delay")
76
77fwd_ref = task_share.Share('l', thread_protect = False, name = "fwd_ref")
78arc_ref = task_share.Share('l', thread_protect = False, name = "arc_ref")
79piv_ref = task_share.Share('f', thread_protect = False, name = "piv_ref")
80c_state = task_share.Share('b', thread_protect = False, name = "c_state")
81
82l_state = task_share.Share('b', thread_protect = False, name = "l_state")
83ready_Black = task_share.Share('b', thread_protect = False, name = "ready_Black")
84ready_White = task_share.Share('b', thread_protect = False, name = "ready_White")
85
86need_Calibrate = task_share.Share('b', thread_protect = False, name = "need calibrate")
87centroid = task_share.Share('f', thread_protect = False, name ="centroid")
88
89automatic_mode = task_share.Share('b', thread_protect = False, name = "automatic_mode" )
90
91imu_flg = task_share.Share('b', thread_protect= False, name = "imu_flg")
92
93yaw = task_share.Share('f', thread_protect = False, name = "yaw")
94yaw_velocity = task_share.Share('f', thread_protect = False, name = "yaw_velocity")
95
96total_dist = task_share.Share("f", thread_protect = False, name = "S")
97
98centroid_goal = task_share.Share("f", thread_protect = False, name = "Goal Centroid")
99yaw_goal = task_share.Share("f", thread_protect = False, name = "Yaw Goal")
100
101bump_on_off = task_share.Share("b", thread_protect = False, name = "Bump Flag")
102bump_flg = task_share.Share("b", thread_protect = False, name = "Bump Flag")
103
104#---------------------------------------------------------------------------------
105# Create All Queues #
106#---------------------------------------------------------------------------------
107velocity = task_share.Queue('f', 50, thread_protect = False, overwrite = False, name = "velocity")
108velocity2 = task_share.Queue('f', 50, thread_protect = False, overwrite = False, name = "velocity2")
109
110position = task_share.Queue('f', 50, thread_protect = False, overwrite = False, name = "position")
111
112times = task_share.Queue('l', 50, thread_protect = False, overwrite = False, name = "times")
113
114#---------------------------------------------------------------------------------
115# Create All Hardware Driver Objects #
116#---------------------------------------------------------------------------------
117tim1 = Timer(1, prescaler=0, period=65535)
118tim3 = Timer(3, prescaler=0, period=65535)
119tim4 = Timer(4, freq=20000)
120
121my_motor_Left = Motor(Pin.cpu.B7, Pin.cpu.C13, Pin.cpu.C14, tim4, 1)
122encoder_Right = Encoder(tim3, Pin.cpu.C6, Pin.cpu.C7)
123
124my_motor_Right = Motor(Pin.cpu.B6, Pin.cpu.B10, Pin.cpu.A10, tim4, 2)
125encoder_Left = Encoder(tim1, Pin.cpu.A8, Pin.cpu.A9)
126
127bump_sensor_right = Bump_Sensor(Pin.cpu.B13, Pin.cpu.B3, Pin.cpu.B5)
128bump_sensor_left = Bump_Sensor(Pin.cpu.B11, Pin.cpu.B15, Pin.cpu.B14)
129
130ir1 = IR_Sensor(Pin.cpu.A6)
131ir2 = IR_Sensor(Pin.cpu.A7)
132ir3 = IR_Sensor(Pin.cpu.C4)
133ir4 = IR_Sensor(Pin.cpu.A0)
134ir5 = IR_Sensor(Pin.cpu.A1)
135ir6 = IR_Sensor(Pin.cpu.C4)
136ir7 = IR_Sensor(Pin.cpu.B0)
137ir8 = IR_Sensor(Pin.cpu.C0)
138ir9 = IR_Sensor(Pin.cpu.C1)
139ir10 = IR_Sensor(Pin.cpu.C2)
140ir11 = IR_Sensor(Pin.cpu.C3)
141ir12 = IR_Sensor(Pin.cpu.C5)
142ir13 = IR_Sensor(Pin.cpu.B1)
143
144line_sensor_obj = Line_Sensor(ir1, ir2, ir3, ir4, ir5, ir6, ir7, ir8, ir9, ir10, ir11, ir12, ir13)
145
146uart_obj = UART(5, 115200)
147uart_obj.init(115200, bits=8, parity = None, stop = 1, timeout = 50)
148
149# create i2c and imu objects
150i2c_obj = I2C(1,I2C.CONTROLLER,baudrate = 400000)
151imu_obj = IMU(i2c_obj)
152
153#---------------------------------------------------------------------------------
154# Create All Controller Objects #
155#---------------------------------------------------------------------------------
156controller_obj_left = Controller(.2,1.95,1, ERR_SAT_LEFT, EFF_SAT_LEFT)
157controller_obj_right = Controller(.2,1.95,1, ERR_SAT_RIGHT, EFF_SAT_RIGHT)
158controller_obj_line = Controller(150, 10, 1, ERR_SAT_LINE, EFF_SAT_LINE)
159controller_obj_yaw = Controller(.8,.3,1, ERR_SAT_YAW, EFF_SAT_YAW)
160
161#---------------------------------------------------------------------------------
162# Create Task Class Objects with Shares. #
163#---------------------------------------------------------------------------------
164motor_Left_class = motor_encoder_left_class(my_motor_Left, encoder_Left, m_state_l, position_l, velocity_l, times, PWM_l, delay)
165motor_Right_class = motor_encoder_right_class(my_motor_Right, encoder_Right, m_state_r, position_r, velocity_r, times, PWM_r, delay)
166ui_obj = ui(testing_flg, m_state_l, m_state_r, position, velocity, times, PWM_l, PWM_r, delay, uart_obj, fwd_ref, arc_ref, piv_ref, c_state, velocity2, need_Calibrate, ready_Black, ready_White, automatic_mode, imu_flg)
167data_collector_obj = data_collector(testing_flg, position, velocity, times, position_l, velocity_l, position_r, velocity_r, velocity2)
168control_task_obj = control_task(fwd_ref, arc_ref, piv_ref, c_state, controller_obj_left, controller_obj_right, position_l, velocity_l, position_r, velocity_r, PWM_l, PWM_r, encoder_Right, encoder_Left, m_state_l, m_state_r, centroid, controller_obj_line, automatic_mode, line_sensor_obj, need_Calibrate, yaw, centroid_goal, yaw_goal, controller_obj_yaw)
169line_task_obj = line_task(line_sensor_obj, l_state, need_Calibrate, ready_Black, ready_White,centroid)
170imu_task_obj = imu_task(imu_obj, imu_flg, yaw, yaw_velocity)
171observer_obj = observer(position_l, position_r, yaw, yaw_velocity, PWM_l, PWM_r, velocity_l, velocity_r, total_dist)
172pathing_obj = pathing_plan(total_dist, automatic_mode, c_state, fwd_ref, piv_ref, yaw, centroid_goal, yaw_goal, bump_on_off, bump_flg)
173bump_obj = Bump_Task(c_state, bump_sensor_right, bump_sensor_left, bump_on_off, bump_flg)
174
175#---------------------------------------------------------------------------------
176# Create All Tasks with Class Object Generator Function #
177#---------------------------------------------------------------------------------
178task1 = cotask.Task(data_collector_obj.run, name="data_collector", priority=2, period=18, profile=True, trace=False, shares=())
179task2 = cotask.Task(motor_Left_class.run, name="motor_encoder_left", priority=7, period=13, profile=True, trace=False, shares=())
180task3 = cotask.Task(motor_Right_class.run, name="motor_encoder_right", priority=7, period=13, profile=True, trace=False, shares=())
181task4 = cotask.Task(ui_obj.run, name="ui", priority=1, period=1, profile=True, trace=False, shares=())
182task5 = cotask.Task(control_task_obj.run, name="control_task", priority=6, period=15, profile = True, trace=False, shares=())
183task6 = cotask.Task(line_task_obj.run, name="line_task", priority = 5, period = 22, profile = True, trace = False, shares=())
184task7 = cotask.Task(imu_task_obj.run, name = "imu_task", priority = 6, period = 20, profile= True, trace= False, shares = ())
185task8 = cotask.Task(observer_obj.run, name = "observer", priority = 4, period = 20, profile = True, trace = False, shares = ())
186task9 = cotask.Task(pathing_obj.run, name= "pathing", priority = 3, period = 30, profile = True, trace = False, shares = ())
187task10 = cotask.Task(bump_obj.run, name = "bump task", priority = 2, period = 50, profile = True, trace = False, shares=())
188
189#---------------------------------------------------------------------------------
190# Add Tasks to Scheduler #
191#---------------------------------------------------------------------------------
192cotask.task_list.append(task1)
193cotask.task_list.append(task2)
194cotask.task_list.append(task3)
195cotask.task_list.append(task4)
196cotask.task_list.append(task5)
197cotask.task_list.append(task6)
198cotask.task_list.append(task7)
199cotask.task_list.append(task8)
200cotask.task_list.append(task9)
201cotask.task_list.append(task10)
202
203#---------------------------------------------------------------------------------
204# Run the Scheduler until Interrupt #
205#---------------------------------------------------------------------------------
206try:
207 while True:
208 cotask.task_list.pri_sched()
209
210except KeyboardInterrupt:
211 # Task Profile once hit Ctr + C
212 print("\nProfiler results:")
213 print(cotask.task_list)
214 raise
bump__sensor.Bump_Sensor
Initializes Bump sensors.
Definition bump__sensor.py:10
bump_task.Bump_Task
Collision detector for cooperative scheduler.
Definition bump_task.py:10
control_task.control_task
This file contains a class to create a control_task object which is compatible with the cotask schedu...
Definition control_task.py:17
controller.Controller
This file contains a class to create a PID Controller object.
Definition controller.py:19
cotask.Task
Implements multitasking with scheduling and some performance logging.
Definition cotask.py:66
data_collector.data_collector
Collects data from shares.
Definition data_collector.py:6
encoder.Encoder
The Encoder Diver Class.
Definition encoder.py:6
imu.IMU
Initializes IMU Sensor.
Definition imu.py:7
imu_task.imu_task
Runs the IMU.
Definition imu_task.py:8
ir_sensor.IR_Sensor
The IR_Sensor class is an object for each individual ir sensor on our line sensor.
Definition ir_sensor.py:14
line_sensor.Line_Sensor
Line_Sensor class which will calibrate the sensors, read data from the entire line,...
Definition line_sensor.py:27
line_task.line_task
line_task class that can be initialized to be run with the cotask scheduler.
Definition line_task.py:27
motor.Motor
The Motor Diver Class.
Definition motor.py:8
motor_encoder_left_class.motor_encoder_left_class
Left Motor Encoder Class.
Definition motor_encoder_left_class.py:9
motor_encoder_right_class.motor_encoder_right_class
Right Motor Encoder Class.
Definition motor_encoder_right_class.py:10
observer.observer
Observer task object compatible with the cotask scheduler.
Definition observer.py:30
pathing_plan_task.pathing_plan
Pathing plan task class for ROMI navigation.
Definition pathing_plan_task.py:91
task_share.Queue
A queue which is used to transfer data from one task to another.
Definition task_share.py:90
task_share.Share
An item which holds data to be shared between tasks.
Definition task_share.py:324
ui.ui
Initializes User Interface.
Definition ui.py:8