<?xml version="1.0" encoding="UTF-8"?><rss xmlns:dc="http://purl.org/dc/elements/1.1/" xmlns:content="http://purl.org/rss/1.0/modules/content/" xmlns:atom="http://www.w3.org/2005/Atom" version="2.0" xmlns:cc="http://cyber.law.harvard.edu/rss/creativeCommonsRssModule.html">
    <channel>
        <title><![CDATA[Stories by Nishant Vikramaditya on Medium]]></title>
        <description><![CDATA[Stories by Nishant Vikramaditya on Medium]]></description>
        <link>https://medium.com/@vikramaditya.nishant?source=rss-5b587d84fbe0------2</link>
        <image>
            <url>https://cdn-images-1.medium.com/fit/c/150/150/1*mYZVvGsVsVuePnqPJriwZA.jpeg</url>
            <title>Stories by Nishant Vikramaditya on Medium</title>
            <link>https://medium.com/@vikramaditya.nishant?source=rss-5b587d84fbe0------2</link>
        </image>
        <generator>Medium</generator>
        <lastBuildDate>Sat, 21 Nov 2026 19:01:37 GMT</lastBuildDate>
        <atom:link href="https://medium.com/@vikramaditya.nishant/feed" rel="self" type="application/rss+xml"/>
        <webMaster><![CDATA[yourfriends@medium.com]]></webMaster>
        <atom:link href="http://medium.superfeedr.com/" rel="hub"/>
        <item>
            <title><![CDATA[Building Sunshine, my meltybrain combat robot]]></title>
            <link>https://medium.com/@vikramaditya.nishant/building-sunshine-my-meltybrain-combat-robot-c30aa733fb97?source=rss-5b587d84fbe0------2</link>
            <guid isPermaLink="false">https://medium.com/p/c30aa733fb97</guid>
            <category><![CDATA[kalman-filter]]></category>
            <category><![CDATA[signal-processing]]></category>
            <category><![CDATA[computer-aided-design]]></category>
            <category><![CDATA[robotics]]></category>
            <dc:creator><![CDATA[Nishant Vikramaditya]]></dc:creator>
            <pubDate>Mon, 24 Aug 2026 21:18:28 GMT</pubDate>
            <atom:updated>2026-09-04T20:12:29.459Z</atom:updated>
            <content:encoded><![CDATA[<blockquote>This design is fully open-source! Find the CAD <a href="https://cad.onshape.com/documents/ed9d985535bddac0e44ba611/w/d1fed914941d1069aed35752/e/4349072e42fbe5c3d290cb70?renderMode=0&amp;uiState=6a8a91dc04acdaf3b4207b1c">here</a> and the code and PCB design <a href="https://github.com/Nv7-GitHub/sunshine">here</a>.</blockquote><p>Ever since I started watching NHRL (3lb) I’ve been wanting to build a combat robot. Specifically, one from this NHRL match (which is one of my favorite):</p><iframe src="https://cdn.embedly.com/widgets/media.html?src=https%3A%2F%2Fwww.youtube.com%2Fembed%2FrHSNbsJJPuc%3Ffeature%3Doembed&amp;display_name=YouTube&amp;url=https%3A%2F%2Fwww.youtube.com%2Fwatch%3Fv%3DrHSNbsJJPuc&amp;image=https%3A%2F%2Fi.ytimg.com%2Fvi%2FrHSNbsJJPuc%2Fhqdefault.jpg&amp;type=text%2Fhtml&amp;schema=youtube" width="854" height="480" frameborder="0" scrolling="no"><a href="https://medium.com/media/f7d06584847a2286e91085323272b9c7/href">https://medium.com/media/f7d06584847a2286e91085323272b9c7/href</a></iframe><p>I initially decided to build a robot like Lynx, a beater bar, but instead in the 1 pound weight class due to cost and time constraints:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*uwxSvs5VpQOVIUte55lCLw.png" /><figcaption>My first robot design</figcaption></figure><p>This design unfortunately didn’t work out for a variety of reasons including weight, material choice, cable management, and weapon design, but I learned a lot about combat robot design. For my next design, I instead decided to develop the archetype of the other robot in the NHRL match above, Project Liftoff, because I felt that it was a more interesting technical challenge. This is because, unlike a traditional robot, a meltybrain has to use sensors and software to measure its position in real-time (at many thousands of times per second) and then modulate the wheel speeds while it is spinning to drive in a desired direction.</p><p>The main goal of this project was to solve the problem of <strong>drift </strong>— where the angle that the meltybrain thinks is “forwards” drifts over time. You can see this happening in the match above, through the heading light (the red and green dots) which are made by turning the LED on when it is pointing the direction it thinks is forwards and leaving it off otherwise. You can see, when Project Liftoff is spinning for a while, that this forwards direction spins around until the drivers get a chance to correct it. This project successfully solved this problem, and along the way I built my first functional combat robot, made custom electronics, a whole software suite from the ground up, and more!</p><h3>Meltybrain Design considerations</h3><p>Before I even began designing I considered the following factors and decided upon the following parameters:</p><ul><li>Drive Motor — Repeat Robotics 2822</li><li>Body diameter — 140mm</li><li>Max body speed — 4,000 RPM</li><li>Wheel diameter — 44mm</li></ul><p>First, I based the body diameter off of other similar robots and general eyeballing. Once I selected this, I used the characteristic BLDC equations to calculate the max body RPM I could spin up to in a reasonable amount of time with a 10A current limit per motor. Finally, I chose the wheel diameter to be a reasonable amount larger than the motor diameter. Picking the motor KV (1100) was simple given these parameters.</p><h4>Where this went wrong</h4><p>While testing my design I had lots of problems with bouncing, wobbling, and I also think I was leaving weapon performance on the table. For future designs I’m going to pursue a more “pancake” design, where I would have a larger body diameter and smaller wheel diameter so that the overall robot is thinner and wider. This increases the moment of inertia, which means it hits harder and stores more energy, and also makes the robot more stable. For this robot, I likely should have chosen a larger body diameter and then made smaller wheels that went on the motor shaft instead of the can instead.</p><h3>Mechanical Design</h3><p>At 4,000 rpm the 35-gram battery in my robot alone exerts ~70 pounds of force outwards. Each wheel assembly exerts ~100lb outwards. Considering the rest of the robot is also being pulled outwards by the centrifugal forces, I decided to add aluminum rings to handle them. This led me to my overall design which was a sandwich of a 3d-printed frame between the load-bearing aluminum rings.</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*7U7LXfoYDBZtjzyiKzqJxg.png" /><figcaption>My final 1lb meltybrain design</figcaption></figure><p>For armor, I decided to have a soft TPU outer layer and a hard PLA+ inner layer:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*-2p-zFJZ9JtJSwuyrhlDqQ.png" /></figure><p>Everything was printed at 100% infill so the CAD center of mass would perfectly reflect the real world (and I found that this worked IRL to an accuracy of ~1%).</p><p>I used a master sketch-based design approach in Onshape which worked out pretty well.</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*W1dcW9-BfqJoasUH_3M2pA.png" /><figcaption>The master sketch</figcaption></figure><p>However, the outer aluminum rings proved to ultimately be the weak point of the robot. When redesigning, I think I will use a single steel plate that goes through the middle of the robot and sandwich TPU or UHMW around it. You can see the full design in the CAD linked at the top of this post.</p><h3>Wheels!</h3><p>Surprisingly, the hardest aspect of the physical robot construction was making the wheels. Originally, I 3d-printed wheel hubs and wrapped them in self-fusing silicone tape:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*ZisuiJwBu5t-yH3qrOEwIA.jpeg" /><figcaption>The blue tape wrapping the wheels is self-fusing silicone tape</figcaption></figure><p>Although this “worked”, it provided basically no cushioning, which led the robot to just constantly bounce up and down. Additionally, it got dirty super quickly and afterwards lost all of its grip, leading to terrible spin-up times as the wheels were just slipping. I didn’t get around to attempting translation with these wheels, but I doubt it would have worked at all.</p><p>After this, I printed TPU wheels (which didn’t work much better due to the same reasons) and then cast polyurethane wheels using Vytaflex 60A. Due to leaks in the first mold, it took multiple tries and a lot of pain in the demolding process to finally get right:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*fIF-xphGW7ULQNXTNXJklw.png" /><figcaption>Some failed attempts at molding wheels</figcaption></figure><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*Pm9t6jalIX5NxUiLxLMegQ.png" /><figcaption>The final, somewhat successful mold design (blue = polyurethane)</figcaption></figure><p>Even despite these wheels, I was still unable to get translation working to the level that I desired. This is mostly because of the robot’s geometry, since when I try translating the robot tilts and rubs against the floor, causing it to slow down, or at high speeds, flip it onto it’s side. As mentioned above, a better design would be more like a “pancake”. However, the problems I faced are also partly because of the wheels, which are still very stiff and bouncy. The robot bounces quite a bit when it hits any bump, limiting the time the wheels spend on the ground and also the contact patch size. In a future design I will try to have thicker urethane that will better absorb the bouncing. Due to these problems, I was unfortunately only able to get translation working well at a low speed (~900 RPM) where the motors are able to overpower the friction between the frame and the ground.</p><h3>Sensor Selection</h3><p>There are many different sensors that can be used to measure a meltybrain’s position, but here are a few that I’ve seen:</p><ul><li><strong>High-G accelerometer</strong> — These can measure centrifugal force, which can be used to determine angular velocity which can be integrated to get position</li><li><strong>Magnetometer</strong> — These measure the magnetic field created by (among other magnetic fields) the Earth’s magnetic field, which forms a sine wave on the signal as the robot spins within it</li><li><strong>RSSI</strong> — Similar to the earth’s magnetic field, the signal strength also varies in a “sine” wave as the robot spins (depending on antenna design)</li><li><strong>IR emitter/receiver </strong>— An IR emitter stationary in field can be detected by an IR receiver on the robot. This seems to be unreliable due to reflections though.</li></ul><p>I decided that I wanted to use a combination of the first two sensors together — the magnetometer could be used for roughly determining the absolute position of the robot, and the high-g accelerometer could provide a velocity signal that could smooth out and improve the position determined by the magnetometer (through a combination of filters described later).</p><h3>Electronics Design + Assembly</h3><p>Since I was using a magnetometer, I didn’t want high currents and electrical noise from the motors/ESC affecting the measurements. Because of this, I decided to separate the high-power circuitry, but consolidate everything else together onto a PCB I designed. In the end, I used the Repeat Dual Brushless Drive ESC and a custom PCB I designed for everything else. Here are some of the major components and important specs that I looked at:</p><ul><li><strong>Motor Controller</strong>: Repeat Dual Brushless Drive ESC [45A per motor]</li><li><strong>Receiver and Processor</strong>: ESP32-S3-WROOM [2 core, has USB support and FPU] (1 thread for sensing/control, 1 thread for communications using ESP-NOW)</li><li><strong>High-g Accelerometer</strong>: ADXL375 [+/-200G] (a huge pain to procure, eventually I got it from Amazon)</li><li><strong>Magnetometer: </strong>LIS3MDL [1kHz sampling]</li></ul><p>The schematic and PCB were designed in KiCad and all the files are linked at the top of this post in the GitHub, but here are the final schematic and PCB:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*dvD3slx7-6oQtr_kHwre5w.png" /><figcaption>Schematic (Note: I switched to ESP-NOW)</figcaption></figure><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*Mf0iovIgX99CWj9TDl6Ynw.png" /><figcaption>PCB layout</figcaption></figure><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*YxYHkHirv2eykWJWyHRQhQ.png" /><figcaption>PCB 3d model</figcaption></figure><p>I designed the PCB outline in Onshape before designing the physical PCB, so I could place the accelerometer precisely 11mm from the center of rotation and rotated 45 degrees so that the centrifugal force is shared across the X and Y axes.</p><p>I also made a screw switch built into the PCB by having a M4 screw with a brass washer glued on squeeze down onto two exposed pads. Underneath the PCB I put a heat-set insert into the frame.</p><p>I had all components besides the ESP32, LIS3MDL, ADXL375, LEDs, XT30 connector and aluminum polymer (don’t use electrolytic!) capacitors assembled on through JLCPCB Economic assembly. I then bought these components off of Digikey (and an Amazon breakout for the ADXL375), manually applied paste in the area, and then used my hot air gun to melt the paste and have it flow onto the pads before placing the ESP32 and other components. I had to do this because these components aren’t supported by economic assembly (either because of the footprint, thermal requirements, or the LEDs because they are double sided).</p><h3>Software Design</h3><p>For this project I decided to design the codebase around AI coding tools (Claude Code &amp; Codex), using some of the principles laid out in this talk:</p><iframe src="https://cdn.embedly.com/widgets/media.html?src=https%3A%2F%2Fwww.youtube.com%2Fembed%2FoTcimMwxRoM%3Ffeature%3Doembed&amp;display_name=YouTube&amp;url=https%3A%2F%2Fwww.youtube.com%2Fwatch%3Fv%3DoTcimMwxRoM&amp;image=https%3A%2F%2Fi.ytimg.com%2Fvi%2FoTcimMwxRoM%2Fhqdefault.jpg&amp;type=text%2Fhtml&amp;schema=youtube" width="854" height="480" frameborder="0" scrolling="no"><a href="https://medium.com/media/96c2c8746a90559027528a4d489fd3d2/href">https://medium.com/media/96c2c8746a90559027528a4d489fd3d2/href</a></iframe><p>The fundamental concept is derived from AdvantageKit in FRC, called <strong>replay</strong>. The core idea of this is that the robot code has two parts: the hardware, and the actual robot logic. Instead of having the robot logic directly interact with hardware, the data from the hardware (e.g. motor position, velocity, power draw) is passed through an “IO layer” to the robot logic. This is important because you can make changes to the robot logic on your computer and then pass data from an existing log file to the IO layer instead, to see if the code changes worked without any real-world testing. This concept is called replay.</p><p>This is especially valuable because it lets you test code changes to the robot as if they were the real thing (at least in the sensing code, like vision and odometry) without any physical interaction with the actual robot. This is extremely helpful for a meltybrain since most of the complexity lies in the sensing code. By collecting all the data needed and building the code around these principles, I was able to tune and test most of the filtering code in just a few hours using some logged data and looking at graphs made through replay.</p><p>I really like the logging, simulation, and replay system in FRC, but I wanted to optimize it for my specific use case. Specifically, unlike FRC’s robot code which runs at 50 Hz, my system runs at 1 kHz, and ESP-NOW also has much less bandwidth than the WiFi used in FRC. Because of this, I instead implemented a system where the inputs are sent at 1 kHz and everything else is sent at 100 Hz. Then, in the logging app all the incoming inputs are automatically replayed and rendered so I get to see graphs from the code as if everything was being logged at 1kHz by reconstructing everything from just the raw sensor input data on my computer.</p><p>In general the software has 3 main components: <strong>inputs</strong>, <strong>variables</strong>, and <strong>state</strong>.</p><ul><li><strong>Inputs</strong>: This is just the raw sensor data. I logged motor RPMs (via dshot eRPM), battery voltage ADC output, acceleration, magnetic field, driver control inputs, and RSSI</li><li><strong>Variables</strong>: These are values that can be determined from the inputs without any prior history, like battery voltage (calculated from the ADC output), or the angular velocity measured through acceleration (calculated from the acceleration inputs)</li><li><strong>State</strong>: These are values that require history to run. The way this worked is that the state is sent at 100 Hz while the inputs are sent at 1kHz. When the app connects, it runs the real robot’s code (which is encapsulated in it’s own C module, so it can be called from both the Rust Tauri backend and the PlatformIO code running on the ESP32), initializes it with the most recently sent state, and then replays the inputs through it at 1kHz to reconstruct the future state. Since the robot code is deterministic, this produces identical values to the actual state being sent by the robot.</li></ul><p>Using Claude Design, I also designed a UI like AdvantageScope but with better support for data units and a driver station also built in. It looks like this in replay:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*A6Awlz0Xrzu64v24P1zNnA.png" /><figcaption>The app in replay mode</figcaption></figure><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*vgI8aBM3dHsUaNDwdD230g.png" /><figcaption>The app in simulation mode</figcaption></figure><p>Overall I’m very happy with how the app and code turned out! It’s definitely a weird experience to give up actual debugging and instead rely on Claude (and honestly, probably not a good idea for more complicated projects), but it worked really well and let me program and test this robot vastly more quickly than could’ve been done otherwise.</p><h3>Filter Design</h3><p>The goal going into the project was to solve drift, and I decided to go about this by using a magnetometer. The main issues with using a magnetometer are hard iron<strong> </strong>offsets and sensor noise. I solved both using a <strong>RBJ 2nd-order</strong> <strong>band-pass filter.</strong></p><p>Although this isn’t how they work mathematically, the way I imagine a band-pass filter working is that they take in a <strong>signal </strong>— a series of measurements from the sensor, extract all the sine waves that it is composed of (look into a <strong>Fourier transform</strong> if this seems unfamiliar) and then only take the sine waves that are in a certain range of frequencies. Here’s how this solves both of the previous issues:</p><h4>Hard Iron Offsets</h4><p>Even when there are no magnets present, a magnetometer will read the Earth’s magnetic field with a constant shift, called the hard iron offset. This can be caused by current draw creating magnetic fields, inconsistencies in the sensor manufacturing, and in my real robot it is also caused by magnetic elements that are part of the robot itself.</p><p>These offsets result in a constant shift in the magnetometer, which show up as a constant offset, and therefore have a frequency of 0 Hz. Since the band-pass filter doesn’t allow frequencies below some amount higher than zero, this gets removed and so when the robot is spinning the filter is left with a zero-centered sine wave on the magnetometer X and Y axes.</p><h4>Sensor Noise</h4><p>The magnetometer also produces very noisy data. This results from electrical noise on the PCB, interference from the ESC, the motors spinning (which have magnetic fields of their own), and even other robots’ motors and other electrical components. However, this noise shows up in the signal at a much higher frequency than the sine wave that is Earth’s magnetic field, and so falls above the highest frequency allowed by the band-pass filter.</p><h4>Determining Position</h4><p>Now that the magnetometer reading on the X and Y axes forms two smooth sine waves (90 deg in phase apart) matching the Earth’s magnetic field on each axis, the angle of the robot can simply be calculated by taking the arctangent of these two measurements.</p><p>After the magnetometer processing resulting in position, it is fused with the velocity in a two-state Kalman filter. Overall, here is a diagram showing the filter’s structure and the result of each step on real-world data:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*3qdUMWMlJSi1wD58WLXHkQ.png" /></figure><h3>Results</h3><iframe src="https://cdn.embedly.com/widgets/media.html?url=https%3A%2F%2Fwww.youtube.com%2Fwatch%3Fv%3D4dahWgo04AI&amp;type=text%2Fhtml&amp;schema=youtube&amp;display_name=YouTube&amp;src=https%3A%2F%2Fwww.youtube.com%2Fembed%2F4dahWgo04AI%3Ffeature%3Doembed" width="854" height="480" frameborder="0" scrolling="no"><a href="https://medium.com/media/eb5519778fa9f4dd2066ee3c9df49eb5/href">https://medium.com/media/eb5519778fa9f4dd2066ee3c9df49eb5/href</a></iframe><p>I tested Sunshine against a 3lb robot without it’s weapon to see where it would fail. The filter worked (mostly) well, and the small areas where it jitters around were fixed later. However, after colliding with the arena steel walls for a while, the weapon tooth broke off:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*ezKOjOFh8Vckk8fv6ndXIA.jpeg" /><figcaption>The weapon broke off of Sunshine</figcaption></figure><p>The capacitors (which I forgot to hot glue down) also managed to flex out despite being very light aluminum polymer capacitors — indicative of the insane accelerations the robot underwent in these collisions! At least the arena has some (small) scars left over as a result of this though, and the sparks created by the Grade 5 Titanium weapon were super cool!</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*m8v1U2Fspl3PJ9x3S3FSQA.jpeg" /><figcaption>Sparks from the Titanium weapon</figcaption></figure><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*lb2wZal0W_nn1TZBoEYFOA.jpeg" /><figcaption>Damage to the steel kick plates in the arena from the weapon</figcaption></figure><p>Thank you to the <a href="https://discord.gg/w2cgnhQAEX">meltybrain Discord server </a>and North End Makerspace for helping, and Circular Reasoning for providing a robot test against!</p><img src="https://medium.com/_/stat?event=post.clientViewed&referrerSource=full_rss&postId=c30aa733fb97" width="1" height="1" alt="">]]></content:encoded>
        </item>
        <item>
            <title><![CDATA[How to use Charge 3B]]></title>
            <link>https://medium.com/@vikramaditya.nishant/how-to-use-charge-3b-0dd7040dbe51?source=rss-5b587d84fbe0------2</link>
            <guid isPermaLink="false">https://medium.com/p/0dd7040dbe51</guid>
            <category><![CDATA[nimh-rechargeable-battery]]></category>
            <category><![CDATA[robotics]]></category>
            <category><![CDATA[ftc]]></category>
            <category><![CDATA[battery]]></category>
            <dc:creator><![CDATA[Nishant Vikramaditya]]></dc:creator>
            <pubDate>Thu, 02 Jul 2026 22:16:42 GMT</pubDate>
            <atom:updated>2026-07-02T22:16:42.015Z</atom:updated>
            <content:encoded><![CDATA[<p>Thank you for buying a Tektite Charge 3B! It’s packed with features, and this blog post guides you through how to use all of them.</p><p>If you have any questions, thoughts, problems, or just want to share your experience, feel free to reach out in the #charge-3b channel in the Tektite discord: <a href="https://discord.gg/6uq7kxy6FZ">https://discord.gg/6uq7kxy6FZ</a></p><h3>Plugging it in</h3><p>To use Charge 3B, you need <strong>at least a 65W USB-C brick</strong>, but ideally you should <strong>use a 100W brick</strong> for full battery charging speed. Charge 3B automatically detects the wattage of the charger, and if it is exceeding the maximum it will reduce the charging speed. For example, when using a 65W brick and trying to charge 3 batteries on Turbo Mode (2A), the current indicator becomes red to indicate the power is limited to 1.6A:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*wbudKEkVZdYBiR6wdPCVww.jpeg" /><figcaption>Power is limited when using bricks under 65W</figcaption></figure><p>If the brick is not a 100W or higher brick, this is indicated when Charge 3B is first turned on. For example, many MacBook bricks only output 94W (even when they are advertised as 96W, since they output 20.5V):</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*AivuYS5H8zhZ4-LuLsPfqg.jpeg" /></figure><p>Another issue you may have is with the iPhone braided USB-C cables. Those cables (and some other ones) have chips inside that limit the current available, so even if you are using a 100W brick, Charge 3B will still only have 60W available:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*DRNLNmw52RhEvtj7ObFcgg.jpeg" /><figcaption>The Charge 3B can only use 60W, even with a 100W brick, due to the cable limiting the power</figcaption></figure><p>Once you have a suitable brick and cable combo, you are ready to use Charge 3B!</p><h3>Charge 3B’s user interface</h3><p>Here is how Charge 3B looks when turned on:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*Hd5f3TgLGKfEoZoA7TDl1w.jpeg" /></figure><p>There are 3 sections, one for each charging port. The left section of the screen corresponds to the left charging port, the right corresponds to the right charging port, and so on. Each section of the screen / port is controlled independently, so you can use multiple different features at the same time.</p><p>Each port has a list of menus that you can scroll through. To scroll to the next menu, press the bottom part of the screen. For example, to go to the next menu (Fast charging mode) for the middle port, you would press here:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*B6q52j50YxmpXjI8kIx0Sg.jpeg" /></figure><p>You can press on the top section to go back a menu. For example, to go to the previous menu (Slow charging mode) you would press here:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*npr8NwyAeppkc0-R0KlSbA.jpeg" /></figure><p>Once you select a menu, you can plug in a battery. <strong>Once the battery is plugged in, you can not switch menus! </strong>If you are in normal mode and you want to go to fast mode, you would have to un-plug the battery, switch to fast mode, and plug the battery back in.</p><h3>The charging modes</h3><p>4 of the menus are charging modes:</p><ul><li>Slow (0.8A)</li><li>Normal (1.2A)</li><li>Fast (1.6A)</li><li>Turbo (2.0A)</li></ul><p>The number is the amount of <strong>current </strong>being used to charge the battery. Using more current means the battery will charge faster, but it also means the battery will heat up more while charging which over the long term is bad for battery health. Because of this, I recommend using Normal or Fast mode to charge batteries during practice and Turbo for competitions, where you need your batteries charged quickly. When you plug in a battery while in one of the charging modes, the battery will automatically begin charging:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*mvZQSe5Y06CzObWtFjOjjw.jpeg" /></figure><p>There are 5 numbers shown:</p><ol><li><strong>Voltage </strong>— The current voltage of the battery. Although every battery charges to a different level (and batteries in worse health charge to lower voltages generally), a voltage around 14 is usually approaching being full while 10–12 is low. This is the same voltage shown in the driver station.</li><li><strong>Current </strong>— The amount of current going into charging the battery. This should correspond to which charging mode you’re using, but can vary if the battery connector is loose or you just plugged in the battery.</li><li><strong>Power </strong>— The number of watts going into charging the battery. This is just voltage * current.</li><li><strong>Charge time </strong>(hours:minutes:seconds) — The amount of time the battery has been charging for.</li><li><strong>Charger temperature </strong>— This is not the temperature of the battery! This is the temperature of the charging circuit inside of Charge 3B. This shouldn’t go over 100 C.</li></ol><p>The main one to look at is voltage, which sort of acts as a progress indicator for how charged the battery is. If you have 3 batteries in charge and you need to get the most charged one, you can generally just grab the one with the highest voltage.</p><p>When done charging, the menu turns green and Charge 3B enters <strong>TOP-UP </strong>mode (shown below), where a small charging current is applied to keep the battery voltage high. This shuts off after an hour, at which point Charge 3B fully disconnects the battery and the screen simply says <strong>DONE.</strong></p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*Mly1SE_wj4banmUZZprzvQ.jpeg" /></figure><h3>Internal resistance (IR)</h3><p>When you start driving or stall a motor on your robot, you will notice that the voltage of the battery goes down, but goes back up when you stop stalling the motor / driving. This is because the battery voltage drops when <strong>drawing current</strong>. For every amp of current drawn, the amount of voltage drop is equal to the <strong>internal resistance </strong>of the battery. For example, if the internal resistance is 0.100 ohms (100 milli-ohms), if a motor is stalling and drawing 9 amps, the battery voltage will drop 0.100 * 9 = 0.9V. Since voltage drop negatively impacts the performance of your robot, you want to use batteries with low internal resistance.</p><p>Charge 3B allows you to measure the internal resistance of your battery, which is often a good sign of the health of the battery. A lot of high-temperature charging, age, and use can all contribute to internal resistance becoming higher. To measure the internal resistance (IR), switch to the IR mode and plug in the battery. Charge 3B will cycle through various currents and average out the voltage changes to calculate IR.</p><p><strong>⚠️ FOR MOST ACCURATE MEASUREMENT, MEASURE IR WHILE THE BATTERY IS DISCHARGED A LITTLE (~12–13V)</strong></p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*TOECCehfZM0oqGAuc2uEIw.jpeg" /><figcaption>IR measurement of a pretty new battery</figcaption></figure><p>The number ending with “m” is the number of milli-ohms of resistance. Here is what to look for:</p><ul><li><strong>90–110m: Very good — </strong>New batteries should be in this range, and these are great for competition</li><li><strong>110–130m: Good </strong>— Probably not new, but still good and usable in practice and competition</li><li><strong>130–160m: Fine</strong> — Definitely usable for practice, and you can choose whether to use this in competition depending on the number of motors, power draw, and power requirements of your robot.</li><li><strong>160m+: Bad </strong>— Probably still usable in practice but it will drain quickly and sag a lot. At this point you may want to consider cycling your battery (explained below)</li></ul><p><strong>IMPORTANT NOTE: TAMIYA CONNECTORS </strong>(shown below)<strong> INTRODUCE 30–80m OF RESISTANCE! </strong>I highly recommend removing them and replacing them with XT30 or PowerPole connectors, which are usually in the single digits.</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/598/1*J7NMqh8KOhq1BQeom_Lzgg.png" /><figcaption>This is what a Tamiya connector looks like, I highly recommend removing it!</figcaption></figure><h3>Charging Statistics</h3><p>Charge 3B keeps track of how much power and time you have spent charging. You can see this in the Stats mode:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*a6fyY6GYmrDU-vPMQ5X67g.jpeg" /></figure><p>There are two numbers displayed in the bottom: total energy (in watt-hours) and total charging time (in days, hours, and minutes). To put the energy in perspective, as of writing this, 1000 watt-hours of electricity (1kWh) costs around 19 cents in the US. I’m excited to see the</p><p>This menu will also display the voltage of the battery plugged in but not charge it, so it can be useful if you just want to measure the voltage of a battery.</p><h3>Using the external resistor</h3><p>The right-most side of Charge 3B has a white port, which you can plug in an external resistor into:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*yktJF85Ew-8BHDQIL2j5wA.jpeg" /></figure><p>This allows Charge 3B to discharge the battery plugged into the right port, which means the right-side has extra menus and features. When cycling through the menus on this port, you will see <strong>CYCLE</strong> and <strong>STORE</strong>. Both of these menus require the external resistor to be plugged in to function.</p><h4>Battery storage</h4><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*9ESv5BljH2ca9sowNpSExQ.jpeg" /><figcaption>The storage mode of Charge 3B</figcaption></figure><p>If you aren’t going to use your battery for a while (1mo+), leaving it fully charged (14V+) or heavily discharged (&lt;11V) can stress the internal chemistry. You can switch your Charge 3B to <strong>STORE</strong> mode on the right-most port and plug in a battery and it will get charged/discharged to 12V at which point you can unplug it and store it somewhere.</p><h4>Battery cycling</h4><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*IyD_If7MDhqwt1LKgYz7PQ.jpeg" /><figcaption>The cycle mode of Charge 3B</figcaption></figure><p>High internal resistance can be caused by crystals that form around the electrodes of the battery in heavy use. If your battery has high internal resistance, this may be the cause. To solve this you can use the <strong>CYCLE </strong>menu on the right-most port, which fully discharges and charges your battery several times at low current. This can break up those crystalline formations and reduce the internal resistance of a heavily used battery, making it useful again for competition or practice use.</p><p>However, note that batteries can only do a finite number of cycles before they go bad, so <strong>use the cycle mode sparingly</strong>!</p><img src="https://medium.com/_/stat?event=post.clientViewed&referrerSource=full_rss&postId=0dd7040dbe51" width="1" height="1" alt="">]]></content:encoded>
        </item>
        <item>
            <title><![CDATA[Introducing torque-current control for FTC]]></title>
            <link>https://medium.com/@vikramaditya.nishant/introducing-torque-current-control-for-ftc-3db75d281258?source=rss-5b587d84fbe0------2</link>
            <guid isPermaLink="false">https://medium.com/p/3db75d281258</guid>
            <category><![CDATA[robotics]]></category>
            <category><![CDATA[physics]]></category>
            <category><![CDATA[electronics]]></category>
            <category><![CDATA[first-robotics]]></category>
            <dc:creator><![CDATA[Nishant Vikramaditya]]></dc:creator>
            <pubDate>Wed, 20 May 2026 05:21:04 GMT</pubDate>
            <atom:updated>2026-05-20T22:22:58.704Z</atom:updated>
            <content:encoded><![CDATA[<p>What if I told you there was a way to dramatically improve the efficiency of your FTC robot, make the driving smoother, never strip a hex or break a mechanism again, never damage a motor from stalling, and never pop a fuse? Well it turns out there is, and it’s both really interesting, simple to implement, and there’s a bunch of lessons along the way that I wish I knew when I was in FTC.</p><p>This post does a somewhat deep dive into motor physics which I think is incredibly useful but can take a bit of time and thinking to process, so if you’re just trying to implement this technique without understanding why it works, feel free to skip ahead to the “Prerequisite: Voltage Control” section!</p><h3>Modeling brushed motors with physics</h3><p>There’s generally three types of motors used in robotics: brushed, brushless, and stepper. Brushless (coming to FTC soon with the A301) is super interesting and I’ve already written a blog post about it <a href="https://medium.com/@vikramaditya.nishant/how-to-make-a-field-oriented-control-esc-from-scratch-948a5d6547b9">here</a>. This blog post, however, is about brushed motors since those are the ones currently used in FTC.</p><h4>Torque, Speed, Voltage, and Current</h4><p>Mechanically, a motor has two things you care about: torque (newton-meters), and speed (radians per second). When you multiply the torque and speed together, you get power. This is how much energy the motor is applying to the mechanism, every second.</p><p>For example: If your motor is spinning at 1 radian per second and applying 1 newton-meter of torque to your mechanism, that means it is applying 1 Nm x 1 rad/s = 1 watt of power = 1 Joule per second. So every second, 1 more joule of energy is given to the mechanism.</p><p>Electrically, a motor also has two things you care about: voltage (volts), and current (amps). When you multiply these numbers together you ALSO get power! If you are applying 1 volt to your motor and it is drawing 1 amp, that means it is drawing 1 V x 1 A = 1 watt of power.</p><p>Note that is <strong>DRAWING</strong> 1 watt! Unfortunately, due to resistance (explained soon) the power that a motor draws is greater than the mechanical power it applies to the mechanism. The ratio between these is the <strong>efficiency</strong> of the motor.</p><p>The fact that current and voltage seem to mirror torque and speed is not a coincidence! It turns out that the torque of a motor is equivalent to the current it is drawing, and that the voltage applied to a motor is (without resistance) is equivalent to it’s speed! This can be explained by looking at the basics of magnetism.</p><h4>A brief dive into magnetism</h4><p>Although magnetism is a broad topic, there are two laws that are fundamental to the physics of motors: <strong>Ampere’s law </strong>and <strong>Faraday’s law</strong>.</p><p>Inside the motor, there are a lot of coils of wire. When current flows through those coils of wire, they produce a magnetic field, and the strength of this magnetic field is equivalent to the torque the motor is producing. <strong>Ampere’s law </strong>tells us that the strength of the magnetic field is equivalent to the amount of current going in, and therefore, <strong>the current going into a motor is equivalent to the torque it produces</strong>.</p><p>Inside the motor there are also magnets that the magnetic field of the coils attract to (which is what actually produces the torque). When the motor starts spinning, the coils begin moving past these magnets quickly. <strong>Faraday’s law </strong>tells us that when a magnet moves past a coil, the coil generates voltage. This is why motors are also generators — when you spin them and connect a load to the input, they generate electricity!</p><blockquote>If you spin a FTC mechanism with a heavily reduced motor while the robot is off, you’ll see that the lights on the Control Hub actually turn on. This is because the motor is generating voltage which is back-powering the hubs!</blockquote><p>This voltage generated by the motor is called <strong>back-emf </strong>(where emf is short for electromotive force, basically a fancy name for voltage), and it is what dictates the speed of a motor. For example, when GoBilda tells you that their motor spins 6,000 RPM at 12 volts, that means that at 6,000 RPM the motor is generating 12 volts. If your battery is 12 volts, that means that you won’t be able to apply any more voltage, and so you can’t spin the motor past 6,000 RPM. However, if you had a higher voltage battery, you could spin the motor faster, and so <strong>voltage is equivalent to speed</strong>.</p><h4>The electrical model</h4><p>To make all the coils in the motor, there is a lot of wire. There are also brushes that connect these coils to the power input of the motor (as part of a mechanism called a commutator which I’m not going to explain but are cool). Together, these have <strong>resistance</strong>, which means that when current flows through them, some energy gets wasted as heat. The amount of resistance of a wire is measured in ohms. The energy being wasted as heat manifests itself as a voltage drop which follow’s Ohm’s law: V = IR.</p><p>Let’s say you have a really long wire from your 12 volt battery to your Control Hub that has 1 ohm of resistance. If the control hub is drawing 1 amp, then the voltage drop from that really long wire is 1 A x 1 ohm = 1 volt. When this happens, 1 volt is getting “wasted” on the wire so the control hub only receives 11 volts even though the battery is supplying 12 volts. We can calculate that the power being wasted on the wire is 1 V x 1 A = 1 watt! This is why you should make sure to have short and thick wire from your battery to your hubs.</p><p>You can model a brushed motor as a resistor in series with a voltage source that represents the back-emf of the motor. We can put in a 12V battery and make a diagram (called a schematic) like so:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*tQvS1GwPQI54QvrujbfbDQ.png" /><figcaption>A schematic of the model of a motor</figcaption></figure><blockquote>Note: Motors have inductance, which means that this model isn’t accurate during current transients. The time constant is pretty short though, so this model works well enough. Also yes, I showed the back-emf with the symbol of an inductor (it was the most motor-looking symbol I could think of)</blockquote><p>This can look a little intimidating at first, but makes a lot of sense when you run through some examples. All you have to know is that the voltage on the left side and right side are equal, and that the current through the resistor is the same as the current draw from the battery and the current draw of the motor. Let’s make a hypothetical motor that has the following properties:</p><ul><li>For every radian per second the shaft spins, the motor generates 1 volt of back-emf</li><li>The resistance of the motor is 1 ohm</li><li>For every amp the motor draws, it produces 1 N-m of torque</li></ul><p>Now, let’s say that the shaft of the motor is spinning at 12 rad/s. That means that the back-emf generated is 12 volts. Since the voltage of the battery is 12 volts, that means the voltage of the resistor has to be 0 volts, so that the voltage of the two sides add up to be the same (12V):</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*LT4M8Fx3CaWIXZdX1wheZA.png" /></figure><p>If the voltage across the resistor is 0 volts, that means that the current must be 0 amps (since V = IR and R = 1). Therefore, when the motor is spinning at 12 rad/s, zero current can flow, and so the motor cannot produce any torque. If the motor cannot produce any torque, it cannot spin any faster, so 12 rad/s is the max speed of this motor at 12V. This is the same reason that a typical FTC motor does not spin faster than 6,000RPM at 12V.</p><p>Now let’s say that our motor is stalling, so the shaft is not spinning and so the back-emf voltage is 0. That means that the voltage across the resistor must be 12V for both sides to add up. Since V = IR and R = 1, that means there must be 12 amps of current for the voltage to be 12V.</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*njhxjHVDZ1p49DS6li8ToQ.png" /></figure><p>Therefore, when our motor is stalled it pulls 12 amps at 12 volts. This is the the <strong>stall current </strong>of the motor. We know that for every amp it is drawing, it produces 1 N-m of torque. Therefore, when stalling, the motor produces 12 N-m of torque, which is the <strong>stall torque</strong>.</p><p>Now, let’s say our motor is at half of it’s maximum speed (6 rad/s). Using the same model, we can calculate that 6 amps are being drawn by the motor:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*ao02lhkP-qG_1hKduiOY_A.png" /></figure><p>When our motor is stalled, the speed is zero, and therefore the power (which is speed x torque) is zero. All the energy is wasted as heat because it is going through the resistor. In this new scenario, the power is non-zero since there is torque being produced and the motor is spinning. However, due to the resistance of the motor, 12 volts is needed even though only 6 volts is going into fighting back-emf. The rest is being wasted as heat due to the resistance of the motor, which is why the input electrical power (12 V x 6 A = 72 W in this case) is not equal to the mechanical power produced (which would be 6 N-m x 6 rad/s = 36 W). At this operating point, the motor is 50% efficient.</p><p>This explanation of how a motor works allows you to fully explain the brushed motor curves:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/998/0*8kk32P4HXfa93wip.png" /></figure><ul><li>You can see that as the motor spins faster, the current draw (blue dotted line) decreases. This is because more voltage is dedicated to overcoming back-emf so less is left for the resistor, and therefore the current through the resistor must drop</li><li>As the current draw drops, the torque drops equivalently because the two are proportional to each other</li><li>Power is torque x speed. Since speed increases as you go right and torque decreases as you go to the right, power forms a parabola which reaches its maximum at half speed and half torque.</li><li>Efficiency increases as current decreases, since less energy is wasted as heat in the resistance (to a point, after which other losses dominate)</li></ul><h3>How current control works</h3><p>So far, we’ve been using a model of a motor just connected to a 12-volt battery. However, the control hub actually lets us “create” a battery of any voltage (from 0 to the hub voltage) for the motor.</p><p>Using this, the actual current control is extremely simple. Once again, using the hypothetical motor presented earlier, let’s say we measured the speed of the motor as 6 rad/s using the encoder and we want the motor to draw 1 amp. We know the back-emf voltage is 6V, and that the voltage across the resistor must be 1 A x 1 ohm = 1 V. Therefore, we have to apply 6 + 1 = 7 V to draw 1 amp:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*YcvAcPcqI53icYTCy623rw.png" /></figure><h4>Current limiting (for drivetrain and PID)</h4><p>Oftentimes, you don’t want to actually control current/torque, but you do want to limit them so that you don’t slip, pop a fuse, or break a mechanism. You can do that very easily with this technique.</p><p>Let’s say in a given loop we measure the speed of the motor shaft to be 6 rad/s and we want to limit the current to 1 amp. Since the voltage from back-emf is 6 V, the voltage across the resistor can be a maximum of 1 volt before the 1 amp limit is exceeded. Therefore, we can apply anywhere from 5 V to 7 V to the motor.</p><p>Let’s say the driver slams the joystick forwards. Instead of just sending 12 volts to the motor like usual and wasting 6 amps just to have the wheels slip and create heat, this method would limit the voltage applied to the drivetrain to 7 V. You don’t waste energy slipping the wheels (which reduces wheel wear), you don’t waste energy making heat in the motor (which reduces current draw over the course of the match), and your drivers seem smoother since the wheels aren’t alternating between slipping and gripping.</p><p>Now let’s say you’re doing defense and pushing another bot. The wheel rpm is zero, so with this technique voltage would be limited from -1 V to 1 V. With 4 drivetrain motors this limits the current to 4 amps, where normally it would be 12 x 4 = 48 A (which would pop your fuse). The motors are also not going to get as hot even though they are stalling, and you have more traction since you are still experiencing static friction.</p><h3>Calculating the constants from a real motor</h3><p>So far we’ve been working with a hypothetical motor. Now, let’s use the real <a href="https://www.gobilda.com/5000-series-12vdc-motor-with-8mm-rex-pinion-shaft/">GoBilda bare motor</a>. There are a few very important specifications given:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*do7r_5_XYSak1-PGF9BYHg.png" /></figure><p>First, let’s start with resistance. They tell us that when stalling, the motor pulls 9.2A (stall current) at 12V. We know that when stalling all the current and voltage is going to the resistor. Therefore, we can calculate the resistance (R = V/I) to be <strong>1.3 ohms</strong>.</p><p>We can’t directly get the speed when the current is zero amps because even at no-load conditions things like air resistance and bearing resistance require the motor to exert some torque to keep spinning. GoBilda tells us that the motor needed 0.25A to spin at 5,800 RPM (607.4 rad/s) under no load. Therefore, we know that at 0.25A the voltage across the resistor is 0.325 V (using the resistance calculated above), and so at 5,800 RPM the motor generates 11.675 V. Therefore, the motor generates <strong>0.0192 V/(rad/s)</strong>.<strong> </strong>You can see this adds up to 12V given the no-load conditions:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*sT8Ucap7vKViwPL7O87Bng.png" /></figure><p>Finally, we can calculate the torque constant given that the motor generates 1.47 kg-cm (0.144 N-m) while stalling. Since the motor pulls 9.2A, that means the motor produces 0.144/9.2 = 0.0157 N-m of torque per amp, which can use to calculate the current needed to produce a desired torque. Using these constants, we can apply the method explained above.</p><h3>Prerequisite: Voltage control</h3><p>Normally, when you call motor.setPower(), it’s controlling <strong>duty cycle</strong>, which is effectively the percent of the battery voltage that is sent to the motor. If you set a power of 0.5 and your battery is at 14V, you are sending 7V to the motor. If the battery is at 12V, you are sending 6V to the motor. As you might imagine, this leads to the robot’s performance changing significantly over the course of the match as the battery voltage drops.</p><p>Compensating this out is very easy — instead of calling setPower from just the raw joysticks or PID output, you can send a voltage instead by dividing the output by the hub voltage in setPower(). For example, if you want to send 6V to your drivetrain, every loop you would divide 6V by the measured voltage of the battery and use that as the power in setPower. That way, even as the voltage sags during the robot’s acceleration, the performance should remain relatively consistent.</p><p>Although this isn’t a magic bullet in making the robot perform consistently across battery voltage, since the overall voltage will still decrease over the course of the match (which will reduce the max torque and max speed as you can see in the model above), it goes a long way in helping improve consistency and so I recommend doing voltage control for all motors in the robot.</p><blockquote>Pro tip: If you have already tuned your PID when the output was getting applied via .setPower(), just multiply the gains by 13 to get it well tuned with voltage compensation (because the PID was likely tuned around 13V).</blockquote><h3>Torque-Current Control with an encoder</h3><p>Using the equations explained above, we can implement torque-current control by measuring the velocity from the encoder. First, we define the constants:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*46PGM9D7QlO3lJHAupw73Q.png" /></figure><blockquote>To modify this for a different motor, just update the no-load RPM (5800), the no-load current (0.25), the stall current (9.2), and ticks per revolution (28).</blockquote><p>Once you have this, every loop:</p><ol><li>Calculate back-emf voltage from the encoder velocity</li><li>Calculate the needed voltage for the given current</li><li>Apply it to the motor via voltage control</li></ol><p>I implemented it like this:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*xTd89F8P8szXkUHe6H1Dsw.png" /></figure><p>If you instead had a goal voltage that you wanted to apply (e.g. if using PID or joysticks), you can calculate vCmd such that the current does not exceed a maximum:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*G7_GxYORVDFn-JM9-0YkEg.png" /></figure><p>Using this method, you can limit or target a current and torque with your motors! Here are my results:</p><iframe src="https://cdn.embedly.com/widgets/media.html?src=https%3A%2F%2Fwww.youtube.com%2Fembed%2FU4u7AS1aCZQ%3Ffeature%3Doembed&amp;display_name=YouTube&amp;url=https%3A%2F%2Fwww.youtube.com%2Fshorts%2FU4u7AS1aCZQ%3Fsi%3DtJyRmWxAz8ksPzVq&amp;image=https%3A%2F%2Fi.ytimg.com%2Fvi%2FU4u7AS1aCZQ%2Fhq2.jpg&amp;type=text%2Fhtml&amp;schema=youtube" width="640" height="480" frameborder="0" scrolling="no"><a href="https://medium.com/media/91489c5c79dea59a9fabe8cf3ae25711/href">https://medium.com/media/91489c5c79dea59a9fabe8cf3ae25711/href</a></iframe><p>With the 0.5A current limit, you can see that the motor accelerates slowly — you can hear it spinning up. Even when it is at the max speed, I can still stop it with my hands, and even twist it in the opposite direction, but when I let go you can see it accelerates in the target direction. You can also feel the constant torque when you hold the motor.</p><p>Note that negative current just means torque in the opposite direction.</p><h3>Torque-Current control without an encoder</h3><p>Although the method described above works well and is reliable, wiring the encoder for many motors can be quite burdensome and difficult to manage. To do torque-current control without an encoder, we can instead take advantage of current sensor internal to the Control Hub.</p><p>Looking back at the motor model, if you know the current flowing into the motor, you can measure the voltage drop across the resistor. If we also know the voltage we applied (which we do), we subtract the two to get the voltage from back-emf. Once we have the back-emf voltage, we can calculate the voltage to apply to the motor the same as above.</p><h4>The problem with current sensing</h4><p>Let’s consider what happens when the motor is spinning clockwise, but the torque applied is counter-clockwise. In this case, the motor is decelerating, which means that the current is being <strong>regenerated</strong>. This doesn’t affect the encoder method, as the regenerated current will simply be limited and the torque will continue to be limited.</p><p>Unfortunately, the current sensor in the Control Hub only returns the magnitude of the current flowing, so we need to figure out the sign ourselves. Since we can’t know whether the motor is accelerating or decelerating without an encoder, I just chose to always assume it was accelerating. This means that the code will correctly limit the current drawn from the battery (which happens during acceleration), but during deceleration it will not correctly limit the torque or regenerated current. That means this method does not control torque or current when decelerating, but that can be an acceptable trade-off when you’re just trying to prevent your fuses from popping.</p><p>Another downside of this technique is that it requires you to read the current sensor, which incurs a loop time penalty (unlike encoders, which are bulk-read).</p><p>I implemented this technique like so:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*Le_0L_LnMWWFv4XgqYniIw.png" /></figure><p>To summarize, this technique can make wiring much simpler, but incurs a loop-time penalty and does not control deceleration torque properly, so is only useful if you’re trying to limit current drawn from the battery (for example, as a quick fix to avoid popping fuses).</p><p>Here are my results with this technique, which are quite similar to the encoder-based technique despite only requiring the two motor wires:</p><iframe src="https://cdn.embedly.com/widgets/media.html?src=https%3A%2F%2Fwww.youtube.com%2Fembed%2FZASuAK9D5Ww%3Ffeature%3Doembed&amp;display_name=YouTube&amp;url=https%3A%2F%2Fwww.youtube.com%2Fshorts%2FZASuAK9D5Ww%3Fsi%3DpbvC1LX17Gn8wVAV&amp;image=https%3A%2F%2Fi.ytimg.com%2Fvi%2FZASuAK9D5Ww%2Fhq2.jpg&amp;type=text%2Fhtml&amp;schema=youtube" width="640" height="480" frameborder="0" scrolling="no"><a href="https://medium.com/media/09f2be851e2cc2e1847185f70ca696da/href">https://medium.com/media/09f2be851e2cc2e1847185f70ca696da/href</a></iframe><h3>Side quest: Encoder-less speed estimation</h3><p>You might notice that we estimated back-emf voltage to do torque control without an encoder. Since we know back-emf is just the motor speed multiplied by Ke, we can divide out Ke to calculate the motor velocity.</p><p>Of course, this is a pretty bad estimate because:</p><ol><li>When decelerating, it reads the speed as much higher than it actually is (due to the regen current problem explained earlier)</li><li>When at a low applied voltage, noise in the current sensing can dominate the back-emf voltage so the accuracy isn’t very good</li><li>In general, the current sensor is very noisy, so the readings are quite noisy</li></ol><p>Still, it works surprisingly well considering these drawbacks:</p><iframe src="https://cdn.embedly.com/widgets/media.html?src=https%3A%2F%2Fwww.youtube.com%2Fembed%2F3v8UZrQXsDg%3Ffeature%3Doembed&amp;display_name=YouTube&amp;url=https%3A%2F%2Fwww.youtube.com%2Fshorts%2F3v8UZrQXsDg%3Fsi%3DJk1409ypzpf38MLm&amp;image=https%3A%2F%2Fi.ytimg.com%2Fvi%2F3v8UZrQXsDg%2Fhq2.jpg&amp;type=text%2Fhtml&amp;schema=youtube" width="640" height="480" frameborder="0" scrolling="no"><a href="https://medium.com/media/e5df9f4fa0bb41e5ebd0f02682fa5b47/href">https://medium.com/media/e5df9f4fa0bb41e5ebd0f02682fa5b47/href</a></iframe><p>I hope that after reading this blog post you learned about motor physics, electricity, magnetism, and why you need to implement voltage, current, and torque control into your FTC robot!</p><img src="https://medium.com/_/stat?event=post.clientViewed&referrerSource=full_rss&postId=3db75d281258" width="1" height="1" alt="">]]></content:encoded>
        </item>
        <item>
            <title><![CDATA[How to make a zero-drift FTC localizer with Kalman Filters]]></title>
            <link>https://medium.com/@vikramaditya.nishant/how-to-make-a-zero-drift-ftc-localizer-with-kalman-filters-911807e0916d?source=rss-5b587d84fbe0------2</link>
            <guid isPermaLink="false">https://medium.com/p/911807e0916d</guid>
            <category><![CDATA[linear-algebra]]></category>
            <category><![CDATA[kalman-filter]]></category>
            <category><![CDATA[robotics]]></category>
            <category><![CDATA[control-system]]></category>
            <dc:creator><![CDATA[Nishant Vikramaditya]]></dc:creator>
            <pubDate>Mon, 19 Jan 2026 06:00:04 GMT</pubDate>
            <atom:updated>2026-01-19T06:13:21.873Z</atom:updated>
            <content:encoded><![CDATA[<p>In FTC, localization is traditionally done with odometry dead wheels, oftentimes with <a href="https://www.gobilda.com/4-bar-odometry-pack-2-pods-1-pinpoint-computer/">goBILDA Pinpoint</a> (if you aren’t using pinpoint, I strongly recommend getting it). However, vibrations from flywheels spinning have been shown to cause pinpoint to <strong>drift</strong>, where the readings change slowly over time, even if the robot is stationary. That is because pinpoint is using <strong>dead reckoning: </strong>it’s adding up the angular velocity and changes in the odometry readings thousands of times every second to get the current position. This is very accurate in the short term, but over time small errors can accumulate. Additionally, vibrations from the flywheel can cause the sensors it uses to report slightly inaccurate data, further compounding the drift over time.</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*RiPj9aR3EdHb40bYPVgdUA.png" /><figcaption>April tags</figcaption></figure><p>Another way of localizing is with <a href="https://limelightvision.io/products/limelight-3a">Limelight</a>. Since this looks at an April tag on the field, which won’t drift over time, this gives an <strong>absolute pose estimate </strong>— regardless of how long you’ve been running, the mean of a bunch of limelight readings should be your true position on the field. Unfortunately the problem lies in that you have to read the mean. This is because the data is very <strong>noisy</strong>, meaning that it jitters around and so using an individual reading or even a few readings averaged out can be inaccurate.</p><p>So, to summarize, we have two different sources of a position measurement: odometry, which is great over the short term, and vision, which is great over the long term. Excitingly, when you have multiple sensors measuring the same thing with different sources of error, there’s an amazing solution to getting the best of both worlds: <strong>Kalman filters</strong>.</p><h3>What are Kalman filters?</h3><blockquote>I learned about Kalman filters from an amazing document called “Controls engineering in FIRST Robotics Competition” by Tyler Veness. You can read it <a href="https://file.tavsys.net/control/controls-engineering-in-frc.pdf">here</a>, and if controls is your thing, I would highly recommend it! The math is somewhat intimidating and it took me two or three read-throughs to understand most of it, but model-based control is a completely different way of thinking about control systems that, although overkill for FTC/FRC, I’ve found incredibly useful in rocketry and is very important for the industry where you oftentimes can’t test your control system in your garage.</blockquote><p>At their core, Kalman filters are just trying to estimate two things: the current position (“<strong>state”, </strong>variable name<strong> x</strong>) and how uncertain the state estimate is (“<strong>error covariance matrix”, </strong>variable name<strong> P</strong>). They do this through two steps: the <strong>predict step</strong>, and the <strong>update step</strong>.</p><p>In the predict step, you predict the future position using a model of your system. In the update step, you incorporate sensor readings to refine this prediction. Thus, you can reason that during the predict step, your uncertainty should go up since you’re simply dead reckoning. In your update step you’re looking at multiple sensors to refine the estimate, so your uncertainty should go down. To actually implement this you use the following matrix equations:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*_-rcxlSIhEH18VNqmNUZFw.png" /><figcaption>The Kalman Filter equations, from Controls Engineering in FRC</figcaption></figure><p>To make a Kalman filter, you start out with a model of your system. We are trying to estimate one thing: our current position, without the Pinpoint’s drift. Thus, the first item of the state is of course the position (x), but to subtract the pinpoint’s drift we have to know what it is. To do this we need to <strong>augment </strong>the filter to add another state that estimates how much the pinpoint has drifted (which I will refer to as xd).</p><p>Although the model is traditionally some sort of physics model, Kalman filters perform well as long as the predict step is something you trust over the short term. In the case of our system we can just use the pinpoint’s estimate. This gives you the following system of equations to run every predict step:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/612/1*hdS8tt7q8sRSDUr9UQaYow.png" /></figure><p>We know that the pinpoint’s drift (xd) isn’t going to change over the short term, so we can just assume that’s constant, and the position estimate can just be updated by adding the latest change in the pinpoint’s estimate.</p><p>To implement this in Java, you would simply just add the change in the Pinpoint to your position estimate. However, for the Kalman filter’s math you need to write this out in the form of an equation x{t+1} = Ax{t} + Bu. Those matrices ultimately end up looking like this:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/620/1*zmwgt51jvkyh99P6Aiwx2A.png" /></figure><p>For the update step, we need a way of getting our sensor measurements from our state. This can be written in a system of equations like so:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/714/1*d95Zd-1pGslfgDNBMVLOvA.png" /></figure><p>Or, when you write it in the Kalman filter y = Cx + Du format, you get the following:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/600/1*xlApPOg4ASU5UxKulREIVg.png" /></figure><h3>Tuning the Filter</h3><p>Now we’ve made a model, but there are two more matrices in the Kalman filter we need: Q (process noise), and R (sensor noise). In the Kalman filter equations (shown above) you can see that Q is added to P every predict step, so it’s essentially capturing the idea reasoned earlier that your uncertainty increases every predict step.</p><p>R is the easiest to fill out, as its just the <strong>variance</strong> (standard deviation squared) of your sensors. It looks like this:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/908/1*Y41fKcCGQGLjLaNdLXqUcQ.png" /></figure><p>And you can just fill it out by measuring the standard deviation of these two sensors while the robot isn’t moving but the flywheel is running.</p><p>Q is the <strong>process noise covariance </strong>matrix, which is what you use to describe the variance of your model. The variance of x is just the variance of the pinpoint (which you’ve already measured for R), but the variance of the bias is more complicated. Essentially, we are modeling the bias as a <strong>random walk</strong>, also known as brownian motion (in continuous time). This is essentially saying that the drift itself isn’t random, but it changes randomly over time in a way such that each change follows a normal distribution. Random walk is visualized nicely in this graph:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/0*nUXoj7kbfgITaHRI.png" /><figcaption>Visualization of random walk from <a href="https://quantgirluk.github.io/Understanding-Quantitative-Finance/brownian_motion.html">https://quantgirluk.github.io/Understanding-Quantitative-Finance/brownian_motion.html</a></figcaption></figure><p>To derive the math around this, let’s say that at each time step, the drift changes by S, for a certain amount of time t. Each value of S follows a normal distribution centered around mean 0 with a certain standard deviation sigma. We can then derive the variance and standard deviation from this using variance addition rules:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*b9Jf1_JtD_fYXCmH7465sA.png" /></figure><p>Therefore, to determine the time-independent standard deviation of the drift, we need to take many samples of the drift (the pinpoint when the robot isn’t moving) at a constant rate. Then, with this data, calculate the differences of two data points and divide by the square root of the change in time. For best accuracy, do this over multiple time periods.</p><p>To do this, reset the pinpoint to zero, and keep the robot stationary with the flywheel running. Then take the average of the pinpoint reading for 1 second, save it to an array, take another average for 1 second, save it to an array, and so on.</p><p>Now, let’s say you have 100 samples that are averages of the drift of the pinpoint over 1 second. You can calculate sigma by subtracting the first and last measurements and dividing by 10 (square root of 100 seconds). You can also calculate it by taking the average of all the differences of 1 second. Or you can take the average difference of samples 4 seconds apart, and divide those by two. Regardless of the time period, you should get roughly the same value, and you can average these out to get your final value for the process noise of the pinpoint drift.</p><p>Then, Q works out to be the following:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/692/1*2w5TN73iW5vaHmE-fweu9g.png" /></figure><p>Where delta t is your loop time, and you’re multiplying the drift random walk by it to calculate the appropriate process noise given your loop time. This can intuitively be reasoned out by thinking about the fact that the pinpoint will drift more over a loop if your loop time is really long. You can also see it works out with dimensional analysis. Here’s the dimensional analysis (where distance is meters):</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/878/1*jXFAUq0GuVgZkLWscBiJnQ.png" /></figure><h3>Implementation notes</h3><h4>Use 1 Kalman Filter per state</h4><p>You are estimating 3 states with the Pinpoint: x, y, and heading. You need to compute this Kalman filter for each state. You most likely need to tune the heading filter separately, but the position standard deviations are most likely the same between x and y.</p><h4>Use Megatag 2</h4><p>This filter makes a few assumptions about the Limelight’s position reading:</p><ol><li>The camera gives you the correct position, just noisy</li></ol><p>This is actually not true for Megatag 1, due to April Tag ambiguity when only one is in view (which is nearly always true in FTC).</p><p>2. The camera’s reading is real-time</p><p>This is also not true for Megatag 1, due to vision processing delays.</p><p>The solution is to use Limelight Megatag 2, which uses the pinpoint’s heading to calculate a non-ambiguous, real-time absolute pose estimate. You can read more about ambiguity and Megatag 2 <a href="https://docs.limelightvision.io/docs/docs-limelight/pipeline-apriltag/apriltag-robot-localization-megatag2">on their documentation</a>.</p><p>If you’ve mounted your limelight on your turret you also need to make sure to transform it based on the turret’s angle and position relative to your robot.</p><h4>Initialization</h4><p>When you reset your odometry to a position (e.g. at the start of auto), you are very confident in your drift and position. However, Kalman filters don’t work correctly if you set P to 0. So make sure to initialize your drift and position uncertainty (the diagonals of P) to your initial setup accuracy, like 1 inch or half an inch.</p><h4>Asynchronous Kalman Filtering</h4><p>You probably are able to read your pinpoint at a much higher frequency than your limelight. Additionally, there isn’t always an April tag in view. Because of this you can’t run the update step every loop, since a lot of the time a camera measurement won’t be available. Luckily, it turns out you can just only run the update step whenever you have a reading available, and the math still works out.</p><p>Therefore, when you implement this filter you would want to run the predict step every loop, and then run the update step whenever a new limelight reading is available and an April tag is in view.</p><p>If you have multiple limelights and multiple April tags in view, you can even run the predict step multiple times with measurements from both cameras for even more accuracy! This is why you see FRC robots covered in limelights, since they take all the measurements from every camera and incorporate them all into the pose estimator Kalman filter.</p><h3>Simplifying the Kalman filter to 1D</h3><p>In reality, the standard deviation of your pinpoint is basically zero, since pinpoint readings aren’t noisy. Because of this you can actually just skip estimating the position, and instead just estimate the drift using the Kalman filter. Since your state is now just 1 dimension (drift) you can use a much simpler version of the Kalman filter, since most of the matrix math cancels out when in 1 dimensions. Here are the equations you ultimately get:</p><p><strong>Predict:</strong></p><figure><img alt="" src="https://cdn-images-1.medium.com/max/698/1*p7TGRaK4RGVEw_1dAFLBWA.png" /></figure><p>(Since the drift doesn’t change at all in the predict step, you only have to update P in your implementation)</p><p>You can really clearly see how the uncertainty increases during the predict step in the 1D version!</p><p><strong>Update:</strong></p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*G3090EABpIKMBIyHvSaOMg.png" /></figure><p>Finally, to get your actual robot position, you just subtract the drift from the pinpoint’s measurement.</p><p>Regarding implementation, the standard deviations/random walk is measured in the same way as before, and you still run it asynchronously and with Megatag 2. You just aren’t incorporating the information about the pinpoint’s noise and correcting for it anymore, which is fine since the pinpoint isn’t noisy anyways.</p><h3>Conclusion</h3><p>In this blog post you hopefully learned about random walk and how to implement an asynchronous Kalman filter to correct the random walk drift in pinpoint using Limelight vision. Good luck in your competitions!</p><img src="https://medium.com/_/stat?event=post.clientViewed&referrerSource=full_rss&postId=911807e0916d" width="1" height="1" alt="">]]></content:encoded>
        </item>
        <item>
            <title><![CDATA[Programming a hooded DECODE shooter]]></title>
            <link>https://medium.com/@vikramaditya.nishant/programming-a-hooded-decode-shooter-463d6a71832f?source=rss-5b587d84fbe0------2</link>
            <guid isPermaLink="false">https://medium.com/p/463d6a71832f</guid>
            <category><![CDATA[decode]]></category>
            <category><![CDATA[control-system]]></category>
            <category><![CDATA[first]]></category>
            <category><![CDATA[ftc]]></category>
            <category><![CDATA[robotics]]></category>
            <dc:creator><![CDATA[Nishant Vikramaditya]]></dc:creator>
            <pubDate>Sun, 05 Oct 2025 08:26:30 GMT</pubDate>
            <atom:updated>2025-10-05T18:54:10.484Z</atom:updated>
            <content:encoded><![CDATA[<p>In FIRST Tech Challenge DECODE, teams must shoot projectiles into goals from various locations on the field. One method of aiming these projectiles is through a hooded flywheel shooter:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*a-_g0kunFUliuTc9A7n5Jg.png" /><figcaption>A hooded shooter design from FTC Team #5189 (not mine)</figcaption></figure><p>Like traditional flywheel shooters, these rely on a quickly-spinning flywheel to shoot up a ball. However, unlike fixed-angle shooters, they can use a hood to adjust the angle of the ball.</p><h3>Why use a hooded shooter?</h3><p>These are very difficult to model analytically, which is why I didn’t discuss them in my <a href="https://medium.com/@vikramaditya.nishant/programming-a-decode-shooter-4ab114dac01f">previous blog post</a>. However, despite being more complex mechanically, they have the advantage that they allow teams to use the optimal angle for shooting, regardless of launch distance:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*XiTdkIfUQARgiFzRFufHSQ.png" /></figure><p>As you can see, the hooded shooter can shoot the projectile at a much more shallow angle when far away from the goal. This limits airtime which reduces the effects of aerodynamics and other sources of error on the ball, while still keeping the same range as a fixed-angle shooter. This should hopefully lead to more accurate, consistent shots, and also make things like shooting while moving easier. Considering these benefits, lets discuss how to program a hooded shooter!</p><h3>Flywheel Velocity PID</h3><p>The first step to implementing a hooded shooter is to control the velocity of the flywheel accurately. Unlike a fixed-angle shooter, a hooded shooter simply needs to maintain a constant velocity. Thus, it may be tempting to just set the power to 1.0 and let it spin at full speed. However, this has a critical problem: the speed will change based on the voltage of the battery, which in turn changes based on things like the power draw of your drivetrain. This will result in inconsistent shooting. Instead, you should use a <strong>velocity PID controller</strong> to accurately reach a speed that you know your flywheel can achieve, even at low battery voltages.</p><p>To determine a target speed, apply something like 10V to the motor (hopefully, your voltage doesn’t drop below this). You can do this by applying a power thats proportional to your battery voltage. For example, if your battery voltage is 13.5V, you would apply 10/13.5 = a power of ~0.75 to your motor. Then, measure the velocity with your encoder and save this as your target velocity.</p><p>As with pretty much every control system, you use PID to do the control. In this case, you use an <strong>integrator </strong>which is a PID controller where P and D are zero. That way, any error in the flywheel’s velocity leads to an increase in the motor’s voltage. This is because in velocity control you need to constantly apply a power to the motor to keep it spinning (unlike slides or an arm, where 0 power makes them stay the same), so the integrator needs to add up the velocity error.</p><p>You can also add a little bit of P to improve the controller’s <strong>transient response </strong>— how quickly the flywheel speeds up and slows down when its commanded to spin up or a disturbance occurs (e.g. a ball is launched). Once kI is tuned well I would slowly increase kP and test via a step response (basically tell it to go from 0 to 50% speed and vice versa) and plot the velocity on something like FTC dashboard. For example, here’s a step response of a (pretty terribly tuned) velocity controller I made a while back:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/0*yfSlda3iHAPiddcN.png" /><figcaption>I believe my kI term was way too large and my kP term was slightly too small when I made this</figcaption></figure><p>You also want to design your flywheel to have a somewhat high moment of inertia so that disturbances like the ball going through the flywheel affect the speed less and spin-up is smoother.</p><h3>Aiming at the goal (localization)</h3><blockquote>This section is copied from my <a href="https://medium.com/@vikramaditya.nishant/programming-a-decode-shooter-4ab114dac01f">previous blog post</a></blockquote><p>The next programming challenge is aiming at the goal. To do this you need to know your position on the field — a problem called <strong>localization</strong>. I’ve found that using dead-wheel odometry with something like GoBilda Pinpoint works pretty well in the short term (maybe 30 seconds? do some testing and find out! the judges will love it (: ) but suffers from something called <strong>drift </strong>in the long term. Essentially, tiny errors in the localization add up over time causing inaccuracies after a lot of driving. To address this, you can use the <strong>April tags </strong>on the field — the weird things that look like QR codes:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/0*qF46uMBBh6HIwkhR.png" /></figure><p>These are called <strong>fiducials</strong>, and computer vision code can be used to calculate the robot’s position by basically looking at how the corners are squished and twisted on the tag from the robot’s perspective.</p><p>There are many ways to determine your position from an April tag, including using the builtin SDK April tag pose estimator with a calibrated webcam or using a Limelight. I haven’t actually compared these so try them out and see which works better! You can simply overwrite your pose with the pose calculated from the April tag, a process called <strong>re-localization.</strong></p><p>For maximum reliability I would recommend manually re-localizing (requiring the driver to press a button) because things like motion blur and camera delay can make automatically re-localizing error prone. Probably implement an algorithm to verify the new pose is correct too, maybe averaging out multiple values or comparing it to the old pose and ensuring they are roughly similar.</p><p>Once you know your position on the field you just need basic trigonometry to determine how to aim your turret or robot. You can use traditional PID, either with the drivetrain’s heading or with your turret to achieve this.</p><h3>Aiming the Hood</h3><p>Unfortunately, it is extremely difficult to derive an equation for how to aim the hood since there are so many complex phenomenon going on. Instead, we can use a more experimental approach!</p><p>In this experiment, the <strong>independent variable</strong> is the distance from the target, which is measured by the localizer. The <strong>dependent variable</strong> is the angle of the hood, which you need to find!</p><p>To figure out the angle of the hood you can position the robot at a series of known distances from the goal. Then, run the flywheel velocity PID and shoot while tuning the angle of the hood until the ball consistently goes in. This means that you would want to do several shots at each distance and ensure that they all go in. You can also do things like bouncing off the wall for more consistency. Once you’ve picked an angle for each distance, do two things:</p><ol><li>Write down the hood angle and distance (most likely the hood servo position)</li><li><strong>If you want to shoot while moving</strong>:<br>- You need to have something like an LED on your robot to indicate when the command to feed the ball into your shooter was issued<br>- Record a slow-mo video of the shot, and make sure to label the video with the shot distance</li></ol><p>Make sure to test a lot of distances — at least five, but the more, the merrier! Additionally, make sure to only test distances that encompass the launch zones. Don’t waste time preparing for shots you don’t need to make :)</p><p>Once you have data for hood angle vs shot distance, you can either fit a regression or use an <a href="https://docs.ftclib.org/ftclib/features/util#interplut-interpolated-look-up-table">InterpLUT</a>. To create a <strong>regression</strong>, you plot hood angle vs shot distance and use a tool like Excel or Desmos to fit a mathematical equation:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/0*_E5sbF6zOIt6KOx5.png" /><figcaption>A polynomial regression for our Into The Deep vision code</figcaption></figure><p>Then, you can just copy this equation into your code and use it to calculate your hood angle.</p><p>The other option is to use an <strong>interpolated look-up table </strong>(you can find a good implementation on <a href="https://docs.ftclib.org/ftclib/features/util#interplut-interpolated-look-up-table">FTCLib</a>). With this one you can just enter your data into your code, and it will linearly interpolate between each data point to calculate the hood angle. However, even if you are using an InterpLUT I would recommend at least plotting the data once, because these graphs help you gain a better understanding of your robot and also help you find any anomalies! (Pro Tip: put this in your engineering portfolio)</p><h3>Combining it all together</h3><p>Now that you understand each component of a hooded shooter, here’s how the process to tune one would work:</p><ol><li>Tune the flywheel velocity PID, using a tool like FTC Dashboard</li><li>Record data for hood position vs shot distance</li><li>Fit a regression or use an InterpLUT</li></ol><p>To actually shoot, here’s how the process would work in the control loop:</p><ol><li>Update localization (e.g. query pinpoint, re-localize with AprilTags)</li><li>Calculate the hood angle using the InterpLUT/regression, and move the servo to this position</li><li>Spin the flywheel using the velocity controller</li><li>Aim the turret/drivetrain using PID</li><li>If the robot is aimed (e.g. turret/drivetrain error is &lt;5deg) and the flywheel is at an appropriate speed (e.g. &lt;50rpm of error), feed the ball into the shooter!</li></ol><p>This method accounts for the time the robot takes to line up, and it also allows you to shoot as fast as possible while still maintaining accuracy, since it waits for the flywheel to spin back up before shooting another projectile.</p><h3>Side Quest: Turret Hysteresis Control</h3><p>To shoot while moving, or just to improve the performance of your shooter in general, a good method is to use a turret. However, these have a problem: range discontinuities. Since they can’t turn an infinite amount, at some point you need to flip between one side of the range and the other side of the range (unless you use a slip ring).</p><p>With a turret that has a range of +/-180 degrees, for every desired turret angle, there is one way to achieve this. This seems fine, but when you are driving backwards, your turret needs to be either at +180 or -180 degrees. If your robot hovers around this angle (due to noise or other factors), you will find your turret trying to rapidly go back and forth between the two, which is very undesirable.</p><p>To solve this you can use hysteresis, which essentially involves adding some buffer around this range. For example, if you had a turret with a range of +/- 200 degrees, you would now have two ways of achieving angles towards the end of the range. The logic is simply to pick the angle closest to the turret’s current angle, and only flip around if the desired angle is out of the range.</p><p>With this hysteresis control, let’s imagine your turret is at 180 degrees. Even if your robot varies by +/- 15 degrees around this, it will still be under 200 degrees, so the turret will not have to flip all the way around. If you vary +/- 15 degrees around a desired angle of 200 degrees, your turret will flip around to -160. Then, it will vary from -175 to -145 which also does not require the turret to flip. Thus, by adding 20 degrees of range to the turret you can eliminate the flipping issue.</p><h3>Shooting while moving</h3><p>To shoot while moving you need to be able to aim at the goal while driving — something only possible with turreted shooters or swerve drives, since Mecanum suffers from different driving speeds dependent on the direction you are driving. However, if you do have a turret or swerve system available, you can implement shooting while moving for two major benefits:</p><ul><li>You can avoid defense much more easily, since it is very difficult to defend against a moving robot</li><li>You can cycle faster, since you don’t have to stop in between cycles</li></ul><p>The core principle of shooting while moving is to basically imagine the goal has moved opposite to the direction you are moving. Specifically, you need to take the velocity of your robot (pinpoint tells you this) and multiply it by the shot time, and subtract that from the goal’s position when calculating the hood and turret/drivetrain angles.</p><p>To determine shot time, you can use (you guessed it) another InterpLUT/regression! This time, however, the independent variable is the hood angle, and the dependent variable is the shot time.</p><p>The shot time includes <strong>both the time for the ball to get fed into the shooter and the airtime</strong>. To fit this regression/InterpLUT you can use your slow-mo videos from before, counting the time in between the LED that indicates a shot has started and the ball reaching the goal.</p><p>This has a problem: you calculate your shot time based on your hood angle, which depends on your distance to the goal. However, your distance to the goal depends on your shot time (since you add that to the goal position), which depends on your hood angle! Therefore, the hood angle calculations depend on themselves! I <em>think</em> it should be fine to just iteratively converge on the right hood/turret angle with a few iterations of a loop like so:</p><ol><li>Define a hood angle variable outside of the loop</li><li>IN THE LOOP (probably do 5–10 iterations, but try running some tests to see how many iterations it takes to converge):</li><li>Calculate the shot time using the hood angle variable</li><li>Calculate the new position of the goal using <strong>-robot velocity * shot time + original goal position</strong></li><li>Calculate the hood angle and turret position to aim towards this new goal position, and save them to the variables outside of the loop</li><li>Repeat!</li></ol><p>If you are using a regression, it’s probably possible to do this analytically as well (but I’m too lazy to derive it).</p><p>Now, if you’re driving towards the goal, the robot will account for the fact that the ball will be launched at a faster speed towards the goal than if the robot was stationary, and thus will aim higher to account for it!</p><blockquote>⚠️ <strong>WARNING</strong>: This shooting while moving method assumes that your robot’s velocity is constant in the time it takes for the ball to be go through the shooter. Make sure that your feeding mechanism is fast enough for this to be true!</blockquote><p>You can now replace the hood and turret angle calculations from the previous control loop with your updated velocity-compensated versions, and you should be able to shoot while moving!</p><h4>Implementation considerations</h4><p>One reason this is significantly more difficult to implement is that your setpoints for the turret and hood angle are constantly changing — unlike with the stationary version, you aren’t guaranteed to achieve some level of low error after letting the turret/drivetrain PID run for a while, since it is constantly trying to follow a changing setpoint. To address this there are a few things I would do:</p><ol><li>Tune your PIDs well! I would spend a lot of time tuning the flywheel and turret/swerve PIDs to ensure that they have great transient response, so that you can shoot multiple shots in quick succession and handle their changing setpoints.</li><li>Figure out the maximum error range that your turret can have before missing the goal. This should prevent your robot from trying to achieve an impossible level of turret accuracy while driving.</li><li>Make sure your hood is faster than your turret! Most hoods are driven by servos that don’t have encoders, so you can’t actually ensure that the hood has reached the setpoint before shooting. However, if your hood moves faster than your turret then you don’t need to worry about your hood being too slow since your turret accuracy criteria will ensure the robot waits until the hood has reached the right spot.</li><li>Slow down the maximum drivetrain speed when trying to shoot. This will ensure the setpoint isn’t changing too fast!</li></ol><h3>Questions?</h3><p>If you have any questions feel free to DM me (nv7_) on Discord! I’m super excited to see what people develop for this game and I especially would love to see a robot that can shoot while moving. Good luck!</p><img src="https://medium.com/_/stat?event=post.clientViewed&referrerSource=full_rss&postId=463d6a71832f" width="1" height="1" alt="">]]></content:encoded>
        </item>
        <item>
            <title><![CDATA[Programming a DECODE shooter]]></title>
            <link>https://medium.com/@vikramaditya.nishant/programming-a-decode-shooter-4ab114dac01f?source=rss-5b587d84fbe0------2</link>
            <guid isPermaLink="false">https://medium.com/p/4ab114dac01f</guid>
            <category><![CDATA[first]]></category>
            <category><![CDATA[control-system]]></category>
            <category><![CDATA[ftc]]></category>
            <category><![CDATA[physics]]></category>
            <category><![CDATA[decode]]></category>
            <dc:creator><![CDATA[Nishant Vikramaditya]]></dc:creator>
            <pubDate>Sun, 07 Sep 2025 00:57:06 GMT</pubDate>
            <atom:updated>2025-09-07T00:57:06.986Z</atom:updated>
            <content:encoded><![CDATA[<p>For the first time since Ultimate Goal, FIRST Tech Challenge has a game in which you can shoot projectiles! Even more interestingly, the projectiles are balls, meaning that they behave like ideal physics projectiles, enabling us to use simple equations to aim our shooters.</p><p>This blog post covers the math and process behind calibrating and controlling a shooter from the ground up, and even explains how to shoot while moving!</p><blockquote><strong>Prerequisite knowledge</strong>: Basic algebra-based physics (e.g. AP Physics 1), basic trig, PID control</blockquote><h3>Designing the Shooter</h3><p>I think the simplest competitive shooter is a fixed-angle flywheel shooter. If we look at a side view of the robot and the field, it would look something like this:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*FDm8lPzQwgx9aWo-Znb9HA.png" /></figure><p><strong>Fixed-angle </strong>means that the angle the ball is launched at (theta) is fixed, so you don’t need some sort of tilting mechanism.</p><p>We can define variables like the heights of the various field items and the launch angle (theta) of the projectile. There is one more important variable: <strong>initial velocity (v0)</strong>.</p><p>To launch the ball you simply need to push it into a mechanism with a wheel spinning very quickly, called a <strong>flywheel</strong>. This flywheel needs to be very grippy and have a high <strong>moment of inertia</strong>, meaning you want to add a lot of weight to the rim of the wheel. This will make controlling it with a motor much easier.</p><p>To change the trajectory of the ball you just need to change the launch velocity by changing the speed of the flywheel, which doesn’t require a shooting mechanism that can tilt.</p><p>If the shooting mechanism can’t tilt, then you need to determine two things when designing your robot: the height of the shooting point (hbot) and the angle it shoots at (theta). The height doesn’t really affect your performance or strategy, so I picked 0.4 meters (~15.75 inches) which is probably close to what most teams will use. The launch angle, however, is important to calculate.</p><h4>Determining the launch angle</h4><p>One way to determine the launch angle is to pick the smallest distance from the goal that you want to shoot from. As you can imagine, if you want to be able to shoot while really close to the goal, you need to shoot nearly straight up. To shoot from far away you would want something closer to 45 degrees. I chose a minimum distance of 0.86 meters — the length of the diagonal of a field tile.</p><p>To achieve the smallest launch angle, we want the ball to just barely go above the edge of the goal. This looks like this:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*Zh7CPj2Lx7b47rJaaStrYg.png" /><figcaption>The path of the ball, with a parabola drawn underneath</figcaption></figure><p>Therefore, to calculate theta you can simply use the equation for the maximum height of a projectile and derive the distance it has travelled in that time, since the maximum height of the ball must be directly above the edge of the goal.</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*ofDYGk_hF4s8WrgPt5fNdA.png" /></figure><p>We know that ymax is equal to the height the ball has to rise — the height of the goal minus the height of the robot — and xmax is equal to the minimum distance we want to shoot for. So, you can get rid of v0 with some simple algebra and solve in something like Desmos:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*Oc1lGZpcQCibhtNnOV2etA.png" /></figure><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*_xG5Xz3SmYXMB9FvJQ5RXg.png" /><figcaption>To solve I set ymax equal to the height of the goal + a 5&quot; margin (to leave 2.5&quot; under the ball) minus the height of the robot, and xmax equal to xmin in the diagram (terrible naming)</figcaption></figure><blockquote>You can find this Desmos graph <a href="https://www.desmos.com/calculator/qbbthw1bh1">here</a>.</blockquote><p>As you can see, at a launch angle of 58.61 degrees the closest we can launch from the goal is 0.86 meters. Using theta and the equations above you can also solve for v0 and see that you need a 4.36m/s (9.75mph) launch velocity to achieve this. Therefore, I think 60 degrees is a pretty good launch angle, enabling you to shoot from 32 inches away from the goal.</p><p>Make sure to go through this process for your robot! This can definitely enhance an engineering portfolio and is a nice application of physics to the real world.</p><h3>Calculating shooter trajectory</h3><p>Let’s assume our turret or robot is pointed towards the target and the robot is not moving for now. We can once again simplify this problem to a 2–dimensional view along the path of the ball. The shooting would look like this:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*dQZ-IVl7O26zaktCrFbxyA.png" /></figure><p>This can once again be solved with the equations of motion. We know these equations for the height of the ball as a function of time:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*pvnopyi5Ln_u9igZU09zLQ.png" /></figure><p>First, we can solve for the time when the height of the ball is equal to the height of the goal, using the quadratic formula:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*mNEuKW6GZdHzqhiigonMUA.png" /></figure><p>We want the <strong>higher value of t</strong>, since as you can see in the diagram of the situation we want the ball to go past the peak of its flight and then fall down into the goal. That means that it has reached the height of the goal once (the earlier value of t), gone above the goal, and then turned around and gone down into the goal (the later value of t).</p><p>In this equation there are two unknowns: v0 (which we want to solve for) and t. To eliminate t we can use the equation for the horizontal position:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*EPkb85j5GarV680MckSA0A.png" /></figure><p>Now, we have an equation with one unknown: v0, which is what we want to solve for! When you rearrange to solve for v0 you get this final equation:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*6MN3wFrFLjzTRa9kSTbX6A.png" /></figure><p>We can plot this to see how the shooter performs at various distances (where the y-axis is the time it takes the ball to fly, and the x-axis is the launch velocity). First, a quick sanity check. If we plug in our minimum distance of 0.86m we can see that it is in fact the minimum distance that works for this equation:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*zGH7RasVG7H7mK4AAlxH9Q.png" /><figcaption>x is slightly higher than 0.86 due to rounding errors. x-axis represents launch velocity, y-axis represents time the ball is flying</figcaption></figure><blockquote>You can find this Desmos graph <a href="https://www.desmos.com/calculator/n9syawsgk2">here</a>.</blockquote><p>Additionally, the equation tells us to use a launch velocity of 4.36m/s which is what we calculated needed to be the launch velocity in the shooter angle calculations. Additionally, the peak height that the projectile reaches in this situation is 1.11m which is equal to the height of the goal with 5&quot; of headroom — exactly our parameters when designing the shooter!</p><p>Now, lets test it at various points on the field. There are two launch zones that we can launch from:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*yfSPKeMc8pjuIIqFbbcV7w.png" /></figure><p>The furthest edge of the close launch zone is approximately 2.5 meters away. Using the calculator we can see that this corresponds to a launch velocity of 5.8m/s (12.9mph) and a max projectile height of 1.6m (5.4ft) which I think are reasonable.</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*1nKpzxLY3WGB_g8fHWY6zQ.png" /></figure><p>The far launch zone is roughly 3.9m away from the goal. Using the calculator we can see that we need a launch velocity of 6.9m/s (15.5mph) and a max projectile height of 7.14ft:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*RgvPJQVN4axNhxLAb2S-xA.png" /></figure><p>These numbers also seem pretty reasonable, considering that most ceilings are 8 feet high. Altogether this means a launcher at a <strong>58 degree angle </strong>(60 is also fine) that can shoot in the range of <strong>9–16mph </strong>should work well for any launch zone, requiring only a simple fixed angle flywheel launcher. Let’s discuss how to implement this:</p><h3>Implementing projectile velocity control</h3><p>There are a few aspects to programming, the most interesting of which is actually controlling the velocity of the ball. The basic idea of doing this is to fit a <strong>regression</strong>, which in this case would be a mathematical model of how the flywheel speed affects the exit velocity.</p><p>To do this, you first need a way of measuring the exit velocity of the ball. One way is with tools that can use two sensors and measure the time it takes for the projectile to get between the two:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*Cp00u4bvsdDR4eQPiMgNgg.png" /><figcaption>I have no idea if this would actually work for DECODE, but its maybe worth a try?</figcaption></figure><p>You could also potentially try implementing your own with beam break sensors, but you would need some very precise way of measuring the tiny amount of time it takes the ball to get between the two.</p><p>Another method is to launch the ball in an open area and measure how far it lands, and then use the equations of motion to derive its exit velocity from the distance it travelled. This seems much more feasible but also more error prone, so let me know if you have any better ways of measuring this! If I was making a DECODE robot I would probably use this method, since its so much easier.</p><h4>Fitting the regression</h4><p>The core idea of this is that you try a bunch of flywheel velocities (use an <strong>encoder</strong> to measure the velocity and a controller as explained below) and plot the resulting ball velocity. Then, you can use a tool like Excel to fit an equation (<strong>regression</strong>) that models this.</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*GE_HKX7wVik1fKCvoQ9J7Q.png" /><figcaption>A regression from Into the Deep for our vision code</figcaption></figure><p>More values are better! Make sure that the range of velocities you achieve has the entire range that you could possibly desire, in my case 9–16mph.</p><p>If you flip the columns around and instead <strong>make a regression of rpm vs velocity </strong>you get an equation that tells you the flywheel angular velocity that you need to target to achieve a certain ball velocity! You can simply put this equation into your code and use it with the equation for target ball velocity determined above to calculate the target flywheel speed.</p><h4>Controlling flywheel velocity</h4><p>As with pretty much every control system, you use PID to do the control. In this case, you use an <strong>integrator </strong>which is a PID controller where P and D are zero. That way, any error in the flywheel’s velocity leads to an increase in the motor’s voltage. This is because in velocity control you need to constantly apply a power to the motor to keep it spinning (unlike slides or an arm, where 0 power makes them stay the same), so the integrator needs to add up the velocity error.</p><p>You can also add a little bit of P to improve the controller’s <strong>transient response </strong>— how quickly the flywheel speeds up and slows down when its commanded to spin up or a disturbance occurs (e.g. a ball is launched). Once kI is tuned well I would slowly increase kP and test via a step response (basically tell it to go from 0 to 50% speed and vice versa) and plot the velocity on something like FTC dashboard. For example, here’s a step response of a (pretty terribly tuned) velocity controller I made a while back:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*2ZY9g9v9DCr5TKRmzCESQw.png" /><figcaption>I believe my kI term was way too large and my kP term was slightly too small when I made this</figcaption></figure><p>You want your flywheel to have a somewhat high moment of inertia so that disturbances like the ball going through the flywheel affect the speed less and spin-up is smoother.</p><h3>Aiming at the goal</h3><p>The next programming challenge is aiming at the goal. To do this you need to know your position on the field — a problem called <strong>localization</strong>. I’ve found that using dead-wheel odometry with something like GoBilda Pinpoint works pretty well in the short term (maybe 30 seconds? do some testing and find out!) but suffers from something called <strong>drift </strong>in the long term. Essentially, tiny errors in the localization add up over time causing inaccuracies after a lot of driving. To address this, you can use the <strong>April tags </strong>on the field — the weird things that look like QR codes:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*xtK1szMeF9QyVxWAr7DIBQ.png" /></figure><p>These are called <strong>fiducials</strong>, and computer vision code can be used to calculate the robot’s position by basically looking at how the corners are squished and twisted on the tag from the robot’s perspective.</p><p>There are many ways to determine your position from an April tag, including using the builtin SDK April tag pose estimator with a calibrated webcam or using a Limelight. I haven’t actually compared these so try them out and see which works better! You can simply overwrite your pose with the pose calculated from the April tag, a process called <strong>re-localization.</strong></p><p>I would recommend manually re-localizing (requiring the driver to press a button) because things like motion blur and camera delay can make automatically re-localizing error prone. Probably implement an algorithm to verify the new pose is correct too, maybe averaging out multiple values or comparing it to the old pose and ensuring they are roughly similar.</p><p>Once you know your position on the field you just need basic trigonometry to determine how to aim your turret or robot. You can use traditional PID, either with the drivetrain’s heading or with your turret to achieve this.</p><h3>Combining it all together</h3><p>Now that you’ve developed all the components necessary to auto-aim and score let’s combine it all together. In a single loop of your robot’s auto-aiming code you would want to:</p><ol><li>Update localization (e.g. query pinpoint, re-localize with AprilTags)</li><li>Calculate the necessary velocity using the v0 equation</li><li>Calculate the flywheel speed needed with the regression</li><li>Spin the flywheel using the velocity controller</li><li>Aim the turret or drivetrain with a heading controller</li><li>If the robot is aimed (e.g. error is &lt;5deg) and the flywheel is spinning at the right speed (e.g. &lt;50rpm of error), then feed a ball into the shooting mechanism and score!</li></ol><p>You could incorporate this into the driver controls as a button so that, when you press the button, the auto-aim takes over the driving and automatically lines up the robot (or aims the turret), spins up the flywheel, and then scores.</p><h3>Scoring while moving</h3><p>Up until now we have assumed the robot is stationary, which is probably a valid assumption early in the season — you can afford to drive to the launch zone, press a button, and allow the robot to align itself with the goal, spin up the flywheel, and shoot. However, this is very easy to defend against — the opposing alliance could just push you and you either wouldn’t meet the conditions to score or potentially miss. You could try implementing something like position PID to hold yourself in place, but this still wastes time and can cause issues if someone collides with your robot at a high speed while you are trying to shoot.</p><p>I think the most guaranteed way to avoid this being a problem is to drive around while scoring, which would force the opposing alliance to chase you (very difficult). This would also enable you to score while driving to intake more game elements, which can speed up cycle times and make you score more.</p><h4>Hardware requirements</h4><p>This is only possible if you have a swerve drivetrain (which allows you to drive independently of the robot’s heading) or a turret, which also would allow you to control the aiming independently of the robot’s drivetrain heading. While a Mecanum drive can technically aim independently of the direction it’s driving (field-centric driving) it doesn’t work very well because a misaligned center of mass can cause it to turn while strafing, there’s often a lot of shaking while strafing, and it’s more prone to slip.</p><h4>How it works</h4><p>Essentially, to score while moving you make the robot imagine that the goal has moved opposite to the robot’s movement. For example, if the robot is quickly moving toward the goal, you would imagine the goal is even closer than it is at the moment, since when you shoot a ball it would be moving faster towards the goal than if you just shot it while stationary.</p><p>This requires you to know the velocity of the robot in the field (pinpoint tells you) and the amount of time the ball will fly for. Then you can simply multiply the velocity by the amount of time the ball will be flying and add this to the goal’s position in your calculation for the turret angle and the shooting distance.</p><h4>Determining ball airtime</h4><p>If you remember back to the v0 equation, you’ll realize that one of the intermediate steps solved for time. Specifically, this one:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/194/1*Gpe5KgzBh40TCRLoPGcJDg.png" /></figure><p>However, you will realize that it depends on the distance to the goal, which depends on the position of the goal, which depends on the time the ball will fly for! This ends up being a circular requirement. Even worse, this also depends on v0 which is also calculated from the distance to the goal.</p><p>I haven’t tested this, but I believe you can just use a few iterations of a loop to converge on roughly the right values of v0 and t. It would look something like this:</p><ol><li>Reset the a variable representing the position of the goal to its regular position</li><li>Repeat 3 times:</li><li>Calculate the distance to the goal using the current value of the variable</li><li>Calculate v0 using the distance to goal</li><li>Calculate time using this distance and v0</li><li>Update the variable for position of the goal using this time value</li></ol><p>Although nested loops inside of the control loop are generally bad, this one isn’t a problem since it just repeats 3 times (or a larger, fixed amount) and it only does basic math, not I/O.</p><p>You can then, while driving, aim the turret (or swerve drive) and update the flywheel target velocity based on this velocity-compensated goal position.</p><h4>Implementation considerations</h4><p>One reason this is significantly more difficult to implement is that your PID controller setpoints for the turret and flywheel are constantly changing — unlike with the stationary version, you aren’t guaranteed to achieve some level of low error after letting the PIDs run for a while, since they are constantly trying to follow a changing setpoint. To address this there are a few things I would do:</p><ol><li>Tune your PIDs well! I would spend a lot of time tuning the flywheel and turret/swerve PIDs to ensure that they have great transient response.</li><li>Implement more advanced shooting criteria — instead of just waiting for turret and flywheel error to be below some hardcoded amount, actually calculate what this amount should be. <br>For example, using the knowledge of the front and back edge of the goal, you can calculate the minimum and maximum velocity bounds with the v0 equation. Using the width of the goal, you can calculate the maximum error for the turret. Since you are calculating these dynamically they can be much larger than a hardcoded error range.</li><li>Slow down the maximum drivetrain speed when trying to shoot. This will ensure the setpoint isn’t changing too fast!</li></ol><h3>Questions?</h3><p>If you have any questions feel free to DM me (nv7_) on Discord! I’m super excited to see what people develop for this game and I especially would love to see a robot that can shoot while moving. Good luck!</p><img src="https://medium.com/_/stat?event=post.clientViewed&referrerSource=full_rss&postId=4ab114dac01f" width="1" height="1" alt="">]]></content:encoded>
        </item>
        <item>
            <title><![CDATA[Using Field-Oriented Control to make the perfect robot drivetrain]]></title>
            <link>https://medium.com/@vikramaditya.nishant/using-field-oriented-control-to-make-the-perfect-robot-drivetrain-d57a45fb14ee?source=rss-5b587d84fbe0------2</link>
            <guid isPermaLink="false">https://medium.com/p/d57a45fb14ee</guid>
            <category><![CDATA[autonomous-vehicles]]></category>
            <category><![CDATA[control-system]]></category>
            <category><![CDATA[robotics]]></category>
            <dc:creator><![CDATA[Nishant Vikramaditya]]></dc:creator>
            <pubDate>Fri, 22 Aug 2025 00:23:21 GMT</pubDate>
            <atom:updated>2025-10-28T20:45:05.471Z</atom:updated>
            <content:encoded><![CDATA[<blockquote><strong>Disclaimer</strong>: Field-oriented control (FOC) is a control technique for <strong>brushless </strong>motors, which I explained and implemented in my <a href="https://medium.com/@vikramaditya.nishant/how-to-make-a-field-oriented-control-esc-from-scratch-948a5d6547b9">previous blog post</a>. This blog post simply applies one component of field-oriented control, the current control, to <strong>brushed</strong> motors. However, you can use FOC to create this drivetrain, and with higher efficiency! This is something that I would like to pursue in the future, but I haven’t done it yet due to cost and time constraints.</blockquote><p><strong>Prerequisite knowledge: </strong>PID control, brushed motor voltage/current relationships, back-emf, PWM, trigonometry, ohm’s law</p><p>A <strong>skid-steer drivetrain </strong>(also known as a differential drive or tank drive) is the simplest drivetrain you can create to move in a 2-dimensional field. A typical setup looks like this:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/554/1*Xk-iig--cBGYHTiCtW7n3A.png" /><figcaption>Differential drive diagram from MATLAB</figcaption></figure><p>It simply utilizes two motors to create forward-backward (linear) motion and turning (angular) motion. When both wheels are moving with the same velocity, the drivetrain moves forwards. When one wheel is faster than the other it arcs. If both are moving the same speed but in opposite directions, the drivetrain turns in place.</p><p>To control this drivetrain (and any other system) there are two methods: <strong>open-loop </strong>and <strong>closed-loop </strong>control. In open-loop control, you simply move based on pre-determined commands. For example, “apply the same voltage to both motors for 10 seconds to move forward 10 centimeters” and “apply opposing voltages to both motors for 1 second to turn 90 degrees”. This is a like walking with your eyes closed; it generally doesn’t work well unless you have an extremely accurate model of your robot and environment.</p><p>In closed-loop control, you use <strong>feedback </strong>from sensors to control the robot. The simplest setup needed to have sensor feedback is to add <strong>encoders </strong>to each wheel, which measure the rotation of each wheel.</p><h3>Localization — How to know where you are</h3><p>Differential drivetrains have three main variables that change over time (known as the <strong>state</strong>):</p><ul><li>Position (x, y)</li><li>Heading (theta)</li></ul><p>Additionally, they have a <strong>track width</strong> (b) which is the distance between the center of the two wheels.</p><p>These can be used with the following equations to calculate the position of the robot in a loop:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/854/1*xM5MAtTwWCP9_k1Nv0puVQ.png" /></figure><p>Essentially, in an iteration of the loop at time <em>t </em>you can use the change in wheel positions (phi) and robot parameters to determine the position of the robot in the next iteration. If you run this algorithm at a very high frequency you will find that it yields decent results. However, a very simple method to greatly improve accuracy is to integrate a bias-calibrated gyroscope to calculate theta instead of using the encoders. By running this encoder loop at ~2kHz I got very good accuracy.</p><h3>The problem with simple closed-loop control</h3><p>Now, to control this drivetrain you can just calculate the heading you need to reach a target point (xref, yref) with the atan2f function. Then, use a heading PD controller to achieve this target heading — it will output a value “w”, representing target angular velocity. Add this to one wheel’s PWM and subtract it from the other wheel’s PWM. Add a fixed linear velocity to both wheels’ PWM to go forwards. When you get close to the target point (within your target turn radius), move on to the next point. This gives you a simple closed-loop path follower!</p><p>That gives you something like this:</p><iframe src="https://cdn.embedly.com/widgets/media.html?src=https%3A%2F%2Fwww.youtube.com%2Fembed%2Fh0ABW57l-FI%3Ffeature%3Doembed&amp;display_name=YouTube&amp;url=https%3A%2F%2Fwww.youtube.com%2Fshorts%2Fh0ABW57l-FI%3Fsi%3Dp20V6pCnFk6rwDTC&amp;image=https%3A%2F%2Fi.ytimg.com%2Fvi%2Fh0ABW57l-FI%2Fhq2.jpg&amp;type=text%2Fhtml&amp;schema=youtube" width="640" height="480" frameborder="0" scrolling="no"><a href="https://medium.com/media/b314e60c68ad7a86499be7a1b25d0297/href">https://medium.com/media/b314e60c68ad7a86499be7a1b25d0297/href</a></iframe><p>This looks fine, but if you look at the wheels while turning you’ll notice that one pretty much just stops instantly at the start of the turn. This is very bad — that means its slipping! This becomes even more apparent at higher speeds, but this vehicle didn’t get very good accuracy because of this. The localization relies on accurate encoder measurements, so if the wheel is slipping the encoder measurements are going to be inaccurate and often in very inconsistent ways since slipping is a very unpredictable phenomenon.</p><p>One way to get around this is to use <strong>dead wheels</strong>, where you have an unpowered wheel spring-loaded against the ground with an encoder. This solves the accuracy issue, but slipping wheels have the other issue that they have less torque than wheels that aren’t slipping. Thus, you still won’t get optimal drivetrain performance if slipping, although this is an easy way to mitigate the slip issue.</p><h3>The solution: current control</h3><p>Basic physics tells us that if a wheel exceeds a certain amount of torque, it slips. So, if we are able to limit the torque to each wheel we won’t have any slipping problems!</p><p>One of the most fundamental facts about DC motors is that the current flowing in the motor is proportional to the torque it is outputting. Thus, by measuring and controlling the current flowing in the motor, we can control and therefore limit the torque on each wheel!</p><p>However, unlike position or velocity which change relatively slowly (you can see these changing), current can change in time periods on the order of microseconds. Thus, you need a very high controller frequency — on the order of kHz — to control this. Additionally, tuning it by hand is extremely difficult. However, since a DC motor can be modeled as resistor and inductor, we can use control theory to determine the PID coefficients! To learn more about how this is done, read the “<strong>Where do the PI coefficients come from?</strong>” of my <a href="https://medium.com/@vikramaditya.nishant/how-to-make-a-field-oriented-control-esc-from-scratch-948a5d6547b9#4c25">FOC blog post</a>.</p><p>It turns out that using a PI controller (PID without the D) with the following coefficients gives us a (sort of) critically damped controller for the motor:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*z06Zb_vm4uK4xm1OwVJtkA.png" /></figure><p><strong>Bandwidth </strong>is the frequency that the controller can react to disturbances. Essentially, if you apply a sine wave to the target current with the frequency of the bandwidth, then it will track it reasonably well. However, past this frequency it won’t.</p><p>Your PID controller frequency should be <strong>10–20x </strong>faster than the bandwidth. For example, since my controller runs at a loop frequency of 2kHz, I chose a bandwidth of 120 Hz. If your bandwidth is too high, you will run into oscillations.</p><p>When the motor is not moving it generally acts like a resistor, since there isn’t any back-emf. Thus, you can calculate resistance by using the stall current of the motor and dividing voltage by it (due to Ohm’s law). However, I prefer to measure it myself since manufacturer values are usually pretty unreliable and this value needs to be accurate for the controller to work well.</p><p><strong>Resistance </strong>is pretty simple to measure. Simply apply a voltage through the motor driver and measure the current flowing through the motor. Then use R = V/I.</p><p>I 3d-printed a jig to lock the rotor to do this, and started with a duty cycle of 15%, although in the end I used 25%. <strong>BE CAREFUL! </strong>Higher duty cycles give you more accurate values, but also risk burning your motors (which smells REALLY bad).</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*Cpyld7QpTE3PwjRJKIEXnQ.jpeg" /><figcaption>I 3d-printed a jig to lock the rotor</figcaption></figure><p>Unfortunately, <strong>inductance</strong> measurement is more involved. Essentially, I took advantage of the fact that the time it takes for an inductor to decay to 36.8% of its peak voltage is equal to L/R:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/469/0*4UGnF3qH5KtIXrUS.png" /></figure><p>Still in the locked jig, I applied a 1000kHz 10% duty cycle PWM to the motor and measured the time it took to decay to 36.8% by plotting current (probing the motor driver current sensor output) over time with my oscilloscope:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*TLUJk2bk6JKNbA8EG3vmjA.jpeg" /></figure><p>I used the MATH function to filter the noisy data, which made it a lot clearer. Note that you need to use a low duty cycle, since <strong>the current must decay to zero</strong>! I then used the time constant equation to calculate inductance.</p><p>Unfortunately, this process is pretty involved and expensive, but luckily measuring inductance accurately doesn’t matter too much since it only affects the P coefficient and the I coefficient does most of the work in this controller. The P coefficient simply improves transient response — how quickly it responds to a change in the target current. For a typical 25mm brushed DC motor you can probably guess around 1mH as a starting point, and I found mine to be 2.13mH.</p><p>By implementing a PI controller with these coefficients and your measured resistance and inductance values you can control current in each motor! Note that this PI controller outputs voltage, so you need to divide the output by the battery voltage to get a PWM duty cycle.</p><h3>Integrator windup</h3><p>One issue is that the torque that a motor outputs reduces linearly with how fast its spinning, as more voltage is dedicated to back-emf. If you try to maintain a constant current on a motor without load, as the motor speeeds up the back-emf will increase and thus the integrator term will try to apply increasingly higher voltages, bringing a problem called <strong>integrator windup</strong>. To address this, I implemented a simple anti-windup algorithm:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*AvMVdGu-0b0zIc5eQXB0mg.png" /><figcaption>Anti-windup algorithm</figcaption></figure><p>Finally, to control velocity I implemented a simple P controller, which I tuned by hand. I also implemented a maximum current limit on the output of the controller, which I initialized with a small value and increased until the wheels started slipping when driving against a wall. To tune the velocity controller, I would recommend putting in a step response and graphing it, since this gives a really clear visual on whether its overshooting or overdamped.</p><p>In the end, you get a nested PID controller with a velocity control outer loop and current control inner loop.</p><h3>Implementation</h3><p>The current controller and localization needs to run at a high frequency, so I decided to use one core of my RP2350 just for localization, velocity, and current control, running a 2kHz loop. I used the FIFO buffer to send data like the reference velocity from the other core, which I used to handle data logging and user interaction code.</p><p>To improve the controller I implemented a lower current limit when the motor is decelerating. Additionally, I split the velocity controllers to control angular velocity (difference in velocity between the wheels) and linear velocity (average velocity of the wheels) instead of having a per-wheel controller. This prevented the linear component of the position controller (described in the “closed-loop control section”) from overpowering the angular portion. I implemented this through a current “budgeting” algorithm where the angular controller was guaranteed a small amount of current and additionally could use any current unused by the linear controller. I also implemented centripetal force limiting, to prevent lateral slippage.</p><p>The motor driver I was using only gave the magnitude of the current in the motor, so I had to implement a method of determining whether the motor was decelerating (regenerative braking) or accelerating. The general idea is that if the voltage applied to the motor is less than the back-emf of the motor, then it is regen braking.</p><p>I calculated the voltage applied to the motor from the duty cycle and bus voltage, and estimated the back-emf of the motor can be estimated from the no-load speed at 12V (which I also recommend measuring yourself). I simply take the speed, measured by the encoders, and scale it based on the no-load speed. In the end I did something like this:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*W2q022DnbvkgNFFFaj6VgQ.png" /></figure><p>And tested it by applying a sinusoidal input to the motor PWM cycle:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/0*AJ5ThrAm-RLIUr6G" /></figure><p>As you can see, the current (yellow line) is sinusoidal, and it looks like the derivative of the velocity (green line)! That means that it is measuring torque, and the sign is correct.</p><h3>Results</h3><p>This system worked extremely well, requiring no modifications to the closed-loop controller described earlier, and resulted in extremely clean motion-profiled paths!</p><iframe src="https://cdn.embedly.com/widgets/media.html?src=https%3A%2F%2Fwww.youtube.com%2Fembed%2FAq9-dWZL13o%3Ffeature%3Doembed&amp;display_name=YouTube&amp;url=https%3A%2F%2Fwww.youtube.com%2Fshorts%2FAq9-dWZL13o%3Fsi%3DxnVUeinnpZvlI7Je&amp;image=https%3A%2F%2Fi.ytimg.com%2Fvi%2FAq9-dWZL13o%2Fhq2.jpg&amp;type=text%2Fhtml&amp;schema=youtube" width="640" height="480" frameborder="0" scrolling="no"><a href="https://medium.com/media/26a8873f820b365f68b2b0641e53dac0/href">https://medium.com/media/26a8873f820b365f68b2b0641e53dac0/href</a></iframe><p>One of the greatest advantages, as shown by the video, is that it can push other objects and handle disturbances without slipping, something not possible with typical trapezoidal motion profiles without extensive tuning for different scenarios.</p><img src="https://medium.com/_/stat?event=post.clientViewed&referrerSource=full_rss&postId=d57a45fb14ee" width="1" height="1" alt="">]]></content:encoded>
        </item>
        <item>
            <title><![CDATA[How to make a Field-Oriented Control ESC from scratch]]></title>
            <link>https://medium.com/@vikramaditya.nishant/how-to-make-a-field-oriented-control-esc-from-scratch-948a5d6547b9?source=rss-5b587d84fbe0------2</link>
            <guid isPermaLink="false">https://medium.com/p/948a5d6547b9</guid>
            <category><![CDATA[electronics]]></category>
            <category><![CDATA[foc]]></category>
            <category><![CDATA[control-system]]></category>
            <category><![CDATA[pcb-design]]></category>
            <category><![CDATA[motor]]></category>
            <dc:creator><![CDATA[Nishant Vikramaditya]]></dc:creator>
            <pubDate>Fri, 15 Aug 2025 03:23:16 GMT</pubDate>
            <atom:updated>2025-08-19T23:08:25.561Z</atom:updated>
            <content:encoded><![CDATA[<figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*jkACARlrAf7TamNJ71OHEA.jpeg" /><figcaption>My custom field-oriented controller “Fluxon mini”</figcaption></figure><p>Torque control instead of voltage control, current limiting, insane efficiency, and silent operation make brushless Field-Oriented Control (FOC) motor controllers incredibly useful for nearly every robotics application. They enable applications like perfect motion profiling, ultra-compact actuators, and long life on battery-powered robots.</p><p>Pursuing this project taught me an incredible amount about embedded electronics, control systems, and PCB design, and I would strongly recommend trying it out yourself. Here’s my experience with making a FOC controller as a high schooler and what I learned along the way so that you can use to make your own, starting from the ground up with your own hardware, software, and control systems!</p><p><strong>Prerequisite knowledge</strong>: Basic circuits (resistors, inductors, capacitors), trigonometry, MOSFETs, microcontrollers, pulse-width modulation, basics of brushless motors (back-emf, KV rating), and PID</p><h3>What is FOC?</h3><p>The core principle of FOC is that Brushless DC motors (BLDCs) have three phases (which is why they have three wires), which I am going to refer to as A, B, and C. The core idea is that if you connect one phase (one of the wires of the motor) to power and the other two to ground, then one phase will be turned on.</p><p>There are two parts to a BLDC motor: the rotor, which moves (<strong>rotates)</strong>, and the stator, which doesn’t move (is <strong>stationary</strong>). In the motor there are coils of wire for each phase, and neodymium magnets on the rotor. When current flows through a coil, it makes a magnetic field, and thus the magnets on the rotor will be attracted to the phase that’s on. For each <strong>pole </strong>in a brushless motor, there is a coil of wire for each phase, and each coil is spaced evenly. If you imagine a motor with just 1 pole, then there would be 3 coils, each 120 degrees apart. Here’s a visualization from MATLAB:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*WCcO259xdIT_wn0CqYQ_pw.png" /><figcaption>Visualization of 3-phase motor from MATLAB</figcaption></figure><p>In this image, the A phase (red) is the only one turned on. Thus, the net magnetic field simply points towards its coil of wire, which is indicated by the pink arrow. This is the magnetic field created by the <strong>stator</strong>, since the coils of wire are part of the stator. The rotor’s magnetic field, created by the permanent magnets, is 90 degrees away from the stator’s magnetic field, so the rotor would spin 90 degrees so that these line up if you were to leave the A phase on and let go of the rotor. In fact, you can connect one wire of your motor to power and the other two to ground and see this happen with any BLDC motor (but don’t do this with too much voltage because you might burn your motor, try out like 0.5–1V).</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*_Q9gJhJwEuUPP9ph0qY6iQ.png" /><figcaption>Wye-wound motor configuration (KiCad only has 45 degree inductors, so bad angles)</figcaption></figure><p>To turn on the A-phase, you need to connect it’s wire to power and the other two to ground because (most) motors are wound in a wye configuration. When you connect voltage to A and ground to B and C, current flows through A. (although there is current flowing through coils B and C, their vectors cancel out)</p><p>In the model above, if you were to spin the rotor by hand more than 180 degrees and let go, it would spin further until it lined up with the A phase again. Thus, there is a “stable point” every 360 degrees of rotation. However, if you were to do this in reality for a typical BLDC motor you would notice that the stable points are much closer. This is because real motors have more than 1 pole:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/693/0*hI019DekBlZKocgN.JPG" /></figure><p>For example, in the 2 pole motor, you would only have to spin it more than 90 degrees for it to click into the next set of coils. And in the 4 pole motor you would just have to spin it 45 degrees before it goes to the next stable point. The A2212 (a common drone motor) has 7 poles, so you have to push it through 7 stable points — you feel these as “clicks” — before you do a full rotation. For the rest of this section, we will imagine the motor as a 1 pole motor, but pole counts will come back later!</p><p>The <strong>core principle of FOC </strong>is that we want to keep the magnetic field inside the stator 90 degrees away from the rotor’s magnetic field. For example, in the image from MATLAB above, this is the case, so the FOC control is functioning in that motor. In the image below, FOC is not happening because the magnetic field inside of the motor is not 90 degrees away from the rotor’s magnetic field:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/882/1*H3kedS_k4OnUn3CQr7wLZA.png" /></figure><p>As you can see, the pink arrow (the stator magnetic field) is not 90 degrees away from the blue arrow (the rotor’s magnetic field). Thus, the torque is not being produced optimally, since these should be 90 degrees apart for optimal torque production. This is because, in a motor, torque is the <strong>cross product </strong>of the two magnetic field vectors, and cross product is maximized when the vectors are perpendicular. The “field-oriented” part of FOC comes from the fact that it always keeps the stator’s magnetic <em>field </em>90 degrees away from the rotor’s magnetic <em>field</em>. And that’s really all FOC is!</p><h3>How do you do FOC?</h3><p>In any DC motor, <strong>current is proportional to torque</strong>. By measuring the current in each phase, you can determine the size and direction of the pink arrow — the stator magnetic field. Then, you can calculate how much of this lines up with the rotor’s magnetic field (<strong>direct axis</strong>) and how much is perpendicular to the rotor’s magnetic field (<strong>quadrature axis</strong>). For FOC to work you simply have to ensure the direct axis current is 0, and for the motor to spin with a constant torque, you simply apply a quadrature current.</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/450/0*4KDZRdQVgtzmB2uY.png" /><figcaption>The core principle of FOC</figcaption></figure><p>For example, if the direct current is 0 amps and the quadrature current is 5 amps, then the direction of the of the stator’s magnetic field will be 90 degrees away from the rotor’s magnetic field, and the torque in the motor will be proportional to the current of 5A.</p><p>There are three “steps” to FOC:</p><ol><li>Determine the quadrature and direct axis currents. You do this by measuring phase currents and using math to convert the A, B, and C phase currents to direct and quadrature. To do this, you need to know the position of the rotor, which I did through a magnetic encoder.</li><li>Use two <strong>PI</strong> (PID without the D) <strong>controllers</strong> to drive the direct axis current to 0 and the quadrature axis current to a current setpoint (e.g. 5A). These controllers tell you the voltage needed on the direct and quadrature axes to get the desired current. If you target a negative current then the motor will spin the opposite direction.</li><li>Convert quadrature &amp; direct axis voltage (from the PI controller) to voltages for each phase and apply that voltage to each phase. Similar to brushed DC motors, you use <strong>pulse-width modulation</strong> (PWM) to create this voltage for each phase. Since there are 3 phases, you need to use some math to calculate the duty cycle for each phase.</li></ol><p>Essentially, we start with current measurements in the <strong>motor frame of reference</strong>, convert it to the direct &amp; quadrature frame of reference (<strong>dq frame</strong>), use that to calculate desired voltages with PI controllers, and then convert that back to <strong>motor frame</strong>. However, before implementing these, we first need hardware to test out our algorithms!</p><h3>Requirements for the ESC hardware</h3><p>There a few things we need to be able to do to execute the algorithm described above:</p><ol><li>Measure the angle of the rotor. I did this with the MagnTek <strong>MT6701</strong>, a hall-effect sensor which measured the angle of a magnet I attached to the rotor. For my ESC, it looked like this:</li></ol><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*JMUz-qNgBN9Y-1LCLryXdw.jpeg" /><figcaption>The MT6701 encoder, with the magnet attached to the motor above it</figcaption></figure><p>2. Write voltage to each phase of the motor. To do this we must measure power supply voltage and be able to do PWM on each phase of the motor. For PWM, we need two things: a <strong>gate driver</strong>, and <strong>MOSFETs</strong>. You might be familiar with MOSFETs, which essentially act as electrically controlled switches. However, when starting this project I didn’t understand the purpose of a gate driver.</p><p>Essentially, we need the ability to connect each phase to either power or ground. To do this we use high-side and low-side MOSFETs, in the following configuration for each phase:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*Teu-YCC-FWjLXJuy2aEaiA.png" /><figcaption>The structure of the driving circuit for each gate</figcaption></figure><blockquote>Note that the shunt resistor (3 milli-ohm one) and SENSE outputs are for current sensing, which will be addressed below.</blockquote><p>This configuration of MOSFETs is called a <strong>half-bridge</strong>. When the high-side MOSFET opens, and the low-side MOSFET closes, the phase is connected to power. When the low-side MOSFET is open and the high-side MOSFET is closed, the phase is connected to ground.</p><p>Note that we are using two N-channel MOSFETs (since they have much better efficiency than P-channel). This means we have to drive the gate 10V higher than the source (labeled “S”) to turn the MOSFET on fully. This means that, if we have a 12V power source, we actually have to be able to drive the high gate to around 22V for it to fully open. Additionally, since the gate of a MOSFET acts as a capacitor it requires a lot of current to open quickly, which is essential for high efficiency. For this reason we need to use a <strong>gate driver </strong>(a chip made for the purpose of driving MOSFETs).</p><p>3. Measure the current of each phase of the motor. For this we use <strong>bidirectional</strong> <strong>current-sense amplifiers</strong> and shunt resistors. Essentially, these rely on Ohm’s law: <strong>V=IR</strong>.</p><p>When current is flowing across the shunt resistor there is a tiny voltage drop (on the order of millivolts). A <strong>current-sense amplifier </strong>takes this tiny voltage drop and amplifies it, usually by some fixed ratio — I used 40V/V — so that the microcontroller’s ADC can read the current. However, you might be wondering why we need a bidirectional amplifier, considering this is a DC circuit!</p><h4>How can current flow out of ground?</h4><p>If current is negative, that means that current is flowing from ground into the phase, which made zero sense to me — that would mean the voltage at the phase is negative! Well, it is negative, and it actually does make sense when you realize the coils of wire for each phase acts as <strong>inductors</strong>.</p><p>In my mind I imagine an inductor as a device that tries to keep the current flowing through it constant. During pulse-width modulation the voltage is rapidly switched from high to low for a phase. However, the phase wants to keep the current constant. So imagine this:</p><ol><li>The phase is connected to +VDC (switched ON) — the high-side MOSFET is open, the low-side MOSFET is closed. Current flows from +VDC through the phase to the center of the Wye. Thus, the inductor wants to have power flowing through it from this half-bridge. At this point, there is zero current across the shunt resistor, because the low-side MOSFET is closed.</li><li>Now, the phase is connected to GND (switched OFF) — the high-side MOSFET is closed, and the low-side MOSFET is open. The inductor still wants to have current flowing from that bridge to the center of the Wye, so it just pulls current from GND, causing the voltage at the phase to be negative! Now, the voltage across the shunt resistor will be negative since the current is negative and V = IR.</li></ol><p>Note that this is actually a situation when we would consider the current to be positive — the power is flowing from +VDC into the phase. However, our low-side shunt sees a negative voltage at this time. Thus, <strong>we need to connect our shunt resistor to the amplifier backwards</strong>, so that negative voltages across the shunt resistor correspond to positive currents after amplified!</p><p>This places two requirements on our system:</p><ol><li>Our current-sense amplifier must be <strong>bidirectional </strong>— it must handle both positive and negative voltages across the shunt resistor. These amplifiers typically output VCC/2 to represent zero voltage across the shunt, and swing towards VCC if there is a positive voltage and towards ground if there is a negative voltage.</li><li>We <strong>must sample when all gates are LOW </strong>— for this to work, there must be center-aligned PWM on each phase, and the ADC measurement must be triggered when all of the PWM pulses are low.</li></ol><figure><img alt="" src="https://cdn-images-1.medium.com/max/600/0*5dVDUfwpDj__RLPG" /><figcaption>A graphic for center-aligned PWM</figcaption></figure><h3>Component Selection</h3><p>To meet all these requirements I picked an all-in-one chip: the <strong>Texas Instruments DRV8305</strong></p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*vMkzAN6kzDvJ2MWefk3q5w.png" /><figcaption>TI DRV8305</figcaption></figure><p>This chip is genuinely amazing — it has 6 gate drivers (so it can handle the 3 high-side and the 3 low-side MOSFETs), a charge pump that’ll handle making the 22V for the high-side MOSFET, and 3 bidirectional current sense amplifiers! It’s configurable over SPI, so I was able to configure the current-sense amplifier gain based on whether I needed high current or low current motors. Plus it works at just 4.5V, meeting another requirement my ESC had of functioning at both 6V and 12V. And it does this all for just $2.75, making my ESC incredibly cheap.</p><p>Because of TI’s gate driver features I didn’t need to include gate resistors, but these might be something to limit gate drive current depending on your gate drive setup.</p><p>I used 3xPWM control, in which you write PWM duty cycles for the A, B, and C phases. There are 6 MOSFETs (2 per half-bridge), so the chip internally handles the logic to turn on the low-side MOSFET when the PWM signal goes low, and turn on the high-side MOSFET when the PWM signal is high. It leaves a <strong>dead time</strong>, which is time in between turning the high-side FET off and low-side FET on, so that we don’t run into a situation where both are on at the same time and shorting power to ground. The STM32 also supports doing this logic, but it wastes HRTIM channels.</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*Fo72fPjxckfs-L_Rc5aADg.png" /><figcaption>AON7544</figcaption></figure><p>For my MOSFET I picked the tiny AON7544, which somehow costs just 13 cents, while having a ridiculously low on resistance of under 5 milli-ohms at 10V and supporting 30A continuous.</p><p>For the magnetic encoder I picked the MT6701 (~$2) in SSI mode, which, at the SPI clock frequency of 10 MHz, made 10 kHz sampling easy. Overall, this ESC cost just $6 for the components! (not including the microcontroller)</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/676/1*1mncpR7aPNw1kRt_yRqJ_A.png" /><figcaption>STM32G474</figcaption></figure><p>To control the ESC I needed a chip that had a high clock frequency for fast calculations and very good peripheral support, since I needed to have extremely precise triggers and interrupts. In the end I chose to use the STM32G474, since it has the HRTIM (High-resolution Timer) that gave me high-resolution PWM and allowed me to configure the ADC triggering and main processor interrupts through their CubeMX GUI. Additionally, the G474 has enough HRTIM channels and ADC channels to control two ESCs simultaneously, which was my goal.</p><p>Since this is an ARM chip make sure to use the CMSIS-DSP functions! There are built-in ARM-optimized functions for the Clarke, Park, and trigonometrical functions used in the ultra-fast FOC control loop.</p><p>I ran my PWM at 20 kHz and setup the ADC triggers and interrupts such that the FOC loop ran at 10 kHz, sampling every other PWM cycle. That way, the STM32 could control two motors and alternate between their control loops.</p><h3>Bus Capacitance</h3><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*Be8DpL8IugO0kt34kusWEg.png" /><figcaption>I used a lot of capacitors to to filter the bus voltage</figcaption></figure><p>Applying a square wave to an inductor creates a lot of high-frequency current-draw. Although inductors try to keep current constant, they actually end up with a (roughly) triangular current waveform:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*etQuTfFi-d8hKVmh48udbQ.png" /></figure><p>On my ESC I determined that I would have roughly 3A of ripple current (amplitude of the triangle wave), so I sized the capacitors accordingly, using a 100uF <strong>aluminum-polymer capacitor </strong>that can do around 2.1A of ripple current by itself. I then added 8 10uF ceramic capacitors, that can do a ton of ripple current, but not store much energy. This is especially so because this is the real world, and ceramic capacitors lose capacitance when the voltage increases:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*inuJP2lIll4uopJmuypitg.png" /><figcaption>DC bias characteristics of the 10uF 1206 Samsung MLCC capacitor I used</figcaption></figure><p>That means that the 80uF of theoretical capacitance is actually just 30.6uF after DC bias derating. Still, with the 100uF of aluminum-polymer capacitance (which doesn’t derate) this was plenty.</p><p><strong>Larger ceramic capacitors derate less than smaller ones!</strong> I used 1206 or 0805 for any capacitors connected to the power bus.</p><h3>Layout Considerations</h3><p>On my final revision I used a 4-layer board, with signal/ground on the top and bottom, a ground plane on the 2nd layer, and a power plane on the 3rd layer. This worked fine for me, but you can put ground in the 3rd layer too and route power along with signals if you want more heat dissipation. I originally routed this on a 2 layer board and it worked, but the 4-layer one was much easier to route and can handle more current while being significantly smaller (as you can see in the images below).</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*kvCooCf4N2ZMA-kDT0TiXQ.png" /><figcaption>The PCB layout for the final version of the ESC, 4 layer</figcaption></figure><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*OtEddfXiL0X2vMoW1JuDcg.png" /><figcaption>The layout for the first 2-layer version, using a 2.54mm IDC connector for easier debugging, 2 layer</figcaption></figure><p>I tried to pack the MOSFETs as close to the MR30 connector to minimize the length of the copper pours needed. Additionally, I placed 1uF capacitors right next to the high-side MOSFET to reduce any high-frequency power supply noise. Finally, I made sure to use thicker traces for power signals and thinner ones for data signals.</p><p>I used 1 via per continuous amp, so there are 10 vias for the high-side MOSFETs. However, an important result of the fact that motors are inductors is that <strong>they can output more current than they draw</strong>. For this reason I chose the shunt resistor and current sense gain to handle 15A, so that my ESC can handle 15A when at low speeds (explained further at the end of this blog post). Keep this in mind throughout the PCB design!</p><p>Although I started out with an STM32 NUCLEO (their development board), I switched to a custom 2-layer control board to clean up the wiring and allow me to connect two ESCs at once:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*W8VwbTz6mDjmsvEzKgFrFA.png" /></figure><h4>Kelvin Shunts</h4><p>You might notice from the half-bridge circuit that one of the legs of the low-side shunt is connected to ground. While it might be tempting to just connect the corresponding current sense amplifier pins to ground (I did this originally) and let the ground plane connect the shunt and the amplifier, this will cause <strong>ground lift (</strong>where the ground plane has uneven voltage due to high currents) to affect your readings. Instead, you should use a <strong>net-tie </strong>to connect that side of the shunt to the amplifier. The implementation of these varies by your design software, so search up how to use them in your specific software.</p><h3>Debugging Tips</h3><p>Unfortunately, nothing complicated ever works first try and the control algorithms for this ESC were just like that. To debug the control algorithms I employed four techniques:</p><ol><li>Put test points EVERYWHERE! I put test pads on the 6 gates, the 3 phases, and I originally used a STM32 NUCLEO board which allowed me to probe all the wires going between the STM32 and the ESC with my oscilloscope:</li></ol><figure><img alt="" src="https://cdn-images-1.medium.com/max/718/1*bW6D4omniq_Wtt2mh0z0mA.png" /></figure><p>2. Use your debugger! The STM32 has great debugging integration with VSCode and I used this heavily to help debug my code.</p><p>3. Start out small. Although in the end I used a custom controller PCB and high-density FPC cables to connect multiple ESCs to a single microcontroller, starting out with easily probe-able headers made debugging significantly easier and using a NUCLEO ensured that the STM32 portion was functional.</p><p>4. USE THE DAC (Digital-to-Analog Converter) WITH THE OSCILLOSCOPE! I outputted variables that changed at a high frequency (for example, direct current, quadrature current, velocity, etc.) and sampled them with the oscilloscope to see how they changed along with things like the phase voltages or the current sense inputs on the other oscilloscope channel! Here’s a picture of an issue in the quadrature current resulting from mistimed ADC reads:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*i5lUlBxxkv3P3RwFih1gYw.jpeg" /><figcaption>Yellow line is phase A pwm, blue line is quadrature current</figcaption></figure><h3>What’s the alpha-beta frame?</h3><p>There are a few reference frames that we must understand for the math behind FOC to make sense:</p><ol><li>Electrical reference frame — This is the model of the motor with 1 pole, where each phase is 120 degrees apart.</li><li>d-q frame — This is the reference frame where the axes are direct and quadrature current (which is where d-q comes from). This frame is in the rotor’s perspective, so converting to this frame requires knowledge of the rotor’s position.</li><li>Mechanical reference frame — this is the position that the magnetic encoder measures. To convert this to electrical reference frame you need to divide by the number of pole pairs (so there are stable points every 360 degrees in this reference frame), and subtract some offset such that when the A phase is on it corresponds to zero degrees. You may also need to multiply by -1 depending on the direction the motor spins vs your encoder</li><li>Alpha-Beta frame — This is just a fancy name for a typical coordinate plane, where alpha is x and beta is y. This is an intermediary between the current measurements, which happen in the electrical reference frame, and any vectors in this frame are basically just simple vectors on a 2d plane:</li></ol><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*NLKKBSBRiWud5B9z10jqwQ.png" /><figcaption>The alpha-beta frame, with the electrical frame drawn on top</figcaption></figure><p>There are two transforms (and inverses) you must use to convert between the electrical, alpha-beta, and d-q reference frames. The <strong>Clarke transform </strong>converts values in the electrical reference frame (e.g. the current measurements) to the alpha-beta frame. The <strong>Park transform </strong>converts values from the alpha-beta frame to the d-q frame. This requires the angle of the rotor <strong>in the electrical frame</strong>.</p><p>To perform the first step of FOC — determining d and q currents — simply use the Clarke and Parke transforms to convert the current measurements. There are implementations for these in the CMSIS-DSP library, but I implemented Clarke myself so that it would use a, b, and c currents instead of just a and b.</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/798/1*IHbc6BUjaLBPqEGzwToKJw.png" /><figcaption>Clarke transform (you can ignore f0, since SPWM and SVM — explained below — keep this equal to zero)</figcaption></figure><figure><img alt="" src="https://cdn-images-1.medium.com/max/856/1*ZlDx8tbjtxtQLLssNUUurw.png" /><figcaption>Park transform</figcaption></figure><p>Make sure to compute sine and cosine at the start of the loop and then just use the computed values for the park and inverse park transform! Sine and cosine are SLOW, even with CMSIS-DSP’s lookup tables.</p><h3>Moving the motor</h3><p>The easiest way to test if your hardware is working is to just apply a steadily increasing angle to your stator’s magnetic field, which causes the motor to spin at a constant velocity. This also happens to be the last step of FOC, where you need to turn the output of your PI controller into PWM duty cycles for each phase.</p><h4>The Naive solution — Sine PWM (SPWM)</h4><p>Let’s say you want to convert a duty cycle (value from 0.0–1.0 for % of time to turn on) in the alpha-beta frame into a duty cycle in the electrical frame so that you can write to your hardware.</p><blockquote>You can convert a rotor angle (theta) and duty cycle magnitude into the alpha-beta frame by simply calculating alpha = duty * cos(theta) and beta = duty * sin(theta).</blockquote><p>First, you can use the inverse Clarke transform. However, as you might infer by the fact that it can handle negative currents, this transform will output values in the range -1 to 1 for each axis (since the input duty cycle is -1 to 1). To fix this you can simply divide each value by two and add 0.5 to have the duty cycles range from 0–1. If you have theta constantly increase with time (effectively spinning the motor at a constant velocity), you will get a sinusoidal graph for each phase:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/0*rZrAPzyAABdXKZQG.png" /><figcaption>Nice visuals from <a href="https://electronics.stackexchange.com/a/501451/370694">this post</a>. This graph is from before you divide by two and add 0.5.</figcaption></figure><p>However, this has a problem! If you look at the top of the graph only, you will see that this method of PWM does not use all the voltage available, as is visible by the fact that there are moments when the top of the graph isn’t at the maximum amplitude:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/426/1*so-Ab0ajkXMEBo2n50DO9A.png" /></figure><p>If you use this method, your top speed will be limited an unnecessary amount because you are not using the full voltage available. Instead, there is a better method — Space-Vector Modulation (SVM, a.k.a SVPWM) that utilizes the voltage better:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/0*ZALmXP9vwqTsVgjS.png" /><figcaption>Graph of SVPWM from the same post, also before dividing by two and adding 0.5</figcaption></figure><p>Note that there are still little bumps, which are necessary to ensure that the torque is constant throughout the range of angles (explained below). However, it uses the available voltage more effectively. Here’s a better way of visualizing these various methods:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/727/0*8tdmct40zLmTFFXY.png" /><figcaption>Visualization of voltage in electrical frame, also from the same post</figcaption></figure><p>This is a graph of the voltage used at various angles in the electrical reference frame. As you can see, SPWM (the red circle) does not use all the available voltage, while SVPWM does (the blue circle). Trapezoidal commutation, used on cheaper ESCs (which you can imagine is just SVPWM, but without the bumps) leads to torque in the shape of the dark blue hexagon, which is why trapezoidal commutation is often noisier and less efficient than FOC.</p><p>When the rotor is between two phases, a trapezoidal commutation ESC will turn on two phases at once which leads to more voltage than when the rotor is only aligned with a single phase. This causes the motor to slightly accelerate and decelerate throughout its rotation, leading to noise and inefficiency.</p><p>I’m not going to include an implementation for SVPWM but you can find many online.</p><h3>PI Current Control</h3><p>To actually perform FOC we must drive the direct axis current to zero and the quadrature axis current to a setpoint that represents our desired motor torque. To do this, we use a PI controller.</p><p>As you might imagine, tuning a PI controller thats running at 10 kHz is extremely difficult. Luckily, there is a mathematical approach to calculating PI coefficients that ensure the controller is critically damped, explained at the end of this blog post. Simply use the following formulas:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*FL_RATu1D-fSXN7I-WPhow.png" /><figcaption>The mathematical solution to the PI coefficients (yes, I know kI is unsimplified. This is what pole placement gives you)</figcaption></figure><p>The <strong>bandwidth </strong>is the frequency at which the controller can handle changes in the setpoint. This means that if you apply a sinusoidal input that has a frequency higher than 160 Hz to this controller, it will not accurately reflect the setpoint, which is plenty for most applications.</p><p>Your PID control loop frequency must be significantly higher than the bandwidth, otherwise your system will be unstable (it starts vibrating in a super scary way, speaking from experience). Generally it is recommended that your controller runs at a 10–20x higher frequency than your bandwidth. For example, with this 160 Hz bandwidth the controller should run at 1.6–3.2 kHz. Mine is running at 10 kHz, so it has plenty of stability margin.</p><p>To use these equations you must know your motor inductance and resistance. While measuring resistance is easy (just apply a voltage and divide by the current you measure), inductance is not so easy. I applied a PWM signal to the motor and plotted the current sense output using my oscilloscope, using the MATH function to filter the output:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*f_MWi-qGWfiIyvXSp71lJQ.jpeg" /><figcaption>Blue is raw current sense data, purple is filtered by the oscilloscope</figcaption></figure><p>Then I used the cursor to measure the time it took for the inductor to rise to 63.2% of its final voltage. If you multiply this by the resistance, you get the inductance of the motor. This is because, in a RL circuit, which models a motor effectively, the <strong>time constant — </strong>the time it takes for the current to increase to ~63.2% of the final voltage — is equal to L/R:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/469/0*WrKo9JDwkwt-3z8m.png" /></figure><p>The PI controller with these parameters will output <strong>phase voltages</strong>. To convert a voltage to a duty cycle, you would usually just divide by the bus voltage. However, as explained earlier, the voltage across the phase is 2/3rd of the voltage from that phase to ground — here’s a good visualization of this, demonstrating how the phases act as a voltage divider:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*IYEhCXE9Z-Wg9rYajI8aNg.png" /></figure><p>Therefore, you need to multiply by 3/2 to account for this. That’s why my code looks like this:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/572/1*UJ3G0p0ZL6VwZrLLt6SGEQ.png" /></figure><p>As the motor speeds up, it will have less voltage headroom. For example, take my 1000 KV motor with 0.1-ohm resistance. If I were to apply 12V when it isn’t spinning, I would get 120A — well over 10A. So when the motor isn’t spinning, the controller can easily reach a setpoint of 10A. However, when the motor is spinning at 12,000RPM, the back-emf is 12V. Therefore, if I apply a voltage of 12V I get 0 amps.</p><p>In this situation if I am trying to target, for instance, 10A, the integrator will just keep trying to apply higher and higher voltages that are well above the actual bus voltage. To address this, I used the following anti-windup algorithm:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*9rsUEkfz5UEHbYQc9KnxtA.png" /></figure><p>If there isn’t enough voltage for the motor it reduces the integrator voltage, otherwise it does PI as normal. Your max duty cycle must be below 100% since you sample the ADC when the PWM is low, so if the PWM never goes low the sampling won’t work. I somewhat arbitrarily chose 90%.</p><p>Finally, you need to convert these duty cycles in the d-q frame to the alpha-beta frame with the inverse Park transform and apply them to the motor via SVPWM.</p><p>To control velocity you can use a PD controller that will control acceleration (creates a current reference) to achieve a target velocity. By limiting the maximum current you can limit torque and current draw!</p><p>That’s it! This blog post has broken down each component in this diagram of FOC:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*uGj_XXagL04vLI62joKz8A.png" /></figure><p>By implementing this yourself, you can create your own FOC controller from scratch!</p><h3>Why doesn’t my ESC work at high speeds?</h3><p>I had an issue where at low speeds my ESC worked great, but at high speeds it made crazy noises, and at high enough speeds it got so crazy it triggered the DRV8305’s overcurrent protection. This issue flabbergasted me for weeks, as I looked everywhere for hardware issues. It turns out that it was because of the magnetic sensor SSI read timing.</p><p>At that point I just had the MT6701 SSI read start when the ADC was sampled, which was basically the same time at low speeds. However, in reality the MT6701 has a 5 microsecond delay (quoting the datasheet) and the actual data transfer takes time too. During these tiny microsecond-level delays the motor can spin significant amounts of electrical rotations (keep in mind there are 7 electrical rotations per mechanical rotation, so it was spinning ~35k electrical rpm) which was making it so that the current measurement was at a different point in the motor’s motion than the measured rotor position! Fixing this by shifting the MT6701 read earlier in the loop resolved the issue.</p><h3>Why is input current not equal to phase current?</h3><p>Once again, this all comes down to the fact that motors are inductors. When you measure 5A on the low-side shunt, this is current that has gone through an inductor.</p><p>Power is constant (energy is conserved), so lets say your quadrature current is 2A and the motor is spinning at 6000rpm, so the voltage across it is 6V (from back-emf, assuming negligible resistance). The motor is consuming 12W.</p><p>If the supply voltage is 12V, that means the motor can only be pulling 1A from the supply! This allows you to draw more current through your motor than your supply can provide, and is the reason that stalling your motor can be so dangerous for simpler ESCs — the MOSFETs can have a lot more current going through them than the supply can provide. This is also why motors can provide more torque when they are spinning slowly.</p><h3>Where do the PI coefficients come from?</h3><p>A BLDC motor phase is just an RL circuit — a circuit for which very neat equations exist, that let us mathematically determine the PI coefficients needed for good performance by looking at the transfer functions.</p><blockquote>As a high schooler, I have very little experience with this. I tried my best to understand the derivation and the choices behind the pole placement, but this is an entire field of study that I am very new to!</blockquote><p>To understand these, you first must understand a <strong>transfer function,</strong> which is built on the Laplace transform. From what I understand, this is essentially a different way of writing differential equations with the complex variable s. x gets converted to 1, the derivative of x gets converted to s, and the second derivative is s². Integrals are 1/s, 1/s², etc. This lets us write differential equations as expressions:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/964/1*mII3nd0pnoEIPWP17CIbpA.png" /></figure><blockquote>In control theory the input variable — control input — is typically called u and the output variable is called x, instead of input of x and output of y</blockquote><p>This has the extremely useful property that differential equations can be nested by just multiplying their transfer functions, making combining components of a system much simpler for control theorists.</p><p>The transfer function that gives the current of a RL circuit from its voltage is as follows:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/280/1*zd1V1DQyLX8LwcTbhRRAUg.png" /></figure><p>The transfer function of a closed-loop controller is as follows:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/755/0*VohrxhSnNOdjuhaI.gif" /></figure><p>R(s) is the reference — in this case, the current setpoint.</p><p>G(s) is the <strong>Plant </strong>— the model of the system, which in this case is the RL current transfer function.</p><p>H(s) is the controller — in this case, the PI controller, which has the transfer function (kP + kI/s) since kP is just a gain and kI is an integrator (so the 1/s component comes in)</p><p>When we plug these into T(s), which gives us the response of the system to changes in the setpoint (reference), we get:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*7PTLpPBpjKRut66miYeylw.png" /></figure><p>This is a second order system, which means we can achieve critical damping by manipulating the denominator. Since this is a second order system, you could re-write in the form:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/906/1*XHf30RxthceNIZndeGmkng.png" /></figure><p>And solve for kP and kI such that the damping ratio is equal to 1, making the system perfectly critically damped. However, as I understand it, this is risky because slight variations in the resistance and inductance can cause the system to become unstable, so even though this theoretically leads to perfect critical damping, in reality this doesn’t work well. Instead, we can employ a technique called <strong>pole placement</strong>.</p><p>A transfer function is composed of a ratio of two polynomials. The roots of the polynomial in the numerator are called the <strong>zeros</strong>, and the roots of the polynomial in the denominator are called the <strong>poles</strong>. If the poles of a transfer function are negative and do not have an imaginary component, the system will be critically damped at the bandwidth of the poles.</p><blockquote>Side note: Real poles have no oscillation. Positive poles are unstable — they will just diverge (think a PID controller, but it’s output is flipped so it becomes a positive feedback loop). Negative poles are stable and critically or overdamped. If there is an imaginary component, then there are damped oscillations, so the controller is underdamped. If the poles are purely imaginary, then the system will always oscillate.</blockquote><p>If you look at the original transfer function for the motor, 1/(R + sL), you can see that it has a pole at -R/L — the denominator is equal to zero when s is equal to -R/L. As I understand, we just keep this as one of the poles in our final transfer function to avoid instability in the case of R and L having measurement errors. Then, we want the other pole to be at -bandwidth, so that our controller is able to handle errors at our desired bandwidth.</p><p>The pole at -R/L is inconsequential since, for my motor, it has a value of 1124 Hz. This means that our controller cannot have a bandwidth exceeding 1124 Hz, which we aren’t trying to do. Since the bandwidth of the controller is just 160 Hz, it dominates in the system’s response to setpoint changes.</p><p>Using these two poles we can create a polynomial that we want our denominator to be:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*n7OWaNcN1Z9nPxUpluO65w.png" /><figcaption>Creating a polynomial from the desired poles</figcaption></figure><p>Note that we can multiply the whole thing by L since it is the coefficient of s² in the controller’s denominator. This is necessary for us to be able to set them equal to each other. Once we do that, we can solve for kP and kI:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1012/1*ZHTxOy8cJGybHAhOLWPk7A.png" /></figure><p>And that’s how the equations for kP and kI were determined!</p><img src="https://medium.com/_/stat?event=post.clientViewed&referrerSource=full_rss&postId=948a5d6547b9" width="1" height="1" alt="">]]></content:encoded>
        </item>
        <item>
            <title><![CDATA[How FusionFlight can make drone delivery a reality]]></title>
            <link>https://medium.com/@vikramaditya.nishant/how-fusionflight-can-make-drone-delivery-a-reality-65363c54a2e8?source=rss-5b587d84fbe0------2</link>
            <guid isPermaLink="false">https://medium.com/p/65363c54a2e8</guid>
            <category><![CDATA[sensor-fusion]]></category>
            <category><![CDATA[drones]]></category>
            <category><![CDATA[uw]]></category>
            <category><![CDATA[fusionflight]]></category>
            <category><![CDATA[machine-learning]]></category>
            <dc:creator><![CDATA[Nishant Vikramaditya]]></dc:creator>
            <pubDate>Wed, 28 May 2025 03:07:05 GMT</pubDate>
            <atom:updated>2025-05-30T01:15:12.082Z</atom:updated>
            <content:encoded><![CDATA[<figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/0*LyskTtF2kDXQZBE1.png" /><figcaption>Guide Air Labs’ FusionFlight system</figcaption></figure><p>Advancements in drone technology have promised to revolutionize industries such as drone-based package delivery, public safety, and construction. However, due to challenges in sensor fusion, size, and power consumption, these have failed to manifest into reality. To learn more about how FusionFlight is addressing these problems I sat down with University of Washington Professor Sep Makhsous and PhD Student Gokul Nathan.</p><h3>Localization in Urban Environments</h3><figure><img alt="" src="https://cdn-images-1.medium.com/max/540/0*DawC3r3g5n0T-RdC.jpg" /><figcaption><a href="https://geoawesome.com/eo-hub/gnss-shadow-matching-improving-gnss-positioning-urban-canyons/">The “Urban Canyon” effect</a> (Geoawesome)</figcaption></figure><p>Although systems like GPS have made outdoor localization much more accessible, accuracy within these environments is limited due to the “urban canyon” effect. As shown above, buildings can block signals from satellites that do not have line-of-sight. Even worse, these urban structures can actually reflect GPS signals, leading to the sensor measuring signals satellites out or view, leading to incorrect measurements.</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/679/0*PcTQmhHxCRdItOOQ.png" /><figcaption><a href="https://www.intechopen.com/chapters/60049">A diagram of multipath error</a> (Intechopen)</figcaption></figure><p>Although solutions exist to handle these issues, they are bulky, power-hungry, and extremely expensive, making them inaccessible to the market of mass-produced drones. This poses the problem of developing a low-cost, low-power consumption, and high-accuracy system for outdoor localization that can operate in the urban environments that drones operate in.</p><h3>From Research to Startup</h3><p>While many startups begin with a product idea, <a href="https://www.guideair-labs.com/">GuideAir labs</a> began with a problem and researchers dedicated to solving it. From developing an advanced system to track airborne pollution in Boeing factories to nutrition monitoring in diabetic patients, Dr. Sep Makhsous has years of experience with sensor fusion and the industry.</p><p>PhD student and cofounder Gokul Nathan adds on to this pool of expertise, developing a machine-learning based system to accurately position mobile phones utilizing the integrated low-cost array of sensors present, over the COVID lockdowns. Together, the founding team has decades of experience in embedded systems and low-cost sensor fusion.</p><blockquote>“A phone by default has more than a GPS sensor, it has radio antennas. It’s near Wi-Fi beacons. It also has its own IMU. So I realized very early on that having these huge variety of sensors, each measuring something, some other physical quantity and each has a different mode of failure. So one sensor fails in one place, whereas the other sensors work at that same point.” <em>[</em>Gokul<em>]</em></blockquote><p>As a spin-off from the lab, GuideAir labs also works in collaboration with the University of Washington <a href="https://www.arcuw.org/">ARC Lab</a> on FusionFlight and a variety of other advanced problems, working to harness the power and talent available in the university to solve problems facing the Aerospace industry as a whole.</p><blockquote>“The other thing that we want to try to do in our lab is how we can connect research and fundamental academic work into the industry and how we can bridge this gap by creating solutions out of the technologies that we’re developing in the lab.” <em>[</em>Prof. Sep Makhsous<em>]</em></blockquote><h3>How FusionFlight is unique</h3><p>Although sensor fusion has been a field of research for over half a century, FusionFlight can outperform more traditional systems through their development of novel techniques for outdoor localization. Traditional systems like Kalman filters rely on the premise that sensors provide accurate data, just with an extra layer of noise that needs to be filtered out. Although valid in many situations, this assumption does not apply to the scenario of urban navigation, where, due to issues like multipath, sensors can confidently report incorrect data.</p><blockquote>“Kalman filters are great in that they give you, like, quite robust estimates when you have good information coming from your sensors, but when information becomes unavailable or unreliable, or even worse, wrong information, we see that Kalman filters don’t really like that kind of information … They don’t know what to do with it when things are unseen … We’re able to fill in the gaps of that information” <em>[</em>Gokul<em>]</em></blockquote><p>Another advantage of a machine-learning based approach is power consumption, a critical factor in power-constrained airborne systems like drones. Although training models requires huge amounts of data, computation, and power, the process results in highly refined and compact models that are capable of running on microcontrollers.</p><h3>Future Technology and Challenges</h3><p>A promising area in the development of this technology is the advent of Neural Processing Units (NPUs). These application-specific devices are designed to run large neural networks, orders of magnitude larger than what is currently possible, on the highly constrained power budget of microcontrollers. NPUs integrated into microcontrollers are becoming increasingly available and GuideAir labs is ready to capitalize on this new technology.</p><p>Unfortunately, you probably won’t see a drone delivering your Amazon package tomorrow. One of the biggest challenges to this development is not the technology, but regulation. The future of drones needs a framework for determining safety and accountability for autonomous systems such as those in development at GuideAir labs.</p><blockquote>“Whenever machines come close to humans, we have to make sure that the human is safe even when the machine doesn’t perform as expected. That level of safety and explainability is something very key when we are having flying systems with like propellers moving at really high speed” <em>[</em>Gokul<em>]</em></blockquote><p>Still, the team is looking forward to a future where drones can play a major role in construction, aid in disaster zones, and deliver packages to customers in hours instead of days.</p><blockquote>“And my question I started asking … why is my package taking two days to deliver? Instead of coming and landing in front of my house, why do I have to go somewhere and pick it up? So essentially that question led me to understand there are a lot of technical challenges flying drones … like, okay, I have this good, promising technology that I did with my smartphones a few years back.” <em>[</em>Gokul<em>]</em></blockquote><img src="https://medium.com/_/stat?event=post.clientViewed&referrerSource=full_rss&postId=65363c54a2e8" width="1" height="1" alt="">]]></content:encoded>
        </item>
        <item>
            <title><![CDATA[Designing and programming a high-speed differentially driven vehicle]]></title>
            <link>https://medium.com/@vikramaditya.nishant/designing-and-programming-a-high-speed-differentially-driven-vehicle-3017b72f5790?source=rss-5b587d84fbe0------2</link>
            <guid isPermaLink="false">https://medium.com/p/3017b72f5790</guid>
            <category><![CDATA[scioly]]></category>
            <category><![CDATA[ev]]></category>
            <category><![CDATA[programming]]></category>
            <category><![CDATA[robotics]]></category>
            <category><![CDATA[control-system]]></category>
            <dc:creator><![CDATA[Nishant Vikramaditya]]></dc:creator>
            <pubDate>Tue, 29 Apr 2025 05:27:38 GMT</pubDate>
            <atom:updated>2025-05-02T06:09:38.999Z</atom:updated>
            <content:encoded><![CDATA[<figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*ouQnqc_c1HgXcHqe8hSDng.jpeg" /></figure><blockquote>Purchase one of these and find a materials list/open-source design information <a href="https://tektitebiz.com/product/tektite-r-electric-vehicle-science-kit/">here</a>!</blockquote><p>Electric Vehicle is a Science Olympiad event in which teams must compete to have a vehicle travel a distance as quickly and accurately as possible from AA battery power. Although a seemingly simple challenge, this has proven to be one of the most difficult robotics challenges I have undertaken. This blog post details how I designed and programmed my new vehicle, the “Nats kit”.</p><iframe src="https://cdn.embedly.com/widgets/media.html?src=https%3A%2F%2Fwww.youtube.com%2Fembed%2FOClnd_G8PK4%3Ffeature%3Doembed&amp;display_name=YouTube&amp;url=https%3A%2F%2Fwww.youtube.com%2Fwatch%3Fv%3DOClnd_G8PK4&amp;image=https%3A%2F%2Fi.ytimg.com%2Fvi%2FOClnd_G8PK4%2Fhqdefault.jpg&amp;type=text%2Fhtml&amp;schema=youtube" width="854" height="480" frameborder="0" scrolling="no"><a href="https://medium.com/media/f0b82ba13b908775dade5ece9b24c9fa/href">https://medium.com/media/f0b82ba13b908775dade5ece9b24c9fa/href</a></iframe><h3>Design</h3><p>The biggest problem with my older design, which had 4 passive wheels, was accuracy. This had 2 problems — it didn’t go straight, and aiming was inconsistent (e.g. aiming 4cm left would only result in a 2cm difference).</p><p>Although these issues could be mitigated by making the vehicle larger, I decided to go a different route with a differential drive, which simply just means there are two independently driven wheels that can be used to control the speed AND heading, effectively allowing for full position control. This design requires several key design decisions:</p><ol><li><strong>Motor </strong>— Ideally these have high torque, low current draw, a small deadband, and are lightweight</li><li><strong>Frame </strong>— Ideally, this is lightweight and rigid</li><li><strong>Wheels </strong>— These need to have high traction and be properly sized. I chose Banebots 2&quot; T81 wheels to fit this description</li><li><strong>Encoders/IMU </strong>— These sense the position of the robot, so are the most critical factor affecting the final accuracy</li><li><strong>Ball caster </strong>— A differential drive only has two wheels, so a 3rd omnidirectional wheel is needed to maintain stability</li></ol><h3>Motor</h3><p>To understand motor selection first lets briefly talk about batteries. A real-world battery is actually made up of two things: an ideal battery cell, and <strong>internal resistance</strong>.</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/0*tT7hLU6w-XgBQymL" /><figcaption><a href="https://thescienceandmathszone.com/internal-resistance/">A diagram of a real-world battery</a></figcaption></figure><p>This internal resistance leads to a phenomenon called “voltage sag”. Essentially, if you draw 20A from an AA battery the internal resistance will lead to a large voltage drop since it is proportional to the current (V=IR).</p><p>The amount of internal resistance depends on the type of battery, but generally the AA batteries we are allowed to use have quite a high value. This is different from Lithium Polymer batteries, which have a very low value so can output a lot more current. I’ve found that you should size your power system so that it draws <strong>no more than 10A</strong>.</p><p>Since a differential drive has 2 motors, I wanted motors that could maximize the torque output on the 5A each they had available. Eventually I found these brushless motors that have an integrated sensored ESC.</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/0*HV5gW6QvjgW6ajme.jpg" /><figcaption>The motor I chose for the vehicle, the <a href="https://nfpshop.com/product/25mm-metal-gear-motor-model-nfp-gm25-bl2838">GM25-BL2838</a></figcaption></figure><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*qCDx8s1VIb_Nlb619krfYw.png" /><figcaption>The specs for the motor</figcaption></figure><p>I chose the 4.4:1 ratio motor, since using this yields a somewhat good speed of 8.1mph, which I calculated with <a href="https://www.omnicalculator.com/everyday-life/rpm">this handy calculator</a>. Unfortunately, in practice this motor only draws about 1A, well under the power budget of 5A, and has a pretty low top speed. Some better motor selection could drastically improve the speed of the vehicle, but unfortunately will be significantly more complex to use since I’ll have to design a dual-motor sensored ESC (a project for the summer!).</p><p>The <strong>sensored ESC </strong>is critical because a sensorless ESC, like the one I used in my previous vehicle, requires the motor to be spinning quickly to function. This effectively creates a minimum speed constraint that leads to jerky braking, further increasing inconsistency.</p><h3>Frame</h3><p>If you’re making 1 or 2 vehicles, a 3d-printed frame is probably the most ideal for a vehicle of this size. However, the aluminum frame with carbon rods achieves a similar weight and price when produced at larger scales, and looks super nice. I designed the frame in <a href="https://onshape.com/">Onshape</a>, and I really enjoy using it.</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*b15sK_3y9F-VVIcJZEqvvw.png" /><figcaption>The CAD of the vehicle</figcaption></figure><p>The <strong>center of gravity </strong>was a big consideration in the design. Ideally, the center of gravity should be <strong>as close to the wheels</strong> as possible (since this will maximize traction), and it should be <strong>as low as possible</strong>, so that the vehicle doesn’t want to tip.</p><p>I chose to create the base frame out of a laser-cut aluminum plate, which has the battery holder, motors, and wheels directly attached, which is the vast majority of the weight. Although the center of gravity is really low, I think the ground clearance is a little lower than I would like, so in future designs I’m going to make it higher off the ground. In the end, the entire vehicle weighed an extremely light 622g (with batteries!)</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*AGwWTr2Z9q_NWVkBaq5-wg.jpeg" /><figcaption>The vehicle weighs just 622g, with batteries</figcaption></figure><h3>Encoders/IMU</h3><p>The vehicle can control both its lateral (side-to-side) and axial (forward and backwards) distance, which means it relies on the code to get it to the right position. For the code to do this, it needs to know the current position of the vehicle throughout the run. This is done with encoders, which measure how much each wheel has spun, and an IMU, which measures how much the vehicle has rotated. Although you can calculate the position using just encoders on each wheel, I found that this is significantly less consistent than using it with the IMU.</p><p>The IMU I selected is the ICM-20689, which I just selected since I read that it has good accuracy. I found it to give similar results to the MPU-6050. For the encoders, I used the MT6701, which measures the orientation of the magnetic field of a magnet to determine the orientation of the wheel. This is measured over time to determine the position.</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/249/0*mvZks0n6sC3vmVDq" /><figcaption>The MT6701 sensor</figcaption></figure><h3>Ball caster</h3><p>With just two wheels, the vehicle will not be stable, so it needs a 3rd point of contact with the ground. This takes the form of an omni-directional ball caster, which essentialy just has the role of sliding against the ground in every direction with minimal friction. Originally, I used the <a href="https://www.adafruit.com/product/3948">Adafruit 20mm ball caster</a>:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/779/1*n-ABqn-orRs4Ycb0OGI9Zw.png" /><figcaption>The adafruit 20mm ball caster</figcaption></figure><p>However, I found that the metal ball doesn’t slide inside the casing very well, so in reality the ball just ends up rubbing against the ground. This is still fine for events like Robot Tour, where the speed is low, but for Electric Vehicle friction needs to be minimized as much as possible. To accomplish this, I designed a custom 3d-printed ball caster inspired by the Pololu ball caster, but specific to my Electric Vehicle design (so the ball size, bearing sizes, dowel sizes, etc. are different). This reduced friction significantly, with the friction being so low that the ball, despite having very little mass, can spin within the caster for several seconds.</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/401/1*4DSk3H5DJyRJR0CrlDdniQ.png" /><figcaption><a href="https://www.pololu.com/product/2692">The Pololu ball bearing caster</a></figcaption></figure><p>With the design done, the real complexity and accuracy of the vehicle depends on the software!</p><h3>Custom Electronics (PCB)</h3><p>I initially implemented the electronics for this vehicle onto a solderless breadboard, but due to the many sensors and wires involved (over 35!) it became extremely messy and susceptible to cables coming loose. As a solution, and to aid with larger production, I designed a custom PCB. However, for small scales the use of soldered breadboard can also be very effective.</p><p>The PCB comes with several distinct components that can achieve the function of multiple breadboards in a tiny space, shown below:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*PgsW3Uy80myr9GFOijAQ9Q.png" /></figure><p>A lot of people ask me how to design a PCB, so a brief summary is you need to choose the components of your PCB (for example, you can see mine above), and read the datasheet for each component. Then, you just combine them together into a single board!</p><h3>Software</h3><p>The code that controls the motion of the robot is called the <strong>control system</strong>. This is an entire field of study (called Control Theory), but in the context of Science Olympiad there are two critical concepts: localization (also known as state estimation or observation), and PID. Localization has the task of determining the position (state) of the robot, and PID has the goal of getting the state to a desired value (the finish point).</p><p>I’ve already explained PID in <a href="https://medium.com/@vikramaditya.nishant/how-to-make-science-olympiad-robot-tour-2024-2025-330ceeaccec8">my Robot Tour blog post</a>, but essentially it is a loop that constantly runs with the goal of <strong>minimizing error</strong>. I decided to break the vehicle into three PID controllers: 1 to minimize axial (forward-to-backward) error, 1 to minimize lateral (side-to-side) error, and 1 to minimize heading (turning) error. Lets break down localization and these PID controllers.</p><h3>Localization</h3><p>The accuracy of localization is what determines the final accuracy for a properly programmed vehicle. For my vehicle, I was able to get the control systems to reach the target point within 0.2cm, but in reality I found the error to be around 0.5–1cm due to errors in localization, and I was able to get this down from ~3cm through various techniques. First, lets begin with the basics.</p><p>There are 3 sensors available to us: a position sensor for each wheel, and the gyroscope on the IMU, which gives angular velocity. First, the angular velocity is integrated to get heading. Then, this is used with the average distance traveled to get position:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*IQBlLQanARbvJtYMyq430g.png" /></figure><p>To convert the absolute position of the wheel (from 0–360 deg) to the relative position (e.g. number of rotations) the change between loops, called the delta, is calculated and normalized:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*BcoCefY_b58DJxSGGYGt5A.png" /></figure><p>I also incorporated a constant delay of 870 microseconds to keep the loop rate at 1kHz. Generally, you want your loop rate to be as high as possible, since the integration of the IMU heading and distance is like taking a Riemann sum:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/526/0*a8qInxUFckeyCDwz.png" /><figcaption>A nice visualization of why a faster loop rate is better (from <a href="https://www.math.net/riemann-sum">math.net</a>)</figcaption></figure><p>Unforutunately, sensors can only update their data at a limited rate, and most of mine were capped at 1kHz. This means that going faster than that is pointless since its just going to read the same numbers over and over, so I limited the loop rate to a constant 1kHz, to make the loop very consistent.</p><h4>IMU Bias Calibration</h4><p>The MEMs sensors used in this vehicle have a <strong>bias</strong>, meaning that when you read the angular velocity while the vehicle is not moving, it will give you some constant value. If you read <a href="https://invensense.tdk.com/wp-content/uploads/2021/03/DS-000143-ICM-20689-TYP-v1.1.pdf">the datasheet for the ICM-20689</a>, you will see that its up to 5 degrees per second (highlighted):</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*FLssxOOiHMSt5Tnlge5tJA.png" /></figure><p>To calibrate this out, I simply wrote a program that averaged out 2,000 samples of the gyro, and found the bias of my gyro to be 7.2 deg/s at around 27 C (which is outside their specifications!).</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*oTxSzQAogxKqC0kYYjQbmA.png" /></figure><p>This value is then just subtracted from every angular velocity measurement. One thing to note is that the gyroscope’s bias will change depending on the temperature, so you might also want to calibrate the IMU bias in a variety of temperatures with a regression, using the IMU’s internal temperature sensor, which could give more accuracy between your home and competition (where temperatures will be different).</p><h3>Axial Control</h3><p>For this control system I chose to use a PDF controller. The P term simply minimizes the error, but the D term is a little more interesting. Essentially, what it does is it subtracts the derivative of the position error, which is just velocity. So, by subtracting velocity, the controller drives the velocity to 0 at the end, which is what you want. If the D term wasn’t there, the vehicle would overshoot the end since it wouldn’t drive the velocity to 0, just the position error.</p><h4>Feedforward control</h4><p>On top of the P and D terms I added a feedforward term. Although the explanations for these are often very complicated, the basic idea is that you take something you know about the vehicle and tell it to the PID controller. For example, I know that the vehicle will not move without a power of at least 0.1. So, I add a constant offset of 0.1 in the direction the PD controller is trying to go.</p><h4>Gain scheduling</h4><p>Although the “correct” way to execute the profile of the vehicle is to pre-compute a trapezoidal motion profile and then use the controller to follow it, I just use the controller’s itself to create a smooth deceleration. This, however, has the problem that you need to make the gain (P value) very low for it to have a smooth deceleration. If you make this value low, then the error will be large. So I implemented gain scheduling, where you adjust the gain of the controller depending on the state of the robot. Using some mins, I implemented a function that will continuously increase the gain as the vehicle gets close to the end of the path, so that its able to smoothly decelerate but then aggressively reach the target position once it gets within 1.5 centimeters:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*g4wCMi2KmnqilF2QOry6ag.png" /><figcaption>A graph of the PID gain, made with <a href="https://desmos.com/">Desmos</a></figcaption></figure><figure><img alt="" src="https://cdn-images-1.medium.com/max/680/1*xVwBXNjZaEqXBLy98EOz5w.png" /><figcaption>The equation for the gain scheduling</figcaption></figure><p>In the end, I implemented it with the following code:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*fIM_iv8mQg2G3vzF8FRb5g.png" /></figure><h3>Heading Control</h3><p>I also employed a PD controller for this, for a similar reason — if there was just a P controller, there could be 0 heading error but the vehicle would be spinning to the left, causing it to overshoot and oscillate. In fact, this happened to me:</p><iframe src="https://cdn.embedly.com/widgets/media.html?src=https%3A%2F%2Fwww.youtube.com%2Fembed%2FdWaUfdkU_XY%3Ffeature%3Doembed&amp;display_name=YouTube&amp;url=https%3A%2F%2Fwww.youtube.com%2Fshorts%2FdWaUfdkU_XY%3Fsi%3DExG12bKHjXy0mEuD&amp;image=https%3A%2F%2Fi.ytimg.com%2Fvi%2FdWaUfdkU_XY%2Fhq2.jpg&amp;type=text%2Fhtml&amp;schema=youtube" width="640" height="480" frameborder="0" scrolling="no"><a href="https://medium.com/media/336793394228db8da8b294de537fe339/href">https://medium.com/media/336793394228db8da8b294de537fe339/href</a></iframe><p>With proper tuning, however, I was able to get it to work very well. The heading controller simply adds to a variable called “w” (omega, for angular velocity), which is added to the right motor’s velocity and subtracted from the left motor’s velocity.</p><h3>Lateral Control</h3><p>To ensure lateral accuracy, the horizontal position must also be controlled. I simply implemented a P controller with the horizontal error that would go into the angular error of the heading controller.</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*2L58t_pXDkXwmZChysrRfQ.png" /></figure><h3>Motion Profiling</h3><p>Minimizing wheel slippage is critical for high accuracy. When the wheels slip, the localizer thinks that the vehicle has moved when it actually hasn’t. A combination of slip and numerical integration error are the primary sources of error for the localizer. To minimize slip, the torque of each motor must be limited throughout the run.</p><p>First, I used an acceleration curve. This just interpolates the velocity over a period of time, effectively limiting acceleration:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*SfYV5MMws5hsEOUMwWyAEw.png" /></figure><p>I also scale the powers to each motor to maintain control authority for the heading and lateral controllers, even when the vehicle is traveling at full speed:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*SfYV5MMws5hsEOUMwWyAEw.png" /></figure><p>Finally, at some point I want to implement code that limits the torque to each wheel individually, to completely prevent slip.</p><h3>Software Aiming</h3><p>Since the vehicle already controls the lateral and axial distance precisely, aiming can be done completely in software to achieve much more linear results. Instead of moving the point the laser pointer aims at, you can just change the code while aiming at the same point and the lateral and axial controllers will take care of the rest.</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*ulqvMxxHDUiimfXJJmKLww.png" /></figure><p>However, since the controllers are very aggressive, theywould try to instantly correct, leading to slip. Initially, I just linearly interpolated the error, but this results in an abrupt turn at the discontinuities of the linear function. So, taking a page of the differential equations book, I decided to use e^x for aiming instead, using Desmos to tune the parameters:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*5wjNNcgYkcSeZzzIjjYkLA.png" /></figure><p>I must admit though, it is a little unsatisfying that it will never actually reach the correct aiming distance, but always be ~2% off (due to the nature of e^x). The implementation in code is as so:</p><figure><img alt="" src="https://cdn-images-1.medium.com/max/1024/1*5qUs86N_1BsACnOxwPZfBg.png" /></figure><h3>Conclusion</h3><p>It took a variety of techniques and control theory to implement an accurate, high-speed actively aimed and controlled Electric Vehicle. You can find my code on GitHub:</p><p><a href="https://github.com/Nv7-GitHub/ev25_nv7">GitHub - Nv7-GitHub/ev25_nv7: My source code for the Tektite Electric Vehicle 2025!</a></p><p>And I’ve created a template repository that shows how to read the sensor hardware and control the PCB:</p><p><a href="https://github.com/TektiteBiz/ev25">GitHub - TektiteBiz/ev25: The source code for the Tektite Electric Vehicle 2025!</a></p><p>Good luck in your competitions!</p><img src="https://medium.com/_/stat?event=post.clientViewed&referrerSource=full_rss&postId=3017b72f5790" width="1" height="1" alt="">]]></content:encoded>
        </item>
    </channel>
</rss>