EDU Mini Controller & 2005 Nav Code
 

I'm attempting to implement Kevin Watson's 2005 Nav code on our EDU Mini controller to give my team a small scale platform to learn about PID contollers and to expirement with their own code. (this will be the first year we attempt to use encoders & gyros) I basically took the EDU code as written and added pid.c/h, robot.c/h, encoder.c/h, gyro.c/h, commands.h as well as adc.c/h. I updated the ISR's for adc & encoder.c. I have also updated all the constants to apply to the smaller bot. The software compiles and runs but I'm having a few problems.

1) I put CMD_GYRO_BIAS at the top of the list and it seems to work fine. However, it seems to go haywire after that. Sometimes it steps through my command list just fine and other times it jumps out of the loop to show "unknown command restarting ...". It's as if the rc variable is getting randomly set, and when it moves to the next command it does not recognize it (even thought it did the last time it ran). I've got the NULL command as the last one.

2) The PID algorythm does not really monitor the error between the left and right motor speeds and so the bot won't go straight. This is a known shortcomming and I've read all the posts and some friendly folks are helping with code examples to fix that. However, I can't even get the bot to go straight when contolling on velocity. No mater what setting (pwm value) I use, my left motors alway go faster than my right motors. I'm wondering if there is some inherant issue with either the EDU mini controller or the motors?

Any advice would be appreciated! Thanks folks!

Jeff Austin
Mentor - Team 372

Re: EDU Mini Controller & 2005 Nav Code
 

Possibly a memory overwrite. You might generate a .map file and check it to see where your arrays are falling relative to the memory location of rc (i.e., those at lower addresses can overwrite higher addresses), and print the values of rc to see if it's really random. Make sure rc is static or global.

For the unbalance power of the left vs. right drive I'd test both at max pwm (forward and backward) and then begin slowing the faster side until the sides are balanced. That'll give you a more concrete measurement of the magnitude of the problem. The motors, chain, wheels all contribute to mismatched power, but it's not usually so severe. This is actually one of the exercises I put the new programmers through, by purposely unbalancing things (through s/w or mechanics) and having them rebalance in software.
P.S. The difference is usually proportional or close to it, so you can apply a percentage of the max correction at slower pwms.

Re: EDU Mini Controller & 2005 Nav Code
 

I still own and use one of the old Robovation kits, and I've observed that there is in fact a HUGE difference in the forward and reverse speeds of the motors. If this is what is causing the problem in your case, when you run the robot backwards the right motors should now rotate faster than the left motors. I think I've had to cut back on the speed of one motor by as much as one half to compensate.

The PID loop in the navigation code probably isn't designed in a way that would allow it to compensate for this much a speed difference (between the expected and actual speeds for forward and reverse), so your best bet is to follow Mark's procedure and slow down the motors for the appropriate direction of rotation in the code, after having run the PID loop.

Re: EDU Mini Controller & 2005 Nav Code
 

Yes you are both (Pat & Mark) correct relative to the speed differences of the EDU motors in fwd & rev. I have been able to slow the one side down in the code to allow it to track fairly straight. I just added a 60/100 factor at the end of the PID routine to the right sides pwm value and it works ok now.

Mark,
On the other problem I too suspect some kind of memory over-write or over-flow problem. I printed out the rc values and they show up as either 1 or 0 99.9% of the time. But occasionally a random 485 or 2046 shows up and then things go south. I'm kind of a novice with the PIC and C. I not familiar with how to create a .map file to check my memory allocations. Is there a website that explains how to do it? Should I use the "extern int" identifier for rc to ensure it's a global variable somewhere outside the robot_command() function? (if that's it, I'm curious why Kevin did not do it that way to start with).

Thanks guys for all your help!

Jeff

Re: EDU Mini Controller & 2005 Nav Code
 

