File content as of revision 3:c61c3feb0729:

#include "Apeiros.h"
#include "mbed.h"

//------------------------------------------------------------------------
// Constructor for Apeiros Class.                  
//------------------------------------------------------------------------
Apeiros::Apeiros(PinName tx, PinName rx, int leftMotorPwmOffset, int rightMotorPwmOffset) : Serial(tx,  rx), leftFrontIR(PC_15),centerFrontIR(PC_14),rightFrontIR(PC_13),
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),
_leftEncoderDirPin(PA_6),_rightEncoderDirPin(PA_7),_leftEncoderClk(PH_1),_rightEncoderClk(PH_0),_rightMotorPWM(PB_10),_leftMotorPWM(PB_4),_gripperPWM(PB_6)
{   
    // Set direction of PWM dir pins to be low so robot is halted. //
    _SN_3A = 0;
    _SN_4A = 0;
    _SN_2A = 0;
    _SN_1A = 0;
    
    // Configure Left & Right Motor PWMs. //
    _rightMotorPWM.period_us(255);
    _rightMotorPWM.pulsewidth_us(0);    
    _leftMotorPWM.period_us(255);
    _leftMotorPWM.pulsewidth_us(0);
    
    // Configure Gripper PWM. //
    _gripperPWM.period_ms(20);
    SetGripperPosition(2300);
    
    // Configure optional Wheel Encoder Inputs & ISRs. //
    _leftEncoderDirPin.mode(PullUp);
    _rightEncoderDirPin.mode(PullUp);
    _leftEncoderClk.mode(PullDown);
    _rightEncoderClk.mode(PullUp);  
    _leftEncoderClk.rise(this, &Apeiros::leftEncoderTick);
    _rightEncoderClk.fall(this, &Apeiros::rightEncoderTick);
    
    // Configure Serial ISR & baud rate. //
    Serial::attach(this, &Apeiros::getUartData,Serial::RxIrq);
    baud(115200);
    
    rxBuffIndx = 0;
    uartDataRdy = false;
    motorUpdateTickCount = 0;
    motionLoopCount = 0;
    
    leftEncoderCount = 0;
    rightEncoderCount = 0;
    leftEncoderDir = 1;
    rightEncoderDir = 1;
    leftMotorOffset = leftMotorPwmOffset;
    rightMotorOffset = rightMotorPwmOffset;
    
    analogIndex = 0;
    buzzerPeriod = 0;
    buzzerDuty = buzzerPeriod/2;
    buzzerTick = 0;
    enableBuzzer = false;
    
    analogUpdateCount = 0;
}

//------------------------------------------------------------------------
// Function to Begin using Apeiros class.                     
//------------------------------------------------------------------------
void Apeiros::Begin()
{
    // Configure motor control ISR & set to interrupt every 205us. //
    _motorControl.attach_us(this, &Apeiros::motorControlISR, 205);
    
    printf("\r\n");
    printf("Hello, my name is Apeiros!\r\n");
    
    // Play initialize buzzer tones. //
    SetBuzzerTone(1);
    wait_ms(250);
    SetBuzzerTone(10);
    wait_ms(250);
    SetBuzzerTone(1);
    wait_ms(250);
    SetBuzzerTone(0);
}

//------------------------------------------------------------------------
// Serial Interrupt Service Routine (ISR) for data received via UART.                  
//------------------------------------------------------------------------
void Apeiros::getUartData()
{    
    if (!uartDataRdy)
    { 
      rxBuff[rxBuffIndx] = getc();
      
      if (rxBuff[rxBuffIndx] == 13)
      {
        for (rxBuffIndx = 0; rxBuffIndx < 16; rxBuffIndx ++) tmpRxBuff[rxBuffIndx] = rxBuff[rxBuffIndx];
        rxBuffIndx = 0;
        uartDataRdy = true;
       }   
       else
       {
            rxBuffIndx++;
            if (rxBuffIndx > 15) 
            {
               rxBuffIndx = 0;
               for (int indx=0;indx<16;indx++) rxBuff[indx] = 0;
            }
       }
    }
}

