This class library has been created for use with the Apeiros Robot by Abe Howell's Robotics.
Embed:
(wiki syntax)
Show/hide line numbers
Apeiros.cpp
00001 #include "Apeiros.h" 00002 #include "mbed.h" 00003 00004 //------------------------------------------------------------------------ 00005 // Constructor for Apeiros Class. 00006 //------------------------------------------------------------------------ 00007 Apeiros::Apeiros(PinName tx, PinName rx, int leftMotorPwmOffset, int rightMotorPwmOffset) : Serial(tx, rx), leftFrontIR(PC_15),centerFrontIR(PC_14),rightFrontIR(PC_13), 00008 leftRearIR(PA_13), rightRearIR(PA_14),ad_0(PA_0), ad_1(PA_1),ad_2(PA_4),ad_3(PB_0),ad_4(PC_1),ad_5(PC_0),_buzzerOut(PA_5),_SN_3A(PB_5),_SN_4A(PC_7),_SN_2A(PB_3),_SN_1A(PA_8), 00009 _leftEncoderDirPin(PA_6),_rightEncoderDirPin(PA_7),_leftEncoderClk(PH_1),_rightEncoderClk(PH_0),_rightMotorPWM(PB_10),_leftMotorPWM(PB_4),_gripperPWM(PB_6) 00010 { 00011 // Set direction of PWM dir pins to be low so robot is halted. // 00012 _SN_3A = 0; 00013 _SN_4A = 0; 00014 _SN_2A = 0; 00015 _SN_1A = 0; 00016 00017 // Configure Left & Right Motor PWMs. // 00018 _rightMotorPWM.period_us(255); 00019 _rightMotorPWM.pulsewidth_us(0); 00020 _leftMotorPWM.period_us(255); 00021 _leftMotorPWM.pulsewidth_us(0); 00022 00023 // Configure Gripper PWM. // 00024 _gripperPWM.period_ms(20); 00025 SetGripperPosition(2300); 00026 00027 // Configure optional Wheel Encoder Inputs & ISRs. // 00028 _leftEncoderDirPin.mode(PullUp); 00029 _rightEncoderDirPin.mode(PullUp); 00030 _leftEncoderClk.mode(PullDown); 00031 _rightEncoderClk.mode(PullUp); 00032 _leftEncoderClk.rise(this, &Apeiros::leftEncoderTick); 00033 _rightEncoderClk.fall(this, &Apeiros::rightEncoderTick); 00034 00035 // Configure Serial ISR & baud rate. // 00036 Serial::attach(this, &Apeiros::getUartData,Serial::RxIrq); 00037 baud(115200); 00038 00039 rxBuffIndx = 0; 00040 uartDataRdy = false; 00041 motorUpdateTickCount = 0; 00042 motionLoopCount = 0; 00043 00044 leftEncoderCount = 0; 00045 rightEncoderCount = 0; 00046 leftEncoderDir = 1; 00047 rightEncoderDir = 1; 00048 leftMotorOffset = leftMotorPwmOffset; 00049 rightMotorOffset = rightMotorPwmOffset; 00050 00051 analogIndex = 0; 00052 buzzerPeriod = 0; 00053 buzzerDuty = buzzerPeriod/2; 00054 buzzerTick = 0; 00055 enableBuzzer = false; 00056 00057 analogUpdateCount = 0; 00058 } 00059 00060 //------------------------------------------------------------------------ 00061 // Function to Begin using Apeiros class. 00062 //------------------------------------------------------------------------ 00063 void Apeiros::Begin() 00064 { 00065 // Configure motor control ISR & set to interrupt every 205us. // 00066 _motorControl.attach_us(this, &Apeiros::motorControlISR, 205); 00067 00068 printf("\r\n"); 00069 printf("Hello, my name is Apeiros!\r\n"); 00070 00071 // Play initialize buzzer tones. // 00072 SetBuzzerTone(1); 00073 wait_ms(250); 00074 SetBuzzerTone(10); 00075 wait_ms(250); 00076 SetBuzzerTone(1); 00077 wait_ms(250); 00078 SetBuzzerTone(0); 00079 } 00080 00081 //------------------------------------------------------------------------ 00082 // Serial Interrupt Service Routine (ISR) for data received via UART. 00083 //------------------------------------------------------------------------ 00084 void Apeiros::getUartData() 00085 { 00086 if (!uartDataRdy) 00087 { 00088 rxBuff[rxBuffIndx] = getc(); 00089 00090 if (rxBuff[rxBuffIndx] == 13) 00091 { 00092 for (rxBuffIndx = 0; rxBuffIndx < 16; rxBuffIndx ++) tmpRxBuff[rxBuffIndx] = rxBuff[rxBuffIndx]; 00093 rxBuffIndx = 0; 00094 uartDataRdy = true; 00095 } 00096 else 00097 { 00098 rxBuffIndx++; 00099 if (rxBuffIndx > 15) 00100 { 00101 rxBuffIndx = 0; 00102 for (int indx=0;indx<16;indx++) rxBuff[indx] = 0; 00103 } 00104 } 00105 } 00106 } 00107 00108 //------------------------------------------------------------------------ 00109 // Motor Control Interrupt Service Routine (ISR). 00110 //------------------------------------------------------------------------ 00111 void Apeiros::motorControlISR() 00112 { 00113 motorUpdateTickCount++; 00114 motionLoopCount++; 00115 analogUpdateCount++; 00116 00117 if (motionLoopCount > 250) 00118 { 00119 CalculateWheelSpeed(); 00120 motionLoopCount = 0; 00121 } 00122 00123 if (analogUpdateCount > 50) 00124 { 00125 // Update analog to digital (A/D) conversions. // 00126 switch (analogIndex) 00127 { 00128 case 0: 00129 adSensors[0] = ad_0.read(); 00130 break; 00131 00132 case 1: 00133 adSensors[1] = ad_1.read(); 00134 break; 00135 00136 case 2: 00137 adSensors[2] = ad_2.read(); 00138 break; 00139 00140 case 3: 00141 adSensors[3] = ad_3.read(); 00142 break; 00143 00144 case 4: 00145 adSensors[4] = ad_4.read(); 00146 break; 00147 00148 case 5: 00149 adSensors[5] = ad_5.read(); 00150 break; 00151 } 00152 analogIndex++; 00153 if (analogIndex > 5) analogIndex = 0; 00154 00155 analogUpdateCount = 0; 00156 } 00157 00158 // Update piezo buzzer tone as needed. // 00159 if (enableBuzzer) 00160 { 00161 buzzerTick++; 00162 if (buzzerTick > buzzerPeriod) 00163 { 00164 buzzerTick = 0; 00165 _buzzerOut = 1; 00166 } 00167 else if (buzzerTick > buzzerDuty) 00168 { 00169 _buzzerOut = 0; 00170 } 00171 } 00172 } 00173 00174 //------------------------------------------------------------------------ 00175 // Interrupt Service Routine (ISR) for Left Wheel Encoder Transitions. 00176 //------------------------------------------------------------------------ 00177 void Apeiros::leftEncoderTick() 00178 { 00179 leftEncoderDir = _leftEncoderDirPin.read(); 00180 if (!leftEncoderDir) leftEncoderCount++; 00181 else leftEncoderCount--; 00182 00183 // Remove the oldest value from the sum. 00184 encoderPeriodSum_L -= encoderPeriodArray_L[encoderArrayIndex_L]; 00185 00186 // Store the newest value, and add it to the sum. 00187 encoderPeriodSum_L += encoderPeriodArray_L[encoderArrayIndex_L] = motorUpdateTickCount - prevT3Count_L; 00188 00189 // Calculate a new average period. 00190 encoderPeriod_L = encoderPeriodSum_L/4; 00191 00192 // Move to the next array position. 00193 encoderArrayIndex_L++; 00194 if (encoderArrayIndex_L > 3) encoderArrayIndex_L = 0; 00195 00196 // Store the current timer3TickCount for next time. 00197 prevT3Count_L = motorUpdateTickCount; 00198 00199 // Set encoder as updated. 00200 encoderUpdated_L = true; 00201 } 00202 00203 //------------------------------------------------------------------------ 00204 // Interrupt Service Routine (ISR) for Right Wheel Encoder Transitions. 00205 //------------------------------------------------------------------------ 00206 void Apeiros::rightEncoderTick() 00207 { 00208 rightEncoderDir = _rightEncoderDirPin.read(); 00209 if (!rightEncoderDir) rightEncoderCount--; 00210 else rightEncoderCount++; 00211 00212 // Remove the oldest value from the sum. 00213 encoderPeriodSum_R -= encoderPeriodArray_R[encoderArrayIndex_R]; 00214 00215 // Store the newest value, and add it to the sum. 00216 encoderPeriodSum_R += encoderPeriodArray_R[encoderArrayIndex_R] = motorUpdateTickCount - prevT3Count_R; 00217 00218 // Calculate a new average period. 00219 encoderPeriod_R = encoderPeriodSum_R/4; 00220 00221 // Move to the next array position. 00222 encoderArrayIndex_R++; 00223 if (encoderArrayIndex_R > 3) encoderArrayIndex_R = 0; 00224 00225 // Store the current timer3TickCount for next time. 00226 prevT3Count_R = motorUpdateTickCount; 00227 00228 // Set encoder as updated. 00229 encoderUpdated_R = true; 00230 } 00231 00232 //------------------------------------------------------------------------ 00233 // Function to return status of UART data. 00234 //------------------------------------------------------------------------ 00235 bool Apeiros::IsSerialDataAvailable() 00236 { 00237 if (uartDataRdy) return true; 00238 else return false; 00239 } 00240 00241 //------------------------------------------------------------------------ 00242 // Function to parse UART data from Rx buffer for SDIO1 (PA9 & PA10). 00243 //------------------------------------------------------------------------ 00244 void Apeiros::ParseUartData() 00245 { 00246 switch (tmpRxBuff[0]){ 00247 00248 case 'A': 00249 printf("Sensors = (%0.3f, %0.3f, %0.3f, %0.3f, %0.3f, %0.3f)\r\n", adSensors[0], adSensors[1], adSensors[2], adSensors[3], adSensors[4], adSensors[5]); 00250 break; 00251 00252 case 'B': 00253 ParseBuzzerCommand(); 00254 printf("b\r\n"); 00255 break; 00256 00257 case 'D': 00258 printf("Sensors = (%d, %d, %d, %d, %d)\r\n", leftFrontIR.read(), centerFrontIR.read(), rightFrontIR.read(),leftRearIR.read(),rightRearIR.read()); 00259 break; 00260 00261 case 'E': 00262 printf("Encoder Ticks = %d, %d\r\n",leftEncoderCount, rightEncoderCount); 00263 break; 00264 00265 case 'G': 00266 ParseGripperCommand(); 00267 printf("g\r\n"); 00268 break; 00269 00270 case 'H': 00271 SetMotorSpeed(0, 0); 00272 printf("h\r\n"); 00273 break; 00274 00275 case 'M': 00276 ParseMotorCommand(); 00277 printf("ma\r\n"); 00278 break; 00279 00280 case 'V': 00281 printf("Motor Velocity = %d, %d\r\n",encoderSpeed_L, encoderSpeed_R); 00282 break; 00283 00284 case 'Y': 00285 _gripperPWM.pulsewidth_us(2000); 00286 printf("ya\r\n"); 00287 break; 00288 00289 case 'Z': 00290 _gripperPWM.pulsewidth_us(1000); 00291 printf("za\r\n"); 00292 break; 00293 00294 default: 00295 printf("Cmd unrecognized!\r\n"); 00296 break; 00297 } 00298 00299 for (int buffIndx = 0; buffIndx < 16; buffIndx++) tmpRxBuff[buffIndx] = 0; 00300 00301 uartDataRdy = false; 00302 } 00303 00304 //------------------------------------------------------------------------ 00305 // Function to set tone of piezo buzzer. 00306 //------------------------------------------------------------------------ 00307 void Apeiros::SetBuzzerTone(int buzzerTone) 00308 { 00309 if (buzzerTone != buzzerPeriod) 00310 { 00311 if (buzzerTone > 0) 00312 { 00313 if (buzzerTone > MAX_BUZZER_PWM) buzzerTone = MAX_BUZZER_PWM; 00314 buzzerTick = 1; 00315 buzzerPeriod = buzzerTone; 00316 buzzerDuty = buzzerPeriod/2; 00317 enableBuzzer = true; 00318 } 00319 else 00320 { 00321 buzzerPeriod = 0; 00322 enableBuzzer = false; 00323 _buzzerOut = 0; 00324 } 00325 } 00326 } 00327 00328 //------------------------------------------------------------------------ 00329 // Calculate Wheel Speeds 00330 // 00331 // Speed in 0.1"/sec = (Cwh / NCLKS) * TSCALE / (TICK * PER) = Ktps / PER 00332 // where Cwh = 8.12" wheel circumference, NCLKS = 128 (32 stripe disk), 00333 // TSCALE = 10 (to convert inches to 0.1"), TICK = 205us per timer2 tick, 00334 // and PER is the measured period in timer 2 ticks 00335 // Ktps = 3098 00336 //------------------------------------------------------------------------ 00337 void Apeiros::CalculateWheelSpeed() 00338 { 00339 // If the wheel is spinning so slow that we don't have current reading. 00340 if (!encoderUpdated_R) encoderPeriod_R = motorUpdateTickCount - prevT3Count_R; // Calculate period since last update. 00341 else encoderUpdated_R = false; // Otherwise use it. 00342 00343 // Converts from 205us ticks per edge to multiples of 0.1 inches per second. 00344 encoderSpeed_R = (signed short)(Ktps / encoderPeriod_R); 00345 00346 // Check direction of wheel rotation & adjust as needed. */ 00347 if (!rightEncoderDir) encoderSpeed_R = -encoderSpeed_R; 00348 00349 if (!encoderUpdated_L) encoderPeriod_L = motorUpdateTickCount - prevT3Count_L; 00350 else encoderUpdated_L = false; 00351 00352 // Converts from 205us ticks per edge to multiples of 0.1 inches per second 00353 encoderSpeed_L = (signed short)(Ktps / encoderPeriod_L); 00354 00355 // Check direction of wheel rotation & adjust as needed. */ 00356 if (leftEncoderDir) encoderSpeed_L = -encoderSpeed_L; 00357 } 00358 00359 //------------------------------------------------------------------------ 00360 // Function to set left & right motor speed and direction. 00361 //------------------------------------------------------------------------ 00362 void Apeiros::SetMotorSpeed(int leftMotorSpeed, int rightMotorSpeed) 00363 { 00364 00365 int tmpLeftSpeed = 0; 00366 int tmpRightSpeed = 0; 00367 00368 if (rightMotorSpeed < 0) 00369 { 00370 _SN_2A = 0; 00371 _SN_1A = 1; 00372 } 00373 else { 00374 _SN_2A = 1; 00375 _SN_1A = 0; } 00376 00377 if (leftMotorSpeed < 0) 00378 { 00379 _SN_3A = 0; 00380 _SN_4A = 1; 00381 } 00382 else { 00383 _SN_3A = 1; 00384 _SN_4A = 0; } 00385 00386 if (leftMotorSpeed != 0) tmpLeftSpeed = abs(leftMotorSpeed) + leftMotorOffset; 00387 else 00388 { 00389 tmpLeftSpeed = 0; 00390 _SN_3A = _SN_4A = 0; 00391 } 00392 00393 if (rightMotorSpeed != 0) tmpRightSpeed = abs(rightMotorSpeed) + rightMotorOffset; 00394 else 00395 { 00396 tmpRightSpeed = 0; 00397 _SN_1A = _SN_2A = 0; 00398 } 00399 00400 if (tmpLeftSpeed > MaxMotorSpeed) tmpLeftSpeed = MaxMotorSpeed; 00401 if (tmpRightSpeed > MaxMotorSpeed) tmpRightSpeed = MaxMotorSpeed; 00402 00403 _leftMotorPWM.pulsewidth_us(tmpLeftSpeed); 00404 _rightMotorPWM.pulsewidth_us(tmpRightSpeed); 00405 00406 } 00407 00408 //------------------------------------------------------------------------ 00409 // Function to parse motor commands from UART data. 00410 //------------------------------------------------------------------------ 00411 void Apeiros::ParseMotorCommand() 00412 { 00413 unsigned int commaPos, endPos, index1, index2; 00414 signed short leftWheel, rightWheel; 00415 char charBuff[5]; 00416 bool foundNegative = false; 00417 00418 commaPos = 0; 00419 endPos = 0; 00420 00421 // Find comma position and return char. 00422 for (index1=2;index1<16;index1++) 00423 { 00424 if (tmpRxBuff[index1] == ',') commaPos = index1; 00425 else if (tmpRxBuff[index1] == 13) 00426 { 00427 endPos = index1; 00428 break; 00429 } 00430 } 00431 00432 // Parse out left motor data. 00433 for (index1=0;index1<5;index1++) charBuff[index1] = ' '; 00434 index2 = 0; 00435 for (index1=2;index1<commaPos;index1++) 00436 { 00437 if (tmpRxBuff[index1] != '-') charBuff[index2] = tmpRxBuff[index1]; 00438 else foundNegative = true; 00439 index2++; 00440 } 00441 leftWheel = atol(charBuff); 00442 if (foundNegative) leftWheel = -leftWheel; 00443 00444 foundNegative = false; 00445 00446 // Parse out right motor data. 00447 for (index1=0;index1<5;index1++) charBuff[index1] = ' '; 00448 index2 = 0; 00449 for (index1=commaPos+1;index1<endPos;index1++) 00450 { 00451 if (tmpRxBuff[index1] != '-') charBuff[index2] = tmpRxBuff[index1]; 00452 else foundNegative = true; 00453 index2++; 00454 } 00455 rightWheel = atol(charBuff); 00456 if (foundNegative) rightWheel = -rightWheel; 00457 00458 SetMotorSpeed(leftWheel, rightWheel); 00459 } 00460 00461 //------------------------------------------------------------------------ 00462 // Function to parse piezo buzzer tone commands from UART data. 00463 //------------------------------------------------------------------------ 00464 void Apeiros::ParseBuzzerCommand() 00465 { 00466 unsigned int index1, index2, endPos; 00467 unsigned int buzzerTonePWM; 00468 char charBuff[3]; 00469 00470 index1 = index2 = endPos = 0; 00471 buzzerTonePWM = 1; 00472 00473 for (index1=0;index1<3;index1++) charBuff[index1] = ' '; 00474 00475 // Find position of return char. 00476 for (index1=1;index1<16;index1++) 00477 { 00478 if (tmpRxBuff[index1] == 13) 00479 { 00480 endPos = index1; 00481 break; 00482 } 00483 } 00484 00485 index2 = 0; 00486 for (index1=1;index1<endPos;index1++) 00487 { 00488 charBuff[index2] = tmpRxBuff[index1]; 00489 index2++; 00490 } 00491 00492 buzzerTonePWM = atol(charBuff); 00493 SetBuzzerTone(buzzerTonePWM); 00494 } 00495 00496 //------------------------------------------------------------------------ 00497 // Function to parse gripper servo commands from UART data. 00498 //------------------------------------------------------------------------ 00499 void Apeiros::ParseGripperCommand() 00500 { 00501 unsigned int index1, index2, endPos; 00502 unsigned int gripperPosition; 00503 char charBuff[4]; 00504 00505 index1 = index2 = endPos = 0; 00506 gripperPosition = 2000; 00507 00508 for (index1=0;index1<4;index1++) charBuff[index1] = ' '; 00509 00510 // Find position of return char. 00511 for (index1=1;index1<16;index1++) 00512 { 00513 if (tmpRxBuff[index1] == 13) 00514 { 00515 endPos = index1; 00516 break; 00517 } 00518 } 00519 00520 index2 = 0; 00521 for (index1=1;index1<endPos;index1++) 00522 { 00523 charBuff[index2] = tmpRxBuff[index1]; 00524 index2++; 00525 } 00526 00527 gripperPosition = atol(charBuff); 00528 SetGripperPosition(gripperPosition); 00529 } 00530 00531 //------------------------------------------------------------------------ 00532 // Function to set gripper servo position using supplied pulsewidth[us]. 00533 //------------------------------------------------------------------------ 00534 void Apeiros::SetGripperPosition(int pulseWidth_us) 00535 { 00536 if (pulseWidth_us > MAX_GRIPPER_PULSE) pulseWidth_us = MAX_GRIPPER_PULSE; 00537 if (pulseWidth_us < MIN_GRIPPER_PULSE) pulseWidth_us = MIN_GRIPPER_PULSE; 00538 00539 _gripperPWM.pulsewidth_us(pulseWidth_us); 00540 } 00541 00542 //------------------------------------------------------------------------ 00543 // Function to return left wheel encoder count [integer]. 00544 //------------------------------------------------------------------------ 00545 int Apeiros::GetLeftEncoder() 00546 { 00547 return leftEncoderCount; 00548 } 00549 00550 //------------------------------------------------------------------------ 00551 // Function to return left wheel encoder count [integer]. 00552 //------------------------------------------------------------------------ 00553 int Apeiros::GetRightEncoder() 00554 { 00555 return rightEncoderCount; 00556 } 00557 00558 //------------------------------------------------------------------------ 00559 // Function to reset both the left & right wheel encoder counts. 00560 //------------------------------------------------------------------------ 00561 void Apeiros::ResetWheelEncoders() 00562 { 00563 leftEncoderCount = 0; 00564 rightEncoderCount = 0; 00565 }
Generated on Fri Jul 15 2022 01:40:07 by 1.7.2