James/arm test 1 #18

jamesi8086 · 2023-02-21T08:25:38Z

Description

DRAFT PR DO NOT MERGE

This is meant as demonstration for students

Prototype arm control code building on Max's work, cleaning up and adding more constructs with the intent of improving the Framework in general.

Main changes:

Based off Max's branch and merged in RevA branch, so there might be some extra changes
Simple FSM classes
recomposed bulk of Arms class into ArmJoint, cleaned up Arms
autonomous vs manual arm control (independent of OI vs Auton mode)
manual control is pretty smooth due to relative angle increments
basic shoulder joint automated movement (demo, moves to random positions)
Arm motion planning (WIP)

How Has This Been Tested?

Please describe the tests that you ran to verify your changes. Be detailed so that your code reviewer can understand exactly how much and what kinds of testing were done, and which might still be worthwhile to do.

Unit tests: [Add your description here]
Simulator testing: [Add your description here]
On-robot bench testing: tested OI mode
On-robot field testing: [Add your description here]

he he haw haw

its almost like pid works now

he he haw haw

what can i say

check config for vals

It works but its a bit janky - no spark max on 33 because old one sux

…throwing

maxspier · 2023-02-27T17:04:05Z

src/main/java/com/team766/library/fsm/FiniteState.java

+ * - Imnplementation is not totally foolproof, human error may still violate state expectations
+ * TODO: add more checks, guards
+ */
+public abstract class FiniteState {


not sure what this does - maybe you can explain to me

you could read this first and ask Debajit / me / someone later in-person

Wikipedia:
https://en.wikipedia.org/wiki/Finite-state_machine

With FRC context:
https://frc3512.github.io/ci/state-machines/

maxspier · 2023-02-27T17:05:01Z

src/main/java/com/team766/library/fsm/FiniteStateMachine.java

+ *  TODO: add more checks, guards
+ *  TODO: we should really have an FSM-specific Exception container
+ */
+public abstract class FiniteStateMachine {


also not sure about this one - most likely I just don't know enough java or something

it seems like we already can change states easily though

Yes there is some form of state in Procedure, but I wanted something rigid and extensible.

maxspier · 2023-02-27T17:09:45Z

src/main/java/com/team766/robot/mechanisms/ArmJoint.java

+	// TODO: max acceleration
+
+	public void setMotorPosition(double angle) {
+		if(angle < angleLimitMin) {


i really like this

maybe we can do same thing with max velocity and max accel

actually smartMotion already does those via trapezoidal velocity curve, so I did not want to do it in software, I hope we can try to get pid variables working in that mode, probably involves reading and testing in Rev dashboard

the pid variables already work in the rev client - what would be the next steps for this then?

To confirm, did you use the pid values with SmartMotion and not Position?
Oddly the last time I tested those pid values in code with SmartMotion, the arm swung wildly. We need to run more tests then.

maxspier · 2023-02-27T17:18:24Z

src/main/java/com/team766/robot/states/ArmAutomatedControlState.java

@@ -0,0 +1,86 @@
+package com.team766.robot.states;


maybe you can also explain this to me

This class extends the FiniteState class.
I basically have 1 class per state, so that only one of them is used at any point in time to determine the arm movement behavior and there is no possibility of conflict during runtime..
I've used the buttons to enforce toggling of states.

rcahoon · 2023-02-27T23:24:33Z

src/main/java/com/team766/robot/mechanisms/Arms.java

+        // TODO: second joint
+    }
+
+    public void periodicUpdate() {


Mechanism has an overloadable method, void run(), which is run as part of the scheduling loop (along with the Procedures)

rcahoon · 2023-02-27T23:25:41Z

src/main/java/com/team766/controllers/PIDController.java

@@ -269,9 +270,63 @@ public void calculate(double cur_input) {
 		pr("	Total Error: " + total_error + "		Current Error: " + cur_error +
 		   "	Output: " + output_value + " 	Setpoint: " + setpoint);
 	}
+	public void calculate(double cur_input, double target_speed) {


target_velocity should be part of the setpoint, so should probably be handled by setSetpoint

ignore changes in [PIDController.java] , was just branching from Max's branch and only noticed these after diff-ing

rcahoon · 2023-02-27T23:32:03Z

src/main/java/com/team766/controllers/PIDController.java


+		if (delta_time > 0) { // This condition basically only false in the simulator
+			error_rate = (cur_error - prev_error) / delta_time;


if you want to incorporate velocity control, I think it should go here

error_rate = (cur_error - prev_error) / delta_time + target_velocity;

you'll probably also want a feed-forward term:

double out = ... + Kffv * target_velocity

rcahoon · 2023-02-27T23:37:53Z

src/main/java/com/team766/robot/states/ArmManualControlState.java

+	public ArmManualControlState() {
+		super();
+
+		debugJoystick = RobotProvider.instance.getJoystick(0);


Joystick control mappings should be kept within OI, otherwise it's going to be very easy to lose track of them and end up with multiple things trying to use the same controls. The OI code should call setters on the Arms mechanism

rcahoon · 2023-02-27T23:47:45Z

src/main/java/com/team766/robot/states/ArmAutomatedControlState.java

+				movementState = 0;
+				break;
+		}
+	}


Procedures already exist as a way to switch between multiple control schemes for a mechanism. They are also designed as a way to permit this kind of stateful code without all the boilerplate of switch/case:

public class ArmAutomatedControlState extends Procedure { double targetAngle = 0; double angleTolerance = 3.0d; Random rand = new Random(); @Override public void run(Context context) { context.takeOwnership(Robot.arms); while (true) { // moving Robot.arms.firstJoint.setMotorPosition(targetAngle); context.waitFor(() -> { double motorAngle = Robot.arms.firstJoint.getMotorPosition(); return (motorAngle > targetAngle - angleTolerance) && (motorAngle < targetAngle + angleTolerance); }); // pausing context.waitForSeconds(1.0); // incrementing targetAngle = rand.nextDouble(-25, 45); } } }

are Procedures also used outside Autonomous mode?

I think what I'm trying to work towards is that, since the team needs to use autonomous arm movements for scoring, while retaining the option for manual control as a last resort, the Arm mechanism would contain bulk of its internalized behavior while still has a mode for manual control as an overwritable fallback

I'm probably not too familiar with Procedure abstraction, could there be multiple concurrent Procedures in an Arm?

they are used outside autonomous mode. Procedures are appropriate whenever the robot is performing actions or sequences of actions that are longer than instantaneous. OI can launch other Procedures using context.startAsync

only one Procedure can control a Mechanism at a time. this is by design, otherwise they could try to send competing commands to the Mechanism's actuators. it is enforced by the takeOwnership calls: if two Procedures try to control a Mechanism at the same time, the one that called takeOwnership most recently wins and the other one is cancelled.

I tried out my take on rewriting your branch using Procedures and some of the other utilities from the Maroon Framework: 62776c7

A couple of things I noted:

There are a lot of try-catches built into the framework already, so you don't need to add them yourself in most places (i.e. the framework will automatically notice when an exception has occurred in OI; it will log the error and relaunch OI to try to recover)

It's recommended to call Mechanism.checkContextOwnership() at the beginning of any of a Mechanism's methods that can mutate it. This ensures that the Procedure that's trying to mutate the Mechanism has properly acquired ownership of it.

The values in ArmJointConfig ought to be loaded from the config file, and the settings that get sent to the motor controller ought to be loaded automatically by the framework (once we add SmartMotion configuration to the HAL). I left it as-is in my version since that support is still TBD

You could potentially use a Procedure for the manual control as well; something like 6f215ba

thank, I'll take a look.
I think Max and I didn't know what is the full extent of config supported by the config file; we probably might need to upgrade that to properly support kSmartMotion's required parameters

add Max's pids for SmartMotion

maxspier and others added 20 commits January 28, 2023 15:20

First test - changing arm height and reading distance works.

89cf042

Stopped robot sentience (maybe?)

4e42aa4

h

5d4c68f

PIDController works - found bug on main file

de36bd1

he he haw haw

Update Pulley.java

6066bc0

he he he ho

c14bf25

its almost like pid works now

1st try big arm

6615e55

he he haw haw

Merge branch 'main' into Swingy-Arm

370ec87

it works

d9e538f

Update Arms.java

261f510

i mean it worked until it didn't

ccd0e5e

what can i say

Added comments and removed unused code

c808ba4

Added comments and removed unused code

dc5a6f9

Pid for all arms work

9f56e63

check config for vals

Rev a little less than a works

0d4cf61

henry would be pretty mad if he saw how the robot works right now

32a0c59

It works but its a bit janky - no spark max on 33 because old one sux

Merge branch 'RevA' into james/arm-test-1

9f516fe

FSM, ArmJoint, control state machine, minor refactoring of exception …

09ef96c

…throwing

bug fixes, manual control via closed loop on motor encoder tested ok

3232f0e

basic auton arm movement

e4d4df6

jamesi8086 requested review from SauquetAlex, dejabot, rcahoon and maxspier February 21, 2023 08:26

maxspier marked this pull request as ready for review February 21, 2023 14:51

maxspier reviewed Feb 27, 2023

View reviewed changes

Update OI.java

996d176

maxspier reviewed Feb 27, 2023

View reviewed changes

Update Arms.java

6be9499

maxspier reviewed Feb 27, 2023

View reviewed changes

rcahoon suggested changes Feb 27, 2023

View reviewed changes

convert FiniteState to use Procedure

647051e

add Max's pids for SmartMotion

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

James/arm test 1 #18

James/arm test 1 #18

jamesi8086 commented Feb 21, 2023 •

edited by maxspier

Loading

maxspier Feb 27, 2023

jamesi8086 Feb 27, 2023

maxspier Feb 27, 2023

maxspier Feb 27, 2023

jamesi8086 Feb 27, 2023

maxspier Feb 27, 2023

maxspier Feb 27, 2023

jamesi8086 Feb 27, 2023

maxspier Feb 28, 2023 •

edited

Loading

jamesi8086 Mar 1, 2023

maxspier Feb 27, 2023

jamesi8086 Feb 27, 2023

rcahoon Feb 27, 2023

rcahoon Feb 27, 2023

jamesi8086 Feb 28, 2023

rcahoon Feb 27, 2023

rcahoon Feb 27, 2023

rcahoon Feb 27, 2023

jamesi8086 Feb 28, 2023 •

edited

Loading

rcahoon Feb 28, 2023

rcahoon Mar 8, 2023

rcahoon Mar 8, 2023 •

edited

Loading

jamesi8086 Mar 9, 2023 •

edited

Loading


		if (delta_time > 0) { // This condition basically only false in the simulator
		error_rate = (cur_error - prev_error) / delta_time;

James/arm test 1 #18

Are you sure you want to change the base?

James/arm test 1 #18

Conversation

jamesi8086 commented Feb 21, 2023 • edited by maxspier Loading

Description

How Has This Been Tested?

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

maxspier Feb 28, 2023 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

jamesi8086 Feb 28, 2023 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

rcahoon Mar 8, 2023 • edited Loading

Choose a reason for hiding this comment

jamesi8086 Mar 9, 2023 • edited Loading

Choose a reason for hiding this comment

jamesi8086 commented Feb 21, 2023 •

edited by maxspier

Loading

maxspier Feb 28, 2023 •

edited

Loading

jamesi8086 Feb 28, 2023 •

edited

Loading

rcahoon Mar 8, 2023 •

edited

Loading

jamesi8086 Mar 9, 2023 •

edited

Loading