//------------------------------------------------------------------------
// Motor Control Interrupt Service Routine (ISR).
//------------------------------------------------------------------------
void Apeiros::motorControlISR()
{
    motorUpdateTickCount++;
    motionLoopCount++;
    analogUpdateCount++;
    
    if (motionLoopCount > 250)
    {
        CalculateWheelSpeed();
        motionLoopCount = 0;
    }
    
    if (analogUpdateCount > 50)
    {
        // Update analog to digital (A/D) conversions. //
        switch (analogIndex)
        {
            case 0:
             adSensors[0] = ad_0.read();
             break;
            
            case 1:
             adSensors[1] = ad_1.read();
             break;
            
            case 2:
             adSensors[2] = ad_2.read();
             break;
            
            case 3:
             adSensors[3] = ad_3.read();
             break;
            
            case 4:
             adSensors[4] = ad_4.read();
             break;
            
            case 5:
             adSensors[5] = ad_5.read();
             break;        
        }
        analogIndex++;
        if (analogIndex > 5) analogIndex = 0;
        
        analogUpdateCount = 0;
    }
    
    // Update piezo buzzer tone as needed. //
    if (enableBuzzer)
    {
        buzzerTick++;
        if (buzzerTick > buzzerPeriod)
        {
            buzzerTick = 0;
            _buzzerOut = 1;
        }
        else if (buzzerTick > buzzerDuty)
        {
            _buzzerOut = 0;
        }
    }
}

//------------------------------------------------------------------------
// Interrupt Service Routine (ISR) for Left Wheel Encoder Transitions.                     
//------------------------------------------------------------------------
void Apeiros::leftEncoderTick()
{
    leftEncoderDir = _leftEncoderDirPin.read();
    if (!leftEncoderDir) leftEncoderCount++;
    else leftEncoderCount--;
    
    // Remove the oldest value from the sum.
    encoderPeriodSum_L -= encoderPeriodArray_L[encoderArrayIndex_L];
    
    // Store the newest value, and add it to the sum.
    encoderPeriodSum_L += encoderPeriodArray_L[encoderArrayIndex_L] = motorUpdateTickCount - prevT3Count_L;
    
    // Calculate a new average period.
    encoderPeriod_L = encoderPeriodSum_L/4;
    
    // Move to the next array position.
    encoderArrayIndex_L++;                   
    if (encoderArrayIndex_L > 3) encoderArrayIndex_L = 0;
      
    // Store the current timer3TickCount for next time.
    prevT3Count_L = motorUpdateTickCount;
    
    // Set encoder as updated. 
    encoderUpdated_L = true;
}

//------------------------------------------------------------------------
// Interrupt Service Routine (ISR) for Right Wheel Encoder Transitions.                     
//------------------------------------------------------------------------
void Apeiros::rightEncoderTick()
{
    rightEncoderDir = _rightEncoderDirPin.read();
    if (!rightEncoderDir) rightEncoderCount--;
    else rightEncoderCount++;
    
    // Remove the oldest value from the sum.
    encoderPeriodSum_R -= encoderPeriodArray_R[encoderArrayIndex_R];
    
    // Store the newest value, and add it to the sum.
    encoderPeriodSum_R += encoderPeriodArray_R[encoderArrayIndex_R] = motorUpdateTickCount - prevT3Count_R;
    
    // Calculate a new average period.
    encoderPeriod_R = encoderPeriodSum_R/4;
    
    // Move to the next array position.
    encoderArrayIndex_R++;                   
    if (encoderArrayIndex_R > 3) encoderArrayIndex_R = 0;
      
    // Store the current timer3TickCount for next time.
    prevT3Count_R = motorUpdateTickCount;   
    
    // Set encoder as updated. 
    encoderUpdated_R = true;
}

//------------------------------------------------------------------------
// Function to return status of UART data.
//------------------------------------------------------------------------
bool Apeiros::IsSerialDataAvailable()
{
    if (uartDataRdy) return true;
    else return false;
}

//------------------------------------------------------------------------
// Function to parse UART data from Rx buffer for SDIO1 (PA9 & PA10).                     
//------------------------------------------------------------------------
void Apeiros::ParseUartData()
{    
    switch (tmpRxBuff[0]){

       case 'A':
        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]);
        break;
        
       case 'B':
        ParseBuzzerCommand();
        printf("b\r\n");
        break;
                
       case 'D':
        printf("Sensors = (%d, %d, %d, %d, %d)\r\n", leftFrontIR.read(), centerFrontIR.read(), rightFrontIR.read(),leftRearIR.read(),rightRearIR.read());
        break;

       case 'E':
        printf("Encoder Ticks = %d, %d\r\n",leftEncoderCount, rightEncoderCount);
        break;
                
       case 'G':
        ParseGripperCommand();
        printf("g\r\n");
        break;
        
       case 'H':
        SetMotorSpeed(0, 0);
        printf("h\r\n");
        break;
       
       case 'M':
        ParseMotorCommand();
        printf("ma\r\n");
        break;
       
       case 'V':
        printf("Motor Velocity = %d, %d\r\n",encoderSpeed_L, encoderSpeed_R);
        break;
       
       case 'Y':
        _gripperPWM.pulsewidth_us(2000);
        printf("ya\r\n");
        break;
        
       case 'Z':
        _gripperPWM.pulsewidth_us(1000);
        printf("za\r\n");
        break;
        
       default:
        printf("Cmd unrecognized!\r\n");
        break;
    }   
    
    for (int buffIndx = 0; buffIndx < 16; buffIndx++) tmpRxBuff[buffIndx] = 0;
    
    uartDataRdy = false;
}

