/* Peeves Control Program for the CRS Dead-Reckoning Course
*
* This open-source software is available under the "MIT license".
* For more information, see http://www.opensource.org
*
* Copyright (c) 2001 by G.W. Lucas
*
* Permission is hereby granted, free of charge, to any person obtaining a
* copy of this software and associated documentation files (the "Software"),
* to deal in the Software without restriction, including without limitation
* the rights to use, copy, modify, merge, publish, distribute, sublicense,
* and/or sell copies of the Software, and to permit persons to whom the
* Software is furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included
* in all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
* OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE
* FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
* OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
* WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
/*
* Physical paramters and units of measurement are expressed in
* in encoder clicks, about 0.1282 clicks/cm. B is the distance
* between wheels, center-to-center.
*
* B = 154 clicks, 19.75 cm
*
*
* At various points, this program requires initial power levels for
* traveling in a straight-line path. These are obtained through
* experimentation and are platform-specific. Ideally, if the
* left and right motors and gear trains are evenly matched and
* loaded, the settings would be (3,3). The "spread" value
* is defined as the difference powerRight-powerLeft.
* Suppose the motors were perfectly matched. If you sent
* more power to the left motor, the spread value would be
* negative and, at the same time, the system would turn to
* the right. In mathematics, the convention is to treat right turns
* as negative. So with perfectly matched motors, the spread relates to the
* direction and speed of the turn. However, if the system is
* out-of-trim, we need to adjust the initial spread value to
* reflect its bias. For example, if the left motor is slow
* compared to the right (at equal power levels), the system
* will tend to turn to the left. A left turn is positive, so
* we say "the system has a positive bias." We need to set the
* initial spread to a negitive value to overcome the positive bias.
*
* parameters are INITIAL_SPREAD, INITIAL_RIGHT, INITIAL_RIGHT.
* these are obtained through experimentation. See logic below
* to find the formula for obtaining right and left as a function
* of the spread.
*
* INITIAL_EXPECTED_TURN is the expected turn radius, in clicks.
* It's value is obtained through experimentation and is highly
* variable, depending on battery age, ambient temperature,
* and floor surface. Just try to get something close to the
* actual value and it should work fine.
*
* LEG_LENGTH_LONG, LEG_LENGTH_SHORT, etc.
* the leg lengths reflect the lengths of the straight-line sections
* of the course (the "corridors") without the contribution of the turns.
* They are based on the dimensions of the CRS course which, following
* that curious American custom, was given in English units. Recall
* that for purposes of this program, all units are given in clicks
* (encoder intervals).
*
*
* itSum and bTheta0
* In the equations used to derive this logic, the variable theta is
* used to represent the robot's orientation (in radians). Since
* we cannot support fractional values, we use B*Theta instead.
* itSum is used to indicate the robot's lateral displacement
* from the centerline. It is scaled by 2*B. A scale factor
* of 4*B would have preserved more precision in the calculation
* (see how we divide itSum by 2 as it computed below), but I
* was worried about it getting too large and causing an integer
* overflow. As used, it should be safe out to 5 cm, and pretty
* safe out to 9 cm.
*
*/
#define B 154
#define INITIAL_RIGHT 3
#define INITIAL_LEFT 4
#define INITIAL_SPREAD -1
#define TURN_COUNT_FWD 4
#define TURN_COUNT_FLOAT 4
#define INITIAL_EXPECTED_TURN 200
#define LEG_LENGTH_LONG 1189 // approx 5 feet
#define LEG_LENGTH_SHORT 594 // approx 2.5 feet
#define STANDARD_TURN 178 // 9 inches (half width of the CRS track)
#define TURN_90 243 // encoder delta for 90-degree turn(B*PI/2)
#define COURSE_CENTERLINE 1544 // approx 6.5 feet
// abc() ---------------------------------------------------------
// the function abc computes result=a*b/c, where a,b,c are all positive.
// It is necessaryin cases where a*b may exceed 32767, and where you need
// to preserve as much precision as possible... the constraints are:
// max result not to exceed 32767
// a*b must be less than 16*32768 = 524288
// c must be less than 32768/16 = 2048
//
// Clearly, abc() involves a lot of computational overhead.
// It's a lot of work that yields a not-especially-impressive extension
// to the limits of NQC/RCX. So only use it when absolutely necessary.
//
// In my speed tests I found the following (loop overhead removed):
//
// standard a*b/c = 0.0082 seconds per calc
// abc func = 0.0914 seconds per calc
//
// I could not use abc within the PID loop because of the high
// overhead associated with the calculation (it messed up the timing).
//
// The algorithm used for abc is of my own devising and therefore
// I am absolutely sure there is some well-known method that's
// a lot better. If you are one of the well-knowers, please
// let me know how this ought to be done -- gary
//
void abc(int n, int m, int c, int &result){
int nr, mr;
int h, hr;
nr = n&0x0f;
mr = m&0x0f;
n/=16;
m/=16;
hr = n*m*16;
h = hr/c;
hr -= h*c;
result=h*16;
hr += (n*mr+m*nr);
h = hr/c;
hr -= h*c;
result+=h*16;
hr*=16;
h = hr/c;
hr -= h*c;
result+=h;
hr += mr*nr;
h = hr/c;
result += h;
}
// ------------------------------------------------------------------------
task main()
{
// persistent, "state" variables ----------- */
int itSum; // accumulated integrated error times (b/2);
int bTheta0; // sum of delta-w values
int wrPrior;
int wlPrior;
int priorT;
int spread;
int legLength;
int iLeg;
int expectedTurn;
int offTrack;
// perishable variables used for calculations. Recall that
// the RCX firmware only supports 32 variables, so economy is
// a serious consideration.
int sensorL, sensorR, clockT;
int wl, wr, T;
int dw, sw;
int x;
int temp;
int temp1;
int temp2;
bTheta0 = 0;
itSum = 0;
wrPrior = 0;
wlPrior = 0;
priorT = 0;
offTrack = 0;
spread = INITIAL_SPREAD;
expectedTurn = INITIAL_EXPECTED_TURN;
CreateDatalog(100);
SetSensor(SENSOR_1, SENSOR_ROTATION);
SetSensor(SENSOR_3, SENSOR_ROTATION);
PlaySound(SOUND_CLICK); // indicates the program is ready
Wait(100); // allow time for me to remove my finger from "run" button
PlaySound(SOUND_CLICK);
// now, laissez les bon temps rouler -----------------------
SetPower( OUT_A, INITIAL_LEFT);
SetPower( OUT_C, INITIAL_RIGHT);
SetDirection(OUT_A+OUT_C, OUT_FWD);
ClearSensor(SENSOR_1);
ClearSensor(SENSOR_3);
ClearTimer(0);
SetOutput( OUT_A+OUT_C, OUT_ON);
legLength= LEG_LENGTH_SHORT + STANDARD_TURN - expectedTurn;
for(iLeg=0; iLeg<5; iLeg++){
// enter the straight-line phase (PID controlled)
while(1){
sensorR = SENSOR_3;
sensorL = SENSOR_1;
x = (sensorR+sensorL)/2;
if(x >= legLength){
// adjust the length of the next leg by our previous
// "offTrack" value. If we have overshot our current
// legLength, the offTrack value becomes that overshoot
AddToDatalog(Timer(0));
AddToDatalog(x);
AddToDatalog(legLength);
x-=legLength;
legLength=offTrack;
offTrack=x;
break;
}
clockT = Timer(0);
T = clockT - priorT;
if(T<1)
continue;
wr = sensorR - wrPrior;
wl = sensorL - wlPrior;
dw = wr-wl;
sw = wr+wl;
if(sw<4)
continue;
itSum += (sw*(dw+2*bTheta0))/2;
// temp is the error(t) term
// temp1 is the integral of error(t)
// temp2 is the derivative of error(t)
// x is the computed correction
// in the example code below, the PID parameters are
// folded in with other constants to preserve numerical precision
// under integer operations:
// Pe = 1.6
// Pi = 0.6
// Pd = 0.46
temp = (24*sw*(dw+bTheta0))/(T*B);
temp1 = itSum/B;
temp2 = (10*sw*dw)/T;
temp2 = 7*temp2/(B*T);
x = INITIAL_SPREAD-(temp+temp1+temp2)/3;
if(x!=spread){
// adjust the spread by the correction, x. then use
// the spread to compute power levels. The logic for
// a positive spread and a negative spread is largely
// symmetric. In the case where the spread is an odd number
// (spread&1 is true), increment the fast wheel by 1 power step.
spread=x;
if(spread<0){
if(spread<-7)
spread = -7;
temp1 = 3-spread/2;
temp2 = 3+spread/2;
if(spread&1)
temp1++;
}else{
if(spread>7)
spread = 7;
temp1 = 3-spread/2;
temp2 = 3+spread/2;
if(spread&1)
temp2++;
}
SetPower(OUT_A, temp1);
SetPower(OUT_C, temp2);
wrPrior = sensorR;
wlPrior = sensorL;
priorT = clockT;
bTheta0+=(wr-wl);
T = 0;
}
if(T>=8){
/* we've gone quite a while without a power adjustment.
to keep the counters from getting too large, we
need to reset them. */
wrPrior = sensorR;
wlPrior = sensorL;
priorT = clockT;
bTheta0+=(wr-wl);
}
}
if(iLeg==4){
break;
}
// enter the curve phase (no PID control) ----------------
wrPrior=SENSOR_3;
wlPrior=SENSOR_1;
ClearSensor(SENSOR_1);
ClearSensor(SENSOR_3);
SetPower(OUT_A, 0);
SetPower(OUT_C, 7);
bTheta0 = wrPrior-wlPrior;
AddToDatalog(itSum);
AddToDatalog(bTheta0);
temp=0;
x=0;
do{
if(x==0){
if(temp==0){
SetOutput(OUT_A, OUT_FLOAT);
temp = 1;
x=TURN_COUNT_FLOAT;
}else{
SetOutput(OUT_A, OUT_FWD);
temp = 0;
x=TURN_COUNT_FWD;
}
}
x--;
wr = SENSOR_3;
wl = SENSOR_1;
dw = wr-wl;
}while(dw+bTheta0 < TURN_90);
// cancel the turn as soon as possible
if(temp==1)
SetOutput(OUT_A, OUT_FWD);
SetPower( OUT_A, INITIAL_LEFT);
SetPower( OUT_C, INITIAL_RIGHT);
spread = INITIAL_SPREAD;
AddToDatalog(wr);
AddToDatalog(wl);
sw = wr+wl;
abc(sw, B, 2*dw, x);
AddToDatalog(x);
// we had already shortened the previous leg based on
// how much the expectedTurn exceeded the standard turn
// or lengthened it if the expectedTurn was tighter than standard).
// so if the actual turn, x, matches the expectedTurn, then we
// should be exactly on-track (on the center line of the corridor).
// unfortunately, the most recent turn probably deviates
// from the expectedTurn value, and we will have to adjust for
// that in the next turn by setting the offTrack value.
temp = x-expectedTurn;
AddToDatalog(temp);
offTrack+=temp;
AddToDatalog(offTrack);
// we expect (or hope) that the behavior on the next turn
// will match that of the turn we just completed.
// so adjust the expectedTurn accordingly.
expectedTurn = x;
// The turn that we just completed probably deviated from
// a standard turn radius. if it exceeded the standard radius,
// it has carried us further into the corridor than a standard
// turn would have and we need to shorten the length of the
// next leg accordingly... if it fell short, we would not yet
// be fully into the corridor and we would lengthen the leg.
// So we make the adjustment legLength based on STANDARD_TURN
// In a similar manner, we also need to apply an adjustment on how
// we expect the next turn to behave.
//
// I have observed that the turn radius changes as the robot
// does it's stuff, probably in response to a gradual increase
// in average speed... as of yet I have not worked out a good
// way to predict it, so I just assume that the next turn will
// be a lot like the one we just completed. The place where
// the estimate counts the most is on the last turn... after
// the each of the early turns we can correct by using the offTrack
// value to adjust the legLength as for the subsequent leg.
// After the last turn, this is not an option. The only feasible
// correction might be to set itSum (integral error times b/2).
// and let the PID logic correct for the problem. I haven't
// tried this yet, in part because I am afraid of of stressing
// the PID logic with too large an error.
temp = STANDARD_TURN-x;
if(iLeg < 3){
legLength += LEG_LENGTH_LONG + temp + STANDARD_TURN-expectedTurn;
}else{
legLength += LEG_LENGTH_SHORT + temp;
}
bTheta0 += (SENSOR_3 - SENSOR_1 - TURN_90);
ClearSensor(SENSOR_1);
ClearSensor(SENSOR_3);
AddToDatalog(bTheta0);
AddToDatalog(itSum);
AddToDatalog(Timer(0));
wrPrior=0;
wlPrior=0;
legLength -= itSum/(2*B);
itSum=0;
ClearTimer(0);
priorT=0;
}
// brake hard and come to a stop ---------------------------
SetOutput(OUT_A+OUT_C, OUT_OFF);
AddToDatalog(SENSOR_1);
AddToDatalog(SENSOR_3);
AddToDatalog(itSum);
// end with a flourish -------------------------------------
Wait(250);
PlayTone(587, 40);
PlayTone(0, 5);
PlayTone(440, 20);
PlayTone(0, 5);
PlayTone(440, 20);
PlayTone(0, 5);
PlayTone(493, 40);
PlayTone(0, 5);
PlayTone(440, 40);
Wait(250);
PlayTone(554, 40);
PlayTone(0, 5);
PlayTone(587, 40);
PlayTone(0, 5);
}