The memory map can be generated by MPLAB whenever you compile your code by going to:
Project -> Build Options... -> Project
then click on the "MPLINK Linker" tab,
then click on "Generate map file".
After you build your project look in your project directory for a text file ending in ".map". You can view it in any text editor.

The *.map file has four parts:
1) Section Info
2) Program Memory Usage
3) Symbols - Sorted by Name
4) Symbols - Sorted by Address

Section 2 is pretty useful as it gives the utilization summary for the Program Space directly, but 90% actually represents full as far as the user is concerned.

Section 4 shows the Data Space utilization that you want (ignore for now the Program Space – see column marked “location”). It provides the starting address of each declared variable. The ending address can be either assumed to be where the next variable starts or calculated from the known size.

-----------------
rc is fine declared the way it is. My fault for snapping off a quick afterthought and not checking the code first.
rc's value is the status returned from each of the driving functions, so the problem is probably within one or more of the driving functions.
Identify the function(s) being executed when it returns the bad values. It could be as simple as just not being assigned before exiting the drive routine.

If it's an overwrite issue it'll either be within the drive function or possibly with any debug printfs that occur before you check rc to proceed to the next command.

As regards the printf, the EDU code because of it's age is probably not using the newest C compiler version, but an old IFI version printf_lib.c/h. That old version has several limitations you should read about in the printf_lib.c header comments. I only mention it because one of the limitations is it will overwrite if you attempt to printf a long message (> 80 characters). I doubt this is your problem because the bad rc numbers you quoted are not the ASCII characters you'd expect if this were happening.

Re: EDU Mini Controller & 2005 Nav Code
 

How many encoder ticks per second are you generating and how fast is the ADC sampling the gyro?

-Kevin

Re: EDU Mini Controller & 2005 Nav Code
 

Mark/Kevin,
I've been sent out on a business trip. I won't be back til later this week so please stay tuned!

Mark,
Thanks for the .map info, I'll give it try.

Kevin,
ACD sampling - I have been trying it at 200, 400 and 800 HZ. It does not seem to matter.

Ticks/sec - I'll have to confirm the numbers when I get home but I'm using the 128 Grayhill 61K encoder. At ~500mm/sec with 60 mm dia wheels and a 1 to 1 ratio between the encoder gear and the wheel gear, it's about 340 ticks/sec I'm guessing.

Re: EDU Mini Controller & 2005 Nav Code
 

Another possibility since you are using ISRs is that a compiler generated temporary variable (or MATH variable) being used by your main code is being overwritten during the execution of the ISR. If this is happening, then your code can behave very unpredictably. As a quick check, look just before the "void InterruptHandlerLow ()" routine in the user_routines_fast.c file, there should be a line similar to the following:

#pragma interruptlow InterruptHandlerLow save=PROD, section(".tmpdata"), section("MATH_DATA")

At a minimum, the PROD & ".tmpdata" section should be in the #pragma statement. While Kevin's code contains both of these, the default EDU code does not. The MATH_DATA section is probably not needed but for testing purposes it won't hurt to add it just in case (except for some additional latency executing the interrupts). Kevin's code usually includes multiple versions of this line, so you may be able to just uncomment the correct one. This may not help, but I thought it was at least worth a mention.

Mike

Re: EDU Mini Controller & 2005 Nav Code
 

Mike Bortfeldt,
This is what was in the position you noted:
#pragma interruptlow InterruptHandlerLow save=PROD /* You may want to save additional symbols. */

I commented that line out and put this line below it:
#pragma interruptlow InterruptHandlerLow save=PROD, section(".tmpdata"), section("MATH_DATA")

The randomness has gone away and the software steps through my command list without jumping around! Thanks for the help!

Now I need to work on the turning. The gyro code does not seem to be working now... stay tuned for more questions!

Re: EDU Mini Controller & 2005 Nav Code
 

Nice catch.

Re: EDU Mini Controller & 2005 Nav Code
 

Ok guys, here is my gyro "issue":

