Project Logs

Collapse

• Revision 2.0: CAN, Dual Motors, and Virtual Model Control

  Jake Wachlin • 02/03/2021 at 04:32 • 0 comments

  In the previous log, I explained how I had got basic closed loop position control working on version 2.0 of the dual motor controller. Profiling showed that this update worked as intended, and the performance is vastly improved. Instead of a 500Hz control loop, this runs its controllers at 4000Hz (including motion primitive planning) for smooth motion control. It was a big jump from a Microchip Cortex M0+ to a STM32 Cortex M4F microcontroller which I was not familiar with. The previous log noted a few next steps, all of which are now complete:

  • Set up CAN bus interface (RX and TX)
  • Test multiple devices on a network
  • Swap out current sense resistors
  • Test and tune current control
  • Test and tune Virtual Model Controller

  ---

  CAN, Dual Motors, and Position Primitives

  I set up the CAN interface, connected two motors, and attached the ESP32-based robot controller via CAN to this device. I then ran a demo running quadratic Bezier curve-based motion primitives. This worked well, as good or better than V1.X. In this case, the performance seems to be mostly limited by the motor, but the control sounds noticeably smoother. The tracking performance is quite good. The image below is tracking the path with a 1500ms period.

  Even moving faster, the tracking performance is quite good. The image below is operating with a 750ms period. There is some limited performance as the foot moves rapidly from positive Y back, but this appears to be a motor limitation more than a controller issue (as indicated by motor saturation.)

  ---

  Current Control
  

  Version 1.X of this project had significant computational limitations which did not allow me to run current control fast enough to get good virtual model control results. With 8X control loop rate improvement, this version promises improvements in current control. Interestingly, the control loops is now fast enough (4kHz) that it is approaching the frequency of the motor driving PWM (currently 8.4kHz). Therefore, each control cycle sets effectively 2 pulses of the PWM on each loop! 

  The original current sense resistors were set to 0.24 Ohm, which did not provide enough resolution. I have a 3.0V reference and a 12-bit ADC, so it was not measuring the small currents of these motors very well. I replaced that with 2.14 Ohm resistors, and have much improved resolution. Ideally, this will be improved in the future with a smaller resistor and a current sense amplifier, but it seems to work for now.

  The current on each motor is sampled at 20kHz and digitally low-pass filtered. The low-pass filter was designed with a 90% rise time of ~11ms. This current feedback signal seems quite reliable so far.

  A PI controller is used for the current control. This controller is tuned to be almost entirely an integral controller, so that the command is ramped up and down and doesn't make major jumps. This seems to work quite well.
  

  ---

  Virtual Model Control
  

  With effective current control, virtual model control can now reliably be performed. As implemented, a virtual spring and damper in both X and Y directions in the cartesian space are connected between the current foot position and some desired position determined based on a motion primitive. The motor torques are determined (assuming no friction and no gravity, which I may account for in the future) using the transpose of the Jacobian. Then, assuming a linear relationship between motor torque and motor current, the torque is applied via current control. The image below shows the basic concept.

  While this control approach makes a number of assumptions that may be much less reasonable on these cheap, high ratio gearmotors than high-end motors used in academic or commercial robotics, it does appear to work reasonably well. The image below shows tracking through a quadratic Bezier curve based motion primitive using this virtual model control with a 3000ms period.
  

  The VMC does struggle with fast movements. While the above motion primitive had roughly constant foot speed throughout the motion, the primitive below...

  Read more »

