dsPid33
|
00001 /* //////////////////////////////////////////////////////////////////////////// 00002 ** File: dsPid33.c 00003 */ 00004 unsigned char Ver[] = "dsPid33 2.2.6 Guiott 12-11"; // 26+1 char 00039 #include "dsPID33_definitions.h" 00040 #include "DEE Emulation 16-bit.h" 00041 00042 int main (void) 00043 { 00044 Settings(); 00045 00046 //[1] 00047 LED1 = 1; 00048 LED2 = 0; 00049 // MOTOR_ENABLE1 = 0; 00050 // MOTOR_ENABLE2 = 0; 00051 00052 Tmr2OvflwCount1=0; 00053 Tmr2OvflwCount2=0; 00054 IC1_FIRST = 0; 00055 IC2_FIRST = 0; 00056 RAMP_FLAG1 = 0; 00057 RAMP_FLAG2 = 0; 00058 PID_REF1 = 0; 00059 PID_REF2 = 0; 00060 VelDecr = 1; 00061 00062 // to start first and second slower cycles over 1ms cycle 00063 Cycle1 = 0; 00064 Cycle2 = 0; 00065 CYCLE1_FLAG = 0; 00066 CYCLE2_FLAG = 0; 00067 00068 ORIENTATION_FLAG = 0; 00069 MAP_SEND_FLAG=0; 00070 00071 /*Delay needed to ensure the program memory is not modified before 00072 verification in programming procedure 00073 http://forum.microchip.com/tm.aspx?m=326442 */ 00074 DelayN1ms(30); 00075 00076 DataEEInit(); // Initialize flash memory writing [33] 00077 dataEEFlags.val = 0; 00078 00079 BlinkPeriod = NORM_BLINK_PER; // LED1 blinking period (ms) 00080 BlinkOn = NORM_BLINK_ON; // LED1 on time (ms) 00081 00082 ConstantsRead(); // get stored constant values from FLASH 00083 00084 InitPid1(); 00085 InitPid2(); 00086 00087 ANGLE_PID_DES=0; 00088 InitAnglePid(); 00089 00090 DIST_PID_DES=0; 00091 InitDistPid(); 00092 00093 00094 CONSOLE_DEBUG = 0; 00095 00096 ADC_CALC_FLAG = 0; 00097 IdleSample = 0; 00098 IdleCount = 0; 00099 00100 DIST_OK_FLAG = 1; // to kick start the scheduler sequence 00101 00102 SchedPtr = 0; // [32] 00103 00104 RT_TIMER_FLAG = 0; // disable real time timer 00105 UsartSetting(); // initialize Usart1 00106 Usart2Setting(); // initialize Usart2 00107 TxContFlag = 0; 00108 00109 ResetCount = 0; 00110 00111 ISR_Settings(); // Configures and enables ISRs 00112 00113 // Send string 'Ver' 00114 for (i=0; i<26; i++) 00115 { 00116 UartTxBuff[i]=Ver[i]; 00117 } 00118 DMA6CNT = 25; // # of DMA requests 00119 DMA6CONbits.CHEN = 1; // Re-enable DMA Channel 00120 DMA6REQbits.FORCE = 1; // Manual mode: Kick-start the first transfer 00121 00122 MAP_BUFF_FLAG=1; 00123 00124 00125 /*===========================================================================*/ 00126 /* Main loop */ 00127 /*===========================================================================*/ 00128 00129 while (1) // start of idle cycle [25a] 00130 { 00133 /* ----------------------------------------- one character coming from UART1 */ 00134 if (UartRxPtrIn != UartRxPtrOut) UartRx(); // [6d] 00135 00136 /* ----------------------------------------- one character coming from UART2 */ 00137 if (Uart2RxPtrIn != Uart2RxPtrOut) Uart2Rx(); // [6zd] 00138 00139 /* ------------------------ a command is coming from serial interface 1 or 2 */ 00140 if ((UartRxStatus == 99) || (Uart2RxStatus == 99)) Parser(); 00141 00142 /* ------------------------------------- PID and speed calculation every 1ms */ 00143 if (PID1_CALC_FLAG) Pid1(); 00144 if (PID2_CALC_FLAG) Pid2(); 00145 00146 /* ----------------------------------------- DeadReckonig field mapping [22] */ 00147 if (CYCLE1_FLAG) DeadReckoning(); 00148 00149 /* ---------------------------------------------------- Orientation PID [23] */ 00150 if (ORIENTATION_FLAG) Orientation(); 00151 00152 /* ----------------------------------------------------------- Distance [24] */ 00153 if (CYCLE2_FLAG) Navigation(); 00154 00155 /*-- continuos parameters transmission without request (just for debug) [31] */ 00156 if (TxContFlag && UartContTxTimer<=0) TxCont(); 00157 00158 /*----------------------------------------- ADC value average calculus [2a] */ 00159 if (ADC_CALC_FLAG) AdcCalc(); 00160 00161 /*---------------------------------------------------------- Real time timer */ 00162 if (RT_TIMER_FLAG) 00163 { 00164 if (RtTimer <= 0) 00165 { 00166 TIMER_OK_FLAG = 1; 00167 RT_TIMER_FLAG = 0; 00168 } 00169 } 00170 00171 /*-------------------------------------------------- Actions scheduler [32] */ 00172 if (SCHEDULER_FLAG) // [32a] 00173 { 00174 if (ANGLE_OK_FLAG || DIST_OK_FLAG || TIMER_OK_FLAG) Scheduler(); 00175 } 00176 00177 /*-------------------------------------------------- To send map to console */ 00178 if (MAP_SEND_FLAG) 00179 { 00180 if (Vel[R]==0 && Vel[L]==0) 00181 { 00182 SendMap(); 00183 } 00184 } 00185 00186 /*--------------------------------------------------- Heartbeat led linking */ 00187 if (Blink == BlinkOn) 00188 { 00189 LED1 = 0; 00190 LED2 = 1; 00191 Blink ++; 00192 } 00193 00194 if (Blink >= BlinkPeriod) 00195 { 00196 LED1 = 1; 00197 LED2 = 0; 00198 Blink = 0; 00199 } 00200 00201 /*------------------------------------------- Idle percentage calculus [25] */ 00202 IdleCount ++; // [25] 00203 if (IdleSample >= IDLE_CYCLE) 00204 { 00205 IdlePerc=(long)(IdleCount * IDLE_TIME_PERIOD) / (long)(IDLE_SAMPLE_TIME); 00206 IdleCount = 0; 00207 IdleSample = 0; 00208 } 00209 00210 Nop(); // end of idle cycle [25a] 00211 00212 }/*....Main loop*/ 00213 00214 }/*.....................................................................Main */ 00215 00216 00217 /*===========================================================================*/ 00218 /* Functions */ 00219 /*===========================================================================*/ 00220 00221 void SendMap(void) 00222 {// send one row of grid map to console 00223 int TmpMap; 00224 00225 if(MAP_BUFF_FLAG) 00226 {// DMA have sent the whole buffer 00227 MAP_BUFF_FLAG=0; // will be set again by DMA ISR 00228 00229 UartTmpBuff[0][SendMapPort]=MapSendIndx; 00230 // starting points for X and Y coordinates 00231 TmpMap = Xshift; 00232 UartTmpBuff[1][SendMapPort]=TmpMap>>8; 00233 UartTmpBuff[2][SendMapPort]=TmpMap; 00234 TmpMap = Yshift; 00235 UartTmpBuff[3][SendMapPort]=TmpMap>>8; 00236 UartTmpBuff[4][SendMapPort]=TmpMap; 00237 00238 for (i=0; i<X_SIZE; i++) 00239 {// put in buffer both nib0 and nib1 for that X coordinate 00240 UartTmpBuff[i+5][SendMapPort]=MapXY[i][MapSendIndx].UC; 00241 } 00242 00243 TxParameters('$', X_SIZE+6, SendMapPort); 00244 MAP_SEND_FLAG=0; 00245 } 00246 } 00247 00248 void ThetaDesF(float Angle) 00249 { 00252 ThetaDes = Angle; 00253 ThetaDesRef = ThetaDes; 00254 PIDInit(&AnglePIDstruct); 00255 } 00256 00257 void Parser (void) 00258 { 00261 int ParserCount; // parser index 00262 int C1; // generic counter 00263 int C2; 00264 int C3; 00265 int TmpChk; 00266 00267 unsigned int Ktmp; // temp for PID coefficients 00268 int Ptmp; // temp for position values 00269 int CurrTmp; // temp for motors current value 00270 int Alpha; // rotation angle in degrees 00271 long Tmp1Long; // temp to combine long 00272 long Tmp2Long; 00273 00274 const float AngleS = 0.785398163; // 45� angle for object on side 00275 00276 if (UartRxStatus == 99) // the command come from UART1 00277 { 00278 TmpPtr = UartRxPtrData; // last data of header 00279 UartRxStatus = 0; 00280 Port=0; 00281 } 00282 else // the command come from UART2 00283 { 00284 TmpPtr2 = Uart2RxPtrData; // last data of header 00285 Uart2RxStatus = 0; 00286 Port=1; 00287 } 00288 00289 00290 if (UartRxCmd[ResetPort] != '*') // [28] 00291 { 00292 ResetCount = 0; 00293 } 00294 00295 switch (UartRxCmd[Port]) 00296 { 00297 // --- Navigation values read 00298 case 'A': // all parameters request (mean) 00299 // VelMes = Int -> 2 byte (mm/s) 00300 // Ptmp = (VelMes[R] + VelMes[L]) >> 1; // average speed 00301 VelInt[R]=(long)(Vel[R] * 1000)>>15; // VelR 00302 VelInt[L]=(long)(Vel[L] * 1000)>>15; // VelL 00303 Ptmp = (VelInt[R] + VelInt[L]) >> 1; // average speed 00304 if (CONSOLE_DEBUG) //[30] 00305 { 00306 Ptmp = VelDesM; 00307 ADCValue[R]=abs(VelDesM); 00308 ADCValue[L]=abs(VelDesM); 00309 ThetaMes=ThetaDes; 00310 } 00311 UartTmpBuff[0][Port]=Ptmp>>8; 00312 UartTmpBuff[1][Port]=Ptmp; 00313 // Curr = int -> 2byte (mA) 00314 CurrTmp = ADCValue[R]+ADCValue[L]; // total current 00315 UartTmpBuff[2][Port]=CurrTmp>>8; 00316 UartTmpBuff[3][Port]=CurrTmp; 00317 // PosXmes rounded in a Int -> 2 byte (mm) 00318 Ptmp = FLOAT2INT(PosXmes); // PosX 00319 UartTmpBuff[4][Port]=Ptmp>>8; 00320 UartTmpBuff[5][Port]=Ptmp; 00321 // PosYmes rounded in a Int -> 2 byte (mm) 00322 Ptmp = FLOAT2INT(PosYmes); // PosY 00323 UartTmpBuff[6][Port]=Ptmp>>8; 00324 UartTmpBuff[7][Port]=Ptmp; 00325 // ThetaMes rounded in a Int -> 2 byte (degrees) 00326 Ptmp = ThetaMes * RAD2DEG; // Theta 00327 Alpha = FLOAT2INT(Ptmp); 00328 UartTmpBuff[8][Port]=Alpha>>8; 00329 UartTmpBuff[9][Port]=Alpha; 00330 // angle reading from sensors board compass (deg * 10) 00331 UartTmpBuff[10][Port]=AngleCmp>>8; 00332 UartTmpBuff[11][Port]=AngleCmp; 00333 // idle time in % 00334 UartTmpBuff[12][Port]=IdlePerc; 00335 00336 TxParameters('A',13, Port); 00337 break; 00338 00339 case 'a': // all parameters request (details) 00340 // VelInt = Int -> 2 byte 00341 VelInt[R]=(long)(Vel[R] * 1000)>>15; // VelL 00342 VelInt[L]=(long)(Vel[L] * 1000)>>15; // VelL 00343 if (CONSOLE_DEBUG) //[30] 00344 { 00345 VelInt[R]= VelDesM; 00346 VelInt[L]= VelDesM; 00347 ADCValue[R]=abs(VelDesM); 00348 ADCValue[L]=abs(VelDesM); 00349 ThetaMes=ThetaDes; 00350 } 00351 UartTmpBuff[0][Port]=VelInt[R]>>8; 00352 UartTmpBuff[1][Port]=VelInt[R]; 00353 UartTmpBuff[2][Port]=VelInt[L]>>8; 00354 UartTmpBuff[3][Port]=VelInt[L]; 00355 // ADCValue = int -> 2byte 00356 UartTmpBuff[4][Port]=ADCValue[R]>>8; // CurrR 00357 UartTmpBuff[5][Port]=ADCValue[R]; 00358 UartTmpBuff[6][Port]=ADCValue[L]>>8; // CurrL 00359 UartTmpBuff[7][Port]=ADCValue[L]; 00360 // Space = int -> 2byte 00361 UartTmpBuff[8][Port]=SpTick[R]>>8; // SpTickR 00362 UartTmpBuff[9][Port]=SpTick[R]; 00363 SpTick[R] = 0; // [19] 00364 UartTmpBuff[10][Port]=SpTick[L]>>8; // SpTickL 00365 UartTmpBuff[11][Port]=SpTick[L]; 00366 SpTick[L] = 0; // [19] 00367 TxParameters('a',12, Port); 00368 break; 00369 00370 00371 //--- Navigation parameters settings 00372 case 'D': // setting reference coord. X, Y computing distance 00373 // [24] Mode C 00374 // High Byte * 256 + Low Byte 00375 Ptmp = (UartRxBuff[IncrCircPtr(Port)][Port] << 8) + 00376 (UartRxBuff[IncrCircPtr(Port)][Port]); // Dist 00377 PosXdes = PosXmes + (Ptmp * sin(ThetaDes)); 00378 PosYdes = PosYmes + (Ptmp * cos(ThetaDes)); 00379 DIST_ENABLE_FLAG = 1; // enable distance computing [24a] 00380 SCHEDULER_FLAG = 0; // [32a] 00381 if (CONSOLE_DEBUG) //[30] 00382 { 00383 PosXmes=PosXdes; 00384 PosYmes=PosYdes; 00385 } 00386 break; 00387 00388 case 'O': // setting ref. orientation angle in degrees (absolute) 00389 // [24] Mode A 00390 // High Byte * 256 + Low Byte 00391 Ptmp = (UartRxBuff[IncrCircPtr(Port)][Port] << 8) + 00392 (UartRxBuff[IncrCircPtr(Port)][Port]); 00393 ThetaDesF(Ptmp * DEG2RAD); 00394 DIST_ENABLE_FLAG = 0; // disable distance computing [24a] 00395 VelDecr = 1; // [24d] 00396 SCHEDULER_FLAG = 0; // [32a] 00397 if (CONSOLE_DEBUG) //[30] 00398 { 00399 ThetaMes=ThetaDes; 00400 } 00401 break; 00402 00403 case 'o': // setting reference orientation angle in degrees 00404 // as a delta of the current orientation (relative) 00405 // [24] Mode A 00406 // High Byte * 256 + Low Byte 00407 Ptmp = (UartRxBuff[IncrCircPtr(Port)][Port] << 8) + 00408 (UartRxBuff[IncrCircPtr(Port)][Port]); 00409 ThetaDesF( ThetaDes + (Ptmp * DEG2RAD)); 00410 DIST_ENABLE_FLAG = 0; // disable distance computing [24a] 00411 VelDecr = 1; // [24d] 00412 SCHEDULER_FLAG = 0; // [32a] 00413 break; 00414 00415 case 'P': // setting reference coord. X, Y in mm 00416 // [24] Mode B 00417 // High Byte * 256 + Low Byte 00418 PosXdes = (float)((UartRxBuff[IncrCircPtr(Port)][Port] << 8) + 00419 (UartRxBuff[IncrCircPtr(Port)][Port])); 00420 PosYdes = (float)((UartRxBuff[IncrCircPtr(Port)][Port] << 8) + 00421 (UartRxBuff[IncrCircPtr(Port)][Port])); 00422 DIST_ENABLE_FLAG = 1; // enable distance computing [24a] 00423 SCHEDULER_FLAG = 0; // [32a] 00424 if (CONSOLE_DEBUG) //[30] 00425 { 00426 PosXmes=PosXdes; 00427 PosYmes=PosYdes; 00428 } 00429 break; 00430 00431 case 'S': // setting reference speed (as mm/s) 00432 // High Byte * 256 + Low Byte 00433 VelDesM = (UartRxBuff[IncrCircPtr(Port)][Port] << 8) + 00434 (UartRxBuff[IncrCircPtr(Port)][Port]); 00435 if (VelDesM > 999) VelDesM = 999; // range check 00436 if (VelDesM < -999) VelDesM = -999; // range check 00437 SCHEDULER_FLAG = 0; // [32a] 00438 break; 00439 00440 case 'H': // immediate Halt without decelerating ramp.In this way it 00441 // uses the brake effect of H bridge in LAP mode 00442 VelDesM = 0; 00443 SCHEDULER_FLAG = 0; // [32a] 00444 if (CONSOLE_DEBUG) //[30] 00445 { 00446 VelMes[R]=0; 00447 VelMes[L]=0; 00448 ADCValue[R]=0; 00449 ADCValue[L]=0; 00450 } 00451 break; 00452 00453 00454 //--- Constant parameters setting 00455 case 'J': // setting PID coefficients for DistPid 00456 TmpChk=0; // checksum to control permanent storage 00457 for (ParserCount=0; ParserCount < 5; ParserCount+=2) 00458 { 00459 // High Byte * 256 + Low Byte 00460 Ktmp = (UartRxBuff[IncrCircPtr(Port)][Port] << 8) + 00461 (UartRxBuff[IncrCircPtr(Port)][Port]); 00462 if (Ktmp > 9999) Ktmp = 9999; // range check 00463 TmpChk+=Ktmp; 00464 DataEEWrite(Ktmp,EE_DIST_KP+ParserCount/2); 00465 DistKCoeffs[ParserCount/2] = Q15((float)(Ktmp)/10000); 00466 } 00467 InitDistPid(); 00468 DataEEWrite(TmpChk,EE_CHK_DIST); 00469 break; 00470 00471 case 'K': // setting PID coefficients for Speed PIDs 00472 TmpChk=0; // checksum to control permanent storage 00473 for (ParserCount=0; ParserCount < 5; ParserCount+=2) 00474 { 00475 // High Byte * 256 + Low Byte 00476 Ktmp = (UartRxBuff[IncrCircPtr(Port)][Port] << 8) + 00477 (UartRxBuff[IncrCircPtr(Port)][Port]); 00478 if (Ktmp > 9999) Ktmp = 9999; // range check 00479 TmpChk+=Ktmp; 00480 DataEEWrite(Ktmp,EE_KP1+ParserCount/2); 00481 kCoeffs1[ParserCount/2] = Q15((float)(Ktmp)/10000); 00482 } 00483 InitPid1(); 00484 00485 for (ParserCount=0; ParserCount < 5; ParserCount+=2) 00486 { 00487 // High Byte * 256 + Low Byte 00488 Ktmp = (UartRxBuff[IncrCircPtr(Port)][Port] << 8) + 00489 (UartRxBuff[IncrCircPtr(Port)][Port]); 00490 if (Ktmp > 9999) Ktmp = 9999; // range check 00491 TmpChk+=Ktmp; 00492 DataEEWrite(Ktmp,EE_KP2+ParserCount/2); 00493 kCoeffs2[ParserCount/2] = Q15((float)(Ktmp)/10000); 00494 } 00495 InitPid2(); 00496 DataEEWrite(TmpChk,EE_CHK_SPEED); 00497 break; 00498 00499 case 'k': // settings PID coefficients for AnglePid 00500 TmpChk=0; // checksum to control permanent storage 00501 for (ParserCount=0; ParserCount < 5; ParserCount+=2) 00502 { 00503 // High Byte * 256 + Low Byte 00504 Ktmp = (UartRxBuff[IncrCircPtr(Port)][Port] << 8) + 00505 (UartRxBuff[IncrCircPtr(Port)][Port]); 00506 if (Ktmp > 9999) Ktmp = 9999; // range check 00507 TmpChk+=Ktmp; 00508 DataEEWrite(Ktmp,EE_ANGLE_KP+ParserCount/2); 00509 AngleKCoeffs[ParserCount/2] = Q15((float)(Ktmp)/10000); 00510 } 00511 InitAnglePid(); 00512 DataEEWrite(TmpChk,EE_CHK_ANGLE); 00513 break; 00514 00515 case 'L': // Kvel 00516 TmpChk=0; // checksum to control permanent storage 00517 Tmp1Long = ((long)UartRxBuff[IncrCircPtr(Port)][Port] << 8) + 00518 ((long)UartRxBuff[IncrCircPtr(Port)][Port]); 00519 TmpChk+=Tmp1Long; 00520 DataEEWrite(Tmp1Long,EE_KVEL1_H); 00521 Tmp2Long = ((long)UartRxBuff[IncrCircPtr(Port)][Port] << 8) + 00522 ((long)UartRxBuff[IncrCircPtr(Port)][Port]); 00523 TmpChk+=Tmp2Long; 00524 DataEEWrite(Tmp2Long,EE_KVEL1_L); 00525 Kvel[R] = (Tmp1Long << 16) + Tmp2Long; 00526 00527 Tmp1Long = ((long)UartRxBuff[IncrCircPtr(Port)][Port] << 8) + 00528 ((long)UartRxBuff[IncrCircPtr(Port)][Port]); 00529 TmpChk+=Tmp1Long; 00530 DataEEWrite(Tmp1Long,EE_KVEL2_H); 00531 Tmp2Long = ((long)UartRxBuff[IncrCircPtr(Port)][Port] << 8) + 00532 ((long)UartRxBuff[IncrCircPtr(Port)][Port]); 00533 TmpChk+=Tmp2Long; 00534 DataEEWrite(Tmp2Long,EE_KVEL2_L); 00535 Kvel[L] = (Tmp1Long << 16) + Tmp2Long; 00536 DataEEWrite(TmpChk,EE_CHK_KVEL); 00537 break; 00538 00539 case 'M': // Mechanical costants: Axle size, KspR, KspL [29] 00540 TmpChk=0; // checksum to control permanent storage 00541 Tmp1Long = ((long)UartRxBuff[IncrCircPtr(Port)][Port] << 8) + 00542 ((long)UartRxBuff[IncrCircPtr(Port)][Port]); 00543 TmpChk+=Tmp1Long; 00544 DataEEWrite(Tmp1Long,EE_AXLE_H); 00545 Tmp2Long = ((long)UartRxBuff[IncrCircPtr(Port)][Port] << 8) + 00546 ((long)UartRxBuff[IncrCircPtr(Port)][Port]); 00547 TmpChk+=Tmp2Long; 00548 DataEEWrite(Tmp2Long,EE_AXLE_L); 00549 Axle = (float)((Tmp1Long << 16) + Tmp2Long)/(float)(10000); 00550 00551 00552 Tmp1Long = ((long)UartRxBuff[IncrCircPtr(Port)][Port] << 8) + 00553 ((long)UartRxBuff[IncrCircPtr(Port)][Port]); 00554 TmpChk+=Tmp1Long; 00555 DataEEWrite(Tmp1Long,EE_KSP1_H); 00556 Tmp2Long = ((long)UartRxBuff[IncrCircPtr(Port)][Port] << 8) + 00557 ((long)UartRxBuff[IncrCircPtr(Port)][Port]); 00558 TmpChk+=Tmp2Long; 00559 DataEEWrite(Tmp2Long,EE_KSP1_L); 00560 Ksp[0] = (float)((Tmp1Long << 16) + Tmp2Long)/(float)(1000000000); 00561 00562 Tmp1Long = ((long)UartRxBuff[IncrCircPtr(Port)][Port] << 8) + 00563 ((long)UartRxBuff[IncrCircPtr(Port)][Port]); 00564 TmpChk+=Tmp1Long; 00565 DataEEWrite(Tmp1Long,EE_KSP2_H); 00566 Tmp2Long = ((long)UartRxBuff[IncrCircPtr(Port)][Port] << 8) + 00567 ((long)UartRxBuff[IncrCircPtr(Port)][Port]); 00568 TmpChk+=Tmp2Long; 00569 DataEEWrite(Tmp2Long,EE_KSP2_L); 00570 Ksp[1] = (float)((Tmp1Long << 16) + Tmp2Long)/(float)(1000000000); 00571 DataEEWrite(TmpChk,EE_CHK_MECH); 00572 break; 00573 00574 case 's': // Scheduler parameters setting [32] 00575 TmpChk=0; // checksum to control permanent storage 00576 VelDesM = 0; 00577 SCHEDULER_FLAG = 0; // [32a] 00578 C3 = EE_SCHED; // starting address for permanent storage 00579 for (C1=0; C1<16; C1++) 00580 { 00581 for (C2=0; C2<4; C2++) 00582 { 00583 SchedValues[C1][C2]=(UartRxBuff[IncrCircPtr(Port)][Port] << 8) + 00584 (UartRxBuff[IncrCircPtr(Port)][Port]); 00585 TmpChk+=SchedValues[C1][C2]; 00586 DataEEWrite(SchedValues[C1][C2],C3); 00587 C3 ++; 00588 } 00589 } 00590 DataEEWrite(TmpChk,EE_CHK_SCHED); 00591 break; 00592 00593 //--- Service 00594 case '*': // Board reset [28] 00595 if (ResetCount < 3) 00596 { 00597 ResetCount ++; 00598 ResetPort = Port; 00599 } 00600 else 00601 { 00602 SET_CPU_IPL( 7 ); // disable all user interrupts 00603 DelayN1ms(200); 00604 asm("RESET"); 00605 } 00606 break; 00607 00608 case 'R': // Firmware version request 00609 // Send string 'Ver' 00610 for (i=0; i<27; i++) 00611 { 00612 UartTmpBuff[i][Port]=Ver[i]; 00613 } 00614 TxParameters('R',26, Port); 00615 break; 00616 00617 case 'c': // Continuos send mode 00618 // sends all data without request. 00619 // 0=OFF 00620 // 1=send mean parameters 00621 // 2=send detailed parameters 00622 TxContFlag = UartRxBuff[RX_HEADER_LEN + 1][Port]; 00623 UartContTxTimer=UART_CONT_TIMEOUT; // timer reset 00624 break; 00625 00626 case 'z': // send back a text string, just for debug 00627 // Send string 'Test' 00628 for (i=0; i<27; i++) 00629 { 00630 UartTmpBuff[i][Port]=Test[i]; 00631 } 00632 TxParameters('z',24, Port); 00633 break; 00634 00635 case 'e': // Read error code and reset error condition 00636 // Send error value 00637 BlinkPeriod = NORM_BLINK_PER;// LED1 blinking period (ms) 00638 BlinkOn = NORM_BLINK_ON; // LED1 on time (ms) 00639 Blink = 0; 00640 UartTmpBuff[0][Port]=ErrCode>>8; 00641 UartTmpBuff[1][Port]=ErrCode; 00642 TxParameters('e',2, Port); 00643 break; 00644 00645 case 'f': // set "Console Debug" mode [30] 00646 CONSOLE_DEBUG = UartRxBuff[IncrCircPtr(Port)][Port]; 00647 break; 00648 00649 case '#': // start scheduler sequence 00650 DIST_OK_FLAG = 1; // to kick start the scheduler sequence 00651 SchedPtr = 0; // [32] 00652 SCHEDULER_FLAG = 1; // [32a] 00653 break; 00654 00655 00656 case '$': // all map grid sending request 00657 // disable any other procedure pending 00658 DIST_ENABLE_FLAG = 0; // disable distance computing [24a] 00659 VelDecr = 1; // [24d] 00660 SCHEDULER_FLAG = 0; // [32a] 00661 VelDesM = 0; 00662 00663 SendMapPort = Port; // to temporay store port number for delayed TX 00664 MAP_SEND_FLAG=1; 00665 00666 // index of row requested by console in 0 to Y_size range 00667 MapSendIndx = UartRxBuff[IncrCircPtr(Port)][Port]; 00668 break; 00669 00670 //--- Sensors 00671 case 'd':// Distance from objects, target and compass readings 00672 // Obj = unsigned int -> 2 byte (mm) 00673 Obj[0] = (UartRxBuff[IncrCircPtr(Port)][Port])*10; // Left object 00674 Obj[1] = (UartRxBuff[IncrCircPtr(Port)][Port])*10; // Center object 00675 Obj[2] = (UartRxBuff[IncrCircPtr(Port)][Port])*10; // Right object 00676 // Target = unsigned char -> 1 byte 00677 Target[0] = (UartRxBuff[IncrCircPtr(Port)][Port]); // Left target 00678 Target[1] = (UartRxBuff[IncrCircPtr(Port)][Port]); // Center target 00679 Target[2] = (UartRxBuff[IncrCircPtr(Port)][Port]); // Right target 00680 // Compass Bearing = int -> 2 byte (deg*10) 00681 AngleCmp = (UartRxBuff[IncrCircPtr(Port)][Port] << 8) + 00682 (UartRxBuff[IncrCircPtr(Port)][Port]); 00683 // [24g] relative position of obstacles. 00684 if (Obj[0] < OBST_THRESHOLD) // from object on the left 00685 { 00686 VObX[0] = Obj[0] * sin(ThetaMes-AngleS); 00687 VObY[0] = Obj[0] * cos(ThetaMes-AngleS); 00688 // mark obstacle in the cell 00689 Slam(PosXmes + VObX[0], PosYmes + VObY[0], 1); 00690 } 00691 00692 if (Obj[1] < OBST_THRESHOLD) // from object on the center 00693 { 00694 VObX[1] = Obj[1] * sin(ThetaMes); 00695 VObY[1] = Obj[1] * cos(ThetaMes); 00696 // mark obstacle in the cell 00697 Slam(PosXmes + VObX[1], PosYmes + VObY[1], 1); 00698 } 00699 00700 if (Obj[2] < OBST_THRESHOLD) // from object on the right 00701 { 00702 VObX[2] = Obj[2] * sin(ThetaMes+AngleS); 00703 VObY[2] = Obj[2] * cos(ThetaMes+AngleS); 00704 // mark obstacle in the cell 00705 Slam(PosXmes + VObX[2], PosYmes + VObY[2], 1); 00706 } 00707 break; 00708 00709 case 'Q':// Raw sensors data, dsNav -> console 00710 // PosXmes rounded in a Int -> 2 byte (mm) 00711 Ptmp = FLOAT2INT(PosXmes); // PosX 00712 UartTmpBuff[0][Port]=Ptmp>>8; 00713 UartTmpBuff[1][Port]=Ptmp; 00714 // PosYmes rounded in a Int -> 2 byte (mm) 00715 Ptmp = FLOAT2INT(PosYmes); // PosY 00716 UartTmpBuff[2][Port]=Ptmp>>8; 00717 UartTmpBuff[3][Port]=Ptmp; 00718 // ThetaMes rounded in a Int -> 2 byte (degrees) 00719 Ptmp = ThetaMes * RAD2DEG; // Theta 00720 Alpha = FLOAT2INT(Ptmp); 00721 UartTmpBuff[4][Port]=Alpha>>8; 00722 UartTmpBuff[5][Port]=Alpha; 00723 // VObX rounded in a Int -> 2 byte (cm) 00724 for(i=0; i<3; i++) // Buffer index from 6 to 17 00725 { 00726 Ptmp = FLOAT2INT(VObX[i]/10); // Obj X coord in cm 00727 UartTmpBuff[6+(i*4)][Port]=Ptmp>>8; 00728 UartTmpBuff[7+(i*4)][Port]=Ptmp; 00729 Ptmp = FLOAT2INT(VObY[i]/10); // Obj Y coord in cm 00730 UartTmpBuff[8+(i*4)][Port]=Ptmp>>8; 00731 UartTmpBuff[9+(i*4)][Port]=Ptmp; 00732 } 00733 00734 TxParameters('Q',18, Port); 00735 break; 00736 00737 default: 00738 UartRxError(-7,Port); // error: not a known command 00739 break; 00740 } 00741 } 00742 00743 void Scheduler(void) // [32] 00744 { 00745 unsigned char SchedCode= 0; // action to execute 00746 float DPosX; // delta PosX 00747 float DPosY; // delta PosY 00748 00749 ANGLE_OK_FLAG = 0; 00750 DIST_OK_FLAG = 0; 00751 TIMER_OK_FLAG = 0; 00752 SCHED_ANGLE_FLAG = 0; 00753 SCHED_DIST_FLAG = 0; 00754 00755 /* 00756 Units 00757 code 00758 Angle(deg) 00759 Speed(mm/s) 00760 X(mm) 00761 Y(mm) 00762 D(mm) 00763 Delay(n x 50ms) 00764 00765 Meaning of parameters 00766 code 1 2 3 00767 0 00768 1 V 00769 2 V(2) D 00770 3 V(2) Theta 00771 4 V(2) X Y 00772 5 V(2) X Y 00773 6 Delay 00774 */ 00775 00776 SchedCode = SchedValues[SchedPtr][0]; 00777 switch (SchedCode) 00778 { 00779 case 0: // stop end of sequence 00780 VelDesM = 0; 00781 SCHEDULER_FLAG = 0; // [32a] 00782 break; 00783 00784 case 1: // keep previous angle - set speed 00785 VelDesM = SchedValues[SchedPtr][1]; 00786 RT_TIMER_FLAG = 1; 00787 RtTimer = 2; // wait 100ms for next step 00788 break; 00789 00790 case 2: // keep angle and speed(2) - set distance 00791 if (SchedValues[SchedPtr][1] != 0xFFFF) 00792 { 00793 VelDesM = SchedValues[SchedPtr][1]; 00794 } 00795 PosXdes=PosXmes+(SchedValues[SchedPtr][2]*sin(ThetaDes)); 00796 PosYdes=PosYmes+(SchedValues[SchedPtr][2]*cos(ThetaDes)); 00797 SCHED_DIST_FLAG = 1; // wait for target distance 00798 DIST_ENABLE_FLAG = 1; // enable distance computing [24a] 00799 break; 00800 00801 case 3: // keep speed(2) - set angle 00802 SCHED_ANGLE_FLAG = 1; // wait for target angle 00803 if (SchedValues[SchedPtr][1] != 0xFFFF) 00804 { 00805 VelDesM = SchedValues[SchedPtr][1]; 00806 } 00807 ThetaDesF(SchedValues[SchedPtr][2] * DEG2RAD); 00808 DIST_ENABLE_FLAG = 0; // disable distance computing [24a] 00809 VelDecr = 1; // [24d] 00810 break; 00811 00812 case 4: // keep speed(2) - set angle toward X, Y 00813 SCHED_ANGLE_FLAG = 1; // wait for target angle 00814 if (SchedValues[SchedPtr][1] != 0xFFFF) 00815 { 00816 VelDesM = SchedValues[SchedPtr][1]; 00817 } 00818 DPosX=(float)(SchedValues[SchedPtr][2])-PosXmes;//delta on X 00819 DPosY=(float)(SchedValues[SchedPtr][3])-PosYmes;//delta on Y 00820 // if ((DPosX != 0) && (DPosY != 0)) 00821 ThetaDesF(atan2f(DPosX,DPosY));//Tan(ThetaDes)=Sin/Cos=X/Y 00822 break; 00823 00824 case 5: // keep speed(2) - walk toward X, Y 00825 if (SchedValues[SchedPtr][1] != 0xFFFF) 00826 { 00827 VelDesM = SchedValues[SchedPtr][1]; 00828 } 00829 PosXdes = (float)(SchedValues[SchedPtr][2]); 00830 PosYdes = (float)(SchedValues[SchedPtr][3]); 00831 SCHED_DIST_FLAG = 1; // wait for target distance 00832 DIST_ENABLE_FLAG = 1; // enable distance computing [24a] 00833 break; 00834 00835 case 6: // wait n ms 00836 RT_TIMER_FLAG = 1; 00837 RtTimer = SchedValues[SchedPtr][1]; // n x 50ms 00838 break; 00839 00840 default:// the same as 0 00841 VelDesM = 0; 00842 SCHEDULER_FLAG = 0; // [32a] 00843 break; 00844 } 00845 00846 if (SchedPtr < 15) 00847 { 00848 SchedPtr ++; // next step 00849 } 00850 } 00851 00852 void AdcCalc(void) 00853 { 00854 extern int DmaAdc[2][64]; 00855 int AdcCount = 0; 00856 long ADCValueTmp[2] = {0,0};// to store intermediate ADC calculations 00857 00858 ADC_CALC_FLAG = 0; // will be set by ISR when all data available 00859 // averages the 64 ADC values for each ANx 00860 for (AdcCount=0;AdcCount<64;AdcCount++) 00861 { 00862 ADCValueTmp[R] += DmaAdc[R][AdcCount]; 00863 ADCValueTmp[L] += DmaAdc[L][AdcCount]; 00864 } 00865 ADCValue[R] = ADCValueTmp[R] >> 6; // [2a] 00866 ADCValue[L] = ADCValueTmp[L] >> 6; // [2a] 00867 00868 if (ADCValue[R] > ADC_OVLD_LIMIT) // [2b] 00869 { 00870 ADCOvldCount[R] ++; 00871 } 00872 else 00873 { 00874 ADCOvldCount[R] = 0; 00875 } 00876 00877 if (ADCValue[L] > ADC_OVLD_LIMIT) // [2b] 00878 { 00879 ADCOvldCount[L] ++; 00880 } 00881 else 00882 { 00883 ADCOvldCount[L] = 0; 00884 } 00885 00886 if ((ADCOvldCount[R]>ADC_OVLD_TIME)||(ADCOvldCount[L]>ADC_OVLD_TIME)) 00887 { 00888 ADCOvldCount[R]=0; 00889 ADCOvldCount[R]=0; 00890 VelDesM = 0; // immediate halt 00891 SCHEDULER_FLAG = 0; // [32a] 00892 BlinkOn = 50; // very fast blink for alarm 00893 BlinkPeriod = 100; 00894 ErrCode = -30; 00895 } 00896 } 00897 00898 00899 void InitDistPid(void) 00900 { 00901 //Initialize the PID data structure: PIDstruct 00902 //Set up pointer to derived coefficients 00903 DistPIDstruct.abcCoefficients = &DistabcCoefficient[0]; 00904 //Set up pointer to controller history samples 00905 DistPIDstruct.controlHistory = &DistcontrolHistory[0]; 00906 // Clear the controler history and the controller output 00907 PIDInit(&DistPIDstruct); 00908 //Derive the a,b, & c coefficients from the Kp, Ki & Kd 00909 PIDCoeffCalc(&DistKCoeffs[0], &DistPIDstruct); 00910 } 00911 00912 void Navigation(void) // called after odometry is performed 00913 { 00914 float DPosX; // delta PosX 00915 float DPosY; // delta PosY 00916 float Dist; // distance 00917 float VelDecrObj; // speed decreasing due to objects found 00918 00919 CYCLE2_FLAG = 0; // will be set again by timer ISR 00920 00921 if (DIST_ENABLE_FLAG) 00922 { 00923 // distance from goal, i.e.: distance from desired to current position 00924 DPosX = PosXdes - PosXmes; // X component of distance 00925 DPosY = PosYdes - PosYmes; // Y component of distance 00926 Dist = sqrtf(powf(DPosX,2) + powf(DPosY,2));// Pythagora's Theorem 00927 } 00928 else 00929 { 00930 Dist = OBST_THRESHOLD; // a reference value 00931 DPosX = Dist * sin(ThetaDesRef); 00932 DPosY = Dist * cos(ThetaDesRef); 00933 } 00934 00935 if (Dist < MIN_DIST_ERR) // goal reached 00936 { 00937 DIST_ENABLE_FLAG = 0; // disable distance computing [24a] 00938 VelDecr = 1; // [24d] 00939 VelDesM = 0; 00940 if (SCHED_DIST_FLAG) 00941 { 00942 DIST_OK_FLAG = 1; // target ok 00943 } 00944 return; // job done 00945 } 00946 else 00947 {// set ThetaDes and VelDecr to reach the goal avoiding obstacles 00948 VelDecrObj=ObstacleAvoidance(DPosX, DPosY, Dist); 00949 } 00950 00951 if (Dist < MIN_GOAL_DIST) // [24c] 00952 { 00953 DIST_PID_DES = 0; // the goal is to have dist=0 from target 00954 DIST_PID_MES = Q15((float)Dist/(float)MIN_GOAL_DIST); // measured input 00955 PID(&DistPIDstruct); 00956 VelDecr = -_itofQ15(DIST_PID_OUT); // [24d] 00957 VelDecr *= VelDecrObj; 00958 } 00959 else 00960 { 00961 VelDecr = VelDecrObj; 00962 } 00963 } 00964 00965 float ObstacleAvoidance(float DPosX, float DPosY, int DistTarget) 00966 { // [24e] 00967 float VX; // X component of resultant vector 00968 float VY; // Y component of resultant vector 00969 float VM; // magnitude of resultant vector 00970 const float Fcr= 0.2; // Force constant (repelling) 00971 const float Fct= 1; // Force constant (attraction to the target) 00972 int Cx; // loop counters 00973 int Cy; 00974 int X_grid; 00975 int Y_grid; 00976 int CellV; // the valued contained in the current cell 00977 float Frx=0; // component vectors of the repulsive force 00978 float Fry=0; 00979 float FrTmp; 00980 float Ftx=0; // component vectors of the actractive force 00981 float Fty=0; 00982 int TableX; 00983 int TableY; 00984 00985 if(Obj[0]<OBST_MIN_DIST||Obj[1]<OBST_MIN_DIST||Obj[2]<OBST_MIN_DIST) 00986 {// set ThetaDes and VelDecr to avoid very close obstacles 00987 return -0.2; // walk backward at a slow speed 00988 } 00989 else 00990 { // index in the range 0-Y_SIZE to point the grid matrix 00991 X_grid=PosIndx(PosXmes); 00992 Y_grid=PosIndx(PosYmes); 00993 00994 TableX=TABLE_SIZE-OBST_FIELD;//define the field scanned into the table 00995 for(Cx = X_grid-OBST_FIELD; Cx <= X_grid+OBST_FIELD; Cx++) 00996 { 00997 if((Cx >= X_POINT_MIN) && (Cx <= X_POINT_MAX)) 00998 { 00999 TableY=TABLE_SIZE-OBST_FIELD;//define the field scanned 01000 for(Cy = Y_grid-OBST_FIELD; Cy <= Y_grid+OBST_FIELD; Cy++) 01001 { 01002 if((Cy >= Y_POINT_MIN) && (Cy <= Y_POINT_MAX)) 01003 { 01004 CellV=GetMap(Cx, Cy); 01005 if((CellV > 0) && (CellV < 8)) 01006 { 01007 if((Cx != 0) && (Cy != 0)) 01008 { 01009 FrTmp = Fcr*CellV; 01010 Frx+=FrTmp*VffTableX[TableX][TableY]; 01011 Fry+=FrTmp*VffTableY[TableX][TableY]; 01012 } 01013 } 01014 } 01015 TableY++; 01016 } 01017 } 01018 TableX++; 01019 } 01020 01021 //The target generates a constant-magnitud attracting force 01022 Ftx=Fct*DPosX; // /DistTarget; 01023 Fty=Fct*DPosY; // /DistTarget; 01024 01025 VX=Frx+Ftx; // Resultant Force Vector 01026 VY=Fry+Fty; 01027 VM = 1; // to be defined (sqrtf(powf(VX,2) + powf(VY,2)))/OBST_THRESHOLD; 01028 01029 #ifdef NO_OBSTACLE 01030 #warning -- compiling with NO OBSTACLE ************************************* 01031 VX = DPosX; 01032 VY = DPosY; 01033 VM = 1; 01034 #endif 01035 01036 if ((VX != 0) && (VY != 0)) 01037 {// phase of new vector [22aa] Tangent(ThetaDes) = Sin/Cos = X/Y 01038 ThetaDesF(atan2f(VX,VY)); 01039 } 01040 return VM; 01041 } 01042 } 01043 01044 void InitAnglePid(void) 01045 { 01046 //Initialize the PID data structure: PIDstruct 01047 //Set up pointer to derived coefficients 01048 AnglePIDstruct.abcCoefficients = &AngleabcCoefficient[0]; 01049 //Set up pointer to controller history samples 01050 AnglePIDstruct.controlHistory = &AnglecontrolHistory[0]; 01051 // Clear the controler history and the controller output 01052 PIDInit(&AnglePIDstruct); 01053 //Derive the a,b, & c coefficients from the Kp, Ki & Kd 01054 PIDCoeffCalc(&AngleKCoeffs[0], &AnglePIDstruct); 01055 } 01056 01057 void Orientation(void) // [23] 01058 { 01059 int DeltaVel; // difference in speed between the wheels to rotate 01060 int RealVel; // VelDesM after reduction controlled by Dist PID 01061 float Error; 01062 01063 ORIENTATION_FLAG = 0; // it will be restarted by DeadReckoning() 01064 01065 if (ThetaDes < 0) ThetaDes += TWOPI; // keep angle value positive 01066 if (ThetaMes < 0) ThetaMes += TWOPI; 01067 Error = ThetaMes - ThetaDes; 01068 if (Error > PI) // search for the best direction to correct error [23a] 01069 { 01070 Error -= TWOPI; 01071 } 01072 else if (Error < -PI) 01073 { 01074 Error += TWOPI; 01075 } 01076 if (SCHED_ANGLE_FLAG) 01077 { 01078 if (fabsf(Error) < MIN_THETA_ERR) 01079 { 01080 ANGLE_OK_FLAG = 1; // target ok 01081 } 01082 } 01083 01084 ANGLE_PID_DES = 0; // ref value translated to 0 [23c] 01085 ANGLE_PID_MES = Q15(Error/PI); // current error [23b] 01086 PID(&AnglePIDstruct); 01087 01088 DeltaVel = (ANGLE_PID_OUT >> 7);// MAX delta in int = 256 [23d] 01089 RealVel = VelDesM * VelDecr; // [24d] 01090 VelDes[R] = RealVel - DeltaVel; // [23e] 01091 VelDes[L] = RealVel + DeltaVel; 01092 01093 if (VelDes[R] > MAX_ROT_SPEED) 01094 { 01095 VelDes[R] = MAX_ROT_SPEED; 01096 VelDes[L] = MAX_ROT_SPEED + (DeltaVel << 1); 01097 } 01098 else if (VelDes[R] < -MAX_ROT_SPEED) 01099 { 01100 VelDes[R] = -MAX_ROT_SPEED; 01101 VelDes[L] = -MAX_ROT_SPEED + (DeltaVel << 1); 01102 } 01103 else if (VelDes[L] > MAX_ROT_SPEED) 01104 { 01105 VelDes[L] = MAX_ROT_SPEED; 01106 VelDes[R] = MAX_ROT_SPEED - (DeltaVel << 1); 01107 } 01108 else if (VelDes[L] < -MAX_ROT_SPEED) 01109 { 01110 VelDes[L] = -MAX_ROT_SPEED; 01111 VelDes[R] = -MAX_ROT_SPEED - (DeltaVel << 1); 01112 } 01113 01114 VelFin[R] = Q15((float)(VelDes[R])/1000); // [23f] 01115 if (VelFin[R] != PID_REF1) 01116 { 01117 if (PID_REF1>=0 && VelFin[R] > PID_REF1) 01118 { 01119 Ramp1 = Q15( ACC); 01120 RAMP_T_FLAG1 = 1; 01121 } 01122 if (PID_REF1>=0 && VelFin[R] < PID_REF1) 01123 { 01124 Ramp1 = Q15(-DEC); 01125 RAMP_T_FLAG1 = 0; 01126 } 01127 if (PID_REF1< 0 && VelFin[R] > PID_REF1) 01128 { 01129 Ramp1 = Q15( DEC); 01130 RAMP_T_FLAG1 = 1; 01131 } 01132 if (PID_REF1< 0 && VelFin[R] < PID_REF1) 01133 { 01134 Ramp1 = Q15(-ACC); 01135 RAMP_T_FLAG1 = 0; 01136 } 01137 RAMP_FLAG1 = 1; // acceleration ramp start 01138 } 01139 01140 VelFin[L] = Q15((float)(VelDes[L])/1000); 01141 if (VelFin[L] != PID_REF2) 01142 { 01143 if (PID_REF2>=0 && VelFin[L] > PID_REF2) 01144 { 01145 Ramp2 = Q15( ACC); 01146 RAMP_T_FLAG2 = 1; 01147 } 01148 if (PID_REF2>=0 && VelFin[L] < PID_REF2) 01149 { 01150 Ramp2 = Q15(-DEC); 01151 RAMP_T_FLAG2 = 0; 01152 } 01153 if (PID_REF2< 0 && VelFin[L] > PID_REF2) 01154 { 01155 Ramp2 = Q15( DEC); 01156 RAMP_T_FLAG2 = 1; 01157 } 01158 if (PID_REF2< 0 && VelFin[L] < PID_REF2) 01159 { 01160 Ramp2 = Q15(-ACC); 01161 RAMP_T_FLAG2 = 0; 01162 } 01163 RAMP_FLAG2 = 1; // acceleration ramp start 01164 } 01165 } 01166 01167 void DeadReckoning(void) // [22] 01168 { 01169 float CosNow; // current value for Cos 01170 float SinNow; // current value for Sin 01171 float DSpace; // delta traveled distance by the robot 01172 float DTheta; // delta rotation angle 01173 float DPosX; // delta space on X axis 01174 float DPosY; // delta space on Y axis 01175 float SaMinusSb; 01176 float SrPlusSl; 01177 float Radius; 01178 01179 CYCLE1_FLAG=0; 01180 01181 Spmm[R]=SpTick[R]*Ksp[R]; // distance of right wheel in mm 01182 SpTick[R] = 0; // rest counter for the next misure 01183 Spmm[L]=SpTick[L]*Ksp[L]; // distance of left wheel in mm 01184 SpTick[L] = 0; // rest counter for the next misure 01185 01186 #ifdef geographic // [22aa] 01187 SaMinusSb=Spmm[L]-Spmm[R]; 01188 #else 01189 SaMinusSb=Spmm[R]-Spmm[L]; 01190 #endif 01191 01192 SrPlusSl=Spmm[R]+Spmm[L]; 01193 if (fabs(SaMinusSb) <= SPMIN) 01194 {// traveling in a nearly straight line [22a] 01195 DSpace=Spmm[R]; 01196 01197 #ifdef geographic // [22aa] 01198 DPosX=DSpace*SinPrev; 01199 DPosY=DSpace*CosPrev; 01200 #else 01201 DPosX=DSpace*CosPrev; 01202 DPosY=DSpace*SinPrev; 01203 #endif 01204 01205 } 01206 else if (fabs(SrPlusSl) <= SPMIN) 01207 {// pivoting around vertical axis without translation [22a] 01208 DTheta=SaMinusSb/Axle; 01209 ThetaMes=fmodf((ThetaMes+DTheta),TWOPI);//current orient. in 2PI range 01210 CosPrev=cosf(ThetaMes); // for the next cycle 01211 SinPrev=sinf(ThetaMes); 01212 DPosX=0; 01213 DPosY=0; 01214 DSpace=0; 01215 } 01216 else 01217 {// rounding a curve 01218 DTheta=SaMinusSb/Axle; 01219 ThetaMes=fmodf((ThetaMes+DTheta),TWOPI);//current orient. in 2PI range 01220 CosNow=cosf(ThetaMes); 01221 SinNow=sinf(ThetaMes); 01222 DSpace=SrPlusSl/2; 01223 Radius = (SemiAxle)*(SrPlusSl/SaMinusSb); 01224 01225 #ifdef geographic // [22aa] 01226 DPosX=Radius*(CosPrev-CosNow); 01227 DPosY=Radius*(SinNow-SinPrev); 01228 #else 01229 DPosX=Radius*(SinNow-SinPrev); 01230 DPosY=Radius*(CosPrev-CosNow); 01231 #endif 01232 01233 CosPrev=CosNow; // to avoid re-calculation on the next cycle 01234 SinPrev=SinNow; 01235 } 01236 01237 Space += DSpace; // total traveled distance 01238 PosXmes += DPosX; // current position 01239 PosYmes += DPosY; 01240 01241 Slam(PosXmes, PosYmes, 2); // mark a presence in this position cell 01242 01243 ORIENTATION_FLAG = 1; // position coordinates computed, Angle PID can start 01244 } 01245 01246 unsigned char Slam(float PosX, float PosY, int Cell) 01247 {//Simultaneous Localization And Mapping 01248 nibble CellValue; // field map value of current cell 01249 int Xpoint; // X coordinate value normalized in matrix range 01250 int Ypoint; // Y index 01251 /* Field shifting still developing 01252 int OldPoint; // temp for previous field boundaries 01253 int TempIndx; 01254 */ 01255 // field mapping [22b] 01256 // index in the range 0-Y_SIZE to point the matrix 01257 Xpoint=PosIndx(PosX); 01258 Ypoint=PosIndx(PosY); 01259 01260 if (PosX >= MaxMapX) 01261 { 01262 Xpoint = X_POINT_MAX; 01263 } 01264 /* [22e] Field shifting still developing 01265 {// compute the next cell beyond old boundary modulus MAP_SIZE 01266 OldPoint=abs((__builtin_modsd((MaxMapX+HALF_MAP_SIZE),MAP_SIZE)) 01267 /CELL_SIZE); 01268 MaxMapX = PosX; 01269 MinMapX = MaxMapX - MAP_SIZE; 01270 Xshift = Xpoint + 1; // [22g] 01271 for (i=OldPoint; i<=Xpoint; i++) 01272 { 01273 for (TempIndx=0; TempIndx<Y_SIZE; TempIndx++) 01274 {// purge the translated portion of the field 01275 SetMap(Xpoint, Ypoint, 0); 01276 } 01277 } 01278 } 01279 */ 01280 else if (PosX < MinMapX) 01281 { 01282 Xpoint = X_POINT_MIN; 01283 } 01284 /* [22e] Field shifting still developing 01285 {// compute the previous cell behind old boundary modulus MAP_SIZE 01286 OldPoint=abs((__builtin_modsd((MaxMapX+(HALF_MAP_SIZE)-CELL_SIZE), 01287 (MAP_SIZE)))/CELL_SIZE); 01288 MinMapX = PosX; 01289 MaxMapX = MinMapX + MAP_SIZE; 01290 Xshift = -Xpoint; // [22g] 01291 for (i=OldPoint; i>=Xpoint; i--) 01292 { 01293 for (TempIndx=0; TempIndx<Y_SIZE; TempIndx++) 01294 {// purge the translated portion of the field 01295 SetMap(Xpoint, Ypoint, 0); 01296 } 01297 } 01298 } 01299 */ 01300 01301 if (PosY >= MaxMapY) 01302 { 01303 Ypoint = Y_POINT_MAX; 01304 } 01305 /* [22e] Field shifting still developing 01306 {// compute the next cell beyond old boundary modulus MAP_SIZE 01307 OldPoint=abs((__builtin_modsd((MaxMapY+HALF_MAP_SIZE),MAP_SIZE)) 01308 /CELL_SIZE); 01309 MaxMapY = PosY; 01310 MinMapY = MaxMapY - MAP_SIZE; 01311 Yshift = Ypoint + 1; // [22g] 01312 for (i=OldPoint; i<=Ypoint; i++) 01313 { 01314 for (TempIndx=0; TempIndx<X_SIZE; TempIndx++) 01315 {// purge the translated portion of the field 01316 SetMap(Xpoint, Ypoint, 0); 01317 } 01318 } 01319 } 01320 */ 01321 01322 else if (PosY < MinMapY) 01323 { 01324 Ypoint = Y_POINT_MIN; 01325 } 01326 /* [22e] Field shifting still developing 01327 {// compute the previous cell behind old boundary modulus MAP_SIZE 01328 OldPoint=abs((__builtin_modsd((MaxMapY+(HALF_MAP_SIZE)-CELL_SIZE), 01329 (MAP_SIZE)))/CELL_SIZE); 01330 MinMapY = PosY; 01331 MaxMapY = MinMapY + MAP_SIZE; 01332 Yshift = -Ypoint; // [22g] 01333 for (i=OldPoint; i>=Ypoint; i--) 01334 { 01335 for (TempIndx=0; TempIndx<X_SIZE; TempIndx++) 01336 {// purge the translated portion of the field 01337 SetMap(Xpoint, Ypoint, 0); 01338 } 01339 } 01340 } 01341 */ 01342 01343 CellValue.nib=GetMap(Xpoint, Ypoint); 01344 01345 if ((Xpoint != XindxPrev) || (Ypoint != YindxPrev))//only when cell changes 01346 { 01347 switch (Cell) // [22d] 01348 { 01349 case 1: 01350 if (CellValue.nib<7) 01351 { 01352 CellValue.nib++; 01353 } 01354 break; 01355 01356 case 2: 01357 if (CellValue.nib<8)// this overrides obstacle info 01358 { 01359 CellValue.nib = 8; 01360 } 01361 else if (CellValue.nib<10) 01362 { 01363 CellValue.nib++; 01364 } 01365 break; 01366 01367 case 5: // gas 01368 CellValue.nib = 12; 01369 break; 01370 01371 case 6: // light 01372 CellValue.nib = 13; 01373 break; 01374 01375 case 7: // sound 01376 CellValue.nib = 14; 01377 break; 01378 } 01379 01380 SetMap(Xpoint, Ypoint, &CellValue); 01381 01382 XindxPrev=Xpoint; 01383 YindxPrev=Ypoint; 01384 } 01385 01386 return CellValue.nib; 01387 } 01388 01389 int PosIndx(float Pos) 01390 { // field mapping [22b] 01391 // index in the range 0-Y_SIZE to point the matrix 01392 int Indx; 01393 Indx=abs((__builtin_modsd((Pos+(HALF_MAP_SIZE)),(MAP_SIZE)))/CELL_SIZE); 01394 return Indx; 01395 } 01396 01397 unsigned char GetMap(int Xpnt, int Ypnt) 01398 { 01399 int XindxH; // High part of X index 01400 int XindxL; // Low part of X index 01401 div_t z; // for div function 01402 01403 //X index in range 0-X_SIZE (H) and remainder 0-(VAR_PER_BYTE-1) (L) 01404 z=div(Xpnt,VAR_PER_BYTE); 01405 XindxH=z.quot; // main index 01406 XindxL=z.rem; // sub-index 01407 01408 if(XindxL==0) 01409 { 01410 return (MapXY[XindxH][Ypnt].TN.nib0); 01411 } 01412 else 01413 { 01414 return (MapXY[XindxH][Ypnt].TN.nib1); 01415 } 01416 } 01417 01418 void SetMap(int Xpnt, int Ypnt, nibble *CellVal) 01419 { 01420 int XindxH; // High part of X index 01421 int XindxL; // Low part of X index 01422 div_t z; // for div function 01423 01424 //X index in range 0-X_SIZE (H) and remainder 0-(VAR_PER_BYTE-1) (L) 01425 z=div(Xpnt,VAR_PER_BYTE); 01426 XindxH=z.quot; // main index 01427 XindxL=z.rem; // sub-index 01428 01429 if(XindxL==0) 01430 { 01431 MapXY[XindxH][Ypnt].TN.nib0 = CellVal->nib; 01432 } 01433 else 01434 { 01435 MapXY[XindxH][Ypnt].TN.nib1 = CellVal->nib; 01436 } 01437 } 01438 01439 void ConstantsDefaultW (void) 01440 { 01441 DataEEWrite(401,EE_KVEL1_H); 01442 DataEEWrite(10405,EE_KVEL1_L); 01443 DataEEWrite(401,EE_KVEL2_H); 01444 DataEEWrite(10405,EE_KVEL2_L); 01445 DataEEWrite(9999,EE_ANGLE_KP); 01446 DataEEWrite(8000,EE_ANGLE_KI); 01447 DataEEWrite(0001,EE_ANGLE_KD); 01448 DataEEWrite(9999,EE_DIST_KP); 01449 DataEEWrite(8000,EE_DIST_KI); 01450 DataEEWrite(0001,EE_DIST_KD); 01451 DataEEWrite(6000,EE_KP1); 01452 DataEEWrite(7000,EE_KI1); 01453 DataEEWrite(1000,EE_KD1); 01454 DataEEWrite(6000,EE_KP2); 01455 DataEEWrite(7000,EE_KI2); 01456 DataEEWrite(1000,EE_KD2); 01457 DataEEWrite(77,EE_KSP1_H); 01458 DataEEWrite(15183,EE_KSP1_L); 01459 DataEEWrite(77,EE_KSP2_H); 01460 DataEEWrite(15183,EE_KSP2_L); 01461 DataEEWrite(28,EE_AXLE_H); 01462 DataEEWrite(17214,EE_AXLE_L); 01463 } 01464 01465 void ConstantsDefaultR (void) // get constant values as default 01466 { 01467 ANGLE_KP=Q15(0.9999); 01468 ANGLE_KI=Q15(0.7); 01469 ANGLE_KD=Q15(0.0001); 01470 01471 // KP, KI, KD x Distance PID in fractional 01472 // fractional, 3 x 2 = 6 bytes 01473 DIST_KP=Q15(0.9999); 01474 DIST_KI=Q15(0.8); 01475 DIST_KD=Q15(0.0001); 01476 01477 KP1=Q15(0.9); 01478 KI1=Q15(0.7); 01479 KD1=Q15(0.1); 01480 KP2=Q15(0.9); 01481 KI2=Q15(0.7); 01482 KD2=Q15(0.1); 01483 01484 Kvel[R] = 26555230; // x 1 value 01485 Kvel[L] = 26487032; 01486 01487 Ksp[R] = 0.005065008; 01488 Ksp[L] = 0.0050520; 01489 Axle = 184.8728; 01490 01491 SemiAxle = Axle/2; 01492 } 01493 01494 void ConstantsError(void) // Constants paramters error occured 01495 { 01496 BlinkPeriod = K_ERR_BLINK_PER;// LED1 blinking period (ms) 01497 BlinkOn = K_ERR_BLINK_ON; // LED1 on time (ms) 01498 Blink = 0; 01499 ErrCode=-20; // store the last Error Code 01500 ConstantsDefaultR(); // use default parameters 01501 } 01502 void ConstantsRead(void) // get constant values from permament memory 01503 { 01504 long Tmp1Long; 01505 long Tmp2Long; 01506 int C1; // generic counter 01507 int C2; 01508 int C3; 01509 int TmpChk; 01510 01511 #ifdef NO_FLASH // [34a] 01512 #warning -- compiling with NO FLASH ************************************* 01513 ConstantsDefaultR(); 01514 return; 01515 #endif 01516 01517 if (DataEERead(EE_KP1) == 0xFFFF) // default values if not programmed 01518 { 01519 ConstantsError(); 01520 return; 01521 } 01522 01523 C3 = EE_SCHED; // starting address for scheduler sequence storage 01524 TmpChk=0; 01525 for (C1=0; C1<16; C1++) 01526 { 01527 for (C2=0; C2<4; C2++) 01528 { 01529 SchedValues[C1][C2]=(DataEERead(C3)); 01530 TmpChk+=SchedValues[C1][C2]; 01531 C3 ++; 01532 } 01533 } 01534 if (TmpChk!=DataEERead(EE_CHK_SCHED)) 01535 { 01536 ConstantsError(); 01537 return; 01538 } 01539 01540 /* Speed calculation K in micron/second = 26290341 [4] [7] [19] 01541 Speed calculation K in m/s as a power of 2 to semplify dsPID elaboration 01542 Kvel[] = (K << 15) 01543 long, 2 x 4 bytes = 8 byte 01544 Kvel[x] = 26290341; x 1 value = 26290341 x 2 value = 13145171 01545 */ 01546 Tmp1Long=(long)DataEERead(EE_KVEL1_H); 01547 TmpChk=Tmp1Long; 01548 Tmp2Long=(long)DataEERead(EE_KVEL1_L); 01549 TmpChk+=Tmp2Long; 01550 Kvel[R] = ((Tmp1Long << 16) + Tmp2Long); 01551 Tmp1Long=(long)DataEERead(EE_KVEL2_H); 01552 TmpChk+=Tmp1Long; 01553 Tmp2Long=(long)DataEERead(EE_KVEL2_L); 01554 TmpChk+=Tmp2Long; 01555 Kvel[L] = ((Tmp1Long << 16) + Tmp2Long); 01556 01557 if (TmpChk!=DataEERead(EE_CHK_KVEL)) 01558 { 01559 ConstantsError(); 01560 return; 01561 } 01562 01563 /* KP, KI, KD x Angle PID [23] 01564 fractional, 3 x 2 = 6 bytes 01565 ANGLE_KP=0.9999 01566 ANGLE_KI=0.8 01567 ANGLE_KD=0 01568 */ 01569 01570 Tmp1Long=DataEERead(EE_ANGLE_KP); 01571 TmpChk=Tmp1Long; 01572 ANGLE_KP=Q15((float)(Tmp1Long)/10000); 01573 Tmp1Long=DataEERead(EE_ANGLE_KI); 01574 TmpChk+=Tmp1Long; 01575 ANGLE_KI=Q15((float)(Tmp1Long)/10000); 01576 Tmp1Long=DataEERead(EE_ANGLE_KD); 01577 TmpChk+=Tmp1Long; 01578 ANGLE_KD=Q15((float)(Tmp1Long)/10000); 01579 01580 if (TmpChk!=DataEERead(EE_CHK_ANGLE)) 01581 { 01582 ConstantsError(); 01583 return; 01584 } 01585 01586 /* KP, KI, KD x Dist PID [24] 01587 fractional, 3 x 2 = 6 bytes 01588 DIST_KP=0.9999 01589 DIST_KI=0.8 01590 DIST_KD=0 01591 */ 01592 01593 Tmp1Long=DataEERead(EE_DIST_KP); 01594 TmpChk=Tmp1Long; 01595 DIST_KP=Q15((float)(Tmp1Long)/10000); 01596 Tmp1Long=DataEERead(EE_DIST_KI); 01597 TmpChk+=Tmp1Long; 01598 DIST_KI=Q15((float)(Tmp1Long)/10000); 01599 Tmp1Long=DataEERead(EE_DIST_KD); 01600 TmpChk+=Tmp1Long; 01601 DIST_KD=Q15((float)(Tmp1Long)/10000); 01602 01603 if (TmpChk!=DataEERead(EE_CHK_DIST)) 01604 { 01605 ConstantsError(); 01606 return; 01607 } 01608 01609 /* KP, KI, KD x Speed PID1 and PID2 in int x 10.000 01610 fractional, 2MCs x 3params x 2bytes = 12 bytes 01611 KP1=0.6 01612 KI1=0.7 01613 KD1=0.1 01614 KP2=0.6 01615 KI2=0.7 01616 KD2=0.1 01617 */ 01618 Tmp1Long=DataEERead(EE_KP1); 01619 TmpChk=Tmp1Long; 01620 KP1=Q15((float)(Tmp1Long)/10000); 01621 Tmp1Long=DataEERead(EE_KI1); 01622 TmpChk+=Tmp1Long; 01623 KI1=Q15((float)(Tmp1Long)/10000); 01624 Tmp1Long=DataEERead(EE_KD1); 01625 TmpChk+=Tmp1Long; 01626 KD1=Q15((float)(Tmp1Long)/10000); 01627 Tmp1Long=DataEERead(EE_KP2); 01628 TmpChk+=Tmp1Long; 01629 KP2=Q15((float)(Tmp1Long)/10000); 01630 Tmp1Long=DataEERead(EE_KI2); 01631 TmpChk+=Tmp1Long; 01632 KI2=Q15((float)(Tmp1Long)/10000); 01633 Tmp1Long=DataEERead(EE_KD2); 01634 TmpChk+=Tmp1Long; 01635 KD2=Q15((float)(Tmp1Long)/10000); 01636 01637 if (TmpChk!=DataEERead(EE_CHK_SPEED)) 01638 { 01639 ConstantsError(); 01640 return; 01641 } 01642 01643 /* constants for traveled distance calculation: SPACE_ENC_4X in mm [21] 01644 float, 2 x 4 = 8 bytes 01645 Ksp[x] = 0.00506145483078356; 01646 */ 01647 Tmp1Long=(long)DataEERead(EE_KSP1_H); 01648 TmpChk=Tmp1Long; 01649 Tmp2Long=(long)DataEERead(EE_KSP1_L); 01650 TmpChk+=Tmp2Long; 01651 Ksp[R] = (float)(((Tmp1Long << 16) + Tmp2Long))/((float)(1000000000)); 01652 Tmp1Long=(long)DataEERead(EE_KSP2_H); 01653 TmpChk+=Tmp1Long; 01654 Tmp2Long=(long)DataEERead(EE_KSP2_L); 01655 TmpChk+=Tmp2Long; 01656 Ksp[L] = (float)(((Tmp1Long << 16) + Tmp2Long))/((float)(1000000000)); 01657 01658 /* base width, distance between center of the wheels [21] 01659 float, 1 x 4 = 4 bytes 01660 Axle = 185.2222; 01661 */ 01662 01663 Tmp1Long = (long)DataEERead(EE_AXLE_H); 01664 TmpChk+=Tmp1Long; 01665 Tmp2Long = (long)DataEERead(EE_AXLE_L); 01666 TmpChk+=Tmp2Long; 01667 Axle = ((float)((Tmp1Long << 16) + Tmp2Long))/((float)(10000)); 01668 if (TmpChk!=DataEERead(EE_CHK_MECH)) 01669 { 01670 ConstantsError(); 01671 return; 01672 } 01673 01674 SemiAxle = Axle/2; 01675 } 01676 01677 void InitPid1(void) 01678 { 01679 //Initialize the PID data structure: PIDstruct 01680 //Set up pointer to derived coefficients 01681 PIDstruct1.abcCoefficients = &abcCoefficient1[0]; 01682 //Set up pointer to controller history samples 01683 PIDstruct1.controlHistory = &controlHistory1[0]; 01684 // Clear the controler history and the controller output 01685 PIDInit(&PIDstruct1); 01686 //Derive the a,b, & c coefficients from the Kp, Ki & Kd 01687 PIDCoeffCalc(&kCoeffs1[0], &PIDstruct1); 01688 } 01689 01690 void InitPid2(void) 01691 { 01692 //Initialize the PID data structure: PIDstruct 01693 //Set up pointer to derived coefficients 01694 PIDstruct2.abcCoefficients = &abcCoefficient2[0]; 01695 //Set up pointer to controller history samples 01696 PIDstruct2.controlHistory = &controlHistory2[0]; 01697 // Clear the controler history and the controller output 01698 PIDInit(&PIDstruct2); 01699 //Derive the a,b, & c coefficients from the Kp, Ki & Kd 01700 PIDCoeffCalc(&kCoeffs2[0], &PIDstruct2); 01701 } 01702 01703 void Pid1(void) 01704 { 01705 #ifdef SLOW_ENC 01706 if (labs(Vel[R]) > VEL_MIN_PID) // more frequent PID cycle above a threshold 01707 { 01708 Pid1Calc(); 01709 } 01710 else // [19f] 01711 { 01712 PidCycle[R] ++; 01713 if (PidCycle[R] >= 10) 01714 { 01715 Pid1Calc(); 01716 } 01717 } 01718 #else 01719 Pid1Calc(); 01720 #endif 01721 } 01722 01723 void Pid1Calc(void) 01724 {// [19] 01725 long IcPeriodTmp; 01726 int IcIndxTmp; 01727 int PWM; 01728 01729 IcPeriodTmp=Ic1Period; // [19a] 01730 IcIndxTmp=Ic1Indx; 01731 Ic1Indx = 0; 01732 Ic1Period=0; 01733 // IC1_FIRST=0; 01734 PID1_CALC_FLAG = 0; 01735 Vel[R]=0; 01736 01737 if (IcIndxTmp) // motor is running [19c] 01738 { 01739 Vel[R] = Kvel[R]*IcIndxTmp/IcPeriodTmp; 01740 } 01741 01742 SpTick[R] += (int)POS1CNT; // cast to signed to store direction [19] 01743 POS1CNT=0; 01744 01745 // calcolo PID 01746 if (RAMP_FLAG1) // the motor is acc/dec-elerating [19f] 01747 { 01748 PID_REF1 += Ramp1; 01749 if (RAMP_T_FLAG1) 01750 { 01751 if (PID_REF1 >= VelFin[R]) 01752 { 01753 PID_REF1 = VelFin[R]; 01754 RAMP_FLAG1 = 0; // acceleration is over 01755 } 01756 } 01757 else 01758 { 01759 if (PID_REF1 <= VelFin[R]) 01760 { 01761 PID_REF1 = VelFin[R]; 01762 RAMP_FLAG1 = 0; // acceleration is over 01763 } 01764 } 01765 } 01766 01767 PID_MES1 = (Vel[R]); // speed in m/s 01768 PID(&PIDstruct1); 01769 PWM = (PID_OUT1 >> 4) + 2048 ; // [19e] 01770 SetDCMCPWM1(1, PWM, 0); 01771 // MOTOR_ENABLE1 = 1; // [1] 01772 } 01773 01774 void Pid2(void) 01775 { 01776 #ifdef SLOW_ENC 01777 if (labs(Vel[L]) > VEL_MIN_PID) // more frequent PID cycle above a threshold 01778 { 01779 Pid2Calc(); 01780 } 01781 else // [19f] 01782 { 01783 PidCycle[L] ++; 01784 if (PidCycle[L] >= 10) 01785 { 01786 Pid2Calc(); 01787 } 01788 } 01789 #else 01790 Pid2Calc(); 01791 #endif 01792 } 01793 01794 void Pid2Calc(void) 01795 {// [19] 01796 long IcPeriodTmp; 01797 int IcIndxTmp; 01798 int PWM; 01799 01800 IcPeriodTmp=Ic2Period; // [19a] 01801 IcIndxTmp=Ic2Indx; 01802 Ic2Indx = 0; 01803 Ic2Period=0; 01804 // IC2_FIRST=0; 01805 PID2_CALC_FLAG = 0; 01806 Vel[L]=0; 01807 01808 if (IcIndxTmp) // motor is running [19c] 01809 { 01810 Vel[L] = Kvel[L]*IcIndxTmp/IcPeriodTmp; 01811 } 01812 01813 SpTick[L] += (int)POS2CNT; // cast to signed to store direction [19] 01814 POS2CNT=0; 01815 01816 // calcolo PID 01817 if (RAMP_FLAG2) // the motor is acc/dec-elerating [19f] 01818 { 01819 PID_REF2 += Ramp2; 01820 if (RAMP_T_FLAG2) 01821 { 01822 if (PID_REF2 >= VelFin[L]) 01823 { 01824 PID_REF2 = VelFin[L]; 01825 RAMP_FLAG2 = 0; // acceleration is over 01826 } 01827 } 01828 else 01829 { 01830 if (PID_REF2 <= VelFin[L]) 01831 { 01832 PID_REF2 = VelFin[L]; 01833 RAMP_FLAG2 = 0; // acceleration is over 01834 } 01835 } 01836 } 01837 01838 PID_MES2 = (Vel[L]); // speed in m/s 01839 PID(&PIDstruct2); 01840 PWM = (PID_OUT2 >> 4) + 2048 ; // [19e] 01841 SetDCMCPWM1(2, PWM, 0); 01842 // MOTOR_ENABLE2 = 1; // [1] 01843 } 01844 01845 01846 void DelayN1ms(int n) 01847 { 01848 int ms; 01849 for (ms = 0; ms < n; ms ++) 01850 { 01851 DelayN10us(100); 01852 } 01853 } 01854 01855 void DelayN10us(int n) // [22] 01856 { 01857 int DelayCount; 01858 for (DelayCount = 0; DelayCount < (57 * n); DelayCount ++); 01859 } 01860 01861 void TxCont(void) 01862 { 01863 int Ptmp; // temp for position values 01864 int CurrTmp; // temp for motors current value 01865 int VelInt[2]; // speed in mm/s as an integer 01866 int Alpha; // rotation angle in degrees 01867 01868 UartContTxTimer=UART_CONT_TIMEOUT; // timer reset 01869 if (TxContFlag == 1) 01870 { 01871 Ptmp = (VelMes[R] + VelMes[L]) >> 1; // average speed 01872 UartTxBuff[0]=Ptmp>>8; 01873 UartTxBuff[1]=Ptmp; 01874 UartTxBuff[2]=9; // tab 01875 // Curr = int -> 2byte (mA) 01876 CurrTmp = ADCValue[R]+ADCValue[L]; // total current 01877 UartTxBuff[3]=CurrTmp>>8; 01878 UartTxBuff[4]=CurrTmp; 01879 UartTxBuff[5]=9; // tab 01880 // PosXmes rounded in a Int -> 2 byte (mm) 01881 Ptmp = FLOAT2INT(PosXmes); // PosX 01882 UartTxBuff[6]=Ptmp>>8; 01883 UartTxBuff[7]=Ptmp; 01884 UartTxBuff[8]=9; // tab 01885 // PosYmes rounded in a Int -> 2 byte (mm) 01886 Ptmp = FLOAT2INT(PosYmes); // PosY 01887 UartTxBuff[9]=Ptmp>>8; 01888 UartTxBuff[10]=Ptmp; 01889 UartTxBuff[11]=9; // tab 01890 // ThetaMes rounded in a Int -> 2 byte (degrees) 01891 Ptmp = ThetaMes * RAD2DEG; // Theta 01892 Alpha = FLOAT2INT(Ptmp); 01893 UartTxBuff[12]=Alpha>>8; 01894 UartTxBuff[13]=Alpha; 01895 UartTxBuff[14]=10; // LF 01896 UartTxBuff[15]=13; // CR 01897 DMA6CNT = 15; // # of DMA requests 01898 } 01899 else 01900 { 01901 // VelInt = Int -> 2 byte 01902 VelInt[R]=(long)(Vel[R] * 1000)>>15; // VelR 01903 UartTxBuff[0]=VelInt[R]>>8; 01904 UartTxBuff[1]=VelInt[R]; 01905 UartTxBuff[2]=9; // tab 01906 VelInt[L]=(long)(Vel[L] * 1000)>>15; // VelL 01907 UartTxBuff[3]=VelInt[L]>>8; 01908 UartTxBuff[4]=VelInt[L]; 01909 UartTxBuff[5]=9; // tab 01910 // ADCValue = int -> 2byte 01911 UartTxBuff[6]=ADCValue[R]>>8; // CurrR 01912 UartTxBuff[7]=ADCValue[R]; 01913 UartTxBuff[8]=9; // tab 01914 UartTxBuff[9]=ADCValue[L]>>8; // CurrL 01915 UartTxBuff[10]=ADCValue[L]; 01916 UartTxBuff[11]=9; // tab 01917 // Space = int -> 2byte 01918 UartTxBuff[12]=SpTick[R]>>8; // SpTickR 01919 UartTxBuff[13]=SpTick[R]; 01920 UartTxBuff[14]=9; // tab 01921 SpTick[R] = 0; // [19] 01922 UartTxBuff[15]=SpTick[L]>>8; // SpTickL 01923 UartTxBuff[16]=SpTick[L]; 01924 SpTick[L] = 0; // [19] 01925 UartTxBuff[17]=10; // LF 01926 UartTxBuff[18]=13; // CR 01927 DMA6CNT = 18; // # of DMA requests 01928 } 01929 01930 DMA6CONbits.CHEN = 1; // Re-enable TX DMA Channel 01931 DMA6REQbits.FORCE = 1; // Manual mode: Kick-start the first TX 01932 } 01933 01934 /*---------------------------------------------------------------------------*/ 01935 /* Interrupt Service Routines */ 01936 /*---------------------------------------------------------------------------*/ 01937 #ifndef TIMER_OFF 01938 void _ISR_PSV _T1Interrupt(void) // Timer 1 [13] 01939 { 01940 _T1IF=0; // interrupt flag reset 01941 UartContTxTimer --; // timer for continuos send mode 01942 Blink ++; // heartbeat LED blink 01943 01944 // cycle 0 actions 01945 PID1_CALC_FLAG = 1; // PID1 and speed calculation enabled 01946 PID2_CALC_FLAG = 1; // PID2 and speed calculation enabled 01947 01948 Cycle1 ++; 01949 // cycle 1 actions 01950 if (Cycle1 >= CICLE1_TMO) 01951 { 01952 Cycle1 = 0; 01953 CYCLE1_FLAG = 1; // it's time to start first cycle actions [23] 01954 01955 Cycle2 ++; 01956 // cycle 2 actions 01957 if (Cycle2 >= CICLE2_TMO) 01958 { 01959 Cycle2 = 0; 01960 CYCLE2_FLAG=1; // it's time to start second cycle actions [24] 01961 IdleSample ++; // [25] 01962 RtTimer --; // real time delay 01963 RndTimer ++; // Timer for randomly avoid central obstacles 01964 } 01965 } 01966 } 01967 01968 void _ISR_PSV _T2Interrupt(void) // Timer 2 [12] 01969 { 01970 _T2IF = 0; // interrupt flag reset 01971 Tmr2OvflwCount1 ++; // TMR2 overflow as occurred 01972 Tmr2OvflwCount2 ++; // TMR2 overflow as occurred 01973 } 01974 01975 #endif 01976 01977 void _ISR_PSV _DMA7Interrupt(void) // DMA for ADC [2] 01978 { 01979 _DMA7IF = 0; // interrupt flag reset 01980 ADC_CALC_FLAG = 1; // enable ADC average calculus 01981 } 01982 01983 01984 void _ISR_PSV _DMA6Interrupt(void) // DMA for UART1 TX [6d] 01985 { 01986 _DMA6IF = 0; // interrupt flag reset 01987 MAP_BUFF_FLAG = 1; // ready to send another map grid row 01988 } 01989 01990 void _ISR_PSV _DMA5Interrupt(void) // DMA for UART2 TX [6zd] 01991 { 01992 _DMA5IF = 0; // interrupt flag reset 01993 MAP_BUFF_FLAG = 1; // ready to send another map grid row 01994 } 01995 01996 void _ISR_PSV _IC1Interrupt(void) // Input Capture 1 [7] 01997 { 01998 _IC1IF = 0; // interrupt flag reset 01999 if (IC1_FIRST)// first sample, stores TMR2 starting value [19b] 02000 { 02001 Ic1CurrPeriod = IC1BUF; 02002 if (Tmr2OvflwCount1 == 0) 02003 { 02004 Ic1Period += (Ic1CurrPeriod - Ic1PrevPeriod); 02005 } 02006 else 02007 {// [7a] 02008 #ifdef SLOW_ENC 02009 Ic1Period += (Ic1CurrPeriod + (0xFFFF - Ic1PrevPeriod) 02010 +(0xFFFF * (Tmr2OvflwCount1 - 1))); 02011 #else 02012 Ic1Period += (Ic1CurrPeriod + (0xFFFF - Ic1PrevPeriod)); 02013 #endif 02014 Tmr2OvflwCount1 = 0; 02015 } 02016 Ic1PrevPeriod = Ic1CurrPeriod; 02017 if (QEI1CONbits.UPDN) // [7b] 02018 { 02019 Ic1Indx ++; 02020 } 02021 else 02022 { 02023 Ic1Indx --; 02024 } 02025 } 02026 else 02027 { 02028 Ic1PrevPeriod = IC1BUF; 02029 IC1_FIRST=1; 02030 } 02031 } 02032 02033 02034 void _ISR_PSV _IC2Interrupt(void) // Input Capture 2 [7] 02035 { 02036 _IC2IF = 0; // interrupt flag reset 02037 if (IC2_FIRST)// first sample, stores TMR2 starting value [19b] 02038 { 02039 Ic2CurrPeriod = IC2BUF; 02040 if (Tmr2OvflwCount2 == 0) 02041 { 02042 Ic2Period += (Ic2CurrPeriod - Ic2PrevPeriod); 02043 } 02044 else 02045 {// [7a] 02046 #ifdef SLOW_ENC 02047 Ic2Period += (Ic2CurrPeriod + (0xFFFF - Ic2PrevPeriod) 02048 +(0xFFFF * (Tmr2OvflwCount2 - 1))); 02049 #else 02050 Ic2Period += (Ic2CurrPeriod + (0xFFFF - Ic2PrevPeriod)); 02051 #endif 02052 Tmr2OvflwCount2 = 0; 02053 } 02054 Ic2PrevPeriod = Ic2CurrPeriod; 02055 if (QEI2CONbits.UPDN) // [7b] 02056 { 02057 Ic2Indx ++; 02058 } 02059 else 02060 { 02061 Ic2Indx --; 02062 } 02063 } 02064 else 02065 { 02066 Ic2PrevPeriod = IC2BUF; 02067 IC2_FIRST=1; 02068 } 02069 } 02070 02071 02072 void __attribute__ ((interrupt, no_auto_psv)) _U1ErrInterrupt(void) 02073 { 02074 IFS4bits.U1EIF = 0; // Clear the UART1 Error Interrupt Flag 02075 } 02076 02077 void __attribute__ ((interrupt, no_auto_psv)) _U2ErrInterrupt(void) 02078 { 02079 IFS4bits.U2EIF = 0; // Clear the UART2 Error Interrupt Flag 02080 } 02081 02082 //{ 02083 /* Disabled------------------ 02084 void _ISR_PSV _CNInterrupt(void) // change Notification [3] 02085 { 02086 _CNIF = 0; // interrupt flag reset 02087 } 02088 02089 void _ISR_PSV _INT1Interrupt(void) // External Interrupt INT1 [8] 02090 { 02091 _INT1IF = 0; // interrupt flag reset 02092 ClrWdt(); // [1] 02093 LED1=0; // [1] 02094 } 02095 02096 void _ISR_PSV _U1TXInterrupt(void) // UART TX [6a] 02097 { 02098 _U1TXIF = 0; // interrupt flag reset 02099 02100 if (UartTxCntr < UartTxBuffSize) 02101 { 02102 WriteUART1(UartTxBuff[UartTxCntr]); 02103 UartTxCntr++; 02104 } 02105 else 02106 {// waits for UART sending complete to disable the peripheral 02107 TxFlag = 2; 02108 } 02109 } 02110 ------------------ Disabled */ 02111 //} 02112 /*****************************************************************************/ 02113 02114