Here is an extract from gyro.c (I added all the printf's)
Specifically it is from the Process_Gyro_data() function:

else
{
// get the latest measured gyro rate
temp_gyro_rate = (int)Get_ADC_Result(GYRO_CHANNEL) - gyro_bias;
printf("gyro_bias = %d\n", gyro_bias);
printf("temp_gyro_rate = %d\n", temp_gyro_rate);

// update reported gyro rate and angle only if
// measured gyro rate lies outside the deadband
if(temp_gyro_rate < -GYRO_DEADBAND || temp_gyro_rate > GYRO_DEADBAND)
{
// update the gyro rate
gyro_rate = temp_gyro_rate;
printf("gyro_rate = %d\n", gyro_rate);
// integrate the gyro rate to derive the heading
gyro_angle += (long)temp_gyro_rate;
printf("gyro_angle = %d\n", gyro_angle);
}

Here is some sample output:
gyro_bias = 1944
temp_gyro_rate = -820
gyro_rate = -820
gyro_angle = -1
gyro_bias = 1944
temp_gyro_rate = -788
gyro_rate = -788
gyro_angle = -1
gyro_bias = 1944
temp_gyro_rate = -766
gyro_rate = -766
gyro_angle = -1

I was worried that the gyro was not working or the ADE was not working but they appears to be working fine. All the calculations seem to be working fine until it gets to the "gyro_angle += (long)temp_gyro_rate;" line of code. All that the variable gyro_angle will ever be is -1 or 0. I can move the gyro and get it to change from 0 to -1 and visa versa. Any ideas? I'm guessing my C abilities are getting in the way?

Re: EDU Mini Controller & 2005 Nav Code
 

I'm not sure about this, but I believe that the EDU controller shares pins for digital and analog inputs. You need to set the input your gyro is plugged into to be an analog input in user_initialization() which is user_routines.c.

Good Luck!

Re: EDU Mini Controller & 2005 Nav Code
 

Considering he gets a valid gyro bias and rate, the problem is not the reading.

jaustin,

gyro_angle is declared as a long in Kevin's code. The comments section of printf_lib.c states that you should use %lx, but I have never tried that.
What will work (and what Kevin's code does in another section) is to type-cast the long variable. You should print it like that:
Depending on what compiler version you're using, it should be \r or \n.
Anyway, on your code you're printing three times inside the Process_Gyro_Data function. This is an interrupt service routine, and it should be processed as fast as possible. Printf takes a lot of time, and it is unwise to use it three times (actually, to use it at all) inside an ISR.
Check user_routines.c on Kevin's code, there are some examples that will print exactly what you want without "side effects".

Re: EDU Mini Controller & 2005 Nav Code
 

Have you run the latest gyro code on your controller without any modifications other than setting the gyro type in gyro.h and compiling? If so, did it work? If not, can you give it a try?

-Kevin

Re: EDU Mini Controller & 2005 Nav Code
 

Re-reading your post, I realized that you're reaching deep into the code and looking at half-baked data. The variable gyro_angle is just a summation of all of the preceeding gyro rate measurements and won't make sense until you examine the return value of Get_Gyro_Angle( ). Here's the function:

long Get_Gyro_Angle(void)
{
// Return the calculated gyro angle to the caller.
return(((gyro_angle * GYRO_SENSITIVITY *5L) / (ADC_RANGE * ADC_UPDATE_RATE)) * GYRO_CAL_FACTOR);
}

I only calculate the angle "on demand" to keep the load on the microcontroller to a minimum.

BTW, I've spent quite a bit of time in Mukilteo -- mostly waiting for the ferry to Whidbey Island <grin>.

-Kevin

Re: EDU Mini Controller & 2005 Nav Code
 

Kevin,

Although you probably know already, I tried your edu_gyro code out on the VEX with ver 5 firmware. I have the ADXRS150EB attached and the software worked flawlessly.

Jon Mittelman
Team236

Re: EDU Mini Controller & 2005 Nav Code
 

Kevin,
Yep, I've spent some time in that ferry line myself! It's a great time to read my Servo Magazine!

Yes, I have run just the gyro code on my EDU controller and it worked fine. When I rotated the bot by hand, the gyro angle changed the apprpriate amount. So I know the hardware is all hooked up correctly. BTW I'm using the ADXRS150EB.

I knew I was digging deep into the code because I was trying to figure out why the angle (as read by a printf statement of "heading" in cmd_turn() in robot.c) was only showing a -1 or a 0.

Could it have anything to do with the fact that in the stand-alone gyro code, you have us read the gyro every cycle in the fast loop i.e. Process_Gyro_Data() is in the fast loop as directed in gyro_readme.txt? In the FRC code, you only read the gyro "on demand". They way I have it currently set up, it's doing both - maybe I'm swamping the controller?

prograid,
Yes, You are right about the sharing issue. I checked my code and I'm declaring IO1 = INPUT in User_Initialization() in user_routines.c.

Manoel,
Thanks, I figured I was too liberal with the printf's! The problem was present though before I put in the printf's so I don't think they are a cause. But you are right that maybe it's not telling me the whole story...

Everyone,
I REALLY appreciate your time and effort!!!

Re: EDU Mini Controller & 2005 Nav Code
 

Can you zip-up your build directory and e-mail it to me?

-Kevin

Re: EDU Mini Controller & 2005 Nav Code
 

Hi Jon,

Most, if not all, of my code will work with the Vex. I don't really advertise it because Vex is more of a commercial enterprise and I'd rather spend my time supporting the FRC.

I remember that you had some problems with your encoders this past build season. Did you get the problem resolved and your 'bot working?

-Kevin

Re: EDU Mini Controller & 2005 Nav Code
 

Yes, we had 4 encoders working flawlessly. I thought I had let you know last year! BTW, FIRST is helping to promote the VEX as a way for teams to
a) test the robotic waters
b) allow programming teams to develop code in parallel with the build phase
VEX is certainly cheaper than the Edubot. If it takes a commercial enterprise to help promote FIRST, then so be it!

Regards and thanks again

Jon

Re: EDU Mini Controller & 2005 Nav Code
 

Hi guys,
I sent our code to Kevin and nothing jumped out at him so if anyone has more ideas I'm all ears! My variable gyro_angle will never been anything but 0 or -1. Turning one direction makes it 0, the other direction makes it -1. No matter what the temp_gyro_rate variable is, the result of the following line of code is always 0 or -1.

gyro_angle += (long) temp_gyro_rate;

Re: EDU Mini Controller & 2005 Nav Code
 

Post the full project zip for us to take a look at.
Bugs draw programmers like honey does flies.

Re: EDU Mini Controller & 2005 Nav Code
 

Ok Mark, here it is. I had to delete some things from the project folder to get under the size limit. Let me know if I deleted anything you need to see! Thanks again for all your input!

Jeff

Re: EDU Mini Controller & 2005 Nav Code
 

Jeff,

I know this was mentioned earlier in the thread (by Manoel), but I'm thinking your code may be working fine, but the print statements are misleading. The %d format expects a 16 bit integer and the gyro_angle variable is 32 bits. The compiler is going to place all 4 bytes on the stack when it calls the printf routine and I think what you are seeing is the display of only the high 16 bits of the 32 bit integer. Until the angle gets large enough (positive or negative), the values displayed will only be 0 (0x0000) or -1 (0xFFFF) since the lower 16 bits are ignored. If you haven't already, try modifying the printf to the following and see if that gives you any different results.

printf ("gyro_angle = %d\n", (int) gyro_angle);

Mike

Re: EDU Mini Controller & 2005 Nav Code
 

Jeff,

MPLAB wants the file EduCode.mcp.

Re: EDU Mini Controller & 2005 Nav Code
 

You don't really need that file.
Just use the Project Wizard to make a new project from scratch.

The printf is wrong because of the non-standard printf limitation and is causing some of your grief. Put in what Mike suggested and you'll see a difference. %lx will work to an extent, but it too has a bug that will return bad values on odd occasions. Of course with (int) if gyro_angle grows larger than 32,767 you'll suddenly seem to be printing out negative numbers.

Here's the output I'm getting.
Note I did change your prints to a single line per pass, but I didn't adjust the code to match the gyro I had handy.

gyro_rate = 2242, gyro_angle = 0, (int)gyro_angle = 2242
gyro_rate = 2313, gyro_angle = 0, (int)gyro_angle = 4555
gyro_rate = 2372, gyro_angle = 0, (int)gyro_angle = 6927
gyro_rate = 2424, gyro_angle = 0, (int)gyro_angle = 9351
gyro_rate = 2459, gyro_angle = 0, (int)gyro_angle = 11810
gyro_rate = 2480, gyro_angle = 0, (int)gyro_angle = 14290
gyro_rate = 2493, gyro_angle = 0, (int)gyro_angle = 16783
gyro_rate = 2495, gyro_angle = 0, (int)gyro_angle = 19278
gyro_rate = 2490, gyro_angle = 0, (int)gyro_angle = 21768
gyro_rate = 2471, gyro_angle = 0, (int)gyro_angle = 24239
gyro_rate = 2449, gyro_angle = 0, (int)gyro_angle = 26688
gyro_rate = 2425, gyro_angle = 0, (int)gyro_angle = 29113
gyro_rate = 2399, gyro_angle = 0, (int)gyro_angle = 31512
gyro_rate = 2373, gyro_angle = 0, (int)gyro_angle = -31651
gyro_rate = 2335, gyro_angle = 0, (int)gyro_angle = -29316

Re: EDU Mini Controller & 2005 Nav Code
 

Here's a modified version of your project that print's the long value correctly.

• I removed the old IFI printf_lib.c/h files and changed everything to use the mcc18 standard printf instead. Use #include "stdio.h" in whatever files you want to use printfs. Instead of using "n" as a carriage return you now have to use "r". Finally, you need to set MPLAB to ignore type 2066 warnings, or just ignore them when they pop-up during builds.
• I added a work-around for the printing of the long value. If you continue to use this it would be cleaner to hide the details in new print_routines.c/h files. Everything I added in gyro.c is marked with "// MLM".
• I did change some of your printfs to remove the "n", so you might want to add those back in as "r".

Re: EDU Mini Controller & 2005 Nav Code
 

Guys,
I can't thank you all enough! The 0 and -1 problem was keeping me from looking in the right place for the real problem. Your help showed me the gyro code was doing what it was supposed to do (as you all suspected!). Manoel, I'm sorry I did not quite get your point in your earlier email. You were right on! Mark, thanks for tweaking the code!

The REAL problem is more embarrasing. When I used the CMD_TURN, the bot would not stop turning, it would just keep going as if the pid routine was not watching it. It turns out my lefts and rights are crossed somehow. When I changed the minus signs in front of TURN_SPEED in the cmd_turn() function, it now works! It kept going because it was looking for a turn in the wrong direction.

Thanks again guys for all your patient help!!

Jeff

Re: EDU Mini Controller & 2005 Nav Code
 

Cool. I hope to have a new, cleaned-up version of the scripting code before kick-off. I'll incorporate the latest gyro and encoder code too.

-Kevin

Re: EDU Mini Controller & 2005 Nav Code
 

It's more fun if you don't have to debug the debug code.;)