• Revision 2.0 Initial Testing

  Jake Wachlin • 01/13/2021 at 03:15 • 0 comments

  As discussed in the previous log, I developed a 2.0 version with a main goal of improved performance. I upgraded the microcontroller from a Cortex-M0 core to a Cortex-M4F alternative. While doing this, I also jumped from Microchip's SAM line of microcontrollers to the STM32 family. I am relatively new to STM32, but this project is a good learning experience.

  The STM32CubeIDE makes it easy to set up all the peripherals and middleware such as FreeRTOS. Because the previous version was built on FreeRTOS, and most of the more complicated code was agnostic to hardware, porting the firmware over was fairly straightforward. So far, I have:

  • System boots up and FreeRTOS is running
  • Position control and motion primitives are working

  ---

  Control Optimization
  

  In previous analysis, the calculations for motion primitives to determine the next setpoint were separate from the low-level control loop calculations. This was done due to limitations with how slow the primitives calculations were. Originally, before optimizations the motion primitive calculation took 1.88ms and the control loop took 236us. By setting up pre-calculations and implementing the control loop with only integer math used, this was much improved. However, the pre-calculations were very memory limited, and it was still not very fast. This new microcontroller is much faster.

  Now, with a floating point PID controller used for motor control, the control loop itself only takes about 11.8us!

  That is great, meaning we can set the controller to run much faster, get better current control, and smoother motion. I therefore updated the control rate to 4kHz (from 500Hz originally). I also set the controller to motion primitive mode, which calculates a Bezier curve, inverse kinematics of the leg, and PID control of both motors on each cycle. Without optimization, this requires 115us. This is about 18X faster than the previous version! We can also see here that the loop is running at 4kHz now.

  While this is probably good enough, I wanted to do a bit more optimization. The inverse kinematics use both arccos and arctan functions. I figure that these are likely slow, so set about to make faster lookup table versions. This is still a work in progress to improve its accuracy, but did speed up the calculations down to 75us total, now 28X faster than V1.0. While not measured, I couldn't tell the difference visually or audibly using the fast versions versus direct calculation.

  ---

  Next Steps
  

  • Set up CAN bus interface (RX and TX)
  • Test multiple devices on a network
  • Swap out current sense resistors
    • My chosen values were too low, I forgot that the SAMD21 ADC has a PGA but the STM32 does not
  • Test and tune current control
  • Test and tune Virtual Model Controller

• Revision 2.0: Microcontroller upgrade

  Jake Wachlin • 12/19/2020 at 19:39 • 0 comments

  I have been using this motor controller for building a small walking robot. During further development of that project, specifically trying to get virtual model control working correctly, I started running into performance issues. Even with performance optimizations, there was only so much I could do without really significant effort. I have been using the SAMD21 microcontroller, a Cortex M0 core running at 48MHz and 32KB of SRAM. It does not have an FPU, which was starting to be a major issue. When I tried to perform significant pre-calculations of the motion profiles, I ran into memory issues. The clear next revision then was to move up to a more powerful microcontroller, particularly one with an FPU. My projects are also as much about learning as building stuff, so I also decided to use an STM32 since I have limited experience with those. I have also been building more PCBs with JLCPCB, and they have numerous STM32 options.

  I chose the STM32F407, which represents a really significant upgrade in many ways:

  • Clock rate increase to 168MHz (3.5X faster)
  • Memory increase to 192KB (6X more)
  • Has an FPU (likely 50+X faster for some operations, not including clock speed upgrades)
  • Has an onboard CAN controller
  • Has multiple onboard quadrature decoders (reduce overhead from interrupts used on SAMD21 version)

  Besides the major upgrade on the MCU, I made a few other usability upgrades:

  • Added 2 LEDs for status indication
  • Voltage reference for more accurate current feedback

  The image below shows the upgraded design. The connectors and mounting holes have not moved, so that I can easily swap out the older versions. This design has been ordered and I'm just waiting for it to arrive. In the meantime I can make some progress on the firmware, pulling in much of the development work I made for the SAMD21 into the STM firmware.

