/* 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); }