Re: EDU Mini Controller & 2005 Nav Code
 

[quote=Mark McLeod]The memory map can be generated by MPLAB whenever you compile your code by going to:
Project -> Build Options... -> Project
then click on the "MPLINK Linker" tab,
then click on "Generate map file".
After you build your project look in your project directory for a text file ending in ".map". You can view it in any text editor.

Mark,
I've been using the .map file recently thanks to your help. I'm having another problem with my rc variable and when I try to look it up in the .map file (in the symbols-sorted by name) it's not even listed. Shouldn't it be there somewhere or am I looking in the wrong spot?

Re: EDU Mini Controller & 2005 Nav Code
 

.map will only list variables with a fixed, permanent location. Those that are declared to be static or are declared within a project or header file, but outside any function.

The way rc is declared (within a function and not static -- int rc) means it doesn't persist between calls to that function, so it doesn't have a permanent location or home so to speak.
You'll also notice that rc is actually declared 14 or so times, but these are all really different variables, not the same one. By design the code happens to reuse the name just to make it easier for a reader to trace the data flow.

Not having a fixed "home" none of these variables will be explicitly called out in the link map, instead they are dynamically allocated only when the function is active. When the variable is required. When active it will get placed in whatever free data space is available, but the actual location will vary based on what other functions are also active at that particular time.
A chunk of Data space is reserved by the linker for these transient variables, but that space is shared by the transient variables of every function.

