chip updates: rethinking test node/system [updates]
So we're having some major issues here. In that in test node, even if I simply allow the joystick to directly command points the robot isn't cooperating. So here's what I'm going to propose for now and for the future.
For The Future:
I think the only thing the rio should be doing is giving us direct access to the data from the motor controllers and allow us to write data to each individual motor controller. Leg, PID tuning, all that funny stuff we did on the RIO except for enabling/disabling should be handled by the Jetson itself. We're going to go straight to this very soon. It seems like a waste of time to try and write a trajectory node on the Jetson. In fact we probable want to do this and start all the software from scratch and make it so much simpler. There are so many things going on right now there's no way to even know what's going wrong you know?
What we really need is to make this software as simple as humanly possible. So what I'm going to do is create a new branch of the code, and wipe out everything except for the main Robot.java and we're going to make a whole new catkin workspace called chip_workspace and start everything from scratch. The reason? Because if we start from scratch we won't want to write all the code over and over again which means we'll make it so much simpler.
So here's the plan:
(1) New Git Branch where we can edit all the files and delete everything without worrying about losing the old code. Completely new program. Keep only exactly what we need.
(2) New catkin package all together, new ROS system all together. SIMPLER, so much simpler, then what's currently there. There's way too much going on right now.
The MAXIMUM that should be happening on the RIO is the three motors are packaged into legs and we send leg commands just so we don't forget what goes where and we don't need to account for things like multiples in the code. But the jetson sends the rio COMMANDS TO THE MOTORS. NOT XYZ COORDINATES.
And you know what, we're going to do PID tuning properly this time, with limits and everything, so we can get smooth leg responses.
That's the new plan. It's a much better plan. We were going to wait, but I think it's best we do that now because otherwise we'll be fumbling around for the next few weeks breaking things and burning out other things. Let's make this thing do what we know it can do.
(1) Create the SparkMAX Objects, set them to PID position control, Zero the motors, set the PID constants. --> we will tune them on the RIO itself.
(2) Create the network tables communication between the robot and the jetson.
(3) In Robot Periodic, periodically publish info about where the position of each motor is at the moment.
(4) In Robot Init, enable the robot.
(5) In teleop Init, ask robot to hold the leg position.
(6) In teleop periodic, ask the robot to read data from network tables and then set the positions.
DON'T EVEN SET THINGS LIKE THE MULTIPLIERS FOR FORWARDS AND BACKWARDS MOTION. ALL OF THAT WILL BE SET BY THE JETSON. AND WE'LL TUNE THE PID FOR EACH INDIVIDUAL MOTOR USING THE CORRECT TECHNIQUE/AND ACTUAL TECHNIQUE. THAT'S WHAT WE'RE GOING TO DO TO FIX ALL OF THIS.
OUR NEW SOFTWARE VERSION IS NOW: v0.1.1 not v0.0.1 like it says on the website.
And from the ROS SIDE: this is what we're going to start with:
We'll start with RIO_READ and RIO_WRITE and ARBITRATOR and make sure when we write set points to the legs they go to those set points. Then we'll do INVERSE_KINEMATICS and make sure an XYZ position goes to the legs. Then we'll do STABILIZER and try to make the robot balance on two legs. And then we will do MASTER. We will likely keep classes like DEFAULTS.py and CONTROL_MODE.py but we'll likely remove things like the CoG node and etc. We will keep leg currents and start to use that to determine which legs are loaded and which ones aren't. I want to get everything through INVERSE_KINEMATICS done by the end of the week so next week/this weekend we can focus on stabilizer getting that to work so we can then move to CHIP_MASTER and making the puppy step and walk. This is a better plan since we now have direct control over the motors. STABILIZER will likely use some form of state-space control we'll have to develop.
Writing the Code for the CHIP Core Controller:
So this evening we're going to write the CHIP CORE and tune the PID. To do that we need to start with the pseudo code. Here it is. The plan is to implement this in a new branch. And then what we will do is just use the robot.java teleop_periodic() function to toggle a set point -10 revolution to 10 revolutions or something. So we can tune the PID individually for each of the joints. We're going to do that when the system is unloaded and then load the system and see what changes. We will use zeigler-nicholas for best results. We're going to have to figure out how to limit the output of the PID controller so that the leg does not over current when we try to move all of them, or we will isolate the RIO and the leg power supply instead of simply connecting them in parallel.
We're going to look into getting what we really need, which is a voltage regulator module. It takes an unregulated DC input from 3V-12V and boosts it to the required 12V: https://www.vexrobotics.com/217-4245.html#Docs_&_Downloads always with the FRC electronics eh?
This thing will allow us to isolate current/voltage dips due to the motors drawing too much power we're going to try tuning the PID without it and see if it overcurrents. I wouldn't be surprised if a VRM doesn't fix this initially so we won't just go ahead and order one - that doesn't make any sense to do. Let's try tuning PID properly first, and then if moving the legs causes a dip in voltage so far that the system browns-out then then we can look into a VRM. Realistically, we probably should limit the PID controller so the over-voltage doesn't happen. We should also test the amount of turns it takes to over current the system, we can write a loop incrementing the number of turns by one until it over currents and then we will know.
Here we go!:
Now what we're doing is actually writing the code for the motor controllers/the RoboRIO. Here is what we came up with.
The first part of the code imports the only other three code files we need. (1) PIDConstants, (2) Networking, (3) DS. These handle setting PID constants of the motor controllers, communication with the jetson, and enabling the robot. We also included the enum MotorControllerTypes from the last code set.
Now what we did was create all the variables the robot needs. So for kIz and kFF as well as maxRPM we set those constant for all 12 motor controllers. And we have separate PID values for the shoulders/hinge, and the knee. We also create a Driverstation (DS) variable and all the sparkmaxs we need.
Now we get to the RoboINIT method which starts be creating all the sparkmax controllers and deals with inverting any of them that need inverting (these were copied from the last time).
Then to finish of RobotINIT, we set the PID constants for all the motors/controllers. We setup networking by creating the tables we will publish to and subscribe to, and we enable the robot by starting the DriverStation (DS).
Then we get to robot periodic where we simply read the position of the motors, publish those, read the currents of the individual motors, publish those.
In teleop init, we just set all the controllers to send the motors to "0.0" position.
In teleop periodic we read the values from network tables (the jetson) and we simply write them to their respective controllers.
Finally, in disable init, we set all the motors to 0 voltage output and turn off the PID controllers.
The only other thing I think we're going to add is the AVERAGE BUS VOLTAGE of the system for debugging reasons. And that's it! That's all it does!
So here's the structure of the PID loops for the sparkmax controllers. I think the kMaxOutput and kMinOutput are on a scale of 0.0-1.0 as it seems in the documentation so we need to play with that. We'll keep kF to 0.0 because that means it'll make the motor move if we give it a non-zero set point no matter what. We'll leave IZone at 0.0 so the PID controller works as a normal PID controller. Basically what we're going to do is limit the PID controller to be a regular PID controller with some output limits.
Now how do we calculate these output limits? Well let's start by saying that the FRC PDP can handle (8x) 40A motors (high power motors) per robot. Now if we assume that assumes full output range. If we have 12 motors, to make the same amount of power for the system work without browning anything out, we should probably set the outputs to... 8/12 the power per motor or 66%!
Now since we're limiting the power of each motor we might want to do this systematically. Figure out which motors need the most power and set those to the max output and then set motors that don't need much power to less output so we can use the power at hand when needed. For example, if we set the shoulders to 1/4 the total power, the four motors together will make up the power of one motors. 9-high power motors should be handled by the system but not 12 you know?
Anyways, we aren't going to do that now. We're going to try that tomorrow. And we'll see if that calculation even works.