• Quadruped Walking and Gait Testing

  Jake Wachlin • 10/19/2020 at 21:58 • 0 comments

  The last log covered some of the preparation for building a walking quadruped robot using four of the dual motor controllers, one for each leg. The parts for doing this have now arrived, and I have been working on getting the system walking.

  ---

  Leg Trajectory Simulation
  

  While I had previously shown the motor controller controlling a leg through a motion profile, this profile was a complete guess and not based on any research. Unsurprisingly, that motion profile did not work well to get the system moving. I therefore decided to write a Python kinematic simulator that could be used to rapidly evaluate motion profiles, see if they are in the leg's reachable area, and ensure my inverse and forward kinematics are correct. I set it up roughly as a mirror of the C code running on the leg controller to ensure that process is correct. I give it a set of keyframes. Each keyframe consists of an x and y cartesian space coordinate and a value that indicates at what part of the cycle period it should reach that point. All other desired positions are linearly interpolated between those points. These desired positions are fed first into the inverse kinematic solver to calculate joint angles, then the forward kinematic solver to calculate knee and foot positions. This step will fail if the desired position is outside of the reachable area. The positive X axis is "down" and the positive Y axis is "forward," with the origin at the hip. The blue line in the image below shows the leg, with circle markers at the hip, knee, and foot. The black dots are the foot positions throughout the trajectory, equally spaced in time. Therefore, they are more bunched together during slower motions and more spaced apart during fast movements.
  

  Clearly, this is an odd and complication motion and it is unsurprising that it doesn't work well. 

  ---

  Gaits 

  With my motion profile guess not working well, I spent some time reviewing research into quadrupedal robots. One of the most well known is the MIT Cheetah. I had the pleasure of taking Professor Sangbae Kim's Bioinspired Robotics course at MIT, and it was a fascinating introduction to the topic in general and to the MIT Cheetah in particular. Researchers have long classified different gaits for walking animals, including the trot, gallop, pronk, and bound. The following video in particular shows these various actions on the MIT Cheetah. Even if you don't know these gait classification terms, you will recognize the motions.

  These motions serve as a starting inspiration for the gaits I will implement on my robot. I started with the slow trot, shown at about 0:25 into that video. From the video, we can gain some insight into how this gait is implemented. The front right leg and rear left leg are in sync, and the front left leg is in sync with the rear right leg. Throughout the motion, the foot is on the ground roughly 3X longer than it is in the air. The trajectory is shifted rearwards of the hip, so that the front of the trajectory is just slightly forward of the hip. Although the trajectory is likely more complex, and is skewed due to the proprioceptive control used, it looks to my eye roughly to be a triangle with side lengths that are roughly equal. The image below shows the trajectory generated for this gait. It matches all these observed characteristics.

  The next gait is called the fast trot at about 0:32 in that video. Again, this is not very precise, but we can extract some rough characteristics from what we see. As with the slow trot, the front right leg and rear left leg are in sync and the front left leg is in sync with the rear right leg. However, here the foot is on the ground roughly the same amount of time that it is in the air. Because of this, most of the time only two feet are touching the ground. In the slow trot, there is a lot of time with all four feet contacting. This motion profile is shifted more forward, with the rear just back of the hip. It looks roughly triangular, but...

  Read more »

