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 }