00001 00005 #pragma once 00006 00010 #define JOYSTICK_BUTTON_COUNT 16 00011 00015 #define NB_ADVANCE_RETRACT_POSITION 20 00016 00020 #define ERROR_DATA_COUNT_MAX 50 00021 00025 #define ERROR_LAYER_COUNT 7 00026 00030 #define LEGACY_CONFIG_NB_ZONES_MAX 10 00031 00035 #define LEGACY_CONFIG_NB_POINTS_COUNT 8 00036 00040 #define CONTROL_MAPPING_COUNT 6 00041 00045 #define MODE_MAP_COUNT 6 00046 00050 #define STICK_EVENT_COUNT 6 00051 00055 #define BUTTON_EVENT_COUNT 26 00056 00060 #define STRING_LENGTH 20 00061 00065 #define JACO_FINGERS_COUNT 3 00066 00071 #define ERROR_UNKNOWFILE 5001 00072 00077 #define ERROR_MEMORY 5002 00078 00083 #define ERROR_FILEREADING 5003 00084 00088 const unsigned short PAGE_SIZE = 2048; 00089 00093 const int ADDRESS_PAGE_SIZE = 4; 00094 00098 const unsigned short PACKET_PER_PAGE_QTY = 40; 00099 00103 const int PAGEPACKET_SIZE = 52; 00104 00108 const int USB_HEADER_SIZE = 8; 00109 00113 const int USB_DATA_SIZE = 56; 00114 00121 enum POSITION_TYPE 00122 { 00123 NOMOVEMENT_POSITION = 0, 00124 CARTESIAN_POSITION = 1, 00125 ANGULAR_POSITION = 2, 00126 CARTESIAN_VELOCITY = 7, 00127 ANGULAR_VELOCITY = 8, 00128 ANY_TRAJECTORY = 11, 00129 TIME_DELAY = 12, 00130 }; 00131 00132 00133 enum PERIPHERAL_TYPE 00134 { 00135 PERIPHERAL_TYPE_NONE = 0, 00136 PERIPHERAL_TYPE_ANY = 1, 00137 PERIPHERAL_TYPE_ACTUATORK01 = 100, 00138 PERIPHERAL_TYPE_FINGERK01 = 200, 00139 PERIPHERAL_TYPE_JOYSTICK = 300, 00140 PERIPHERAL_TYPE_VIRTUAL_JOYSTICK = 301, 00141 PERIPHERAL_TYPE_CAN_INTERFACE = 400, 00142 }; 00143 00147 enum HAND_MODE 00148 { 00149 HAND_NOMOVEMENT, 00150 POSITION_MODE, 00151 VELOCITY_MODE, 00152 }; 00153 00157 enum ArmLaterality 00158 { 00159 RIGHTHAND, 00160 LEFTHAND, 00161 }; 00162 00167 enum Controller 00168 { 00169 THREE_AXIS_JOYSTICK = 0, 00170 TWO_AXIS_JOYSTICK = 1, 00171 API = 2, 00172 EASY_RIDER = 3, 00173 UNIVERSAL_INTERFACE = 4, 00174 EXTERNAL_CUSTOMINTERFACE = 5, 00175 NONE = 6, 00176 OLED_DISPLAY = 7 00177 }; 00178 00183 enum CONTROL_TYPE 00184 { 00185 CONTROL_TYPE_CARTESIAN = 0, 00186 CONTROL_TYPE_ANGULAR = 1 00187 }; 00188 00192 enum CONTROL_MODULE 00193 { 00197 CONTROL_MODULE_NONE, 00198 00202 CONTROL_MODULE_ANGULAR_VELOCITY, 00203 00207 CONTROL_MODULE_ANGULAR_POSITION, 00208 00213 CONTROL_MODULE_CARTESIAN_VELOCITY, 00214 00219 CONTROL_MODULE_CARTESIAN_POSITION, 00220 00225 CONTROL_MODULE_RETRACT, 00226 00230 CONTROL_MODULE_TRAJECTORY, 00231 00235 CONTROL_MODULE_PREDEFINED, 00236 00240 CONTROL_MODULE_TIMEDELAY, 00241 }; 00242 00246 enum RETRACT_TYPE 00247 { 00251 RETRACT_TYPE_NORMAL_TO_READY = 0, 00252 00256 RETRACT_TYPE_READY_STANDBY = 1, 00257 00261 RETRACT_TYPE_READY_TO_RETRACT = 2, 00262 00266 RETRACT_TYPE_RETRACT_STANDBY = 3, 00267 00271 RETRACT_TYPE_RETRACT_TO_READY = 4, 00272 00276 RETRACT_TYPE_NORMAL_STANDBY = 5, 00277 00281 RETRACT_TYPE_NOT_INITIALIZED = 6 00282 }; 00283 00287 struct AngularInfo 00288 { 00294 float Actuator1; 00295 00301 float Actuator2; 00302 00308 float Actuator3; 00309 00315 float Actuator4; 00316 00322 float Actuator5; 00323 00329 float Actuator6; 00330 00331 00335 void InitStruct() 00336 { 00337 Actuator1 = 0.0f; 00338 Actuator2 = 0.0f; 00339 Actuator3 = 0.0f; 00340 Actuator4 = 0.0f; 00341 Actuator5 = 0.0f; 00342 Actuator6 = 0.0f; 00343 } 00344 }; 00345 00349 struct CartesianInfo 00350 { 00356 float X; 00357 00363 float Y; 00364 00370 float Z; 00371 00378 float ThetaX; 00379 00385 float ThetaY; 00386 00392 float ThetaZ; 00393 00394 00398 void InitStruct() 00399 { 00400 X = 0.0f; 00401 Y = 0.0f; 00402 Z = 0.0f; 00403 ThetaX = 0.0f; 00404 ThetaY = 0.0f; 00405 ThetaZ = 0.0f; 00406 } 00407 }; 00408 00414 struct SensorsInfo 00415 { 00419 float Voltage; 00420 00424 float Current; 00425 00429 float AccelerationX; 00430 00434 float AccelerationY; 00435 00439 float AccelerationZ; 00440 00444 float ActuatorTemp1; 00445 00449 float ActuatorTemp2; 00450 00454 float ActuatorTemp3; 00455 00459 float ActuatorTemp4; 00460 00464 float ActuatorTemp5; 00465 00469 float ActuatorTemp6; 00470 00474 float FingerTemp1; 00475 00479 float FingerTemp2; 00480 00484 float FingerTemp3; 00485 00486 00490 void InitStruct() 00491 { 00492 Voltage = 0.0f; 00493 Current = 0.0f; 00494 AccelerationX = 0.0f; 00495 AccelerationY = 0.0f; 00496 AccelerationZ = 0.0f; 00497 ActuatorTemp1 = 0.0f; 00498 ActuatorTemp2 = 0.0f; 00499 ActuatorTemp3 = 0.0f; 00500 ActuatorTemp4 = 0.0f; 00501 ActuatorTemp5 = 0.0f; 00502 ActuatorTemp6 = 0.0f; 00503 FingerTemp1 = 0.0f; 00504 FingerTemp2 = 0.0f; 00505 FingerTemp3 = 0.0f; 00506 } 00507 00508 }; 00509 00513 struct FingersPosition 00514 { 00518 float Finger1; 00519 00523 float Finger2; 00524 00528 float Finger3; 00529 00533 void InitStruct() 00534 { 00535 Finger1 = 0.0f; 00536 Finger2 = 0.0f; 00537 Finger3 = 0.0f; 00538 } 00539 }; 00540 00548 struct CartesianPosition 00549 { 00553 CartesianInfo Coordinates; 00554 00558 FingersPosition Fingers; 00559 00560 00564 void InitStruct() 00565 { 00566 Coordinates.InitStruct(); 00567 Fingers.InitStruct(); 00568 } 00569 }; 00570 00574 struct AngularPosition 00575 { 00579 AngularInfo Actuators; 00580 00584 FingersPosition Fingers; 00585 }; 00586 00593 struct Limitation 00594 { 00598 float speedParameter1; 00599 00603 float speedParameter2; 00604 float speedParameter3; 00605 float forceParameter1; 00606 float forceParameter2; 00607 float forceParameter3; 00608 float accelerationParameter1; 00609 float accelerationParameter2; 00610 float accelerationParameter3; 00612 void InitStruct() 00613 { 00614 speedParameter1 = 0.0f; 00615 speedParameter2 = 0.0f; 00616 speedParameter3 = 0.0f; 00617 forceParameter1 = 0.0f; 00618 forceParameter2 = 0.0f; 00619 forceParameter3 = 0.0f; 00620 accelerationParameter1 = 0.0f; 00621 accelerationParameter2 = 0.0f; 00622 accelerationParameter3 = 0.0f; 00623 } 00624 }; 00625 00630 struct UserPosition 00631 { 00635 POSITION_TYPE Type; 00636 00640 float Delay; 00641 00645 CartesianInfo CartesianPosition; 00646 00650 AngularInfo Actuators; 00651 00655 HAND_MODE HandMode; 00656 00660 FingersPosition Fingers; 00661 00662 void InitStruct() 00663 { 00664 Type = CARTESIAN_POSITION; 00665 Delay = 0.0f; 00666 CartesianPosition.InitStruct(); 00667 Actuators.InitStruct(); 00668 HandMode = POSITION_MODE; 00669 Fingers.InitStruct(); 00670 } 00671 }; 00672 00676 struct TrajectoryPoint 00677 { 00681 UserPosition Position; 00682 00686 int LimitationsActive; 00687 00691 Limitation Limitations; 00692 00693 void InitStruct() 00694 { 00695 Position.InitStruct(); 00696 LimitationsActive = 0; 00697 Limitations.InitStruct(); 00698 } 00699 }; 00700 00704 struct TrajectoryFIFO 00705 { 00709 unsigned int TrajectoryCount; 00710 00714 float UsedPercentage; 00715 00719 unsigned int MaxSize; 00720 }; 00721 00726 struct SingularityVector 00727 { 00728 int TranslationSingularityCount; 00729 int OrientationSingularityCount; 00730 float TranslationSingularityDistance; 00731 float OrientationSingularityDistance; 00732 CartesianInfo RepulsionVector; 00733 }; 00734 00738 struct JoystickCommand 00739 { 00743 short ButtonValue[JOYSTICK_BUTTON_COUNT]; 00744 00749 float InclineLeftRight; 00750 00755 float InclineForwardBackward; 00756 00761 float Rotate; 00762 00767 float MoveLeftRight; 00768 00773 float MoveForwardBackward; 00774 00779 float PushPull; 00780 00781 void InitStruct() 00782 { 00783 for(int i = 0; i < JOYSTICK_BUTTON_COUNT; i++) 00784 { 00785 ButtonValue[i] = 0; 00786 } 00787 00788 InclineLeftRight = 0.0f; 00789 InclineForwardBackward = 0.0f; 00790 Rotate = 0.0f; 00791 MoveLeftRight = 0.0f; 00792 MoveForwardBackward = 0.0f; 00793 PushPull = 0.0f; 00794 } 00795 }; 00796 00805 struct ClientConfigurations 00806 { 00810 char ClientID[STRING_LENGTH]; 00811 00815 char ClientName[STRING_LENGTH]; 00816 00820 char Organization[STRING_LENGTH]; 00821 00825 char Serial[STRING_LENGTH]; 00826 00830 char Model[STRING_LENGTH]; 00831 00836 ArmLaterality Laterality; 00837 00841 float MaxTranslationVelocity; 00842 00846 float MaxOrientationVelocity; 00847 00851 float MaxTranslationAcceleration; 00852 00856 float MaxOrientationAcceleration; 00857 00861 float MaxForce; 00862 00868 float Sensibility; 00869 00874 float DrinkingHeight; 00875 00881 int ComplexRetractActive; 00882 00886 float RetractedPositionAngle; 00887 00891 int RetractedPositionCount; 00892 UserPosition RetractPositions[NB_ADVANCE_RETRACT_POSITION]; 00893 00898 float DrinkingDistance; 00899 00903 int Fingers2and3Inverted; 00904 00909 float DrinkingLenght; 00910 00914 int DeletePreProgrammedPositionsAtRetract; 00915 00919 int EnableFlashErrorLog; 00920 00924 int EnableFlashPositionLog; 00925 00929 int Expansion[198]; 00930 }; 00931 00937 enum ControlFunctionalityTypeEnum 00938 { 00942 CF_NoFunctionality = 0, 00943 00947 CF_Disable_EnableJoystick = 1, 00948 00955 CF_Retract_ReadyToUse = 2, 00956 00960 CF_Change_TwoAxis_ThreeAxis = 3, 00961 00965 CF_Change_DrinkingMode = 4, 00966 00970 CF_Cycle_ModeA_list = 5, 00971 00975 CF_Cycle_ModeB_list = 6, 00976 00980 CF_DecreaseSpeed = 7, 00981 00985 CF_IncreaseSpeed = 8, 00986 00990 CF_Goto_Position1 = 9, 00991 00995 CF_Goto_Position2 = 10, 00996 01000 CF_Goto_Position3 = 11, 01001 01005 CF_Goto_Position4 = 12, 01006 01010 CF_Goto_Position5 = 13, 01011 01015 CF_RecordPosition1 = 14, 01016 01020 CF_RecordPosition2 = 15, 01021 01025 CF_RecordPosition3 = 16, 01026 01030 CF_RecordPosition4 = 17, 01031 01035 CF_RecordPosition5 = 18, 01036 01041 CF_X_Positive = 19, 01042 01047 CF_X_Negative = 20, 01048 01053 CF_Y_Positive = 21, 01054 01059 CF_Y_Negative = 22, 01060 01065 CF_Z_Positive = 23, 01066 01071 CF_Z_Negative = 24, 01072 01077 CF_R_Positive = 25, 01078 01083 CF_R_Negative = 26, 01084 01089 CF_U_Positive = 27, 01090 01095 CF_U_Negative = 28, 01096 01101 CF_V_Positive = 29, 01102 01107 CF_V_Negative = 30, 01108 01112 CF_OpenHandOneFingers = 31, 01113 01117 CF_CloseHandOneFingers = 32, 01118 01122 CF_OpenHandTwoFingers = 33, 01123 01127 CF_CloseHandTwoFingers = 34, 01128 01132 CF_OpenHandThreeFingers = 35, 01133 01137 CF_CloseHandThreeFingers = 36, 01138 01142 CF_ForceAngularVelocity = 37, 01143 01147 CF_ForceControlStatus = 38, 01148 01149 CF_Trajectory = 39, 01150 01154 CF_AutomaticOrientationXPlus = 40, 01155 01159 CF_AutomaticOrientationXMinus = 41, 01160 01164 CF_AutomaticOrientationYPlus = 42, 01165 01169 CF_AutomaticOrientationYMinus = 43, 01170 01174 CF_AutomaticOrientationZPlus = 44, 01175 01179 CF_AutomaticOrientationZMinus = 45, 01180 01184 CF_AdvanceGOTO_1 = 46, 01185 01189 CF_AdvanceGOTO_Clear_1 = 47, 01190 01194 CF_AdvanceGOTO_Add_1 = 48, 01195 }; 01196 01200 struct StickEvents 01201 { 01206 unsigned char Minus; 01207 01212 unsigned char Plus; 01213 }; 01214 01218 struct ButtonEvents 01219 { 01223 unsigned char OneClick; 01224 01228 unsigned char TwoClick; 01229 01233 unsigned char HoldOneSec; 01234 01238 unsigned char HoldTwoSec; 01239 01243 unsigned char HoldThreeSec; 01244 01248 unsigned char HoldFourSec; 01249 01253 unsigned char HoldDown; 01254 }; 01255 01259 enum ControlMappingMode 01260 { 01264 OneAxis, 01265 01269 TwoAxis, 01270 01274 ThreeAxis, 01275 01279 SixAxis 01280 }; 01281 01286 struct ControlsModeMap 01287 { 01291 int DiagonalsLocked; 01292 01296 int Expansion; 01297 01301 StickEvents ControlSticks[STICK_EVENT_COUNT]; 01302 01306 ButtonEvents ControlButtons[BUTTON_EVENT_COUNT]; 01307 }; 01308 01316 struct ControlMapping 01317 { 01321 int NumOfModesA; 01322 01326 int NumOfModesB; 01327 01332 int ActualModeA; 01333 01338 int ActualModeB; 01339 01343 ControlMappingMode Mode; 01344 01349 ControlsModeMap ModeControlsA[MODE_MAP_COUNT]; 01350 01355 ControlsModeMap ModeControlsB[MODE_MAP_COUNT]; 01356 }; 01357 01361 struct ControlMappingCharts 01362 { 01366 int NumOfConfiguredMapping; 01367 01371 int ActualControlMapping; 01372 01376 ControlMapping Mapping[CONTROL_MAPPING_COUNT]; 01377 }; 01378 01382 enum errorLoggerType 01383 { 01387 ERROR_NOTINITIALIZED, 01388 01392 keos_err1, 01393 01397 keos_err2, 01398 01402 keos_err3, 01403 01407 User_err_start_marker, 01408 01412 errorlog_Actuator_Temperature, 01413 01417 errorlog_Actuator_TemperatureOK, 01418 01422 errorlog_Finger_Temperature, 01423 01427 errorlog_Finger_TemperatureOK, 01428 01432 errorlog_voltage, 01433 01437 errorlog_voltageOK, 01438 01442 errorlog_current_FingersClosing, 01443 01447 errorlog_current_FingersOpening, 01448 01452 errorlog_current_FingersOK, 01453 01457 errorlog_current_Actuators, 01458 01462 errorlog_current_ActuatorsOK, 01463 01467 errorLog_RobotStatus_Build_Incomplete, 01468 01472 errorLogger_END 01473 }; 01474 01478 struct SystemError 01479 { 01483 unsigned int ErrorHeader; 01484 01488 errorLoggerType ErrorType; 01489 01493 int FirmwareVersion; 01494 01498 int KeosVersion; 01499 01503 unsigned int SystemTime; 01504 01508 bool LayerErrorStatus[ERROR_LAYER_COUNT]; 01509 01513 int LifeTime; 01514 01518 int DataCount; 01519 01523 unsigned int Data[ERROR_DATA_COUNT_MAX]; 01524 01525 }; 01526 01530 struct ZoneLimitation 01531 { 01536 float speedParameter1; 01537 01542 float speedParameter2; 01543 01547 float speedParameter3; 01548 01552 float forceParameter1; 01553 01557 float forceParameter2; 01558 01562 float forceParameter3; 01563 01567 float accelerationParameter1; 01568 01572 float accelerationParameter2; 01573 01577 float accelerationParameter3; 01578 }; 01579 01583 enum ShapeType 01584 { 01588 PrismSquareBase_X = 0, 01589 01593 PrismSquareBase_Y = 1, 01594 01598 PrismSquareBase_Z = 2, 01599 01603 PrismTriangularBase_X = 3, 01604 01608 PrismTriangularBase_Y = 4, 01609 01613 PrismTriangularBase_Z = 5, 01614 01618 Pyramid = 6 01619 }; 01620 01624 struct ForcesInfo 01625 { 01629 float Actuator1; 01630 01634 float Actuator2; 01635 01639 float Actuator3; 01640 01644 float Actuator4; 01645 01649 float Actuator5; 01650 01654 float Actuator6; 01655 01659 float X; 01660 01664 float Y; 01665 01669 float Z; 01670 01674 float ThetaX; 01675 01679 float ThetaY; 01680 01684 float ThetaZ; 01685 }; 01686 01690 struct QuickStatus 01691 { 01695 unsigned char Finger1Status; 01696 01700 unsigned char Finger2Status; 01701 01705 unsigned char Finger3Status; 01706 01710 unsigned char RetractType; 01711 01715 unsigned char RetractComplexity; 01716 01720 unsigned char ControlEnableStatus; 01721 01725 unsigned char ControlActiveModule; 01726 01730 unsigned char ControlFrameType; 01731 01735 unsigned char CartesianFaultState; 01736 01740 unsigned char ForceControlStatus; 01741 01745 unsigned char CurrentLimitationStatus; 01746 01750 unsigned char RobotType; 01751 01755 unsigned char RobotEdition; 01756 01760 unsigned char TorqueSensorsStatus; 01761 }; 01762 01766 struct Finger 01767 { 01771 char ID[STRING_LENGTH]; 01772 01776 float ActualCommand; 01777 01781 float ActualSpeed; 01782 01786 float ActualForce; 01787 01791 float ActualAcceleration; 01792 01796 float ActualCurrent; 01797 01801 float ActualPosition; 01802 01806 float ActualAverageCurrent; 01807 01811 float ActualTemperature; 01812 01816 int CommunicationErrors; 01817 01821 int OscillatorTuningValue; 01822 01826 float CycleCount; 01827 01831 float RunTime; 01832 01836 float PeakMaxTemp; 01837 01841 float PeakMinTemp; 01842 01846 float PeakCurrent; 01847 01851 float MaxSpeed; 01852 01856 float MaxForce; 01857 01861 float MaxAcceleration; 01862 01866 float MaxCurrent; 01867 01871 float MaxAngle; 01872 01876 float MinAngle; 01877 01881 unsigned int DeviceID; 01882 01886 unsigned int CodeVersion; 01887 01891 unsigned short IsFingerInit; 01892 01896 unsigned short Index; 01897 01901 unsigned short FingerAddress; 01902 01906 unsigned short IsFingerConnected; 01907 }; 01908 01912 struct Gripper 01913 { 01917 char Model[STRING_LENGTH]; 01918 01922 Finger Fingers[JACO_FINGERS_COUNT]; 01923 }; 01924 01928 struct ZoneShape 01929 { 01933 ShapeType shapeType; 01934 01938 int Expansion1; 01939 01943 CartesianInfo Points[LEGACY_CONFIG_NB_POINTS_COUNT]; 01944 }; 01945 01949 struct Zone 01950 { 01954 int ID; 01955 01959 int Expansion1; 01960 01964 ZoneShape zoneShape; 01965 01970 ZoneLimitation zoneLimitation; 01971 01975 int Expansion2; 01976 }; 01977 01981 struct ZoneList 01982 { 01986 int NbZones; 01987 01991 int Expansion1; 01992 01996 Zone Zones[LEGACY_CONFIG_NB_ZONES_MAX]; 01997 01998 }; 01999 02003 struct SystemStatus 02004 { 02008 unsigned int JoystickActive; 02009 02013 unsigned int RetractStatus; 02014 02018 unsigned int DrinkingMode; 02019 02023 unsigned int ArmLaterality; 02024 02028 unsigned int TranslationActive; 02029 02033 unsigned int RotationActive; 02034 02038 unsigned int FingersActive; 02039 02043 unsigned int WarningOverchargeForce; 02044 02048 unsigned int WarningOverchargeFingers; 02049 02053 unsigned int WarningLowVoltage; 02054 02058 unsigned int MajorErrorOccured; 02059 }; 02060 02064 struct GeneralInformations 02065 { 02069 double TimeAbsolute; 02070 02074 double TimeFromStartup; 02075 02079 unsigned int IndexStartup; 02080 02084 int ExpansionLong1; 02085 02089 float TimeStampSavings; 02090 02094 float ExpansionFloat; 02095 02099 float SupplyVoltage; 02100 02104 float TotalCurrent; 02105 02109 float Power; 02110 02114 float AveragePower; 02115 02119 float AccelerationX; 02120 02124 float AccelerationY; 02125 02129 float AccelerationZ; 02130 02134 float SensorExpansion1; 02135 02139 float SensorExpansion2; 02140 02144 float SensorExpansion3; 02145 02149 unsigned int CodeVersion; 02150 02154 unsigned int CodeRevision; 02155 02159 unsigned short Status; 02160 02164 unsigned short Controller; 02165 02169 unsigned short ControlMode; 02170 02174 unsigned short HandMode; 02175 02179 unsigned short ConnectedActuatorCount; 02180 02184 unsigned short PositionType; 02185 02189 unsigned short ErrorsSpiExpansion1; 02190 02194 unsigned short ErrorsSpiExpansion2; 02195 02199 unsigned short ErrorsMainSPICount; 02200 02204 unsigned short ErrorsExternalSPICount; 02205 02209 unsigned short ErrorsMainCANCount; 02210 02214 unsigned short ErrorsExternalCANCount; 02215 02219 SystemStatus ActualSystemStatus; 02220 02224 UserPosition Position; 02225 02229 UserPosition Command; 02230 02234 UserPosition Current; 02235 02239 UserPosition Force; 02240 02244 ZoneLimitation ActualLimitations; 02245 02249 float ControlIncrement[6]; 02250 02254 float FingerControlIncrement[3]; 02255 02259 JoystickCommand ActualJoystickCommand; 02260 02264 unsigned int PeripheralsConnected[4]; 02265 02269 unsigned int PeripheralsDeviceID[4]; 02270 02274 float ActuatorsTemperatures[6]; 02275 02279 float FingersTemperatures[3]; 02280 02284 float FutureTemperatures[3]; 02285 02289 int ActuatorsCommErrors[6]; 02290 02294 int FingersCommErrors[3]; 02295 02299 int ExpansionLong2; 02300 02304 double ControlTimeAbsolute; 02305 02310 double ControlTimeFromStartup; 02311 02315 unsigned char ExpansionsBytes[192]; 02316 }; 02317 02321 struct AngularAcceleration 02322 { 02327 float Actuator1_X; 02328 02333 float Actuator1_Y; 02334 02339 float Actuator1_Z; 02340 02345 float Actuator2_X; 02346 02351 float Actuator2_Y; 02352 02357 float Actuator2_Z; 02358 02363 float Actuator3_X; 02364 02369 float Actuator3_Y; 02370 02375 float Actuator3_Z; 02376 02381 float Actuator4_X; 02382 02387 float Actuator4_Y; 02388 02393 float Actuator4_Z; 02394 02399 float Actuator5_X; 02400 02405 float Actuator5_Y; 02406 02411 float Actuator5_Z; 02412 02417 float Actuator6_X; 02418 02423 float Actuator6_Y; 02424 02429 float Actuator6_Z; 02430 02434 void InitStruct() 02435 { 02436 Actuator1_X = 0.0f; 02437 Actuator1_Y = 0.0f; 02438 Actuator1_Z = 0.0f; 02439 Actuator2_X = 0.0f; 02440 Actuator2_Y = 0.0f; 02441 Actuator2_Z = 0.0f; 02442 Actuator3_X = 0.0f; 02443 Actuator3_Y = 0.0f; 02444 Actuator3_Z = 0.0f; 02445 Actuator4_X = 0.0f; 02446 Actuator4_Y = 0.0f; 02447 Actuator4_Z = 0.0f; 02448 Actuator5_X = 0.0f; 02449 Actuator5_Y = 0.0f; 02450 Actuator5_Z = 0.0f; 02451 Actuator6_X = 0.0f; 02452 Actuator6_Y = 0.0f; 02453 Actuator6_Z = 0.0f; 02454 } 02455 }; 02456 02460 struct PeripheralInfo 02461 { 02465 unsigned int Handle; 02466 02470 unsigned int Type; 02471 02475 unsigned int Port; 02476 02480 unsigned int Address; 02481 02485 unsigned int CodeVersion; 02486 };