functions.inl

乐高机器人的源码,开发平台是IAR_for_AVR.

//
// Programmer
//
// Date init       26.04.2005
//
// Reviser         $Author:: Dktochpe                                        $
//
// Revision date   $Date::                                                   $
//
// Filename        $Workfile::                                               $
//
// Version         $Revision::                                               $
//
// Archive         $Archive::                                                $
//
// Platform        C
//

//******* cUiBtTest **********************************************************

const     UBYTE NONVOLATILE_NAME[]      = UI_NONVOLATILE;     // Non volatile filename without extention
const     UBYTE DEFAULT_PROGRAM_NAME[]  = UI_PROGRAM_DEFAULT; // On brick programming filename without extention
const     UBYTE TEMP_PROGRAM_FILENAME[] = UI_PROGRAM_TEMP;    // On brick programming tmp filename
const     UBYTE VM_PROGRAM_READER[]     = UI_PROGRAM_READER;  // On brick programming script reader filename without extention
const     UBYTE DEFAULT_PIN_CODE[]      = UI_PINCODE_DEFAULT; // Default blue tooth pin code
const     UBYTE TXT_INVALID_SENSOR[]    = "??????????????";   // Display invalid sensor data


#define   SENSORS   (MENU_SENSOR_INVALID - MENU_SENSOR_EMPTY)

const     UBYTE SENSORTYPE[SENSORS] =  // for view and datalog
{
  0,                //  MENU_SENSOR_EMPTY
  SOUND_DB,         //  MENU_SENSOR_SOUND_DB
  SOUND_DBA,        //  MENU_SENSOR_SOUND_DBA
  LIGHT_ACTIVE,     //  MENU_SENSOR_LIGHT
  LIGHT_INACTIVE,   //  MENU_SENSOR_LIGHT_AMB
  REFLECTION,       //  MENU_SENSOR_LIGHT_OLD
  SWITCH,           //  MENU_SENSOR_TOUCH
  0,                //  MENU_SENSOR_MOTOR_DEG
  0,                //  MENU_SENSOR_MOTOR_ROT
  ANGLE,            //  MENU_SENSOR_ROTATION
  LOWSPEED_9V,      //  MENU_SENSOR_ULTRASONIC_IN
  LOWSPEED_9V,      //  MENU_SENSOR_ULTRASONIC_CM
  TEMPERATURE,      //  MENU_SENSOR_TEMP_C
  TEMPERATURE       //  MENU_SENSOR_TEMP_F
};

const     UBYTE SENSORMODE[SENSORS] =  // for view and datalog
{
  0,                //  MENU_SENSOR_EMPTY
  PCTFULLSCALEMODE, //  MENU_SENSOR_SOUND_DB
  PCTFULLSCALEMODE, //  MENU_SENSOR_SOUND_DBA
  PCTFULLSCALEMODE, //  MENU_SENSOR_LIGHT
  PCTFULLSCALEMODE, //  MENU_SENSOR_LIGHT_AMB
  PCTFULLSCALEMODE, //  MENU_SENSOR_LIGHT_OLD
  BOOLEANMODE,      //  MENU_SENSOR_TOUCH
  0,                //  MENU_SENSOR_MOTOR_DEG
  0,                //  MENU_SENSOR_MOTOR_ROT
  ANGLESTEPSMODE,   //  MENU_SENSOR_ROTATION
  0,                //  MENU_SENSOR_ULTRASONIC_IN
  0,                //  MENU_SENSOR_ULTRASONIC_CM
  CELSIUSMODE,      //  MENU_SENSOR_TEMP_C
  FAHRENHEITMODE    //  MENU_SENSOR_TEMP_F
};

const     UBYTE SENSORFORMAT[SENSORS][10] =
{
  "",               //  MENU_SENSOR_EMPTY
  "%3.0f %%",       //  MENU_SENSOR_SOUND_DB
  "%3.0f %%",       //  MENU_SENSOR_SOUND_DBA
  "%3.0f %%",       //  MENU_SENSOR_LIGHT
  "%3.0f %%",       //  MENU_SENSOR_LIGHT_AMB
  "%3.0f %%",       //  MENU_SENSOR_LIGHT_OLD
  "%1.0f",          //  MENU_SENSOR_TOUCH
  "%8.0f `",        //  MENU_SENSOR_MOTOR_DEG
  "%8.0f R",        //  MENU_SENSOR_MOTOR_ROT
  "%6.0f T",        //  MENU_SENSOR_ROTATION
  "%3.0f In",       //  MENU_SENSOR_ULTRASONIC_IN
  "%3.0f cm",       //  MENU_SENSOR_ULTRASONIC_CM
  "%5.1f `C",       //  MENU_SENSOR_TEMP_C
  "%5.1f `F"        //  MENU_SENSOR_TEMP_F
};

