|
#16
|
||||
|
||||
|
Re: Accelerometer code
Quote:
|
|
#17
02-06-2005, 14:53
|
||||
|
||||
|
Re: Accelerometer code
Quote:
|
|
#18
02-06-2005, 14:54
|
||||
|
||||
|
Re: Accelerometer code
Quote:
Last edited by Joel J : 02-06-2005 at 14:56. Reason: added suggestion.. |
|
#19
02-06-2005, 14:57
|
|||
|
|||
|
Re: Accelerometer code
yea, it looks like i'm gonna try to push Carroll to get us the 203 one. For whatever reason he was adamant about using a PWM and not getting something to convert it. hopefully he goes for getting a new accelerometer entirely.
also, http://microchip.com/stellent/idcplg...&part=DM163022 that's what we plan on using for the 2nd PIC |
|
#20
02-06-2005, 15:09
|
|||
|
|||
|
Re: Accelerometer code
One thing I don't see in your code is where you reset the Port B interrupt flag (INTCONBits.RBIF), but that could easily be in your InterruptHandlerLow routine. If possible could you also post your user_routines_fast.c code as it might help.
Another thing you will probably encounter later on is when you calculate your yacceleration you are dividing a 16 bit integer Int_4_Period (maximum 65535) by 206124 which will always result in 0 as the PIC will be performing integer math. |
|
#21
02-06-2005, 15:13
|
|||
|
|||
|
Re: Accelerometer code
is the PIC capable of doing floating point math at all?
if not i really should just give up on this pwm accelerometer and get the 203 joel is talking about which is what i'm planning on doing now. Thanks for all the help you guys. Its much appreciated |
|
#22
02-06-2005, 15:19
|
||||
|
||||
|
Re: Accelerometer code
Quote:
Code:
float kVariableName; // Float variable with name kVariableName |
|
#23
02-06-2005, 15:47
|
|||
|
|||
|
Re: Accelerometer code
ok, would a 40 Mhz PIC do a better job at it or is it just a bad idea in general because then i need an analog signal (203 model) since if you look in the comments in the accel.c file i need float point math to convert pwm periods into anything useful
|
|
#24
02-06-2005, 16:19
|
||||
|
||||
|
Re: Accelerometer code
Quote:
If you are using the offboard processor, and it is at that speed, then you'd probably get the accuracy you need. In all implementations of this type of accelerometer, the interrupts become, for the most part, the main focus -- ie, they are set on high priority interrupts -- and not much else is going on. So, you decide. Also, as far as floating point goes, I used a fixed-point setup (10 bits for the decimal) to get the accuracy I wanted, without having to resort to floats. For some reason, Dr. Carroll didn't like that (added complexity or some other claim), but its commonly used. Chris Hibner did a presentation on it recently ( http://www.usfirst.org/robotics/2005...rogramming.ppt ). Oh, and here is a "relevant" example of said fixed-point setup in use: Code:
#define RADIX_SHIFT_TIMES 10/* Shift a value this number of places. In other words or multiply (or divide) by 2^number_of_places */ #define RADIX_SHIFT_MULTIPLIES_BY 1024/* this is the effective multiplier/divider. it is 2^RADIX_SHIFT_TIMES */ Code:
void applyMotionControl(pDriveInfo stream)
{
long er = 0, el = 0, VID, WID;
//er = (2.0*Mrobot*R*Kd1*Vd-2.0*Mrobot*R*Kd1*V+2.0*Mrobot*R*Kp1*Vd_int-2.0*Mrobot*R*Kp1*V_int+2.0*Mrobot*R*Ki1*Vd_int_int-2.0*Mrobot*R*Ki1*V_int_int+Kg*Kg*Kt*Kb*wr*rout+rout*rout*b*Mrobot*R*Kp2*Wd_int-rout*rout*b*Mrobot*R*Kp2*W_int+rout*rout*b*Mrobot*R*Ki2*Wd_int_int-rout*rout*b*Mrobot*R*Ki2*W_int_int+rout*rout*b*Mrobot*R*Kd2*Wd-rout*rout*b*Mrobot*R*Kd2*W);
//el = (2.0*Mrobot*R*Kd1*Vd-2.0*Mrobot*R*Kd1*V+2.0*Mrobot*R*Kp1*Vd_int-2.0*Mrobot*R*Kp1*V_int+2.0*Mrobot*R*Ki1*Vd_int_int-2.0*Mrobot*R*Ki1*V_int_int-rout*rout*b*Mrobot*R*Kp2*Wd_int+rout*rout*b*Mrobot*R*Kp2*W_int-rout*rout*b*Mrobot*R*Ki2*Wd_int_int+rout*rout*b*Mrobot*R*Ki2*W_int_int-rout*rout*b*Mrobot*R*Kd2*Wd+rout*rout*b*Mrobot*R*Kd2*W+Kg*Kg*Kt*Kb*wl*rout);
if (stream->gear == HIGH)
{
VID = (Kd1_high*(stream->Vd - stream->V)) + (Kp1_high*(stream->Vd_int - stream->V_int)) + (Ki1_high*(stream->Vd_int_int - stream->V_int_int));
} else {
VID = (Kd1_low*(stream->Vd - stream->V)) + (Kp1_low*(stream->Vd_int - stream->V_int)) + (Ki1_low*(stream->Vd_int_int - stream->V_int_int));
}
VID = VID >> RADIX_SHIFT_TIMES; // Squared radix value, so divide by it, to restore original
VID = VID * DD_Mrobot_R_2; // Squared radix value, but don't divide because of later
if (stream->gear == HIGH)
{
WID = (Kd2_high*(stream->Wd - stream->W)) + (Kp2_high*(stream->Wd_int - stream->W_int)) + (Ki2_high*(stream->Wd_int_int - stream->W_int_int));
} else {
WID = (Kd2_low*(stream->Wd - stream->W)) + (Kp2_low*(stream->Wd_int - stream->W_int)) + (Ki2_low*(stream->Wd_int_int - stream->W_int_int));
}
WID = WID >> RADIX_SHIFT_TIMES;// Squared radix value, so divide by it, to restore original
WID = WID * DD_rout_rout_b_Mrobot_R;// Squared radix value, but don't divide because of later
if (stream->gear == HIGH)
{
er = (VID + WID + (DD_Kg_Kg_Kt_Kb_rout_high*stream->wr)) / DD_rout_Kg_Kt_high; // Added already squared radix value to newly squared radix value
el = (VID - WID + (DD_Kg_Kg_Kt_Kb_rout_high*stream->wl)) / DD_rout_Kg_Kt_high; // No need to divide by radix to even out, because the already present division evens things out
} else {
er = (VID + WID + (DD_Kg_Kg_Kt_Kb_rout_low*stream->wr)) / DD_rout_Kg_Kt_low; // Added already squared radix value to newly squared radix value
el = (VID - WID + (DD_Kg_Kg_Kt_Kb_rout_low*stream->wl)) / DD_rout_Kg_Kt_low; // No need to divide by radix to even out, because the already present division evens things out
}
er = er >> RADIX_SHIFT_TIMES;// Since we are ready to use the value, get rid of multiplier
el = el >> RADIX_SHIFT_TIMES;// Since we are ready to use the value, get rid of multiplier
stream->rx = el;// TEMPORARY -- REMOVE!!!!!!
stream->ry = er;
// Do not integrate unless the outputs are below saturation
if (er > stream->max_output)
{
er = stream->max_output;
} else if (er < stream->min_output)
{
er = stream->min_output;
} else if (el > stream->max_output)
{
el = stream->max_output;
} else if (el < stream->min_output)
{
el = stream->min_output;
} else {
// Integrate values (remember to add check for saturation)
stream->Vd_int = safeAdd_long(stream->Vd_int, ((Ts * (stream->Vd + stream->Vd_prev)) >> (RADIX_SHIFT_TIMES + 1)));
stream->Vd_int_int = safeAdd_long(stream->Vd_int_int, ((Ts * (stream->Vd_int + stream->Vd_int_prev)) >> (RADIX_SHIFT_TIMES + 1)));
stream->Wd_int = safeAdd_long(stream->Wd_int, ((Ts * (stream->Wd + stream->Wd_prev)) >> (RADIX_SHIFT_TIMES + 1)));
stream->Wd_int_int = safeAdd_long(stream->Wd_int_int, ((Ts * (stream->Wd_int + stream->Wd_int_prev)) >> (RADIX_SHIFT_TIMES + 1)));
}
// Store previous values (do this no matter what)
stream->Vd_prev = stream->Vd;
stream->Vd_int_prev = stream->Vd_int;
stream->Wd_prev = stream->Wd;
stream->Wd_int_prev = stream->Wd_int;
// Return value to user
stream->left_output = (unsigned char)limit_to_range(((signed int)el+(signed int)127), (signed int)MIN_JOY, (signed int)MAX_JOY);
stream->right_output = (unsigned char)limit_to_range(((signed int)er+(signed int)127), (signed int)MIN_JOY, (signed int)MAX_JOY);
// stream->left_output = p1_y;
// stream->right_output = p2_y;
}
Last edited by Joel J : 02-06-2005 at 16:24. Reason: added an example.. |
|
#25
02-06-2005, 16:51
|
||||
|
||||
|
Re: Accelerometer code
Okay, lets take a deep breath... Ready? Okay, here's my thinking:
1) Stick with the PWM output because, as Joel pointed out, this device has got a higher resolution ADC on-chip and as all the analog signals stay on-chip, you'll be dealing with fewer noise sources. 2) If you're going to use an external microcontroller to handle the accelerometer interface, use the CCP capture hardware to do the work for you. This won't work on the full-size FRC robot controller because IFI has output buffers on the CCP pins, which precludes them being used as inputs (the smaller EDU robot controller does not have these buffers). I've already written code that will work with the device you're using that uses the CCP capture hardware. If you promise to send me a copy of your project write-up, I'll publish a link to it <grin>. 3) What resistor values did you choose to program the t2 frequency (If you don't know what I'm blathering about, you'd better read the data sheet <grin>). If you're using this accelerometer in a control application, anything higher than 50-100Hz is overkill. If you're integrating the output, you'll want to set it to at least twice the accelerometer's bandwidth (lest Harry Nyquist rise from his grave, and smite thy project), but ideally you'll want to set it as high as possible. -Kevin (Who didn't get nearly enough sleep and has consumed way too much coffee) |
|
#26
02-06-2005, 17:07
|
|||
|
|||
|
Re: Accelerometer code
You could also do your fixed point math another way by calculating your result in units mg's rather than g's by multiplying the appropriate values by 1000. It is not as efficient as bit shifting since you actually have to use multiply and divide statements, but may be a bit more readable.
If you do stick with just the IFI controller, you can probably get reasonable accuracy as the code comments indicate a period of 9.6 milliseconds for the accelerometer. While a low priority interrupt can be delayed depending on what the process is doing, you should be able to maintain an error of less than 1% due to interrupt delay. Mike |
|
#27
02-06-2005, 17:31
|
|||
|
|||
|
Re: Accelerometer code
The resistor value was preset when the device made it into our hands at 1.2 Mega ohms making T2 somewhere in the range of 250 Hz if i remember correctly. The exact calculation is written in the comments i have in the code.
We have to write up a paper for the symposium at the end of the summer and i would be more than willing to send you a copy but as of now there is no formal write up I'll see about sticking with the PWMs. I have to wait until the 2nd PIC gets in. Thanks for the advice ~Will |
|
#28
02-06-2005, 19:18
|
||||
|
||||
|
Re: Accelerometer code
Quote:
-Kevin |
|
#29
04-06-2005, 01:02
|
|||
|
|||
|
Re: Accelerometer code
So far the discussion has been on low range accelerometers. Remember, it's on a robot and there may be times when there is a impact or bump that will drive the unit to saturation. It can take a considerable amount of time to recover from a saturated condition. It's enough to affect the integration. Accelerometers are also excellent vibration sensors. One thing that is often over looked is the natural resonance frequency of the sensor. Both the chip package and the sensor have a resonate point that is high enough not to worry about but your going to have to mount the chip on a carrier. You have to worry about the resonance point of the chip-carrier-mounting system.
Glue your chip to the board. Use a thick high grade PC board. More ridged the better. Use some isolation mounting to mount the board to the bot and pay attention to what part of the robot you mount it to. Try to isolate it from the drive train vibration. I think you might have better results if you went to a mid range analog unit. Run the output through and op amp with a gain of one and a 400 HZ low pass filter. Mount the pic18, op amp and sensor on a small ridged carrier that is mounted to a ridge large mass part of the robot with some isolation mounting hardware. Over sample in software to further reduce noise. Let the pic coproc do the calculations and send a periodic update to the FRC . The Freescale MMA3201D has a low pass fiter on the chip. Last edited by Gdeaver : 04-06-2005 at 01:55. |
 |
| Thread Tools | |
| Display Modes | Rate This Thread |
|
|
Similar Threads
|
||||
| Thread | Thread Starter | Forum | Replies | Last Post |
| Out of the Box Camera Code | russell | Programming | 9 | 21-10-2009 05:28 |
| Some things i'm trying to understand on Kevin's Accelerometer Code | Squall | Programming | 2 | 03-02-2005 12:01 |
| Team THRUST - Kevin's Code and Camera Code Combine | Chris_Elston | Programming | 3 | 31-01-2005 22:28 |
| Example accelerometer code released. | Kevin Watson | Programming | 0 | 20-01-2005 03:45 |
| heres the code. y this not working | omega | Programming | 16 | 31-03-2004 15:18 |
Linear Mode