//------------------------------------------------------------------------
// Function to set tone of piezo buzzer.
//------------------------------------------------------------------------
void Apeiros::SetBuzzerTone(int buzzerTone)
{    
    if (buzzerTone != buzzerPeriod)
    {
        if (buzzerTone > 0)
        {
            if (buzzerTone > MAX_BUZZER_PWM) buzzerTone = MAX_BUZZER_PWM;
            buzzerTick = 1;
            buzzerPeriod = buzzerTone;
            buzzerDuty = buzzerPeriod/2;
            enableBuzzer = true;
        }
        else
        {
            buzzerPeriod = 0;
            enableBuzzer = false;
            _buzzerOut = 0;
        }
    }
}

//------------------------------------------------------------------------
// Calculate Wheel Speeds                                                        
//                                                                            
// Speed in 0.1"/sec = (Cwh / NCLKS) * TSCALE / (TICK * PER) = Ktps / PER     
// where Cwh = 8.12" wheel circumference, NCLKS = 128 (32 stripe disk),       
// TSCALE = 10 (to convert inches to 0.1"), TICK = 205us per timer2 tick,     
// and PER is the measured period in timer 2 ticks                            
// Ktps = 3098                                                                
//------------------------------------------------------------------------
void Apeiros::CalculateWheelSpeed()
  {
    // If the wheel is spinning so slow that we don't have current reading.
    if (!encoderUpdated_R) encoderPeriod_R = motorUpdateTickCount - prevT3Count_R;  // Calculate period since last update.
    else encoderUpdated_R = false;  // Otherwise use it.

    // Converts from 205us ticks per edge to multiples of 0.1 inches per second.
    encoderSpeed_R = (signed short)(Ktps / encoderPeriod_R); 

    // Check direction of wheel rotation & adjust as needed. */
    if (!rightEncoderDir) encoderSpeed_R = -encoderSpeed_R;

    if (!encoderUpdated_L) encoderPeriod_L = motorUpdateTickCount - prevT3Count_L;
    else encoderUpdated_L = false;

    // Converts from 205us ticks per edge to multiples of 0.1 inches per second
    encoderSpeed_L = (signed short)(Ktps / encoderPeriod_L);

    // Check direction of wheel rotation & adjust as needed. */
    if (leftEncoderDir) encoderSpeed_L = -encoderSpeed_L;
  }

//------------------------------------------------------------------------
// Function to set left & right motor speed and direction.
//------------------------------------------------------------------------
void Apeiros::SetMotorSpeed(int leftMotorSpeed, int rightMotorSpeed)
{
 
 int tmpLeftSpeed = 0;
 int tmpRightSpeed = 0;
 
 if (rightMotorSpeed < 0)
 {
    _SN_2A = 0;
    _SN_1A = 1;
 }
 else {
    _SN_2A = 1;
    _SN_1A = 0; }
 
 if (leftMotorSpeed < 0)
 {
    _SN_3A = 0;
    _SN_4A = 1;
 }
 else {
    _SN_3A = 1;
    _SN_4A = 0; }
  
  if (leftMotorSpeed != 0) tmpLeftSpeed = abs(leftMotorSpeed) + leftMotorOffset;
  else
  {
      tmpLeftSpeed = 0;
      _SN_3A = _SN_4A = 0;
  }
  
  if (rightMotorSpeed != 0) tmpRightSpeed = abs(rightMotorSpeed) + rightMotorOffset;
  else
  {
      tmpRightSpeed = 0;
      _SN_1A = _SN_2A = 0;
  }
  
  if (tmpLeftSpeed > MaxMotorSpeed) tmpLeftSpeed = MaxMotorSpeed;
  if (tmpRightSpeed > MaxMotorSpeed) tmpRightSpeed = MaxMotorSpeed;
  
  _leftMotorPWM.pulsewidth_us(tmpLeftSpeed);
  _rightMotorPWM.pulsewidth_us(tmpRightSpeed);
    
}