const     float SENSORDIVIDER[SENSORS] =
{
  1.0,              //  MENU_SENSOR_EMPTY
  1.0,              //  MENU_SENSOR_SOUND_DB
  1.0,              //  MENU_SENSOR_SOUND_DBA
  1.0,              //  MENU_SENSOR_LIGHT
  1.0,              //  MENU_SENSOR_LIGHT_AMB
  1.0,              //  MENU_SENSOR_LIGHT_OLD
  1.0,              //  MENU_SENSOR_TOUCH
  1.0,              //  MENU_SENSOR_MOTOR_DEG
  360.0,            //  MENU_SENSOR_MOTOR_ROT
  1.0,              //  MENU_SENSOR_ROTATION
  2.54,             //  MENU_SENSOR_ULTRASONIC_IN
  1.0,              //  MENU_SENSOR_ULTRASONIC_CM
  10.0,             //  MENU_SENSOR_TEMP_C
  10.0              //  MENU_SENSOR_TEMP_F
};


//******* cUiSetupUltrasonic *************************************************

void      cUiSetupUltrasonic(UBYTE Port)
{
  Port -= MENU_PORT_1;
  pMapLowSpeed->OutBuf[Port].InPtr    = 0;
  pMapLowSpeed->OutBuf[Port].OutPtr   = 0;

  pMapLowSpeed->OutBuf[Port].Buf[pMapLowSpeed->OutBuf[Port].InPtr] = ULTRA_SONIC;      // Device Adress
  pMapLowSpeed->OutBuf[Port].InPtr++;
  pMapLowSpeed->OutBuf[Port].Buf[pMapLowSpeed->OutBuf[Port].InPtr] = 0x41;             // Function => Set command
  pMapLowSpeed->OutBuf[Port].InPtr++;
  pMapLowSpeed->OutBuf[Port].Buf[pMapLowSpeed->OutBuf[Port].InPtr] = 0x02;             // Function => Set to continiues measurement
  pMapLowSpeed->OutBuf[Port].InPtr++;

  pMapLowSpeed->InBuf[Port].BytesToRx = 0;
  pMapLowSpeed->ChannelState[Port]    = LOWSPEED_INIT;
  pMapLowSpeed->State                |= (COM_CHANNEL_ONE_ACTIVE << Port);
}



//******* cUiAskUltrasonic ***************************************************

void      cUiAskUltrasonic(UBYTE Port)
{
  Port -= MENU_PORT_1;
  pMapLowSpeed->OutBuf[Port].InPtr    = 0;
  pMapLowSpeed->OutBuf[Port].OutPtr   = 0;

  pMapLowSpeed->OutBuf[Port].Buf[pMapLowSpeed->OutBuf[Port].InPtr] = ULTRA_SONIC;       // Device Adress
  pMapLowSpeed->OutBuf[Port].InPtr++;
  pMapLowSpeed->OutBuf[Port].Buf[pMapLowSpeed->OutBuf[Port].InPtr] = 0x42;              // Function => Read result 1
  pMapLowSpeed->OutBuf[Port].InPtr++;

  pMapLowSpeed->InBuf[Port].BytesToRx = 1;
  pMapLowSpeed->ChannelState[Port]    = LOWSPEED_INIT;
  pMapLowSpeed->State                |= (COM_CHANNEL_ONE_ACTIVE << Port);
}



//******* cUiReadUltrasonic **************************************************

UBYTE     cUiReadUltrasonic(UBYTE Port)
{
  UBYTE   Result;

  Port -= MENU_PORT_1;
  if ((pMapLowSpeed->ChannelState[Port] == LOWSPEED_IDLE) || (pMapLowSpeed->ChannelState[Port] == LOWSPEED_DONE))
  {
    Result = (UBYTE)pMapLowSpeed->InBuf[Port].Buf[pMapLowSpeed->InBuf[Port].OutPtr];
    pMapLowSpeed->InBuf[Port].OutPtr++;
    if (pMapLowSpeed->InBuf[Port].OutPtr >= SIZE_OF_LSBUF)
    {
      pMapLowSpeed->InBuf[Port].OutPtr = 0;
    }
    if (Result == 0)
    {
      Result = 0xFF;
    }
  }
  else
  {
    Result = 0xFF;
  }

  return (Result);
}



