Accelerometer code

[Joel J]

Originally Posted by ImmortalAres

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

the PWM accelerometer comes with a 14-bit A/D converter, whereas the PICS usually have a 10-bit converter. That was their claimed advantage, but quite frankly "PWM interfaces" are a real pain, and for that reason, Analog is phasing out those line of accelerometers. So you may as well be with the game.

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:

#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 */

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;
}

Heh.