If you really want to see the dynamic location allocated for rc you can add a debug printf within the function to print the address of rc, e.g., printf(" rc address=%dr", &rc);

Re: EDU Mini Controller & 2005 Nav Code
 

Mark,
Thanks so much for the continued help! I'll try your suggesion. I know (I think I know) it's an overwrite issue because I can change the value of rc by adding or removing printf's like you mentioned in your previous post. I can't change it to zero or one like it is supposed to be, but rc either equals 10 or 24 depending on what printf's I use. I'll keep trying. Thanks again for your help!

Re: EDU Mini Controller & 2005 Nav Code
 

Mark,
Thanks again for your insight and patience! We are off and running again!

Re: EDU Mini Controller & 2005 Nav Code
 

Hi Guys,
I hope we are not "debugging the debug code" again, but here is the latest situation:

We are trying to modify Kevin's Nav Code (pid.c specifically) to allow a third motor (arm shoulder joint) to be pid controlled using a potentiometer for the feedback device. We have the adc stuff all worked so we are correctly reading the pot and the command.h and robot.c have been modified to include the commands and code for a third motor. But we are currently stuck at the point where robot.c calls the set_pos() function so that pid.c knows where to go. Robot.c is passing the correct motor number and command position to the set_pos function, but when set_pos() function goes to load it into the motor_info structure, the value changes. This change only occurs when we run a command for the third motor. When we use a CMD_DRIVE for example, pos and pos_cmd are always the same value but when we use our CMD_SHOULDER, pos = 760 (correct value) and pos_cmd = 25637.

Kevin's Code from pid.c:
void set_pos(int motor, long int pos) // pos = encoder counts or pot input)
// 256 = 1 rev
{
motor_info[motor].pid_mode = PID_POS;
motor_info[motor].pos_cmd = pos;
pid_time = 1; // Start over
}

Printing the output looks like this:
motor = 3 This is correct
pos = 760 This is correct
pos_cmd = 25637 Not sure what this is
pos_cmd & pos should be the same value

Any ideas?

Re: EDU Mini Controller & 2005 Nav Code
 

Jeff,

Looking at the base EDU PID code, the first two motors, LEFT & RIGHT are defined with values of 0 & 1. I would expect your new motor would have a value of 2 not 3. However, 3 should work fine as long as you've sized the motor_info array with a minimum value of 4. If this is not the problem, perhaps you could post your actual code. This may help isolate the issue.

Mike

Re: EDU Mini Controller & 2005 Nav Code
 

Mike,
You are right on! I defined our shoulder motor as 3 instead of 2. Thanks man, I knew a set of experienced eyes would catch the problem quickly! Now it's on to find the next problem!

Jeff