//******* cUiResetSensor *****************************************************

void      cUiResetSensor(UBYTE Port,UBYTE Sensor) // Set sensor parameters
{
  switch (Sensor)
  {
    case MENU_SENSOR_MOTOR_DEG :
    {
      pMapOutPut->Outputs[Port - MENU_PORT_A].Flags     |= UPDATE_RESET_COUNT;
    }
    break;

    case MENU_SENSOR_MOTOR_ROT :
    {
      pMapOutPut->Outputs[Port - MENU_PORT_A].Flags     |= UPDATE_RESET_COUNT;
    }
    break;

    case MENU_SENSOR_ROTATION :
    {
      pMapInput->Inputs[Port - MENU_PORT_1].SensorValue  = 0;
    }
    break;

  }
}



//******* cUiSetupSensor *****************************************************

void      cUiSetupSensor(UBYTE Port,UBYTE Sensor) // Set sensor parameters
{
  switch (Sensor)
  {
    case MENU_SENSOR_MOTOR_DEG :
    {
      pMapOutPut->Outputs[Port - MENU_PORT_A].Mode      &= ~(BRAKE | MOTORON);
      pMapOutPut->Outputs[Port - MENU_PORT_A].Flags     |= UPDATE_MODE | UPDATE_SPEED;
      pMapOutPut->Outputs[Port - MENU_PORT_A].TachoCnt   = 0;
    }
    break;

    case MENU_SENSOR_MOTOR_ROT :
    {
      pMapOutPut->Outputs[Port - MENU_PORT_A].Mode      &= ~(BRAKE | MOTORON);
      pMapOutPut->Outputs[Port - MENU_PORT_A].Flags     |= UPDATE_MODE | UPDATE_SPEED;
      pMapOutPut->Outputs[Port - MENU_PORT_A].TachoCnt   = 0;
    }
    break;

    case MENU_SENSOR_ULTRASONIC_IN :
    {
      pMapInput->Inputs[Port - MENU_PORT_1].SensorType   = SENSORTYPE[Sensor - MENU_SENSOR_EMPTY];
      cUiSetupUltrasonic(Port);
    }
    break;

    case MENU_SENSOR_ULTRASONIC_CM :
    {
      pMapInput->Inputs[Port - MENU_PORT_1].SensorType   = SENSORTYPE[Sensor - MENU_SENSOR_EMPTY];
      cUiSetupUltrasonic(Port);
    }
    break;

    default :
    {
      pMapInput->Inputs[Port - MENU_PORT_1].SensorType   = SENSORTYPE[Sensor - MENU_SENSOR_EMPTY];
      pMapInput->Inputs[Port - MENU_PORT_1].SensorMode   = SENSORMODE[Sensor - MENU_SENSOR_EMPTY];
    }
    break;

  }
}



//******* cUiAskSensor *******************************************************

void      cUiAskSensor(UBYTE Port,UBYTE Sensor) // Ask for sensor data
{
  switch (Sensor)
  {
    case MENU_SENSOR_ULTRASONIC_IN :
    {
      cUiAskUltrasonic(Port);
    }
    break;

    case MENU_SENSOR_ULTRASONIC_CM :
    {
      cUiAskUltrasonic(Port);
    }
    break;

    default :
    {
    }
    break;

  }
}



//******* cUiGetSensorValue **************************************************

UBYTE     cUiGetSensorValue(UBYTE Port,UBYTE Sensor,SLONG *Value)
{
  UBYTE   Result = FALSE;

  *Value = 0L;
  switch (Sensor)
  {
    case MENU_SENSOR_MOTOR_DEG :
    {
      *Value = pMapOutPut->Outputs[Port - MENU_PORT_A].TachoCnt;
      Result = TRUE;
    }
    break;

    case MENU_SENSOR_MOTOR_ROT :
    {
      *Value = pMapOutPut->Outputs[Port - MENU_PORT_A].TachoCnt;
      Result = TRUE;
    }
    break;

    case MENU_SENSOR_ULTRASONIC_IN :
    {
      Result = cUiReadUltrasonic(Port);
      if (Result != 0xFF)
      {
        *Value = Result;
        Result = TRUE;
      }
      else
      {
        Result = FALSE;
      }
    }
    break;

    case MENU_SENSOR_ULTRASONIC_CM :
    {
      Result = cUiReadUltrasonic(Port);
      if (Result != 0xFF)
      {
        *Value = Result;
        Result = TRUE;
      }
      else
      {
        Result = FALSE;
      }
    }
    break;

    default :
    {
      if (pMapInput->Inputs[Port - MENU_PORT_1].InvalidData != INVALID_DATA)
      {
        *Value = pMapInput->Inputs[Port - MENU_PORT_1].SensorValue;
        Result = TRUE;
      }
    }
    break;

  }

  return (Result);
}