//------------------------------------------------------------------------
// Function to parse motor commands from UART data.                     
//------------------------------------------------------------------------
void Apeiros::ParseMotorCommand()
{
   unsigned int commaPos, endPos, index1, index2;
   signed short leftWheel, rightWheel;
   char charBuff[5];
   bool foundNegative = false;
   
   commaPos = 0;
   endPos = 0;
   
   // Find comma position and return char.
    for (index1=2;index1<16;index1++)
    {
      if (tmpRxBuff[index1] == ',') commaPos = index1;
      else if (tmpRxBuff[index1] == 13)
      {
        endPos = index1;
        break;
      }
    }
    
    // Parse out left motor data.
    for (index1=0;index1<5;index1++) charBuff[index1] = ' ';
    index2 = 0;
    for (index1=2;index1<commaPos;index1++)
    {
      if (tmpRxBuff[index1] != '-') charBuff[index2] = tmpRxBuff[index1];
      else foundNegative = true;
      index2++; 
    }
    leftWheel = atol(charBuff);
    if (foundNegative) leftWheel = -leftWheel;
    
    foundNegative = false;
    
    // Parse out right motor data.
    for (index1=0;index1<5;index1++) charBuff[index1] = ' ';
    index2 = 0;
    for (index1=commaPos+1;index1<endPos;index1++)
    {
      if (tmpRxBuff[index1] != '-') charBuff[index2] = tmpRxBuff[index1];
      else foundNegative = true;
      index2++;
    }
    rightWheel = atol(charBuff);
    if (foundNegative) rightWheel = -rightWheel;
    
    SetMotorSpeed(leftWheel, rightWheel);
}

//------------------------------------------------------------------------
// Function to parse piezo buzzer tone commands from UART data.                     
//------------------------------------------------------------------------
void Apeiros::ParseBuzzerCommand()
{
    unsigned int index1, index2, endPos;
    unsigned int buzzerTonePWM;
    char charBuff[3];
    
    index1 = index2 = endPos = 0;
    buzzerTonePWM = 1;
    
    for (index1=0;index1<3;index1++) charBuff[index1] = ' ';
    
    // Find position of return char.
    for (index1=1;index1<16;index1++)
    {
      if (tmpRxBuff[index1] == 13)
      {
        endPos = index1;
        break;
      }
    }
    
    index2 = 0;
    for (index1=1;index1<endPos;index1++)
    {
      charBuff[index2] = tmpRxBuff[index1];
      index2++; 
    }
    
    buzzerTonePWM = atol(charBuff);
    SetBuzzerTone(buzzerTonePWM);
}

//------------------------------------------------------------------------
// Function to parse gripper servo commands from UART data.                     
//------------------------------------------------------------------------
void Apeiros::ParseGripperCommand()
{
    unsigned int index1, index2, endPos;
    unsigned int gripperPosition;
    char charBuff[4];
    
    index1 = index2 = endPos = 0;
    gripperPosition = 2000;
    
    for (index1=0;index1<4;index1++) charBuff[index1] = ' ';
    
    // Find position of return char.
    for (index1=1;index1<16;index1++)
    {
      if (tmpRxBuff[index1] == 13)
      {
        endPos = index1;
        break;
      }
    }
    
    index2 = 0;
    for (index1=1;index1<endPos;index1++)
    {
      charBuff[index2] = tmpRxBuff[index1];
      index2++; 
    }
    
    gripperPosition = atol(charBuff);
    SetGripperPosition(gripperPosition);
}

//------------------------------------------------------------------------
// Function to set gripper servo position using supplied pulsewidth[us].                     
//------------------------------------------------------------------------
void Apeiros::SetGripperPosition(int pulseWidth_us)
{
    if (pulseWidth_us > MAX_GRIPPER_PULSE) pulseWidth_us = MAX_GRIPPER_PULSE;
    if (pulseWidth_us < MIN_GRIPPER_PULSE) pulseWidth_us = MIN_GRIPPER_PULSE;
    
    _gripperPWM.pulsewidth_us(pulseWidth_us);
}

//------------------------------------------------------------------------
// Function to return left wheel encoder count [integer].                     
//------------------------------------------------------------------------
int Apeiros::GetLeftEncoder()
{
    return leftEncoderCount;
}

//------------------------------------------------------------------------
// Function to return left wheel encoder count [integer].                     
//------------------------------------------------------------------------
int Apeiros::GetRightEncoder()
{
    return rightEncoderCount;
}

//------------------------------------------------------------------------
// Function to reset both the left & right wheel encoder counts.                     
//------------------------------------------------------------------------
void Apeiros::ResetWheelEncoders()
{
    leftEncoderCount = 0;
    rightEncoderCount = 0;      
}