 |
dsPid33
|
|
dsPid33
|
00001 /* //////////////////////////////////////////////////////////////////////////// 00002 ** Included in "dsPID.c", it contains definitions and variables initialization 00004 00005 /*---------------------------------------------------------------------------*/ 00006 /* standard include */ 00007 /*---------------------------------------------------------------------------*/ 00008 #include "dsPID33_common.h" 00009 00010 /*---------------------------------------------------------------------------*/ 00011 /* Status bits */ 00012 /*---------------------------------------------------------------------------*/ 00013 00014 /* Boot Segment Program Memory:No Boot Segment Program Memory 00015 ** Write Protect:Disabled **/ 00016 //_FBS(BSS_NO_FLASH & BWRP_WRPROTECT_OFF); 00017 00018 // External Oscillator 00019 _FOSCSEL(FNOSC_PRI); // Primary (XT, HS, EC) Oscillator 00020 00021 // Clock Switching is enabled and Fail Safe Clock Monitor is disabled 00022 // OSC2 Pin Function: OSC2 is Clock Output 00023 // Primary Oscillator Mode: XT Crystanl 00024 _FOSC(FCKSM_CSECMD & OSCIOFNC_OFF & POSCMD_XT); 00025 00026 /* Background Debug Enable Bit: Device will Reset in user mode 00027 ** Debugger/Emulator Enable Bit: Reset in operational mode 00028 ** JTAG Enable Bit: JTAG is disabled 00029 ** ICD communication channel select bits: 00030 ** communicate on PGC1/EMUC1 and PGD1/EMUD1 **/ 00031 //_FICD(BKBUG_ON & COE_ON & JTAGEN_OFF & ICS_PGD1); // valid until C30 3.11 00032 _FICD(JTAGEN_OFF & ICS_PGD1); 00033 /*Enable MCLR reset pin and turn on the power-up timers with 64ms delay. 00034 PIN managed by PWM at reset*/ 00035 _FPOR(FPWRT_PWR64 & PWMPIN_ON & HPOL_ON & LPOL_ON); 00036 00037 /* Code Protect: Code Protect off 00038 ** Code Protect: Disabled 00039 ** Write Protect: Disabled **/ 00040 _FGS(GSS_OFF & GCP_OFF & GWRP_OFF); 00041 00042 // Watchdog Timer Enable: 00043 _FWDT(FWDTEN_OFF) //Watchdog timer enabled/disabled by user software 00044 00045 00046 /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ 00047 /* External defininitions */ 00048 /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ 00049 00050 //UART 00051 //{ 00052 extern volatile unsigned char UartRxBuff[][2]; 00053 extern unsigned char 00054 UartTxBuff[] __attribute__((space(dma),aligned(128))); 00055 extern volatile unsigned char UartRxPtrIn; 00056 extern unsigned char UartRxPtrOut; 00057 extern volatile int UartRxStatus; 00058 00059 extern volatile unsigned char Uart2RxPtrIn; 00060 extern unsigned char Uart2RxPtrOut; 00061 extern volatile int Uart2RxStatus; 00062 extern unsigned char TmpPtr; 00063 extern unsigned char UartRxPtrData; 00064 extern unsigned char UartRxCmd[]; 00065 extern unsigned char UartTmpBuff[][2]; 00066 extern const unsigned char Test[]; 00067 extern unsigned char TmpPtr2; 00068 extern unsigned char Uart2RxPtrData; 00069 int Port; // Port = 0 for UART1, Port = 1 for UART2 00070 int SendMapPort; // to temporay store port number for delayed TX 00071 int ResetPort; // to temporay store port number for reset 00072 //} 00073 00074 /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ 00075 /* Global defininitions */ 00076 /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ 00077 00078 struct Bits{ 00079 unsigned bit0:1; 00080 unsigned bit1:1; 00081 unsigned bit2:1; 00082 unsigned bit3:1; 00083 unsigned bit4:1; 00084 unsigned bit5:1; 00085 unsigned bit6:1; 00086 unsigned bit7:1; 00087 unsigned bit8:1; 00088 unsigned bit9:1; 00089 unsigned bit10:1; 00090 unsigned bit11:1; 00091 unsigned bit12:1; 00092 unsigned bit13:1; 00093 unsigned bit14:1; 00094 unsigned bit15:1; 00095 }; 00096 00097 struct Bits VARbits1; 00098 struct Bits VARbits2; 00099 volatile struct Bits VOLbits1; 00100 00101 //----- Flags 00102 00103 #define RC6Tick(x) (Delay1KTCYx(4*x),Delay100TCYx(4*x),Delay10TCYx(3*x)) 00104 #define DELAY100MS Delay10KTCYx(100) // 100ms 00105 00106 #if defined(__dsPIC33FJ64MC802__) || defined(__dsPIC33FJ128MC802__) 00107 // -- compiling for a 28 pin DSC *************************************** 00108 00109 #define LED1 _LATA4 00110 #define LED2 _LATB4 00111 // #define MOTOR_ENABLE1 _LATB1 00112 // #define MOTOR_ENABLE2 _LATB0 00113 #define DIR_485 _LATB9 00114 00115 #elif defined(__dsPIC33FJ64MC804__) || defined(__dsPIC33FJ128MC804__) 00116 // -- compiling for a 44 pin DSC *************************************** 00117 00118 #ifdef DSNAVCON33 00119 00120 #define LED1 _LATA8 00121 #define MOTOR_ENABLE1 _LATA7 00122 #define MOTOR_ENABLE2 _LATA10 00123 00124 #endif 00125 00126 #ifdef ROBOCONTROLLER 00127 00128 #define LED1 _LATA8 00129 #define LED2 _LATA9 00130 #define AUX1 _LATA7 00131 #define AUX2 _LATA10 00132 #define MOTOR_ENABLE1 _LATA0 00133 #define MOTOR_ENABLE2 _LATA1 00134 #define DIR_485 _LATC3 00135 00136 #endif 00137 00138 #else 00139 00140 #error -- dsPIC33FJ not recognized. Accepted only 64/128MC802 or 64/128MC804 00141 00142 #endif 00143 00144 const long Tcy = 1000000/(float)(FCY)* 100000000; 00145 00146 //----- Variabili 00147 int i = 0; // generic index 00148 int j = 0; // generic index 00149 long Blink = 0; // heartbeat blink index 00150 00151 #define UART_CONT_TIMEOUT 100 // continuos parameters TX in ms(debug) 00152 unsigned int TxContFlag=0; // continuos parameters TX flag (debug) 00153 volatile int UartContTxTimer=UART_CONT_TIMEOUT; // timer for cont parameters TX 00154 #define UART_CONT_TIMEOUT 100 // continuos parameters TX in ms(debug) 00155 00156 // ADC 00157 int ADCValue[2] = {0,0}; // 64 sample average ADC AN0, AN1 reading 00158 #define ADC_CALC_FLAG VOLbits1.bit6 // enable ADC value average calculus 00159 #define ADC_OVLD_LIMIT 800 // in mA 00160 #define ADC_OVLD_TIME 100 // n x 10ms 00161 char ADCOvldCount[2] = {0,0}; // how long overload status last 00162 00163 // Input Capture (speed measurement) 00164 volatile int Ic1Indx = 0; // samples buffer index for IC1 00165 volatile int Ic2Indx = 0; // samples buffer index for IC2 00166 volatile unsigned int Ic1PrevPeriod; // previous sample for IC1 00167 volatile unsigned int Ic2PrevPeriod; // previous sample for IC2 00168 volatile long Ic1Period = 0; // n samples average value for IC1 00169 volatile long Ic2Period = 0; // n samples average value for IC2 00170 volatile unsigned int Ic1CurrPeriod; // current value for IC1 00171 volatile unsigned int Ic2CurrPeriod; // current value for IC2 00172 long Tmr2OvflwCount1; // timer 2 overflow for IC1 00173 long Tmr2OvflwCount2; // timer 2 overflow for IC2 00174 #define IC1_FIRST VOLbits1.bit7 // first encoder pulse for IC1 00175 #define IC2_FIRST VOLbits1.bit8 // first encoder pulse for IC2 00176 00177 #define PID1_CALC_FLAG VOLbits1.bit0 // PID and speed elaboration flag 00178 #define PID2_CALC_FLAG VOLbits1.bit1 // PID and speed elaboration flag 00179 00180 /* 00181 -> [4] [7] [19] 00182 THIS PARAMETERS ARE VALID FOR RINO ROBOTIC PLATFORM. 00183 HERE JUST AS AN EXAMPLE ON HOW TO CALCULATE THEM 00184 Encoder = 300 cpr 00185 Gear reduction ratio = 30:1 00186 Wheel speed = 200 rpm 00187 Encoder pulses for each wheel turn = 9.000 00188 Wheel diameter = 58mm -> circumference = 182,2123739mm 00189 Space for each encoder pulse 1x mode Delta S = 0,020245819mm 00190 Space for each encoder pulse 2x mode Delta S = 0,01012291mm 00191 Space for each encoder pulse 4x mode Delta S = 0,005061455mm 00192 Speed calculation K in micron/second = 298536736 -> [19] 00193 */ 00194 #define CPR 300 00195 #define GEAR_RATIO 30 00196 #define CPR_WHEEL CPR*GEAR_RATIO // 9000 00197 #define DIAMETER 0.058 00198 #define CIRC DIAMETER*PI // 0.182212374 00199 #define SPACE_ENC_1X (CIRC / (CPR_WHEEL)) // 2.0245819E-05 00200 #define SPACE_ENC_2X (CIRC / (CPR_WHEEL * 2)) // 1.012291E-05 00201 #define SPACE_ENC_4X (CIRC / (CPR_WHEEL * 4)) 00202 #define K_VEL ((SPACE_ENC_2X) / (TCY)) // TCY = 25,2341731266 ns 00203 00204 #define R 0 // right index 00205 #define L 1 // right index 00206 00207 // Speed calculation K in m/s 00208 long Kvel[2]; // K_VEL << 15 00209 int VelDes[2] = {0,0}; // desired speed mm/s 00210 int VelMes[2] = {0,0}; // measured speed mm/s 00211 int VelDesM = 0; // mean measured speed mm/s [23] 00212 // threshold to change PID calculation frequency. = Speed in mm/s *2^15/1000 00213 #define VEL_MIN_PID 1600 // Approx 50mm/s 00214 unsigned char PidCycle[2]; // counter for longer cycle PID 00215 // constants for traveled distance calculation: SPACE_ENC_4X in mm 00216 // see [2] in descrEng.exe in dsPID folder 00217 long double Ksp[2]; 00218 float Axle;// base width, distance between center of the wheels 00219 float SemiAxle;// Axle/2 useful for odometry calculation 00220 float SemiAxle; // Axle / 2 00221 int SpTick[2] = {0,0}; // distance traveled by a wheel as encoder pulses 00222 00223 float Spmm[2] = {0,0}; // distance traveled by a wheel as mm(SpTick[x]*Ksp[x]) 00224 float Space = 0; // total distance traveled by the robot 00225 #define SPMIN 0.01 // Minimum value to perform odometry 00226 float CosPrev = 1; // previous value for Cos(ThetaMes) 00227 float SinPrev = 0; // previous value for Sin(ThetaMes) 00228 float PosXmes = 0; // current X position coordinate 00229 float PosYmes = 0; // current Y position coordinate 00230 float ThetaMes = 0; // Current orientation angle 00231 // #define PI 3.1415926536 180� 00232 #define TWOPI 6.2831853072 // 360� 00233 #define HALFPI 1.570796327 // 90� 00234 #define QUARTPI 0.7853981634// 45� 00235 00236 long Vel[2]; // speed in m/s << 15 (long) for PID1 and PID2 00237 00238 // PID [19d] 00239 tPID PIDstruct1; 00240 fractional abcCoefficient1[3] __attribute__ ((section (".xbss, bss, xmemory"))); 00241 fractional controlHistory1[3] __attribute__ ((section (".ybss, bss, ymemory"))); 00242 fractional kCoeffs1[] = {0,0,0}; 00243 #define KP1 kCoeffs1[0] 00244 #define KI1 kCoeffs1[1] 00245 #define KD1 kCoeffs1[2] 00246 #define PID_OUT1 PIDstruct1.controlOutput 00247 #define PID_MES1 PIDstruct1.measuredOutput 00248 #define PID_REF1 PIDstruct1.controlReference 00249 00250 tPID PIDstruct2; 00251 fractional abcCoefficient2[3] __attribute__ ((section (".xbss, bss, xmemory"))); 00252 fractional controlHistory2[3] __attribute__ ((section (".ybss, bss, ymemory"))); 00253 fractional kCoeffs2[] = {0,0,0}; 00254 #define KP2 kCoeffs2[0] 00255 #define KI2 kCoeffs2[1] 00256 #define KD2 kCoeffs2[2] 00257 #define PID_OUT2 PIDstruct2.controlOutput 00258 #define PID_MES2 PIDstruct2.measuredOutput 00259 #define PID_REF2 PIDstruct2.controlReference 00260 00261 #define RAMP_FLAG1 VARbits1.bit6 // acc/dec-eleration flag 00262 #define RAMP_T_FLAG1 VARbits1.bit7 // acc/dec-eleration direction 00263 #define RAMP_FLAG2 VARbits1.bit1 // acc/dec-eleration flag 00264 #define RAMP_T_FLAG2 VARbits1.bit15 // acc/dec-eleration direction 00265 fractional VelFin[2] = {0,0}; // temp for speed during acceleration 00266 //#define ACC 0.00025 // acceleration 00267 //#define DEC 0.0005 // deceleration 00268 #define ACC 0.00005 // acceleration 00269 #define DEC 0.001 // deceleration 00270 00271 fractional Ramp1; // acc/dec-eleration ramp slope 00272 fractional Ramp2; // acc/dec-eleration ramp slope 00273 00274 int Curr[2] = {0,0}; // motor current 00275 00276 /* Field mapping, regular size*/ 00277 #define CELL_SIZE 100 // Dimension of grid cell side in mm 00278 #define MAP_SIZE 15000 // Abs Dimension of field (X = Y) in mm 00279 #define HALF_MAP_SIZE 7500 // + - Dimension of field (X = Y) in mm 00280 #define VAR_PER_BYTE 2 // # of cell var per byte 00281 00282 // ATTENTION when updating matrix size values, update as well the 00283 // equivalent values on dsNavConsole software code 00284 #define X_SIZE 75 // (MAP_SIZE/CELL_SIZE/VAR_PER_BYTE) MAX X array index 00285 #define Y_SIZE 150 //(MAP_SIZE/CELL_SIZE) MAX Y array index 00286 #define X_POINT_MAX 150 // [22g] 00287 #define X_POINT_MIN 0 // [22g] 00288 #define Y_POINT_MAX 150 // [22g] 00289 #define Y_POINT_MIN 0 // [22g] 00290 00291 /*Field mapping, regular size */ 00292 00293 /* Field mapping =========== FOR DEBUGGING PURPOSES ================== 00294 because MPLAB watch window becomes extremely slow with a big array 00295 of struct, during debugging this array must be decresead in size 00296 #define CELL_SIZE 100 // Dimension of grid cell side in mm 00297 #define MAP_SIZE 5000 // Abs Dimension of field (X = Y) in mm 00298 #define HALF_MAP_SIZE 2500 // + - Dimension of field (X = Y) in mm 00299 #define VAR_PER_BYTE 2 // # of cell var per byte 00300 00301 // ATTENTION when updating matrix size values, update as well the 00302 // equivalent values on dsNavConsole software code 00303 #define X_SIZE 25 // (MAP_SIZE/CELL_SIZE/VAR_PER_BYTE) MAX X array index 00304 #define Y_SIZE 50 //(MAP_SIZE/CELL_SIZE) MAX Y array index 00305 #define X_POINT_MAX 50 // [22g] 00306 #define X_POINT_MIN 0 // [22g] 00307 #define Y_POINT_MAX 50 // [22g] 00308 #define Y_POINT_MIN 0 // [22g] 00309 Field mapping =========== FOR DEBUGGING PURPOSES ==================*/ 00310 00311 int Xshift = 0; // [22g] 00312 int Yshift = 0; // [22g] 00313 00314 typedef struct 00315 { 00316 unsigned char nib:4; 00317 } 00318 nibble; 00319 00320 typedef union 00321 { 00322 unsigned char UC; 00323 struct 00324 { 00325 unsigned char nib0:4; 00326 unsigned char nib1:4; 00327 }TN; 00328 }PackedBit; 00329 00330 void SetMap(int Xpoint, int Ypoint, nibble *CellVal); 00331 00332 PackedBit MapXY[X_SIZE][Y_SIZE]; // to store field mapping [22b] 00333 00334 // unsigned char MapXY[X_SIZE][Y_SIZE]; // to store field mapping [22b] 00335 int XindxPrev=10000; // previous X index 00336 int YindxPrev=10000; // previous Y index 00337 #define SPACE_FLAG VARbits1.bit2 00338 float MaxMapX = HALF_MAP_SIZE; // current field limits 00339 float MinMapX = -HALF_MAP_SIZE; 00340 float MaxMapY = HALF_MAP_SIZE; 00341 float MinMapY = -HALF_MAP_SIZE; 00342 #define MAP_SEND_FLAG VOLbits1.bit4 // to send map to console 00343 #define MAP_BUFF_FLAG VOLbits1.bit5 // to send another map grid row 00344 unsigned char MapSendIndx; 00345 00346 // Angle PID [23] 00347 tPID AnglePIDstruct; 00348 fractional AngleabcCoefficient[3] 00349 __attribute__ ((section (".xbss, bss, xmemory"))); 00350 fractional AnglecontrolHistory[3] 00351 __attribute__ ((section (".ybss, bss, ymemory"))); 00352 fractional AngleKCoeffs[] = {0,0,0}; 00353 #define ANGLE_KP AngleKCoeffs[0] // K parameters 00354 #define ANGLE_KI AngleKCoeffs[1] 00355 #define ANGLE_KD AngleKCoeffs[2] 00356 #define ANGLE_PID_DES AnglePIDstruct.controlReference // desired angle 00357 #define ANGLE_PID_MES AnglePIDstruct.measuredOutput // measured angle 00358 #define ANGLE_PID_OUT AnglePIDstruct.controlOutput // PID output 00359 #define MAX_ROT_SPEED 512 // MAX speed (+ or -) of wheels during rotation 00360 #define RAD2DEG 57.295779513 // = 180/PI 00361 #define DEG2RAD 0.0174532925 // = PI/180 00362 float ThetaDes = 0; // desired orientation angle (set point) [23] (Rad) 00363 float ThetaDesRef; // to temporarely store angle (Rad) 00364 int AngleCmp; // compass bearing from sensors board (Deg * 10) 00365 #define ORIENTATION_FLAG VARbits1.bit3 // to control Orientation() execution 00366 #define ANGLE_OK_FLAG VARbits1.bit12 // target angle reached 00367 #define MIN_THETA_ERR DEG2RAD // acceptable angle error in radians = 1� 00368 00369 // **debug** 00370 // unsigned char ErrNo[2][16]; // store the occurrence of any kind of error 00371 // unsigned char ErrNo2[16]; // for both MCs and for RX2 00372 // **debug** 00373 00374 // Distance [24] 00375 // field boundaries in mm. Diagonal is 10m 00376 #define BOUND_X_MIN -7070 // left bottom 00377 #define BOUND_Y_MIN -7070 00378 #define BOUND_X_MAX 7070 // top right 00379 #define BOUND_Y_MAX 7070 00380 00381 #define BOUND_FREE 500 // if close to goal don't care of boundaries 00382 #define OBST_THRESHOLD 1500.00 // if far enough don't care of obstacles 00383 #define OBST_MIN_DIST 50 // if too close another avoiding procedure 00384 00385 #define OBST_FIELD 13 // obstacle visibility (13 cells around robot) 00386 #define TABLE_SIZE 16 // precomputed table dimension (16 cells around robot) 00387 00388 // Table to compute Y component for the repulsive vectors [24e] 00389 float VffTableY[33][33] __attribute__((space(auto_psv)))= 00390 { 00391 {-1.38,-1.42,-1.46,-1.48,-1.5,-1.5,-1.49,-1.45,-1.4,-1.31,-1.2,-1.06,-0.89,-0.7,-0.48,-0.24,0,0.24,0.48,0.7,0.89,1.06,1.2,1.31,1.4,1.45,1.49,1.5,1.5,1.48,1.46,1.42,1.38}, 00392 {-1.52,-1.57,-1.62,-1.66,-1.69,-1.71,-1.71,-1.68,-1.63,-1.54,-1.42,-1.26,-1.07,-0.84,-0.58,-0.29,0,0.29,0.58,0.84,1.07,1.26,1.42,1.54,1.63,1.68,1.71,1.71,1.69,1.66,1.62,1.57,1.52}, 00393 {-1.66,-1.74,-1.8,-1.86,-1.91,-1.95,-1.96,-1.95,-1.91,-1.83,-1.7,-1.52,-1.3,-1.02,-0.71,-0.36,0,0.36,0.71,1.02,1.3,1.52,1.7,1.83,1.91,1.95,1.96,1.95,1.91,1.86,1.8,1.74,1.66}, 00394 {-1.83,-1.92,-2.01,-2.09,-2.17,-2.23,-2.27,-2.28,-2.25,-2.17,-2.04,-1.85,-1.59,-1.26,-0.88,-0.45,0,0.45,0.88,1.26,1.59,1.85,2.04,2.17,2.25,2.28,2.27,2.23,2.17,2.09,2.01,1.92,1.83}, 00395 {-2,-2.12,-2.23,-2.35,-2.46,-2.55,-2.62,-2.67,-2.67,-2.61,-2.48,-2.28,-1.98,-1.59,-1.11,-0.57,0,0.57,1.11,1.59,1.98,2.28,2.48,2.61,2.67,2.67,2.62,2.55,2.46,2.35,2.23,2.12,2}, 00396 {-2.19,-2.33,-2.48,-2.63,-2.78,-2.92,-3.04,-3.13,-3.18,-3.16,-3.05,-2.83,-2.49,-2.02,-1.43,-0.74,0,0.74,1.43,2.02,2.49,2.83,3.05,3.16,3.18,3.13,3.04,2.92,2.78,2.63,2.48,2.33,2.19}, 00397 {-2.38,-2.56,-2.75,-2.95,-3.15,-3.35,-3.54,-3.7,-3.81,-3.85,-3.78,-3.58,-3.2,-2.64,-1.89,-0.99,0,0.99,1.89,2.64,3.2,3.58,3.78,3.85,3.81,3.7,3.54,3.35,3.15,2.95,2.75,2.56,2.38}, 00398 {-2.59,-2.8,-3.04,-3.29,-3.56,-3.83,-4.11,-4.36,-4.58,-4.72,-4.74,-4.58,-4.19,-3.51,-2.55,-1.35,0,1.35,2.55,3.51,4.19,4.58,4.74,4.72,4.58,4.36,4.11,3.83,3.56,3.29,3.04,2.8,2.59}, 00399 {-2.8,-3.05,-3.34,-3.66,-4,-4.37,-4.76,-5.15,-5.52,-5.83,-6,-5.96,-5.59,-4.81,-3.57,-1.91,0,1.91,3.57,4.81,5.59,5.96,6,5.83,5.52,5.15,4.76,4.37,4,3.66,3.34,3.05,2.8}, 00400 {-3,-3.31,-3.65,-4.04,-4.48,-4.96,-5.5,-6.07,-6.66,-7.22,-7.66,-7.85,-7.63,-6.79,-5.18,-2.83,0,2.83,5.18,6.79,7.63,7.85,7.66,7.22,6.66,6.07,5.5,4.96,4.48,4.04,3.65,3.31,3}, 00401 {-3.21,-3.56,-3.96,-4.43,-4.97,-5.59,-6.31,-7.11,-8,-8.93,-9.82,-10.49,-10.67,-9.94,-7.91,-4.44,0,4.44,7.91,9.94,10.67,10.49,9.82,8.93,8,7.11,6.31,5.59,4.97,4.43,3.96,3.56,3.21}, 00402 {-3.4,-3.79,-4.26,-4.81,-5.46,-6.24,-7.16,-8.25,-9.53,-11,-12.59,-14.14,-15.24,-15.13,-12.81,-7.54,0,7.54,12.81,15.13,15.24,14.14,12.59,11,9.53,8.25,7.16,6.24,5.46,4.81,4.26,3.79,3.4}, 00403 {-3.57,-4.01,-4.54,-5.17,-5.93,-6.86,-8,-9.42,-11.18,-13.36,-16,-19.05,-22.1,-24,-22.36,-14.27,0,14.27,22.36,24,22.1,19.05,16,13.36,11.18,9.42,8,6.86,5.93,5.17,4.54,4.01,3.57}, 00404 {-3.71,-4.19,-4.77,-5.47,-6.34,-7.42,-8.79,-10.54,-12.83,-15.85,-19.88,-25.22,-32,-39.28,-42.67,-31.62,0,31.62,42.67,39.28,32,25.22,19.88,15.85,12.83,10.54,8.79,7.42,6.34,5.47,4.77,4.19,3.71}, 00405 {-3.82,-4.33,-4.95,-5.71,-6.66,-7.87,-9.43,-11.48,-14.27,-18.14,-23.72,-32.02,-44.72,-64,-88.39,-89.44,0,89.44,88.39,64,44.72,32.02,23.72,18.14,14.27,11.48,9.43,7.87,6.66,5.71,4.95,4.33,3.82}, 00406 {-3.88,-4.41,-5.06,-5.87,-6.87,-8.16,-9.85,-12.12,-15.27,-19.8,-26.66,-37.71,-57.07,-94.87,-178.89,-353.55,0,353.55,178.89,94.87,57.07,37.71,26.66,19.8,15.27,12.12,9.85,8.16,6.87,5.87,5.06,4.41,3.88}, 00407 {-3.91,-4.44,-5.1,-5.92,-6.94,-8.26,-10,-12.35,-15.63,-20.41,-27.78,-40,-62.5,-111.11,-250,-1000,0,1000,250,111.11,62.5,40,27.78,20.41,15.63,12.35,10,8.26,6.94,5.92,5.1,4.44,3.91}, 00408 {-3.88,-4.41,-5.06,-5.87,-6.87,-8.16,-9.85,-12.12,-15.27,-19.8,-26.66,-37.71,-57.07,-94.87,-178.89,-353.55,0,353.55,178.89,94.87,57.07,37.71,26.66,19.8,15.27,12.12,9.85,8.16,6.87,5.87,5.06,4.41,3.88}, 00409 {-3.82,-4.33,-4.95,-5.71,-6.66,-7.87,-9.43,-11.48,-14.27,-18.14,-23.72,-32.02,-44.72,-64,-88.39,-89.44,0,89.44,88.39,64,44.72,32.02,23.72,18.14,14.27,11.48,9.43,7.87,6.66,5.71,4.95,4.33,3.82}, 00410 {-3.71,-4.19,-4.77,-5.47,-6.34,-7.42,-8.79,-10.54,-12.83,-15.85,-19.88,-25.22,-32,-39.28,-42.67,-31.62,0,31.62,42.67,39.28,32,25.22,19.88,15.85,12.83,10.54,8.79,7.42,6.34,5.47,4.77,4.19,3.71}, 00411 {-3.57,-4.01,-4.54,-5.17,-5.93,-6.86,-8,-9.42,-11.18,-13.36,-16,-19.05,-22.1,-24,-22.36,-14.27,0,14.27,22.36,24,22.1,19.05,16,13.36,11.18,9.42,8,6.86,5.93,5.17,4.54,4.01,3.57}, 00412 {-3.4,-3.79,-4.26,-4.81,-5.46,-6.24,-7.16,-8.25,-9.53,-11,-12.59,-14.14,-15.24,-15.13,-12.81,-7.54,0,7.54,12.81,15.13,15.24,14.14,12.59,11,9.53,8.25,7.16,6.24,5.46,4.81,4.26,3.79,3.4}, 00413 {-3.21,-3.56,-3.96,-4.43,-4.97,-5.59,-6.31,-7.11,-8,-8.93,-9.82,-10.49,-10.67,-9.94,-7.91,-4.44,0,4.44,7.91,9.94,10.67,10.49,9.82,8.93,8,7.11,6.31,5.59,4.97,4.43,3.96,3.56,3.21}, 00414 {-3,-3.31,-3.65,-4.04,-4.48,-4.96,-5.5,-6.07,-6.66,-7.22,-7.66,-7.85,-7.63,-6.79,-5.18,-2.83,0,2.83,5.18,6.79,7.63,7.85,7.66,7.22,6.66,6.07,5.5,4.96,4.48,4.04,3.65,3.31,3}, 00415 {-2.8,-3.05,-3.34,-3.66,-4,-4.37,-4.76,-5.15,-5.52,-5.83,-6,-5.96,-5.59,-4.81,-3.57,-1.91,0,1.91,3.57,4.81,5.59,5.96,6,5.83,5.52,5.15,4.76,4.37,4,3.66,3.34,3.05,2.8}, 00416 {-2.59,-2.8,-3.04,-3.29,-3.56,-3.83,-4.11,-4.36,-4.58,-4.72,-4.74,-4.58,-4.19,-3.51,-2.55,-1.35,0,1.35,2.55,3.51,4.19,4.58,4.74,4.72,4.58,4.36,4.11,3.83,3.56,3.29,3.04,2.8,2.59}, 00417 {-2.38,-2.56,-2.75,-2.95,-3.15,-3.35,-3.54,-3.7,-3.81,-3.85,-3.78,-3.58,-3.2,-2.64,-1.89,-0.99,0,0.99,1.89,2.64,3.2,3.58,3.78,3.85,3.81,3.7,3.54,3.35,3.15,2.95,2.75,2.56,2.38}, 00418 {-2.19,-2.33,-2.48,-2.63,-2.78,-2.92,-3.04,-3.13,-3.18,-3.16,-3.05,-2.83,-2.49,-2.02,-1.43,-0.74,0,0.74,1.43,2.02,2.49,2.83,3.05,3.16,3.18,3.13,3.04,2.92,2.78,2.63,2.48,2.33,2.19}, 00419 {-2,-2.12,-2.23,-2.35,-2.46,-2.55,-2.62,-2.67,-2.67,-2.61,-2.48,-2.28,-1.98,-1.59,-1.11,-0.57,0,0.57,1.11,1.59,1.98,2.28,2.48,2.61,2.67,2.67,2.62,2.55,2.46,2.35,2.23,2.12,2}, 00420 {-1.83,-1.92,-2.01,-2.09,-2.17,-2.23,-2.27,-2.28,-2.25,-2.17,-2.04,-1.85,-1.59,-1.26,-0.88,-0.45,0,0.45,0.88,1.26,1.59,1.85,2.04,2.17,2.25,2.28,2.27,2.23,2.17,2.09,2.01,1.92,1.83}, 00421 {-1.66,-1.74,-1.8,-1.86,-1.91,-1.95,-1.96,-1.95,-1.91,-1.83,-1.7,-1.52,-1.3,-1.02,-0.71,-0.36,0,0.36,0.71,1.02,1.3,1.52,1.7,1.83,1.91,1.95,1.96,1.95,1.91,1.86,1.8,1.74,1.66}, 00422 {-1.52,-1.57,-1.62,-1.66,-1.69,-1.71,-1.71,-1.68,-1.63,-1.54,-1.42,-1.26,-1.07,-0.84,-0.58,-0.29,0,0.29,0.58,0.84,1.07,1.26,1.42,1.54,1.63,1.68,1.71,1.71,1.69,1.66,1.62,1.57,1.52}, 00423 {-1.38,-1.42,-1.46,-1.48,-1.5,-1.5,-1.49,-1.45,-1.4,-1.31,-1.2,-1.06,-0.89,-0.7,-0.48,-0.24,0,0.24,0.48,0.7,0.89,1.06,1.2,1.31,1.4,1.45,1.49,1.5,1.5,1.48,1.46,1.42,1.38} 00424 } ;//in program memory space 00425 00426 // Table to compute X component for the repulsive vectors [24e] 00427 float VffTableX[33][33] __attribute__((space(auto_psv)))= 00428 { 00429 {-1.38,-1.52,-1.66,-1.83,-2.00,-2.19,-2.38,-2.59,-2.80,-3.00,-3.21,-3.40,-3.57,-3.71,-3.82,-3.88,-3.91,-3.88,-3.82,-3.71,-3.57,-3.40,-3.21,-3.00,-2.80,-2.59,-2.38,-2.19,-2.00,-1.83,-1.66,-1.52,-1.38}, 00430 {-1.42,-1.57,-1.74,-1.92,-2.12,-2.33,-2.56,-2.80,-3.05,-3.31,-3.56,-3.79,-4.01,-4.19,-4.33,-4.41,-4.44,-4.41,-4.33,-4.19,-4.01,-3.79,-3.56,-3.31,-3.05,-2.80,-2.56,-2.33,-2.12,-1.92,-1.74,-1.57,-1.42}, 00431 {-1.46,-1.62,-1.80,-2.01,-2.23,-2.48,-2.75,-3.04,-3.34,-3.65,-3.96,-4.26,-4.54,-4.77,-4.95,-5.06,-5.10,-5.06,-4.95,-4.77,-4.54,-4.26,-3.96,-3.65,-3.34,-3.04,-2.75,-2.48,-2.23,-2.01,-1.80,-1.62,-1.46}, 00432 {-1.48,-1.66,-1.86,-2.09,-2.35,-2.63,-2.95,-3.29,-3.66,-4.04,-4.43,-4.81,-5.17,-5.47,-5.71,-5.87,-5.92,-5.87,-5.71,-5.47,-5.17,-4.81,-4.43,-4.04,-3.66,-3.29,-2.95,-2.63,-2.35,-2.09,-1.86,-1.66,-1.48}, 00433 {-1.50,-1.69,-1.91,-2.17,-2.46,-2.78,-3.15,-3.56,-4.00,-4.48,-4.97,-5.46,-5.93,-6.34,-6.66,-6.87,-6.94,-6.87,-6.66,-6.34,-5.93,-5.46,-4.97,-4.48,-4.00,-3.56,-3.15,-2.78,-2.46,-2.17,-1.91,-1.69,-1.50}, 00434 {-1.50,-1.71,-1.95,-2.23,-2.55,-2.92,-3.35,-3.83,-4.37,-4.96,-5.59,-6.24,-6.86,-7.42,-7.87,-8.16,-8.26,-8.16,-7.87,-7.42,-6.86,-6.24,-5.59,-4.96,-4.37,-3.83,-3.35,-2.92,-2.55,-2.23,-1.95,-1.71,-1.50}, 00435 {-1.49,-1.71,-1.96,-2.27,-2.62,-3.04,-3.54,-4.11,-4.76,-5.50,-6.31,-7.16,-8.00,-8.79,-9.43,-9.85,-10.00,-9.85,-9.43,-8.79,-8.00,-7.16,-6.31,-5.50,-4.76,-4.11,-3.54,-3.04,-2.62,-2.27,-1.96,-1.71,-1.49}, 00436 {-1.45,-1.68,-1.95,-2.28,-2.67,-3.13,-3.70,-4.36,-5.15,-6.07,-7.11,-8.25,-9.42,-10.54,-11.48,-12.12,-12.35,-12.12,-11.48,-10.54,-9.42,-8.25,-7.11,-6.07,-5.15,-4.36,-3.70,-3.13,-2.67,-2.28,-1.95,-1.68,-1.45}, 00437 {-1.40,-1.63,-1.91,-2.25,-2.67,-3.18,-3.81,-4.58,-5.52,-6.66,-8.00,-9.53,-11.18,-12.83,-14.27,-15.27,-15.63,-15.27,-14.27,-12.83,-11.18,-9.53,-8.00,-6.66,-5.52,-4.58,-3.81,-3.18,-2.67,-2.25,-1.91,-1.63,-1.40}, 00438 {-1.31,-1.54,-1.83,-2.17,-2.61,-3.16,-3.85,-4.72,-5.83,-7.22,-8.93,-11.00,-13.36,-15.85,-18.14,-19.80,-20.41,-19.80,-18.14,-15.85,-13.36,-11.00,-8.93,-7.22,-5.83,-4.72,-3.85,-3.16,-2.61,-2.17,-1.83,-1.54,-1.31}, 00439 {-1.20,-1.42,-1.70,-2.04,-2.48,-3.05,-3.78,-4.74,-6.00,-7.66,-9.82,-12.59,-16.00,-19.88,-23.72,-26.66,-27.78,-26.66,-23.72,-19.88,-16.00,-12.59,-9.82,-7.66,-6.00,-4.74,-3.78,-3.05,-2.48,-2.04,-1.70,-1.42,-1.20}, 00440 {-1.06,-1.26,-1.52,-1.85,-2.28,-2.83,-3.58,-4.58,-5.96,-7.85,-10.49,-14.14,-19.05,-25.22,-32.02,-37.71,-40.00,-37.71,-32.02,-25.22,-19.05,-14.14,-10.49,-7.85,-5.96,-4.58,-3.58,-2.83,-2.28,-1.85,-1.52,-1.26,-1.06}, 00441 {-0.89,-1.07,-1.30,-1.59,-1.98,-2.49,-3.20,-4.19,-5.59,-7.63,-10.67,-15.24,-22.10,-32.00,-44.72,-57.07,-62.50,-57.07,-44.72,-32.00,-22.10,-15.24,-10.67,-7.63,-5.59,-4.19,-3.20,-2.49,-1.98,-1.59,-1.30,-1.07,-0.89}, 00442 {-0.70,-0.84,-1.02,-1.26,-1.59,-2.02,-2.64,-3.51,-4.81,-6.79,-9.94,-15.13,-24.00,-39.28,-64.00,-94.87,-111.11,-94.87,-64.00,-39.28,-24.00,-15.13,-9.94,-6.79,-4.81,-3.51,-2.64,-2.02,-1.59,-1.26,-1.02,-0.84,-0.70}, 00443 {-0.48,-0.58,-0.71,-0.88,-1.11,-1.43,-1.89,-2.55,-3.57,-5.18,-7.91,-12.81,-22.36,-42.67,-88.39,-178.89,-250.00,-178.89,-88.39,-42.67,-22.36,-12.81,-7.91,-5.18,-3.57,-2.55,-1.89,-1.43,-1.11,-0.88,-0.71,-0.58,-0.48}, 00444 {-0.24,-0.29,-0.36,-0.45,-0.57,-0.74,-0.99,-1.35,-1.91,-2.83,-4.44,-7.54,-14.27,-31.62,-89.44,-353.55,-1000.00,-353.55,-89.44,-31.62,-14.27,-7.54,-4.44,-2.83,-1.91,-1.35,-0.99,-0.74,-0.57,-0.45,-0.36,-0.29,-0.24}, 00445 {0.00,0.00,0.00,0.00,0.00,0.00,0.00,0.00,0.00,0.00,0.00,0.00,0.00,0.00,0.00,0.00,0.00,0.00,0.00,0.00,0.00,0.00,0.00,0.00,0.00,0.00,0.00,0.00,0.00,0.00,0.00,0.00,0.00}, 00446 {0.24,0.29,0.36,0.45,0.57,0.74,0.99,1.35,1.91,2.83,4.44,7.54,14.27,31.62,89.44,353.55,1000.00,353.55,89.44,31.62,14.27,7.54,4.44,2.83,1.91,1.35,0.99,0.74,0.57,0.45,0.36,0.29,0.24}, 00447 {0.48,0.58,0.71,0.88,1.11,1.43,1.89,2.55,3.57,5.18,7.91,12.81,22.36,42.67,88.39,178.89,250.00,178.89,88.39,42.67,22.36,12.81,7.91,5.18,3.57,2.55,1.89,1.43,1.11,0.88,0.71,0.58,0.48}, 00448 {0.70,0.84,1.02,1.26,1.59,2.02,2.64,3.51,4.81,6.79,9.94,15.13,24.00,39.28,64.00,94.87,111.11,94.87,64.00,39.28,24.00,15.13,9.94,6.79,4.81,3.51,2.64,2.02,1.59,1.26,1.02,0.84,0.70}, 00449 {0.89,1.07,1.30,1.59,1.98,2.49,3.20,4.19,5.59,7.63,10.67,15.24,22.10,32.00,44.72,57.07,62.50,57.07,44.72,32.00,22.10,15.24,10.67,7.63,5.59,4.19,3.20,2.49,1.98,1.59,1.30,1.07,0.89}, 00450 {1.06,1.26,1.52,1.85,2.28,2.83,3.58,4.58,5.96,7.85,10.49,14.14,19.05,25.22,32.02,37.71,40.00,37.71,32.02,25.22,19.05,14.14,10.49,7.85,5.96,4.58,3.58,2.83,2.28,1.85,1.52,1.26,1.06}, 00451 {1.20,1.42,1.70,2.04,2.48,3.05,3.78,4.74,6.00,7.66,9.82,12.59,16.00,19.88,23.72,26.66,27.78,26.66,23.72,19.88,16.00,12.59,9.82,7.66,6.00,4.74,3.78,3.05,2.48,2.04,1.70,1.42,1.20}, 00452 {1.31,1.54,1.83,2.17,2.61,3.16,3.85,4.72,5.83,7.22,8.93,11.00,13.36,15.85,18.14,19.80,20.41,19.80,18.14,15.85,13.36,11.00,8.93,7.22,5.83,4.72,3.85,3.16,2.61,2.17,1.83,1.54,1.31}, 00453 {1.40,1.63,1.91,2.25,2.67,3.18,3.81,4.58,5.52,6.66,8.00,9.53,11.18,12.83,14.27,15.27,15.63,15.27,14.27,12.83,11.18,9.53,8.00,6.66,5.52,4.58,3.81,3.18,2.67,2.25,1.91,1.63,1.40}, 00454 {1.45,1.68,1.95,2.28,2.67,3.13,3.70,4.36,5.15,6.07,7.11,8.25,9.42,10.54,11.48,12.12,12.35,12.12,11.48,10.54,9.42,8.25,7.11,6.07,5.15,4.36,3.70,3.13,2.67,2.28,1.95,1.68,1.45}, 00455 {1.49,1.71,1.96,2.27,2.62,3.04,3.54,4.11,4.76,5.50,6.31,7.16,8.00,8.79,9.43,9.85,10.00,9.85,9.43,8.79,8.00,7.16,6.31,5.50,4.76,4.11,3.54,3.04,2.62,2.27,1.96,1.71,1.49}, 00456 {1.50,1.71,1.95,2.23,2.55,2.92,3.35,3.83,4.37,4.96,5.59,6.24,6.86,7.42,7.87,8.16,8.26,8.16,7.87,7.42,6.86,6.24,5.59,4.96,4.37,3.83,3.35,2.92,2.55,2.23,1.95,1.71,1.50}, 00457 {1.50,1.69,1.91,2.17,2.46,2.78,3.15,3.56,4.00,4.48,4.97,5.46,5.93,6.34,6.66,6.87,6.94,6.87,6.66,6.34,5.93,5.46,4.97,4.48,4.00,3.56,3.15,2.78,2.46,2.17,1.91,1.69,1.50}, 00458 {1.48,1.66,1.86,2.09,2.35,2.63,2.95,3.29,3.66,4.04,4.43,4.81,5.17,5.47,5.71,5.87,5.92,5.87,5.71,5.47,5.17,4.81,4.43,4.04,3.66,3.29,2.95,2.63,2.35,2.09,1.86,1.66,1.48}, 00459 {1.46,1.62,1.80,2.01,2.23,2.48,2.75,3.04,3.34,3.65,3.96,4.26,4.54,4.77,4.95,5.06,5.10,5.06,4.95,4.77,4.54,4.26,3.96,3.65,3.34,3.04,2.75,2.48,2.23,2.01,1.80,1.62,1.46}, 00460 {1.42,1.57,1.74,1.92,2.12,2.33,2.56,2.80,3.05,3.31,3.56,3.79,4.01,4.19,4.33,4.41,4.44,4.41,4.33,4.19,4.01,3.79,3.56,3.31,3.05,2.80,2.56,2.33,2.12,1.92,1.74,1.57,1.42}, 00461 {1.38,1.52,1.66,1.83,2.00,2.19,2.38,2.59,2.80,3.00,3.21,3.40,3.57,3.71,3.82,3.88,3.91,3.88,3.82,3.71,3.57,3.40,3.21,3.00,2.80,2.59,2.38,2.19,2.00,1.83,1.66,1.52,1.38} 00462 } ;//in program memory space 00463 00464 float VObX[3]; // relative position of obstacles 00465 float VObY[3]; 00466 00467 tPID DistPIDstruct; 00468 fractional DistabcCoefficient[3] 00469 __attribute__ ((section (".xbss, bss, xmemory"))); 00470 fractional DistcontrolHistory[3] 00471 __attribute__ ((section (".ybss, bss, ymemory"))); 00472 fractional DistKCoeffs[] = {0,0,0}; 00473 #define DIST_KP DistKCoeffs[0] // K parameters 00474 #define DIST_KI DistKCoeffs[1] 00475 #define DIST_KD DistKCoeffs[2] 00476 #define DIST_PID_DES DistPIDstruct.controlReference // desired distance 00477 #define DIST_PID_MES DistPIDstruct.measuredOutput // measured distance 00478 #define DIST_PID_OUT DistPIDstruct.controlOutput // PID output 00479 00480 #define DIST_ENABLE_FLAG VARbits1.bit14 // compute distance if enabled 00481 float PosXdes = 0; // desired X position coordinate 00482 float PosYdes = 0; // desired Y position coordinate 00483 #define MIN_GOAL_DIST 450 // if less then decrease speed proportionally 00484 float VelDecr; // multiplied by VelDesM correct the speed [24d] 00485 00486 #define DIST_OK_FLAG VARbits1.bit11 // target distance reached 00487 #define MIN_DIST_ERR 3 // acceptable distance error in mm 00488 00489 // slower cycles setting [13] 00490 volatile unsigned char Cycle1; 00491 volatile unsigned char Cycle2; 00492 #define CYCLE1_FLAG VOLbits1.bit2 00493 #define CYCLE2_FLAG VOLbits1.bit3 00494 #define CICLE1_TMO 10 // starts first cycle every Xms 00495 #define CICLE2_TMO 5 // starts second cycle every N first cycle times 00496 00497 // Idle time [25] 00498 unsigned long IdleCount = 0; // how many times it executes main loop 00499 volatile unsigned long IdleSample=0;//sampling time to compute idle percentage 00500 #define IDLE_CYCLE 20 00501 // LOOP_TIME = CICLE1_TMO * CICLE2_TMO = 50ms 00502 // so -> IDLE_SAMPLE_TIME = LOOP_TIME * IDLE_CYCLE = 1000ms 00503 // in order to have percentage as int IDLE_SAMPLE_TIME * 10,000 00504 #define IDLE_SAMPLE_TIME 10000000 00505 // time in ns to execute main without any call 00506 // measured with stopwatch in SIM [25a] 00507 #define IDLE_TIME_PERIOD 3000 // without optimizations 00508 // #define IDLE_TIME_PERIOD 2225 // with optimizations 3 00509 // #define IDLE_TIME_PERIOD 2525 // with optimizations s 00510 00511 int IdlePerc; // idle percentage 00512 00513 volatile unsigned int BlinkPeriod ; // LED1 blinking period (ms) 00514 unsigned int BlinkOn; // LED1 on time (ms) 00515 #define NORM_BLINK_PER 1000 // Period for OK condition 00516 #define NORM_BLINK_ON 200 // ON TIME for OK condition 00517 #define K_ERR_BLINK_PER 4000// Period for ERR condition 00518 #define K_ERR_BLINK_ON 2000 // ON TIME for ERR condition 00519 00520 00521 int ErrCode; // Error Code 00522 00523 #define CONSOLE_DEBUG VARbits1.bit13 // [30] 00524 00525 int VelInt[2]; // speed in mm/s as an integer 00526 00527 // Scheduler 00528 unsigned char SchedPtr = 0; // point to current step into the sequence 00529 #define SCHED_ANGLE_FLAG VARbits1.bit8 // wait for target angle 00530 #define SCHED_DIST_FLAG VARbits1.bit0 // wait for target distance 00531 00532 /* 00533 // sequence array initialized for a 360� turn cw in 4 steps 00534 int SchedValues[16][4]= { {6,100,0,0}, 00535 {3,0,90,0}, 00536 {6,50,0,0}, 00537 {3,0,180,0}, 00538 {6,50,0,0}, 00539 {3,0,270,0}, 00540 {6,50,0,0}, 00541 {3,0,0,0}, 00542 {6,50,0,0}, 00543 {0,0,0,0}, 00544 {0,0,0,0}, 00545 {0,0,0,0}, 00546 {0,0,0,0}, 00547 {0,0,0,0}, 00548 {0,0,0,0}, 00549 {0,0,0,0} }; 00550 */ 00551 /* 00552 // sequence array initialized for a 360� turn ccw in 4 steps 00553 int SchedValues[16][4]= { {6,100,0,0}, 00554 {3,0,270,0}, 00555 {6,50,0,0}, 00556 {3,0,180,0}, 00557 {6,50,0,0}, 00558 {3,0,90,0}, 00559 {6,50,0,0}, 00560 {3,0,0,0}, 00561 {6,50,0,0}, 00562 {0,0,0,0}, 00563 {0,0,0,0}, 00564 {0,0,0,0}, 00565 {0,0,0,0}, 00566 {0,0,0,0}, 00567 {0,0,0,0}, 00568 {0,0,0,0} }; 00569 */ 00570 00571 // sequence array initialized for a mini UMBmark of 1m x 1m CW 00572 int SchedValues[16][4]= { {6,100,0,0}, 00573 {4,0,0,1000}, 00574 {5,300,0,1000}, 00575 {4,0,1000,1000}, 00576 {5,300,1000,1000}, 00577 {4,0,1000,0}, 00578 {5,300,1000,0}, 00579 {4,0,0,0}, 00580 {5,300,0,0}, 00581 {6,80,0,0}, 00582 {3,0,0,0}, 00583 {0,0,0,0}, 00584 {0,0,0,0}, 00585 {0,0,0,0}, 00586 {0,0,0,0}, 00587 {0,0,0,0} }; 00588 00589 /* 00590 // sequence array initialized for a mini UMBmark of 2.5m x 3.5m CCW 00591 // point the robot to 90� 00592 int SchedValues[16][4]= { {6,100,0,0}, 00593 {4,0,0,2500}, 00594 {5,300,0,2500}, 00595 {4,0,-3500,2500}, 00596 {5,300,-3500,2500}, 00597 {4,0,-3500,0}, 00598 {5,300,-3500,0}, 00599 {4,0,0,0}, 00600 {5,300,0,0}, 00601 {6,80,0,0}, 00602 {3,0,0,0}, 00603 {0,0,0,0}, 00604 {0,0,0,0}, 00605 {0,0,0,0}, 00606 {0,0,0,0}, 00607 {0,0,0,0} }; 00608 */ 00609 /* 00610 // sequence array initialized for a maxi UMBmark of 4m forward 00611 int SchedValues[16][4]= { {6,100,0,0}, 00612 {4,0,0,4000}, 00613 {5,300,0,4000}, 00614 {4,0,4000,4000}, 00615 {5,300,4000,4000}, 00616 {4,0,4000,0}, 00617 {5,300,4000,0}, 00618 {4,0,0,0}, 00619 {5,300,0,0}, 00620 {3,0,0,0}, 00621 {0,0,0,0}, 00622 {0,0,0,0}, 00623 {0,0,0,0}, 00624 {0,0,0,0}, 00625 {0,0,0,0}, 00626 {0,0,0,0} }; 00627 */ 00628 /* 00629 // sequence array initialized for a UMBmark of 4m reward 00630 int SchedValues[16][4]= { {6,100,0,0}, 00631 {4,0,4000,0}, 00632 {5,300,4000,0}, 00633 {4,0,4000,4000}, 00634 {5,300,4000,4000}, 00635 {4,0,0,4000}, 00636 {5,300,0,4000}, 00637 {4,0,0,0}, 00638 {5,300,0,0}, 00639 {6,80,0,0}, 00640 {3,0,0,0}, 00641 {0,0,0,0}, 00642 {0,0,0,0}, 00643 {0,0,0,0}, 00644 {0,0,0,0}, 00645 {0,0,0,0} }; 00646 */ 00647 /* 00648 // sequence array initialized for a 10m straight forward 00649 int SchedValues[16][4]= { {6,50,0,0}, 00650 {5,300,0,10000}, 00651 {3,0,0,0}, 00652 {0,0,0,0}, 00653 {0,0,0,0}, 00654 {0,0,0,0}, 00655 {0,0,0,0}, 00656 {0,0,0,0}, 00657 {0,0,0,0}, 00658 {0,0,0,0}, 00659 {0,0,0,0}, 00660 {0,0,0,0}, 00661 {0,0,0,0}, 00662 {0,0,0,0}, 00663 {0,0,0,0}, 00664 {0,0,0,0} }; 00665 */ 00666 /* 00667 // sequence array initialized for a RTC 10m diagonal 00668 int SchedValues[16][4]= { {6,100,0,0}, 00669 {4,0,7200,7200}, 00670 {5,300,7200,7200}, 00671 {4,0,0,0}, 00672 {5,300,0,0}, 00673 {3,0,0,0}, 00674 {0,0,0,0}, 00675 {0,0,0,0}, 00676 {0,0,0,0}, 00677 {0,0,0,0}, 00678 {0,0,0,0}, 00679 {0,0,0,0}, 00680 {0,0,0,0}, 00681 {0,0,0,0}, 00682 {0,0,0,0}, 00683 {0,0,0,0} }; 00684 */ 00685 00686 volatile int RtTimer; // real time delay using T1 timer 00687 #define RT_TIMER_FLAG VARbits1.bit10 // real time timer enable 00688 #define TIMER_OK_FLAG VARbits1.bit9 // time elapsed 00689 #define SCHEDULER_FLAG VARbits1.bit5 // enable scheduler [32a] 00690 00691 // dist in cm from objects on the three sides 00692 unsigned int Obj[3] = {0x1000, 0x1000, 0x1000}; 00693 unsigned char Target[3] = {0, 0, 0};//highest sensor value on the three sides 00694 volatile unsigned int RndTimer=0;// Timer for randomly avoid central obstacles 00695 #define RND_TIMEOUT 100 // 100 * 50ms = 5 seconds 00696 #define RND_FLAG VARbits1.bit4 // randomly decide in which direction to turn 00697 00698 // simulated EEPROM data addresses [33] 00699 #define EE_KVEL1_H 0 00700 #define EE_KVEL1_L 1 00701 #define EE_KVEL2_H 2 00702 #define EE_KVEL2_L 3 00703 #define EE_CHK_KVEL 4 00704 00705 #define EE_ANGLE_KP 5 00706 #define EE_ANGLE_KI 6 00707 #define EE_ANGLE_KD 7 00708 #define EE_CHK_ANGLE 8 00709 00710 #define EE_KP1 9 00711 #define EE_KI1 10 00712 #define EE_KD1 11 00713 #define EE_KP2 12 00714 #define EE_KI2 13 00715 #define EE_KD2 14 00716 #define EE_CHK_SPEED 15 00717 00718 #define EE_KSP1_H 16 00719 #define EE_KSP1_L 17 00720 #define EE_KSP2_H 18 00721 #define EE_KSP2_L 19 00722 #define EE_AXLE_H 20 00723 #define EE_AXLE_L 21 00724 #define EE_CHK_MECH 22 00725 00726 #define EE_DIST_KP 23 00727 #define EE_DIST_KI 24 00728 #define EE_DIST_KD 25 00729 #define EE_CHK_DIST 26 00730 00731 #define EE_CHK_SCHED 27 00732 #define EE_SCHED 28 // starting address for 128 locations 00733 00734 unsigned char ResetCount = 0; // [28] 00735 00736 // VARbits2.bit0 and up available 00737 // VOLbits1.bit9 and up available 00738 00739 /* Definitions end ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ 00740
1.7.6 
