CAN Controlled Dual Closed-Loop Motor Controller

SAMD21 powered dual micro-gearmotor controller with CAN bus interface

Similar projects worth following
There are not many COTS solutions for closed-loop control of small gearmotors. This board can control two Pololu-style micro gearmotors with encoder and current feedback. Could be used to control a leg of a robot, a SCARA robot, or anything else you need motor control for.

Project Overview

Many low cost robotics projects tend to use stepper motors, as they are easy to use to create precision applications. Small robots are often made with hobby servos, which allow for some level of position control. However, neither give the developer full control. Stepper motors only provide open loop position control, and can miss steps under load with no easy way to detect it. Hobby servos tend to be "jerky" and also typically provide no feedback to their actual position.

This project aims to develop a low-cost design which can be used for closed-loop control of two micro-gearmotors. The current to the motors will also be monitored for current limiting and possible impedance control applications. It can be interfaced with over CAN bus, ensuring robustness and scalability in robotics applications.

So far, this motor controller is capable of speed, position, current, and open loop duty cycle control. It supports motion primitives - motion profiles calculated using onboard inverse kinematics for walking robots that offload a main controller. They are highly configurable, with a CAN ICD detailing the external interface. I have developed (somewhat) working demos of a stabilized wheeled inverted pendulum, robotic legs, crawling robot, and recorded motion playback.

Covered by and

Project Logs

  1. Design
  2. Control and Inverse Kinematics
  3. Current Control and Virtual Model/Impedance Control
  4. CAN Bus Network
  5. Design Update
  6. Hardware Revision and Dumb Demo
  7. Motion Primitives
  8. CAN ICD, Quadruped Prep, and Motion Playback
  9. Quadruped Walking and Gait Testing

Adobe Portable Document Format - 50.25 kB - 10/15/2020 at 13:42


Adobe Portable Document Format - 152.67 kB - 09/07/2020 at 19:06


Adobe Portable Document Format - 234.86 kB - 09/07/2020 at 19:06


Adobe Portable Document Format - 108.47 kB - 09/07/2020 at 19:06