• CAN ICD, Quadruped Prep, and Motion Playback

  Jake Wachlin • 10/04/2020 at 21:59 • 0 comments

  CAN ICD
  

  This log is a mixed bag of a variety of related efforts. The first task was to generate a proper CAN interface control document (ICD). I had previously been adding new CAN messages simply by manually synchronizing the Arduino code for the ESP32 controller and the Atmel Studio project for the SAMD21 on the motor controller. Instead, these should both reference back to a ground-truth ICD. This document is a work in progress and will continue to be modified, but revision 1.0 is on the project page.

  ---

  Quadruped Prep
  

  While I intend to eventually build a bipedal robot, I want to start with a simpler version first. I plan to build a small quadruped robot that I want to be able to, at minimum:

  • Walk forward
  • Walk backwards
  • Turn in place

  I also want to dig into some more complicated and difficult tasks:

  • Walking on non-level or cluttered surfaces
    • Detect foot contact with current increase?
  • Ambulate with various gaits

  In preparation for this, I needed something far less janky than my cardboard structure previously shown. I had a square of HDPE that I will use as the backbone, and will use 3D printed legs. The previous 3D printed legs I had used had a large amount of backlash built in. This was because the motors were not actually bolted to any frame. Instead, a structure was printed to jam the motor in place, and the set screws on the hubs held the assembly together. The image below shows this at the knee joint.

   Clearly, that is not a great long-term solution. The motors' front plates have two M1.6 mounting screws. I purchased some of those, and printed new parts with counterbored holes for those screws. I am still waiting on the screws to arrive, but the following image compares the two "femur" designs. Obviously these are not great feats of mechanical engineering, but to start out I think they should work OK.

  The legs will attach to 3D printed mounts bolted to the HDPE sheet backbone. The image below shows the underside of this with the mounts attached. The mounts all have the appropriate counterbored holes for the motor mounting screws.

  Although a square clearly is an odd shape for the backbone, it provides copious space to mount all of the electronics. With four legs, we need 4 dual motor controllers. We also have the ESP32 main controller in the center. All of these PCBs are ziptied to the sheet. I only have 6 motors so far, 1 of which is 50:1 (the rest are 150:1), so I can't test it fully yet.

  I did however, run motion primitives on all 3 controllers with attached motors and it works as expected. As per the CAN ICD, I added a message to set the motor ticks-per-revolution, so I can configure the one controller with the 50:1 gearmotor to still follow the motion primitive properly, and that works great.

  For now, I am waiting on more motors and mounting screws, and then I'll be able to assemble it. I may even be able to get a 3-legged walker going first.

  ---

  Motion Playback
  

  Many industrial robots meant to operate near people (so called co-bots) have a mode where the user can move around the arm, it saves the motion, then it plays it back. Since this motor controller sends its position in telemetry feedback periodically to the ESP32 main controller, we can do something like this.

  I wrote this setup all in Arduino. Basically, we want to periodically save keyframes which consist of the time and the positions of both motors. In order to save memory, we filter out keyframes that are too similar to the previous keyframe. Then, on playback, we command the keyframes at the appropriate time. A better setup might interpolate between keyframes and get less jerky motion, but this demo does show the promise of this use case.

  The code to perform this is here. It is not pretty, but gets the job done.

  #include <CAN.h>
  #include "dual_motor_can.h"
  
  #define CAN_RX_PIN      4
  #define CAN_TX_PIN      5
  #define LED2_PIN              16
  
  #define FRONT_LEFT            (1)
  #define FRONT_RIGHT           (2)
  #define REAR_LEFT             (4)
  #define REAR_RIGHT            (3)
  
  #define MOTOR_0                (0)
  #define MOTOR_1                (1)
  
  #define TICKS_PER_REV (4200.0)...

  Read more »

