1+ # import " /packages.typ" notebookinator , codetastic , gentle-clues
2+ # import notebookinator : *
3+ # import themes . radial . components : *
4+ # import gentle-clues : *
5+
6+ # import codetastic : qrcode
7+
8+ # show : create-body-entry . with (
9+ title : " Program: Driver Control"
10+ type : " program"
11+ date : datetime (year : 2023 , month : 11 , day : 28 ), // TODO: fix date
12+ author : " Andrew Curtis"
13+ )
14+
15+ = Driver Control
16+ The key design philosophy behind the button mapping is to keep the driver's
17+ thumbs on the joysticks whenever possible. This means that often used
18+ functionality must be accessible without moving the driver's thumbs and thus
19+ these functions are mapped to the trigger buttons. We also must reduce the
20+ likelihood of the driver accidentally pressing buttons that could be
21+ detrimental, such as the emergency stops, and thus these function require that
22+ two buttons be pressed.
23+
24+ == Control Scheme
25+
26+ Before we can look at the code, we must first set out the requirements for the
27+ code:
28+ - Left Joystick vertical axis # sym . arrow . r . double left side of the drive (tank
29+ drive)
30+ - Right Joystick vertical axis # sym . arrow . r . double right side of the drive (tank
31+ drive)
32+ - UP # sym . arrow . r . double move lift up slowly
33+ - DOWN # sym . arrow . r . double move lift down slowly
34+ - UP double press # sym . arrow . r . double move lift to highest position
35+ - DOWN double press # sym . arrow . r . double move lift to lowest position
36+ - R1 # sym . arrow . r . double Toggle Wings
37+ - R2 # sym . arrow . r . double Toggle Blocker
38+ - L1 # sym . arrow . r . double Intake
39+ - L2 # sym . arrow . r . double Outtake
40+ - RIGHT # sym . arrow . r . double Toggle automatic firing functionality of the catapult
41+ - LEFT # sym . arrow . r . double manually fire catapult
42+ - X & A # sym . arrow . r . double toggle catapult emergency stop
43+ - Y & B # sym . arrow . r . double toggle lift emergency stop
44+
45+ == Terms
46+ - Boolean: A value that can only be true or false
47+ - Rising Edge: Often times you only want to do an action once per button press,
48+ such as a toggle. One way to approach this is to do this action when the button
49+ changes from false to true. This kind of boolean transition is called the rising
50+ edge.
51+ - Falling Edge: The transition from true to false.
52+
53+ == Code
54+ now let's look at some code:
55+ - Tank drive: ```cpp
56+ // takes each side's drive power in the range [-127, 127] and a curve gain
57+ Robot::chassis->tank(
58+ // gets left joystick y axis in the range [-127, 127]
59+ Robot::control.get_analog(pros::E_CONTROLLER_ANALOG_LEFT_Y),
60+ // gets left joystick y axis in the range [-127, 127]
61+ Robot::control.get_analog(pros::E_CONTROLLER_ANALOG_RIGHT_Y),
62+ // drive curve gain to enable greater control of the robot.
63+ 15);
64+ ```
65+
66+ Ok, but whats this "drive curve"? The idea is to make it easier to control the
67+ robot at slow speeds, but still be able to go at max speed. This enhances our
68+ driver's ability to accurately control the robot. In this graph the x is the
69+ vertical axis of the joystick, and the y is the output voltage (this specific
70+ curve is the work of team 5225A _The Pilons_ ):
71+
72+ # figure (image (" ./curve.png" width : 50% ))
73+
74+ - Lift: The lift must be capable of moving from its topmost position to its lowest
75+ position as quickly as possible, but sometimes its also beneficial to have the
76+ lift somewhere in the middle. This is useful for minimizing the chance of
77+ tipping when blocking or shooting. We do this by moving the lift to its max or
78+ min with a double press, but the driver may also just press up or down to move
79+ the intake to a middle position.
80+
81+ How do you detect a double press? A double press boils down to a button being
82+ pressed twice with little delay between the two presses. That begs the question
83+ of what is a press? You can either listen to the falling edge or the rising edge
84+ of the button to detect a button press. We use the falling edge for the first
85+ press and rising edge for the second press.
86+
87+ This double press algorithm looks like so:
88+ - On falling edge of button, record the current time and store it in a variable
89+ - On the rising edge of a button, find how long it has been since the last button
90+ press. If it has been less than 150 ms, then go to the min / max of the lift.
91+ Always perform the normal functionality of the button no matter whether the
92+ button is being double pressed or not
93+ # warning [
94+ For our lift, we can let this happen, because moving the lift up when its target
95+ is at its max position, because we prevent it from going above the max angle of
96+ the lift. But, when using double presses for different subsystems this may cause
97+ problems unwanted behavior.
98+ ]
99+ ```cpp
100+ const bool up = Robot::control.get_digital(pros::E_CONTROLLER_DIGITAL_UP);
101+ const bool down =
102+ Robot::control.get_digital(pros::E_CONTROLLER_DIGITAL_DOWN);
103+
104+ // lift granular control
105+ // change the goal position of the lift by the liftIncrement
106+ Robot::Subsystems::lift->changeTarget(
107+ liftIncrement *
108+ (up - down) // ensures that if up and down are both pressed, then nothing happens
109+ );
110+
111+ // lift max/min angle
112+ // on the rising edge of up, if up was pressed recently,
113+ // then set target to max angle
114+ if (up && !prevUp &&
115+ pros::millis() - lastUpPress < maxTimeBetweenDoublePress)
116+ Robot::Subsystems::lift->setTarget(LiftArmStateMachine::maxAngle);
117+ // on the rising edge of down, if down was pressed recently,
118+ // then set target to max angle
119+ if (down && !prevDown &&
120+ pros::millis() - lastDownPress < maxTimeBetweenDoublePress)
121+ Robot::Subsystems::lift->setTarget(LiftArmStateMachine::minAngle);
122+
123+ // on falling edge of up & down, update the last press time
124+ if (!up && prevUp) lastUpPress = pros::millis();
125+ if (!down && prevDown) lastDownPress = pros::millis();
126+
127+ // update previous values of up and down
128+ prevUp = up;
129+ prevDown = down;
130+ ```
131+ - Toggles: Wings / Blocker / Automatic Catapult firing ```cpp
132+ // wings toggle
133+ // retrieve the value of the R2 button
134+ const bool r1 = Robot::control.get_digital(pros::E_CONTROLLER_DIGITAL_R1);
135+ // if on the rising edge of the button
136+ if (r1 && !prevR1) {
137+ // flip the state of the wings
138+ wingsState = !wingsState;
139+ // apply the state of the wings to the actual pistons
140+ if (wingsState) Robot::Actions::expandWings();
141+ else Robot::Actions::retractWings();
142+ }
143+ // update the previous value of R1
144+ prevR1 = r1;
145+ // the other toggles are implemented in much the same way
146+ ```
147+ - Intake / Outtake ```cpp
148+ // if pressing L1, then spin the intake inwards
149+ if (Robot::control.get_digital(pros::E_CONTROLLER_DIGITAL_L1))
150+ Robot::Motors::intake.move(127);
151+ // if pressing L2, then spin the intake outwards
152+ else if (Robot::control.get_digital(pros::E_CONTROLLER_DIGITAL_L2))
153+ Robot::Motors::intake.move(-127);
154+ // otherwise, don't sent power to the intake
155+ else Robot::Motors::intake.move(0);
156+ ```
157+ - Catapult/Lift emergency stop toggle ```cpp
158+ // get whether both emergency stop buttons are currently being pressed
159+ const bool cataEStopCombo =
160+ Robot::control.get_digital(pros::E_CONTROLLER_DIGITAL_X) &&
161+ Robot::control.get_digital(pros::E_CONTROLLER_DIGITAL_A);
162+ // if both buttons have just become pressed (rising edge), then toggle the
163+ // emergency stop of the catapult
164+ if (cataEStopCombo && !prevCataEStopCombo) {
165+ // if the catapult is currently emergency stopped, then disable the
166+ // emergency stop
167+ if (Robot::Subsystems::catapult->getState() ==
168+ CatapultStateMachine::STATE::EMERGENCY_STOPPED)
169+ Robot::Subsystems::catapult->cancelEmergencyStop();
170+ // otherwise emergency stop the catapult
171+ else Robot::Subsystems::catapult->emergencyStop();
172+ }
173+ // update the previous value of the emergency stop buttons
174+ prevCataEStopCombo = cataEStopCombo;
175+ ```
0 commit comments