Adobe Portable Document Format - 67.18 kB - 09/07/2020 at 19:06


  • Quadruped Walking and Gait Testing

    Jake Wachlin10/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. 


    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 Wachlin10/04/2020 at 21:59 0 comments


    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 Wachlin09/27/2020 at 18:48 0 comments


    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.


    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 Wachlin09/27/2020 at 01:15 0 comments


    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.


    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 Wachlin09/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 Wachlin09/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_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_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 Wachlin09/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 »

  • Foot Position Tracking Using Inverse Kinematics

    Jake Wachlin09/01/2020 at 01:22 0 comments

    This log is mostly focused on the code needed to achieve this: tracking of a circle with a 2-jointed robotic leg.

    The first step to achieving this is setting up the drivers for interacting with the motor. I am using the ATSAMD21 microcontroller and using the Atmel Studio IDE for the firmware development. In order to make multitasking easier, I am running FreeRTOS. If you have never used a real-time operating system (RTOS) on a microcontroller project, I highly recommend you try one out. In exchange for some added complexity on setup, an RTOS makes many common tasks much easier to implement. To speed development, I am also using Microchip's Advanced Software Framework (ASF) driver support.

    To interact with the motors, we need to set up the External Interrupt Controller (EIC) module to decode the signals from the quadrature encoders. Sparkfun has a great document laying out the states of a quadrature encoder and what transitions should be considered as moving forward or backward. The following code snippet implements this. This is an ISR triggered at every state transition of the 4 encoder pins. In the future I could speed this up by monitoring which pin triggered the ISR and only responding to that event, but it seems OK for now.

    void extint_detection_callback(void)
         bool m1_in1_state = port_pin_get_input_level(MOTOR_1_IN1_GPIO);
         bool m1_in2_state = port_pin_get_input_level(MOTOR_1_IN2_GPIO);
         bool m2_in1_state = port_pin_get_input_level(MOTOR_2_IN1_GPIO);
         bool m2_in2_state = port_pin_get_input_level(MOTOR_2_IN2_GPIO);
         // get state
         encoder_state[0] = ((m1_in1_state & 0x01) << 0x00) | ((m1_in2_state & 0x01) << 0x01);
         encoder_state[1] = ((m2_in1_state & 0x01) << 0x00) | ((m2_in2_state & 0x01) << 0x01);
         // increment if needed
         int32_t i;
         for(i = 0; i < NUMBER_MOTORS; i++)
                 case 0:
                     if(last_encoder_state[i] == 2)
                     else if(last_encoder_state[i] == 1)
                 case 1:
                     if(last_encoder_state[i] == 0)
                     else if(last_encoder_state[i] == 3)
                 case 2:
                     if(last_encoder_state[i] == 3)
                     else if(last_encoder_state[i] == 0)
                 case 3:
                     if(last_encoder_state[i] == 1)
                     else if(last_encoder_state[i] == 2)
             // Save state
             last_encoder_state[i] = encoder_state[i];

    Next, we need a way to control the motor. I have used the A3906 dual motor controller. By applying a PWM signal to the appropriate pins on the motor controller, I can apply various levels of drive effort to the motor. I am using the SAMD21's Timer/Counter for Control Applications (TCC) module for this. With the SAMD21's highly configurable clocks, I can choose a wide variety of PWM frequencies and resolutions. I have set up the system to run at ~4.8kHz with a resolution of 10,000 ticks. This code snippet shows how to set up the module. If you use this as well, be careful of how TCC maps a limited number of channels to a larger amount of pins.

            struct tcc_config config_tcc;
        tcc_get_config_defaults(&config_tcc, TCC0);
        config_tcc.counter.clock_source = GCLK_GENERATOR_0; // 48 MHz
        config_tcc.counter.clock_prescaler = TCC_CLOCK_PRESCALER_DIV1; // PWM freq = clk / (N (TOP + 1)), where N is prescaler
        config_tcc.counter.period = PWM_PERIOD; = TCC_WAVE_GENERATION_SINGLE_SLOPE_PWM;
       [1] = 0; // channel 1 mapped to pin 5
        config_tcc.pins.enable_wave_out_pin[5] = true;
        config_tcc.pins.wave_out_pin[5]        = MOTOR_1_CONTROL1_GPIO;
        config_tcc.pins.wave_out_pin_mux[5]    = PINMUX_PA15F_TCC0_WO5;
        config_tcc.wave_ext.invert[5] = true;
   [0] = 0; // channel 0 mapped to pin 4
        config_tcc.pins.enable_wave_out_pin[4] = true;
        config_tcc.pins.wave_out_pin[4] = MOTOR_1_CONTROL2_GPIO;
    Read more »

  • Design

    Jake Wachlin08/02/2020 at 20:03 0 comments

    This design must meet a few requirements:

    • Control 2 Pololu-style micro gearmotors, 6V versions
      • Handle motor driving
      • Handle quadrature encoder feedback
    • Monitor motor current for both motors
    • Computational ability to run PID (or more advanced) controllers for two motors simultaneously 
    • CAN bus interface for external commands and feedback to main controller

    There were a few use cases I was thinking about with this:

    1) Walking Robot

    One of these boards can be used for each leg to control 2 joints. A main controller can make decisions about how to move each leg, and this board will handle the realtime, low-level motor control to achieve the desired response.

    2) SCARA Arm

    One of these boards can be used to control the SCARA arm. Instead of stepper motor control, the system could move faster and have knowledge of when the arm is in the correct position.

    With those requirements and use cases in mind, the design was started. An ATSAMD21G18 was chosen for the microcontroller. This is a 48 MHz ARM Cortex M0+ microcontroller with enough power to run FreeRTOS, perform the needed motor control, and interface with the CAN bus. It does not have a CAN controller built in, so an external controller will be used, the MCP2515. The A3906 motor controller can handle both motors, up to 9V and 1A per motor. It has built to support current monitoring. Finally, a high-speed nonvolatile memory in the form of FRAM was included so the system could continuously store the current motor positions. In some use cases, this could be loaded on boot so no homing process would be required to determine the motor positions.

    The ATSAMD21 is used in many Arduino compatible boards. In this case, the firmware will be developed with Atmel Studio, but theoretically this board could be made Arduino compatible.

    In the next logs I will look at the initial firmware development, design of an example walking robot leg, and initial motor control.

View all 9 project logs

Enjoy this project?



dekutree64 wrote 10/03/2020 at 19:28 point

FYI you can easily add a position feedback wire to any hobby servo. Open it up and solder on another wire where the potentiometer output pin connects to the control board. The board already has a resistor to make a voltage divider for its own reading of the position, so you can just run it straight to an ADC input pin on Arduino or whatever.

But servos are still imprecise, since the motor is either full on or full off, which makes for jerky motion and overshoot, which is dealt with by having a large "deadband" around the target position where it won't start up again until the target gets some distance away from the current position. So even if you gradually change the target position, it just results in repeated jerks rather than smooth motion.

But with the feedback wire you probably could do a little bit of current limiting/velocity control by using a servo that tolerates a wide voltage range, like SG90 can handle 3v to 7.2v. By switching to 3v supply when starting moving or approaching the target, you could get a little bit smoother movement. And the fact that it can survive 7.2v while starting up means you could probably push it to 12v or maybe even higher once it's up to speed. Would be interesting to experiment with.

  Are you sure? yes | no

Jake Wachlin wrote 10/04/2020 at 16:05 point

Yeah I've done that before, and as you say it helps somewhat in that you know what the servo is doing at least, but the control is still not great.. Adafruit also sells servos with a feedback wire already provided (e.g. That said, this project allows for arbitrary feedback over the CAN bus as well as more advanced, smoother, tracking control. Thanks for checking out the project!

  Are you sure? yes | no

Similar Projects

Does this project spark your interest?

Become a member to follow this project and never miss any updates