Drive s2 in mm/s
My goal is to use a robot that can drive according to step function velocity and acceleration profiles to use as part of my physics classes. For example, I want a student to be able to tell it to go 15cm/s for 3 seconds, then accelerate at 2cm/s^2 for 5 seconds, then slow to a stop at -5cm/s^2, etc. Measurements with a motion detector (already in my lab) would confirm that it moved as specified (within some reasonable tolerances).
I just received an S2, and my initial tests programming it with the S2GUI indicate that its capable of doing everything I want. As far as I can tell, the GUI only allows me to drive it by specifying speeds as a % of the top speed rather than in mm/s or cm/s. So several questions.
Matt
I just received an S2, and my initial tests programming it with the S2GUI indicate that its capable of doing everything I want. As far as I can tell, the GUI only allows me to drive it by specifying speeds as a % of the top speed rather than in mm/s or cm/s. So several questions.
- Has anyone written routines to drive the S2 in mm/s or cm/s already? No need for me to reinvent the wheel if its already been done.
- If its not been written already, I assume I'll have to use spin or propbasic or C or assembly language (hopefully not) or some other lower-level language to get access to the input from the wheel encoders as feedback to adjust the output level to the motors. Would spin be the right language for this job? I would just like confirmation that it will have the features I'll need before investing the time to learn it, or else get directed to the right language to accomplish what I would like.
- By my calculation, an 8cm diameter wheel rolling at 80rpm should go about 33cm/s. Top speed using the GUI seems to be only about 19cm/s. Am I missing something obvious here?
Matt
Comments
As you noted, the S2 speeds in the GUI are referenced to a % of top speed, which is relative. Max speed depends on battery voltage level as well as individual motor speeds. I'm betting that the encoders (in straight line mode) make sure that the total wheel pulse counts match (as opposed to simply matching RPM), so top speed is limited by the weaker motor.
All this to say that that the time domain factors for controlling absolute speed and acceleration may require different code than the GUI's capabilities provide. The Propeller is a very capable controller, and the necessary timing functions & accuracy are there. I'm sure it can be done if it hasn't been done already.
PhiPi???
It's been a few months since I've written a program for my S2.
The GUI creates a Spin file when it loads the program on to the S2. This lets you see the Spin equivalent of the GUI program. So you can combine the GUI subroutines into your Spin code if you like.
I found the GUI much too limiting. I mainly use the GUI for it's sound effects. I write a simple program using the GUI and copy the methods from the generated Spin code into my own program.
I was just looking through the S2 object. I find a "set_speed" method, but the speed value can only be from zero to 15. Not very useful for your purposes.
I'm sure there's a way to control the S2's speeds with more precision, but it might take a bit of work to figure out how to do it (or a quick answer from Phil).
The problem I see with using the S2 for your acceleration experiments is I think you'd need to use very low acceleration values in order to reach the S2's maximum speed very quickly.
Was the 80rpm measured with the S2 on the ground? I imagine a free spinning S2 wheel could rotate faster than a wheel under load.
The 80rpm was just from the S2 specs, which say 20 to 80rpm. That may well refer to no load speed. The specs don't say as far I can tell.
It would have more cool factor for my students if the top speed were faster, but first I need to get the basic functions working. One thing at a time.
When I saw the drive function in spin, it made me wonder whether spin had direct access to the encoder readings and PWM output levels, or whether those could only be accessed at yet a lower level.
Matt
Welcome to the Parallax forum!
Unfortunately, the news I bring is not what you hoped to hear. The S2 object provides for 16 discrete speeds, and the acceleration profile is hard-coded. Because the motors are driven directly from the batteries, the top speed was calibrated for the lowest battery voltage, along with some margin. That's the reason it's slower than you might otherwise get with fresh batteries and a full-on drive signal. The idea was to provide a uniform -- and simple -- experience for novice programmers.
All of these things can be changed, of course, but it would require some programming at the assembly level to modify or rewrite the motor driver.
-Phil
What's the best reference and/or tutorial to help me learn the propeller assembly language? My previous programming experience is in C, Python and PHP, and I can quickly pick up the basics of other languages. This will be my first experience trying to program a parallel processor. I would like to have something that works by the beginning of next fall's school year. Should I be asking for programming help, or is it reasonable to expect that I can pick up programming at the assembly level over the course of a summer's work?
I won't mislead you into thinking that such a project will be easy. It took me several months to write and debug the S2 object. You'd basically be doing that all over again.
-Phil
I have several links to good sources to learn to program the Propeller in post #3 of my index (see my signature).
My Mecanum wheeled robot (see post #2 of index) is another example of using encoders to control motor speed. It's not nearly as well done as Phil's S2 object, but it might help to see another approach.
The Propeller is a really useful microcontroller to use. I think you'll find lots of other uses for it once you start learning how to program it.
Questions in the Propeller forum are usually quickly answered so don't get stuck on some aspect of your program for very long before asking a question there.
I haven't used C on the Propeller myself yet.
I use C with some other microcontrollers, but IMO, Spin and PASM have been the best option for the projects I've done.
IMO, Spin and PASM would likely be your best option as well.
There my be a few who would disagree with my opinion, but I think most other forum members would agree with me.
Spin and PASM are really the Propeller's "native" languages and I think there are more programming examples in these languages for the Propeller than there are for the other language options.
I think it would also be easier to get help with a Spin/PASM programming problem than getting help using a different language (I'm not sure about this since I've never tried getting help programming the Propeller using a different language).
I've started reading and deciphering the motor driver code from the s2 object. It seems my tasks will be to increase the speed resolution from 16 to 256 speeds, write a PID algorithm to use encoder feedback to regulate speed in mm/s, write some higher-level routines for constant acceleration motion, and possibly add a 10-12 volt voltage regulator to give consistent performance at higher speeds. I haven't decided yet whether the programming tasks mean writing a new motor driver from the ground up or just modifying the existing driver. I'm also hopeful that the last chapter of Harprit Sandu's book will be helpful. I've just started reading it.
And now for my question. What is the idler wheel? Why does it have an encoder? And why does it seemingly need to be handled differently than the other sensors? (i.e. the code often seems to check whether its dealing with the idler wheel and then process it with some extra bit of code. Its treated as an exception.)
I'll periodically post my progress and questions here.
Matt
Duane's right: the tail-wheel encoder is only used for detecting stall conditions.
You should not need to add any kind of voltage regulation. The S2's built-in A-to-D circuitry gives you enough info regarding overall motor current draw and battery voltage to get the high-speed consistency you're after. Don't try to overdo it, though, because there's a separate hardware overcurrent detector that will cut out until reset if the motors draw too much current.
-Phil
Never mind. I see it. IR LED shining horizontally across to IR receiver and lined up to shoot through the holes in the wheel.
Propeller pin 12 output controls idler IRED, but IR receiver for idler encoder goes straight to ADC unit and from there back to propeller through the ADC cog?
Propeller pins 13 and 14 receive left and right motor encoder readings as inputs, which are read and processed by the motor cog and not the ADC unit?
(Presumably there's some electronic signal processing between the motor IR receivers and the pins, so no need for ADC.)
S2 uses at most 5 cogs = cog0 + adc cog + motor cog + tones cog + microphone cog?
If my above understanding is correct, might it be worthwhile for me to split the motor cog into two cogs, one to handle the encoders on its counters and process that information into position, velocity and acceleration information, and the other to handle PID and motor output?
Duane and Phil, I really appreciate your interest in my project.
Matt
Yes to all.
That might make your job a little easier.
-Phil
midler_addr long 0-0
And why pass the idler count hub variable to the motor hub this way:
midler_addr := @Results + (CNT_IDLER << 1)
rather than just as an offset from @motor_cmd which is passed through par?
I see that Results is passed into the adc hub the same way, even though its par remains unused. There must be some advantage gained by doing it this way, but I can't see it.
I do see that the left shift is just accounting for the difference between words and bytes.
Finally, how does this work?
put_stat_ret
clr_stat_ret ret
As I read the manual, if put_stat is called, the return address is stored in the put_stat_ret instruction, which has no ret command. Then the clr_stat_ret instruction has a ret command but no address. Is the return address loaded somewhere else when the put_stat_ret instruction is executed? And then apparently whatever is there is not cleared when clr_stat_ret is executed with no address?
-Phil
I'm amazed and impressed! I've just figured out that you do all of your encoder sampling, bookkeeping and calculations in about 1/8th of a 20kHz PWM period, and then wait to send the next pulse. Wow! I never imagined that was possible before reading your code. I'm used to slower systems where the feedback has to happen on the time frame of many pulses. A quick calculation shows that the longest assembly program that could run in a cog would be 496 instructions taking 2000 clocks or so, still less than the 4000clock period of a 20kHz pulse. If I can keep things simple enough to fit in those limits, assembly plus an 80MHz clock gives a lot of advantages over trying something more complex distributed over several cogs. This realization is now unlocking the rest of the code so that its all making sense.
More soon.
Matt
Why the factor of 2 in the left and right distance counters? As in the following from :do_cmd
shl right_dist, #1 'Double it.
min right_dist, #1 'Have to move at least one encoder pulse.
Also still curious about how put_stat_ret works when the ret command is on the next line (see a couple posts above). Its obvious what it does, but I don't understand how.
Finally, I still have a velocity question. Motor stats confirms that with max_vel = 15, the encoders record 75 counts in 0.1 seconds or 15 counts each 20ms sampling period. Setting max_vel = 6 results in 30 counts per .1s, etc. As I read the program, this is exactly what your proportional feedback on the velocity should achieve. However, 75*.491mm/.1sec = 368mm/s, but the scribbler moves at almost exactly half that speed. I'll do more tests tomorrow. I'm willing to believe that it can go faster, and I'll test that soon, but regardless I still think I'm missing something basic about the speed calculation. And it seems that the .491mm/encoder count must be pretty accurate, or the turns would be way off when drawing.
Still would like an explanation of how the put_stat_ret works without a ret command on the same line. Here's some more detail.
Elsewhere in the program, call clr_stat results in all the above code executing and control returning to the next command after the caller.
And call put_stat executes just the code below put_stat and then returns to the caller just as if the last command had been put_stat_ret ret. But its not. Since the return address is supposed to be stored in the s-field of the put_stat_ret command, how does the ret command on the next line "know" to look on the line above for the return address. So far as I know, there is no dedicated jmp register for the return address to be copied into.
I think this is the code in question?
"put_stat_ret" is an alias for "clr_stat_ret". The code associated with a label doesn't have to be on the same line as the label.
The routines "clr_stat" and "put_stat" have separate starting points but both end with the same "ret" statement.
If one wanted to label a "blank" line of code, you'd need to do this:
If the blank line was within some PASM code, you'd do this:
"nop" stands for 'Not an OPeration".
If just a label is entered, the compiler will associate it with the next valid line of code.
It makes it easier to read code when code blocks are used. The very talented Phil Pilgrim made a little tutorial here:
Did a top speed test yesterday when I came to the part of the motor driver code where I really needed to know it. Seems like it is geared for just a little under 20cm/s. I just put the motor PWM pins to full on and it went a little less than 60cm in 3.0 seconds. Here's my code.
I'm not sure what motors the scribbler2 uses, but does the company make a gear motor with the same physical dimensions but a different ratio?
Matt
One key was developing a set of units and conversion factors, so that each value could be stored as an integer, but with sufficient range and resolution. With conversion factors in hand, I could easily convert to compatible units whenever necessary for a calculation and thereby avoid floating point completely.
distance units: 1encoder count (c) = .247mm (approximately, may be adjusted), 20,000Wc = 1c, 20,000Zc = 1Wc, 400,000,000Zc = 1count
acceleration units: 1counts/sec^2 = 1Zc/P^2'
Phil's motor driver coordinates the wheels by each pulse to see whether one wheel is ahead of the other (on a proportional basis if unequal) and drops pulses for the leading wheel (i.e. setting the pulse width to zero until the other wheel catches up). This is simple, and incredibly effective at driving the S2 straight. I imitated this by creating a "phantom wheel," whose position is updated each pulse. A phantom counter is incremented every time the phantom wheel travels one encoder count. By doing the calculation each pulse and using Zc and pulses for distance and time units, the calculations for the phantom wheel involve only adds, subtracts and shifts. The phantom counter is incremented when accumulated Zc's exceed 400,000,000. As I don't want to slow down the phantom wheel, I bump (set to 100% pulse width) or drop pulses of the dominant wheel to coordinate it with the phantom. The non-dominant wheel is compared directly to the dominant one just as Phil does, although pulses are both bumped and dropped. This alone works incredibly well, even if the nominal pulse width is set to 50% and just left there.
The number of bumps and drops in a 20ms period (400 pulses) is already an indicator of how much the nominal pulse width is off the desired value. If the pulse width were perfect, presumably it would not be necessary to bump or drop any pulses. So instead of doing a standard PID that would have involved divisions to calculate velocities, I just use the number of bumps and drops as my error terms to adjust the nominal pulse width, with an emphasis on having as few bumps as possible.
I also added a little bit of code to increase or decrease the pulse width a bit in anticipation of the acceleration during the next 20ms period. With the anticipation in place, there are typically less than 20 total bumped or dropped pulses out of 400 in every 20ms period. The acceleration is very smooth, and has good performance at low speed. I've attached a screen shot from my motion detector operating at 20Hz.
Phil's code was a beautiful example to follow, and I consider myself lucky to have learned assembly using that example. The power of having pulse-by-pulse feedback and control definitely trumps a more mathematically sophisticated, but slower algorithm. Phil, please let me know how you want to be credited. Right now, I've just included a statement that my code is strongly based on your original S2 motor driver. Perhaps more is in order.
I suspect that propeller assembly is easier to learn than other assembly languages, although I don't know for sure since I don't know any other languages or processors. So thanks to the propeller design team for that as well.
I've titled all my spin functions by adding _mms at the end. In addition to completing the detail work, as stated above, I plan to write a wrapper for my low-level spin function, run_motors_mms that mimics the existing run_motors function, so that my driver can be incorporated back into the S2 object. That way, I won't have to re-write all the drawing routines, and I can use the S2's other capabilities along with my motor driver.
Thank you to both Duane and Phil for tuning in and answering questions along the way. There's more to my project now, particularly writing graphical GUI's for my students to interact with the robot, but writing this driver was the part I was least confident of being able to complete. The rest will just take time.
Matt
Congratulations on your success with the program! You've really put a lot of work into it, and I'm thrilled that you were able to meet your objectives. (As to crediting my work, what you've proposed is more than sufficient.)
Now that you're already done with the project, what will you do with the rest of your summer?
-Phil
Yes, but some can be hard to track down. I may be able to assist. Are you looking for gearmotors with higher (slower RPM & more torque) or lower (faster RPM, lower torque) reduction ratios? I'm assuming you'll want the latter for more gradual acceleration and higher top speed.
Well, still more to do on this project, plenty for a summer. Then there's a summer course to teach and family vacation. I'll be busy. But I've really enjoyed the work so far. The constraints of assembly language require some cleverness and puzzle solving, and its satisfying to see the effort pay off with better performance.
Fortunately I was able to get my other robot working in a satisfactory manner without understanding Phil's code.
I'm hope you'll be willing to post your code. I'd really like to see what you've done.
It's always nice when a robot will do what you want it to do and not just what you tell it to do (since these often end up being two different things).
Come on, Pal: you don't fail until you quit! Up until then, you're empirically determining the differential coefficients of the dynamic performance envelope.