• Motion Primitives

  Jake Wachlin • 09/27/2020 at 18:48 • 0 comments

  Overview

  In robotics, online motion planning can be computationally intensive and/or add significant network load. To avoid this, motion primitives are sometimes used. The concept is that the higher level controller can command pre-determined trajectories that the lower level "local" controller can implement. To understand this better, first consider the following picture. Here, the main controller is doing the lower level control of the motors. It receives information about the motor (position, speed, etc.) and calculates motor throttle commands to the motor controller. The motor controller then does the low level control (e.g. PWM control). The communication between the main controller and motor controller must be fast for this to work. In addition, it requires significant computation on the main controller.

  The next option is to offload the lower-level position control to the local controller, but still perform the motion planning on the main controller. This is what I've previously shown, where the ESP32 commands positions over the CAN bus to the dual motor controller. This works, but as the system has more and more motors (currently, we can address 16 motors), the main controller has to work harder to provide near real-time position commands to track trajectories. It could also potentially overload the CAN bus.

  Finally, consider a motion primitive approach. Here, the motion controller simply tells the local controller to follow a pre-determined profile. The local controller then calculates its own position commands online, and controls the motor to follow the trajectory. This can potentially fully offload the main controller and the CAN bus.

  While the concept is simple, implementation is more difficult. We will dig into the implementation next.
  

  ---

  Implementation

  Thankfully, we already have much of the supporting structure needed to implement simple motion primitives. Namely, we have the structure to unpack CAN messages, change control modes, and calculate inverse kinematics of a two-joint leg.

  In general, we will need a set of keypoints (provided in cartesian space for ease of understanding), and timing information of each of the keypoints. The primitives will be assumed to be periodic. We want the main controller to be able to slightly modify the primitive if needed. For example, we may want to stretch out a motion in one direction, change the period, or change the phase of the motion (time offset). We also may want to invert the motion to support left vs. right legs. I therefore created a few structures that help define the motion.

  typedef struct  
  {
  	float x;
  	float y;
  	float t_part;
  } keyframe_t;
  
  typedef struct  
  {
  	uint8_t num_keyframes;
  	uint8_t invert;
  	float tau; // period
  	float t_offset;
  	keyframe_t frames[MAX_NUMBER_KEYFRAMES];
  } primitive_t;

   I started with a single example primitive, although it should be clear that adding further primitives is straightforward. We have 8 keyframes here. The motion is sort of a pointy, stretched rectangle, with positions in mm. Note that based on the t_part of each keyframe, the section from frame 4 to 5 is the slowest. The thought is that this could be a walking/crawling motion where the leg moves slower while pushing against the ground and faster in mid-air.

  void motion_primitive_init(void)
   {
  	// Leg straight, motors at zero is leg straight down +x. Forward is +y
  
  	// t_part must be always increasing, never > 1.0. Must be a cyclical motion primitive
  
  	// Curved rectangle primitive
  	primitives[0].num_keyframes = 8;
  	primitives[0].tau = 2.0;
  	primitives[0].t_offset = 0.0;
  	primitives[0].invert = 0;
  	primitives[0].frames[0].t_part = 0.0 / primitives[0].tau;
  	primitives[0].frames[0].x = 0.07;
  	primitives[0].frames[0].y = 0.00;
  
  	primitives[0].frames[1].t_part = 0.2 / primitives[0].tau;
  	primitives[0].frames[1].x = 0.07;
  	primitives[0].frames[1].y = 0.06;
  
  	primitives[0].frames[2].t_part = 0.3 / primitives[0].tau;
  	primitives[0].frames[2].x = 0.083;
  	primitives[0].frames[2].y =...

  Read more »

• Revision 1 Hardware Testing and Dumb Demo

  Jake Wachlin • 09/27/2020 at 01:15 • 0 comments

  Hardware

  The new version of the motor controller and the first version of the ESP32-based main controller arrived this week, and I've been testing them out. The hardware turned out nicely. The screw terminals and better labeling all help the design look more professional.

  The ESP32-based main controller is shown below. It is a simple board, with the ESP32 for computation and wireless communications, a MPU6050 IMU, and a CAN transceiver.

  Next, let's look at the Rev 1.1 motor controller. The screw terminals and bulk capacitor are the most obvious changes, but the most important update is the 16MHz crystal added to the MCP2515 CAN controller.

  ---

  Networking
  

  With a main controller, and working CAN controller on the motor controller, I was able to move forward with the CAN communication layer. In a previous log, I explained how we split the bits of the frame IDs to indicate different messages. I also said the network would run at 500kbps. However, I bumped that up to 1Mbps, because why not? I added a software timer to send motor telemetry at 20Hz. For now, this is the motor position and current, but I plan to add more information in the future. 

  I also added more support for various control schemes. Each received CAN frame is put onto a FreeRTOS queue, and parsed in the CAN bus task. For each different received control command, the control approach is changed, so that the main controller can switch from closed loop position commands to open loop duty cycle commands without issue.

  The video below shows the first time the networking was working. The main controller was sending position commands, and the motor was moving correctly.

  ---

  Unbelievably Dumb Demo
  

  As an example of soft real-time interaction between the main controller and the dual motor controller board, I set up an "inverted pendulum" self-balancer using a Clif Bar box as the main structure. The main controller is taped to the top, and there is a proportional controller on roll (and, differentially, yaw) that commands open loop duty cycle commands to the motors. I had selected these motors for the high torque needed to drive the joints in a small walker robot, so they are not fast. The metal hubs used as wheels also get no traction on the desk and slip a lot. 

  The motors are mounted on a 3D printed frame I made in preparation for building a walking robot, which is then taped onto the box. Both PCBs are taped on as well, and a power tether runs to my power supply.

  The peanut butter banana bars are the good ones.

  What could possibly go wrong?

  In the video, I clearly have to help it stay upright. With limited traction and slow motors, it is pretty limited. The hardware is clearly working though, which is what really matters here!

  ---

  Next Steps
  

  • Run system on a battery
  • Control 2+ dual motor controllers from main controller
  • Continue to add software support
    • Motion primitives
    • Time synchronization
    • Position zero-ing command
    • Further telemetry
    • Improve CAN RX handling approach
    • Speed control
  • Build more demo projects
    • SCARA arm/plotter
    • Crawling robot
    • Bipedal walking robot?
    • Quadruped walking robot?
    • RC drift car w/ torque vectoring and traction control (using faster motors)

• Redesign and Main Controller

  Jake Wachlin • 09/07/2020 at 19:05 • 0 comments

  Dual CAN Motor Controller Redesign

  While most of the V1.0 dual CAN motor controller works, the CAN interface does not. In addition, there were a number of usability improvements I wanted to make for a V1.1 update. I used JLCPCB as the assembly service, and EasyEDA for its clean interface to that assembly service. Their assembly service only supports a limited number of components and only single-sided SMD parts. Therefore I will assemble all the through hole parts myself.

  The updates were:

  • Add a 16MHz crystal to the MCP2515 to fix the CAN issue
  • Add pads at logic level CAN TX/RX for debugging support
  • Label all external connections on silkscreen
  • Label maximum input voltage
  • Add optional CAN 120 Ohm termination resistor with solder jumper
  • Add 3-bit device numbering with solder jumpers to address devices without firmware changes
  • The first version just used 0.1" pitch through holes for the motor and power/CAN connections. On V1.1 I chose actual screw terminal connections so wires don't need to be soldered permanently in
  • Add holes that can be used for mounting
  • Add bulk capacitance for motor controller (doesn't seem strictly necessary, but not a bad idea to have)

  The PCB got slightly larger due mainly to the larger external connectors. The image below shows the updated layout. PDFs of the layout and schematic have also been uploaded to the project page.

  ---

  Main Robot Controller
  

  These dual motor controllers are meant to be peripherals on a larger network. They receive higher level commands from some external device over CAN, and handle the low-level motor control. I therefore built an example simple main controller. The main requirements are an attitude sensor and sufficient computational capability to theoretically run complex walking gait controllers.

  For this design I used the ESP32. With 2 cores at 240MHz, it has the computational capability to run fairly complex control schemes. It also has a built in CAN controller. Finally, it supports wireless connectivity so I can send commands remotely if desired. I also used the MPU6050 as an attitude sensor. I can either use its internal sensor fusion, or write my own Kalman filter to fuse the accelerometer and gyroscope data.

  Just like the dual motor controller, I made sure to label all external pins, add CAN TX/RX pads for debugging, as well as I2C debugging support. It has 2 LEDs which tentatively will indicate when it has good wireless communication and when it has a good orientation estimate.

  Thanks to JLCPCB's insanely cheap assembly service, I bought 10 of each board for ~$200 total. Getting each board nearly fully built and delivered in <2 weeks for ~$10/board is crazy!

• CAN Bus Network Layer and Hardware Fix Failure

  Jake Wachlin • 09/06/2020 at 18:18 • 0 comments

  With the project now able to control two motors well, and much of the supporting code for more complicated use cases written, the next step is to set up the communication between devices. For this, the Controller Area Network (CAN) bus is used. Originally developed for automotive use, CAN allows for relatively high-speed, robust, and long-range multi-master communication. It uses two wires which are differentially driven by a dedicated transceiver. I used the TCAN334 here. While some microcontrollers have a CAN controller built in, the ATSAMD21 does not, so I used the ubiquitous MCP2515. The MCP2515 is commonly used in all sorts of low-cost hobbyist (and serious commercial) CAN applications. It has an SPI interface to the ATSAMD21. The two steps here are writing the code to support commands over CAN, and wiring up the physical interface.

  ---

  CAN Network Software Implementation
  

  The Wikipedia page for CAN bus is very enlightening if you are not familiar with how CAN works. CAN is a multi-master protocol, so any device on the bus can send a frame at any time. Each frame has many parts, but the most important are the Identifier (ID) and data sections. The ID is either a 11 or 29 bit value that indicates what is in the message. The data section then holds 0-8 bytes of message data. At the lowest level, the IDs don't mean anything, it is up to the developer to determine how they should be set up. To de-conflict messages, lower ID values are sent first and are thus higher priority messages. When determining how to lay out the IDs, this should be accounted for and lower IDs used for higher priority messages. Finally, different nodes on the bus should never try to send frames with the same ID.

  Therefore, we take some inspiration from the way that the FIRST Robotics Competition lays out IDs using sections of the ID bitfield. Unlike FRC, we will use standard (11 bit) identifiers and communicate at 500kbps (may increase speed in the future). We use the most significant bit as a message type identifier: either a command or info frame. Therefore, all commands are higher priority than all info messages. The next most significant 3 bits are a message class identifier. The next 3 most significant bits are message indices. Finally, the last 4 bits are device indices. For commands, the device index indicates the receiver. For info messages, the device index indicates the sender.

  I then started to think about what messages I would want to send. These can and will be changed in the future but I started with the following:

  #define CAN_MSG_TYPE_SHIFT				(10ul)
  #define CAN_MSG_TYPE_MASK				(1ul << CAN_MSG_TYPE_SHIFT)
  
  #define CAN_MSG_CLASS_SHIFT				(7ul)
  #define CAN_MSG_CLASS_MASK				(7ul << CAN_MSG_CLASS_SHIFT)
  
  #define CAN_MSG_INDEX_SHIFT				(4ul)
  #define CAN_MSG_INDEX_MASK				(7ul << CAN_MSG_INDEX_SHIFT)
  
  #define CAN_MSG_DEVICE_SHIFT			(0ul)
  #define CAN_MSG_DEVICE_MASK				(15ul << CAN_MSG_DEVICE_SHIFT)
  
  #define CAN_MSG_TYPE_CMD					(0)
  #define CAN_MSG_TYPE_INFO					(1)
  
  #define CAN_MSG_CLASS_CMD_CONTROL			(0)
  #define CAN_MSG_CLASS_CMD_TIME				(1)
  #define CAN_MSG_CLASS_CMD_SET_PARAM			(2)
  
  #define CAN_MSG_INDEX_CMD_POSITION			(0)
  #define CAN_MSG_INDEX_CMD_SPEED				(1)
  #define CAN_MSG_INDEX_CMD_CURRENT			(2)
  #define CAN_MSG_INDEX_CMD_PRIMITIVE			(3)
  
  #define CAN_MSG_CLASS_INFO_TELEMETRY		(0)
  
  #define CAN_MSG_INDEX_INFO_POSITION			(0)
  #define CAN_MSG_INDEX_INFO_CURRENT			(1)
  #define CAN_MSG_INDEX_INFO_SPEED			(2)

  There are three obvious command classes I might want: control commands, time (for synchronization), and setting parameters. Then, within control I might want position, speed, current, or motion primitive commands.

  Within info, there are numerous telemetry messages we could send. To start, I listed motor position, current, and speed.

  This does not currently show how the data in these messages are packed, I will need to create an ICD for that.

  The first one to implement support for is a position command. I will set up an Arduino Uno + CAN bus shield...

  Read more »

• Current Control to Virtual Model Control

  Jake Wachlin • 09/05/2020 at 17:54 • 0 comments

  
  The last log covered the implementation of position PID control, then inverse kinematics which allow the system to track a motion in the Cartesian space. As another demo, I tried increasing the frequency of actuation. This leg is hauling. This is really where this project is much better than hobby servos: it can go faster and all parameters can be tuned. Note I also bought some steel hubs which greatly reduce the backlash in the leg.

  Just as position control is a prerequisite for a tracking controller with inverse kinematics, those same inverse kinematics and current control are prerequisites for a Virtual Model/Impedance Controller.

  The Virtual Model Controller implemented here is a simple Cartesian spring setup. The idea is that there are virtual springs attached between the real "foot" of the leg and some desired position for the leg to be. Based on the Hooke's Law for linear springs, we then calculate a desired force to apply to the foot. We then calculate the appropriate torque needed at each joint to apply that force. Depending on the environment, there may be nothing to push against to achieve that force, but this setup nonetheless allows for a pseudo-position control as well, as the foot will be "pulled" towards the desired position by the virtual springs. For ambulatory robots, this sort of controller is common, as it adds virtual elasticity which can help stabilize the walking controller.

  In the simplest motor models, motor current is proportional to motor torque. In ambulatory robots, this is often used to achieve torque control via motor current control. This works well with high-quality motors, low friction, and minimal gearing. The cheap, highly geared motors I am using are not ideal, to say the least. Furthermore, I do not have directional current sensing, which during transitions between direction can cause issues. Nonetheless, we will look at how the implementation would be achieved.

  ---

  Current Control
  

  The current control is implemented using the previously shown PID control library, but instead of providing encoder ticks inputs I use current in mA. Since I do not have directional current measurement, I don't want to allow the controller to command an opposite direction command, which could destabilize the controller. Instead, it clamps the command to zero. In reality, this should be OK, as the current will drop rapidly if command is set to zero. As a demo, I set a motor to drive +100mA and then -200mA periodically. The gifs below show the current being limited appropriately. It is not perfectly tuned, but is certainly working.

  Note that unless the motor pushes against something, it likely will not reach the desired current, and will simply spin in the correct direction indefinitely. This can be a challenge with the virtual model controller. If the controller is not fast enough, the leg will move so much that instabilities in the controller can be reached.

  ---

  Virtual Model Controller
  

  Assuming that current control is linearly equivalent to torque control (again, not very true for these motors, but they are related) the next step is to be able to transform desired force in the Cartesian frame to desired torque in joint space. This is directly related to the inverse kinematics discussed previously. Ignoring friction, gravity, and other forces, the transformation uses the Jacobian, which here is the partial derivatives of the positions with respect to the motor orientations. The values of this matrix change as the leg moves, so it must be recalculated online. Then, the torque can be calculated from the forces with the transpose of the Jacobian.

  The code for this looks like the following:

   void calculate_impedance_control(const impedance_control_params_t params, const leg_ik_t leg, const pos_joint_space_t current_pos, const pos_cartesian_t desired_pos, impedance_control_cmds_t * cmds)
   {
  	pos_cartesian_t current_pos_cart;
  	calculate_fk(&leg, ¤t_pos_cart, current_pos);
  
  	// Calculate desired force from springs
  	// TODO add...

  Read more »

View all 12 project logs