//******* cUiPrintSensorInDisplayBuffer **************************************

void      cUiPrintSensorInDisplayBuffer(UBYTE Port,UBYTE Sensor,UBYTE Valid,SLONG Value)
{
  SWORD   Tmp;

  Tmp = sprintf((char*)VarsUi.DisplayBuffer,(char*)SENSORFORMAT[Sensor - MENU_SENSOR_EMPTY],(float)0);
  if (Valid == TRUE)
  {
    if (Tmp < sprintf((char*)VarsUi.DisplayBuffer,(char*)SENSORFORMAT[Sensor - MENU_SENSOR_EMPTY],(float)Value / SENSORDIVIDER[Sensor - MENU_SENSOR_EMPTY]))
    {
      sprintf((char*)VarsUi.DisplayBuffer,"%*.*s",Tmp,Tmp,(char*)TXT_INVALID_SENSOR);
    }
  }
  else
  {
    sprintf((char*)VarsUi.DisplayBuffer,"%*.*s",Tmp,Tmp,(char*)TXT_INVALID_SENSOR);
  }
}



//******* cUiReleaseSensors **************************************************

void      cUiReleaseSensors(void)
{
  UBYTE   Tmp;

  for (Tmp = 0;Tmp < NO_OF_INPUTS;Tmp++)
  {
    pMapInput->Inputs[Tmp].SensorType   = NO_SENSOR;
  }
}



//******* cUiBtCommand *******************************************************

enum
{
  UI_BT_CTRL,

  UI_BT_GET_DEVICES,          // (UI_BT_GET_DEVICES,Known,*pDevices,NULL)             [Known = 0,1]
  UI_BT_GET_DEVICE_NAME,      // (UI_BT_GET_DEVICE_NAME,Known,*pIndex,*pDeviceName)   [Known = 0,1]
  UI_BT_GET_DEVICE_TYPE,      // (UI_BT_GET_DEVICE_TYPE,Known,*pIndex,*pDeviceType)   [Known = 0,1]

  UI_BT_GET_CONNECTION_NAME,  // (UI_BT_GET_CONNECTION_NAME,NULL,*pConnection,*pConnectionName)
  UI_BT_GET_CONNECTION_TYPE,  // (UI_BT_GET_CONNECTION_TYPE,NULL,*pConnection,*pConnectionType)
  UI_BT_GET_CONNECTION_VALID, // (UI_BT_GET_CONNECTION_NAME,NULL,*pConnection,NULL)

  UI_BT_DUMMY
};


#define   UI_BT_FAILED          0x8200  // General command failed
#define   UI_BT_SUCCES          0x0000  // Command executed succesfully


UBYTE     cUiBTGetDeviceType(UBYTE *pCOD)
{
  ULONG   COD;
  UBYTE   Result;
  UBYTE   Tmp;

  COD      = 0;
  for (Tmp = 0;Tmp < SIZE_OF_CLASS_OF_DEVICE;Tmp++)
  {
    COD  <<= 8;
    COD   |= (ULONG)*pCOD;
    pCOD++;
  }

  Result   = DEVICETYPE_UNKNOWN;
  if ((COD & 0x00001FFF) == 0x00000804)
  {
    Result = DEVICETYPE_NXT;
  }
  if ((COD & 0x00001F00) == 0x00000200)
  {
    Result = DEVICETYPE_PHONE;
  }
  if ((COD & 0x00001F00) == 0x00000100)
  {
    Result = DEVICETYPE_PC;
  }

  return (Result);
}


UBYTE     cUiBTGetDeviceIndex(UBYTE Known,UBYTE No,UBYTE *pIndex)
{
  UBYTE   Result = 0;
  UBYTE   Tmp;

  *pIndex  = 0;
  if (Known)