While doing some basic experiments for the charger I also connected the RS-1501 servo to the AT90CAN. Driving the servo is simple using a PWM output that pulses every 20ms (50Hz). If the high time of the pulse is 1.5ms, the servo is in it’s center position. If decreased to 1.0ms is turns 45 degrees clockwise, if increased to 2.0ms it turns 45 degrees counter clockwise.
Although this works fine and the servo is very powerful, I came to the conclusion this will not work for SnakeBot. The main reason for this is that the speed can not be controlled, only the position. In SnakeBot there will be multiple segments connected together, while moving each segment makes a different angle compared to the others. When moving forward, each servo needs to make a different “delta-angle” in the same amount of time depending it’s position on the path to follow. For this, the speed must be varied since the time available for each segment reaching it’s new angle is exactly the same. Segments that must travel a larger angle must move faster than segments that must travel a small angle (or no angle at all).
This changes the whole mechanical, electrical and software concept of a segment…. Instead of a servo now a simple DC engine can be used with a large gearbox. This can than be driven with a H-bridge using a PWM signal so the speed can be varied. Conrad has a nice power 6V engine engine with a 50:1 gearbox attached, part number is 233130-89.
This engine delivers a torque of 588Ncm at 116RPM of the 6mm shaft while consuming a maximum of 1.8A. 116 turns per minute is about 2 turns per second, based on the servo selection a speed of 0.75sec per 60 degrees would be fast enough allowing a further reduction in speed by a factor of 9 and so increase the torque by a factor of 9 to 5292Ncm! The size of the engine is relative large, it’s length is about 9cm and the shaft is not at a 90 degrees angle. This means the engine should be placed vertical make the segment at least 10cm in diameter making SnakeBot look like an Anaconda! It might be an option to place it horizontal and use two 45 degrees angled gears, to be investigated later. Position can be measured using a potentiometer and an analogue input, there are types available with a metal shaft of 6mm so this can also be used for the axis of the last gear.
Since this setup will allow the speed of the engine to be varied, the question is how to inform the segment that it should move at what speed. In nature, the brains will make the decision to move forward to a target position with a target speed. The Primary Motor Cortex (PMC) will translate this decision into several signals to each individual muscle that is involved, this is done by sending an electrical signal called action potential (spike) through the nerve. When it reaches the end of the nerve it releases a neurotransmitter that triggers a series of events that finally result in contraction of the muscle. For skeletal muscles the force exerted by the muscle is controlled by varying the frequency at which action potentials are sent to muscle fibers. When the frequency changes, also the force changes.
This same principle can be used for the segments, however driving each segment with a frequency modulated signal using the RoboCAN interface would not be very practical. But if we take a look on how the pulses vary during a contraction we can define three phases:
- An increase in pulses from 0Hz to “x”Hz resulting in an increase of force
- A constant frequency of “x”Hz resulting in a constant force being applied
- A decrease in pulses from “x”Hz to 0Hz resulting in a decrease of force
These phases combined result in moving a part of the skeleton (for example a finger) from one location to another location, so frequency does not translate into position, it translates into force applied and so finally speed depending how much resistance is encountered. When resistance is encountered, the speed decreases and might totally stop. The brain monitors the speed and determines that the speed is not what was required and as a response it will increase the frequency of action potentials being fired resulting in a larger force applied until the speed is at the right level. With me so far?
If you are than you will agree that frequency is equal to torque applied by the engine in the segment. And torque translates into current. So basically the PMC determines what the speed must be, the segment starts driving current trough the engine that normally matches the speed. If resistance is encountered, the speed will decrease and the PMC should give the command to increase the current.
Here it might be more practical to have the feedback loop implemented on the segment itself. In practice the PMC has the target to “go there” no matter what resistance is encountered. Only when too much resistance is encountered it might be better to inform the brain it should try something else. Based on this concept the PMC sends the command to increase the speed to “x” in time “y”. The segments starts driving the engine by increasing slowly the current and continiously checks if the speed is reached, if not more current is applied. When it overshoots the current is reduced and so on. When the speed is still not met while the current is at it’s maximum than it should inform the PMC that the command can not be executed but continuous to hold the current until the PMC gets the instruction from the brain to release. When it overheats in the mean time than the segment can protect itself by releasing, same as in nature. Your brains can tell you to hold your fingers firm when hanging over a cliff but when they overload they will release no matter what the brain tells them to do.
What would happen if the segments reaches the speed and no new command is received from the PMC? In nature that would mean that the PMC did not determine what to do next so it continues to fire action potentials. The force applied only changes when the PMC decides to change the frequency, so in theory this would mean the speed is maintained until the PMC gives a new command. That would not work for SnakeBot since it can be that the PMC is occupied with something else. In nature this will not happen since than the PMC is truly multitasking while the micro controller implementation can not. As a result the absolute position of the segments angle is no longer guaranteed. If the PMC calculates the distance in angle to travel and splits this in three parts as mentioned above and sends it to the segment in one command it might work since at that time the segment can compensate by itself the actual speed required versus the given target speed. For example, if the ramp up takes a bit more time since more resistance was detected, the time at constant speed can be reduced a bit so the segment is back on track when it is time to decrease the speed.
Another option is that the PMC only gives command: go to that position “x” within “y” seconds which is in fact force since Force is mass times acceleration (Newtons law). If the delta in position is large, the acceleration required to be there in time “y” is high. Since the mass of SnakeBot is constant, the force is in direct relation to the acceleration and so the required delta distance in time. The segment than executes this acceleration using the same feedback algorithm to increase (or decrease) the speed until it reaches position x. If no new command is given, now it should continue to move at that speed. If the new position is than given with a small delay the segment can compensate the acceleration so it still reaches the newly required position on time.
This option might work the best since I feel this will make the most natural movement possible. The brain decides to move forward ”x” cm in ”y” seconds of time and informs the PMC. Based on the distance and the allowed time the PMC defines three time periods:
- t0-t1, increase the speed until position p1 is reached
- t1-t2, maintain speed until position p2 is reached
- t2-t3, decrease speed until position p3 is reached
This can than be translated into actual angles for each segment at times t1, t2 and t3 since it knows the current position of each segment and the final position of each segment based on the vector models for movement as defined before. Than it is a matter of sending combinations of (p1, t1) to each segment and when t1 is reached sending combinations of (p2, t2). When the time t2 is reached it becomes interesting, if the brain gives the command to move further forward at the same speed than the PMC should re-calculate above mentioned combinations and skip sending (p3, t3). At constant speed the combinations (p1, t1) are equal to (p2, t2) since no ramp up is required but that is still fine for the segments. (p3, t3) will be of a different value (slowing down) since the PMC was told to go to a specific location. If the brain than tells again before reaching t2 to move forward this will repeat itself over and over making the movement constant in speed.
Changing speed works similar, the brain simply changes it’s mind and informs the PMC at any given time of the new position and time. The PMC calculates again the combinations and the segments will adjust the speed accordingly based on the speed and position they have when receiving the new instruction. Simple, right? In all fairness, I’m not sure I’m right here but it seems logic enough to give it a try.
It makes sense to use units that available in the segment for defining position and time, this will prevent complex conversion calculations. For example, the angle is measured using a potentiometer connected to the reference voltage of the analogue to digital converter, the variable voltage from the potentiometer is than measured with a 10-bit resolution resulting in a value of $000 to $3FF. By defining the required angle in an value between $000 and $3FF instead of degrees the segment does not need to convert anything. The potentiometer has a rotation angle of 300 degrees, so $3FF/300=3.41 units per degree rotated. Since only 90 degrees are required the voltage from the potentiometer can also be multiplied by 3 giving a beter resolution of the measurement. This does however mean that the potentiometer must be adjusted in the segment so it is in it’s zero position when the angle is -45 degrees (0V, $000) and reaches 90 degrees turn when the segment is is at 45 degrees ((Vref/300)*90)*3=$3FF. With a bit of tuning the offset and multiplication factor this can reach 10 units per degree rotated.
For time, RoboCAN executes a maintenance routine every 25ms. Due to the resolution of the timer in the AT90CAN, this routine is called every 12.5ms and in the routine it is skipped every other call. When it is not executed, a call can be made to a function that is part of the segment that adjust the speed and checks for overload and if the target position is reached. As such, a time interval of 25ms is achieved and the CPU load is balanced: t=0ms: RoboCAN, t=12.5ms: the engine, t=25ms: RoboCAN, t-37.5ms: the engine and so on. By defining time in units of 25ms also here no calculations have to be made.
So, bottom line, the segment will only implement one command for movement. This will take two parameters: target position ranging from $000 to $3FF and target delta time in units of 25ms. The segment will try to execute this within its safety limits.
As mentioned, the engine will be driven using a PWM signal were the frequency is constant but the pulse varies in width. By experience, I find that most smaller engine like higher frequencies better than lower, using a 20kHz frequency has the additional benefit that you can not hear the engines making a high pitch noise.
A standard H-bridge uses 4 FET’s that each have a very low impedance, the lower the better since resistance means voltage drop which means heat being produced. Selecting a low resistance like 0.1E prevents the need to add heatsinks. I will use the FQP19N10 and the FQP27P06, in principle an overkill on allowed current but these are standard in my stock. Based on the the test schematic looks like this:
Q1 and Q2 are used for switching on and off the power supply, only one of them is open at the time and is kept constant and not driven by the PWM signal. Main reason for this is that it is not required, if a PWM signal is applied to Q5 or Q6 the engine connected to J1 can be varied in speed. Anything that switches causes electrical noise which should be prevented. Secondly, P-channel FET’s have different switching behaviors than N-channel FET’s, N-channel FET’s are typically faster, by keeping the P-channel always on the N-channel FET now controls the engine. Needless to say only the pair Q1/Q6 should be used or the pair Q2/Q5, so special care must be taken by software to prevent a short being created when reversing direction.
The coil in the engine tries to resist when power is applied, when the voltage is removed is resist again in the opposite direction. This results in voltages spikes of 20 to 30V on the power supply! To prevent these spikes, several counter measures are implemented. It starts with capacitor C1 which acts as a short for high frequencies, that is the best action, short it at the source. The diode D1..D4 are fast switching schottky diodes, they prevent that the reverse voltages generated by the engine coil destroy the FET’s. The filters created by R5/C2 and R6/C3 counteracts the coil, the values depend on the characteristics of the engine and are tuned by experiments. Measurements show that the spikes are almost eliminated by this setup, it is reduces to +/- 0.2V which is a job well done.
The 4 FET’s 2N7000 are used to drive the high voltage required for the engine FET’s from the low voltage AT90CAN outputs. When the AT90CAN is held in reset all pins are input, the gates of these 4 FET’s are all tied to ground using 10k resistors so that Q1/Q2 are closed and Q5/A6 are open. As a result both poles of the engine are tied to ground keeping it in brake mode and as a safe starting point until the software has bee initialized.
Resistor R11 will be used in a later stage to measure the current running through the engine.
Step one is to put this schematic on a piece of breadboard, step two is to write some basic code and step three is to build a mechanical setup for holding the engine and drive a larger gear that is connected to a potentiometer so position can be included to provide feedback.