diff --git a/comm/include/comm.h b/comm/include/comm.h index c6ca16db0f5509e672ac3199c464dd8969b28533..6509ae74248c72b8c108e7efa6b06f407de43989 100644 --- a/comm/include/comm.h +++ b/comm/include/comm.h @@ -1,4 +1,4 @@ -/** @file */ +/** @file */ #ifndef _COMM_H #define _COMM_H @@ -35,7 +35,7 @@ typedef enum {COMM_SUCCESS=0x00,/*!< The communication module has ended successf * * Configure the DMA transactions (in case they are enabled). * * Implement the functions required by this structure to initiate and cancel * RX and TX transactions using both IRQ and DMA. - * * Implement the basic IRQ and DMa handler functions. + * * Implement the basic IRQ and DMA handler functions. * * On the user application side, another set of functions must be also implemented * and assigned to this structure at initialization time. The user application @@ -43,15 +43,15 @@ typedef enum {COMM_SUCCESS=0x00,/*!< The communication module has ended successf * required. * * In blocking mode, the IRQ driven transactions are used except when the use of - * DMA is enbaled, in which case the DMA driven transactions are used. + * DMA is enabled, in which case the DMA driven transactions are used. * - * The following diagram descrives the transmission operation when using + * The following diagram describes the transmission operation when using * interrupts, and how the user application, the communication module and the * actual communication device work together: * *  * - * The user application starts a TX driven transmission by calling the comm_send_irq() + * The user application starts a IRQ driven transmission by calling the comm_send_irq() * function, which in turn calls the low level communication function pointed by * the parameter send_irq. At this point the interrupt handler is called each time * a byte is actually sent, which calls the comm_do_irq_send() function to update @@ -61,9 +61,9 @@ typedef enum {COMM_SUCCESS=0x00,/*!< The communication module has ended successf * While the data transfer is in progress the user can call the comm_is_send_done() * function to check the status of the transmission and the comm_get_error() * function to check for errors (if any). This functions can be used with any kind - * of non-blobking transactions (IRQ and DMA). + * of non-blocking transactions (IRQ and DMA). * - * The following diagram descrives the reception operation when using + * The following diagram describes the reception operation when using * interrupts, and how the user application, the communication module and the * actual communication device work together: * @@ -79,7 +79,50 @@ typedef enum {COMM_SUCCESS=0x00,/*!< The communication module has ended successf * turn, calls the user callback function pointed by irq_receive_cb. The reception * remains active until the desired number of bytes are received, or the user * application cancels it. + * + * To get the received data, use the comm_get_received_data() function. + * + * The following diagram descrives the transmission operation when using + * DMA, and how the user application, the communication module and the + * actual communication device work together: + * + *  + * + * The user application starts a DMA driven transmission by calling the comm_send_dma() + * function, which in turn calls the low level communication function pointed by + * the parameter send_dma. This function should configure the DMA transfer and start + * it. + * + * When the last byte to be transmitted is written to the communication device, + * the DMA interrupt handler is called, which calls the comm_do_dma_send() + * function which enables the TX interrupts to wait until the last byte has + * been actually sent. + * + * Finally, when the TX interrupt handler is called, the comm_do_irq_send() + * function is called, in the same way it is called in a regular IRQ driven + * transmission. In this case however, its behavior is different, because + * the user application callback function pointed by dma_send_cb is called, + * so that the user can perform any required action. + * + * The following diagram descrives the reception operation when using + * DMA, and how the user application, the communication module and the + * actual communication device work together: * + *  + * + * The user application starts a DMA driven reception by calling the comm_receive_dma() + * function, which in turn calls the low level communication function pointed by + * the parameter receive_dma. This function should configure the DMA transfer and start + * it. + * + * When the desired number of bytes have been received, the reception DMA interrupt + * handler is called, which calls the comm_do_dma_receive() function, calls the + * user application function pointed by the dma_receive_cb parameter, so that the + * user can perform any required action. + * + * In the case of DMA driven reception, the received data can be accessed by the + * data pointer provided to the comm_receive_dma() function, once the reception + * has ended. */ typedef struct { @@ -93,7 +136,7 @@ typedef struct /** * \brief Active transmission using IRQ flag * - * This parameter is used to indicate if there is an acive transmission of data + * This parameter is used to indicate if there is an active transmission of data * using interrupts to handle it. This flag is set when a transmission starts and * it is cleared when it ends. */ @@ -101,7 +144,7 @@ typedef struct /** * \brief Active transmission using DMA flag * - * This parameter is used to indicate if there is an acive transmission of data + * This parameter is used to indicate if there is an active transmission of data * using DMA to handle it. This flag is set when a transmission starts and it is * cleared when it ends. */ @@ -123,7 +166,7 @@ typedef struct /** * \brief Active reception using IRQ flag * - * This parameter is used to indicate if there is an acive reception of data + * This parameter is used to indicate if there is an active reception of data * using interrupts to handle it. This flag is set when a reception starts and * it is cleared when it ends. */ @@ -131,7 +174,7 @@ typedef struct /** * \brief Active reception using DMA flag * - * This parameter is used to indicate if there is an acive reception of data + * This parameter is used to indicate if there is an active reception of data * using DMA to handle it. This flag is set when a reception starts and it is * cleared when it ends. */ @@ -139,7 +182,7 @@ typedef struct /** * \brief Number of bytes pending to be received * - * This parameter holds the number of bytes pending to be recevied in the current + * This parameter holds the number of bytes pending to be received in the current * transaction. This parameter is initialized when the reception starts, and * is decremented after the reception of each byte. */ @@ -183,7 +226,7 @@ typedef struct void *data; /* IRQ functions */ /** - * \defgroup harware_irq_pointers Hardware IRQ related function pointeris + * \defgroup hardware_irq_pointers Hardware IRQ related function pointers * * This set of functions, that must be implemented by the low level hardware * communication module (either USART, I2C, SPI or any other communication @@ -227,7 +270,7 @@ typedef struct /**@}*/ /* DMA functions */ /** - * \defgroup harware_dma_pointers Hardware DMA related function pointers + * \defgroup hardware_dma_pointers Hardware DMA related function pointers * * This set of functions, that must be implemented by the low level hardware * communication module (either USART, I2C, SPI or any other communication @@ -242,20 +285,20 @@ typedef struct * \brief Pointer to a function to start a transmission using DMA * * This function should configure and enable the corresponding DMA channel - * to transfer the desired ammount of data from the internal buffer to the + * to transfer the desired amount of data from the internal buffer to the * communication device. TX DMA interrupts should be enabled so that the * corresponding IRQ handler is called when the transaction is complete or - * when an error ocurrs. + * when an error occurs. */ unsigned char (*send_dma)(unsigned char *data,unsigned short int length); /** * \brief Pointer to a function to start a reception using DMA * * This function should configure and enable the corresponding DMA channel - * to transfer the desired ammount of data from the communication device to + * to transfer the desired amount of data from the communication device to * the internal buffer. RX DMA interrupts should be enabled so that the * corresponding IRQ handler is called when the transaction is complete or - * when an error ocurrs. + * when an error occurs. */ unsigned char (*receive_dma)(unsigned char *data,unsigned short int length); /** @@ -271,7 +314,7 @@ typedef struct * \defgroup callback_irq_pointers User IRQ related function pointers * * This set of functions, that must be implemented by the user module that - * uses the coomunication structure, are intended to handle the specific + * uses the communication structure, are intended to handle the specific * requirements of each application using interrupt driven communications. * * This function pointers must be assigned by the initialization function @@ -303,7 +346,7 @@ typedef struct * \defgroup callback_dma_pointers User DMA related function pointers * * This set of functions, that must be implemented by the user module that - * uses the coomunication structure, are intended to handle the specific + * uses the communication structure, are intended to handle the specific * requirements of each application using DMA driven communications. * * This function pointers must be assigned by the initialization function @@ -337,16 +380,16 @@ typedef struct * * This functions allows to enable or disable the use of the DMA. This * option can not be modified afterwards. The TTime structure, if provided, - * will allow the comunicatin device to use timeouts during reception. + * will allow the communication device to use timeouts during reception. * Otherwise, the timeout feature will be disabled. * * \param dev pointer to a valid TComm structure to be initialized. If * memory is not pre-allocated before calling this function, its * behavior is unpredictable. * \param use_dma flag to enable (0x01) or disable (0x00) the use of - * the DMA for the communcations structure. Any other value is + * the DMA for the communications structure. Any other value is * interpreted as an enable. - * \param time a pointer to an initiliazed TTime structure to enable the + * \param time a pointer to an initialized TTime structure to enable the * use of timeouts. If timeouts are not desired or required, this * parameter must be set to NULL (0x00000000). */ @@ -367,11 +410,11 @@ void comm_init(TComm *dev,unsigned char use_dma,TTime *time); * behavior is unpredictable. * \param data pointer to a vector where the data to be transmitted is stored. * Memory for this parameter must be pre-allocated before calling this - * function to avoid unexpected behaviour. + * function to avoid unexpected behavior. * \param length Number of bytes to be transmitted. * * \return the status of the transmission. The possible values are the elements - * in the comm_error ennumeraiton. + * in the comm_error enumeration. */ comm_error comm_send(TComm *dev,unsigned char *data,unsigned short int length); /** @@ -383,28 +426,28 @@ comm_error comm_send(TComm *dev,unsigned char *data,unsigned short int length); * reception is used. * * This function blocks until all bytes have been received, there has been an - * error or the ammount of time specified as timeout has elapsed. + * error or the amount of time specified as timeout has elapsed. * * \param dev pointer to a valid TComm structure to be initialized. If * memory is not pre-allocated before calling this function, its * behavior is unpredictable. * \param data pointer to a vector where the received data is to be stored. * Memory for this parameter must be pre-allocated before calling this - * function to avoid unexpected behaviour. + * function to avoid unexpected behavior. * \param length Number of bytes to be received. - * \param timeout maximum time in micro-seconds to wait for the desired ammount + * \param timeout maximum time in micro-seconds to wait for the desired amount * of bytes. If the TTime structure is not valid this parameter is ignored. * * \return the status of the transmission. The possible values are the elements - * in the comm_error ennumeraiton. + * in the comm_error enumeration. */ comm_error comm_receive(TComm *dev,unsigned char *data,unsigned short int *length,unsigned long int timeout); /* IRQ functions */ /** * \brief Function to start an interrupt driven transmission * - * This function is used to start a reception in IRQ driven mode. This function - * starts sending the data but returns immediatelly, reporting an error, if any. + * This function is used to start a transmission in IRQ driven mode. This function + * starts sending the data but returns immediately, reporting an error, if any. * The data in the input parameter is copied to the internal buffer before it * returns. * @@ -416,11 +459,11 @@ comm_error comm_receive(TComm *dev,unsigned char *data,unsigned short int *lengt * behavior is unpredictable. * \param data pointer to a vector where the data to be transmitted is stored. * Memory for this parameter must be pre-allocated before calling this - * function to avoid unexpected behaviour. + * function to avoid unexpected behavior. * \param length Number of bytes to be transmitted. * * \return the status of the transmission. The possible values are the elements - * in the comm_error ennumeraiton. + * in the comm_error enumeration. */ comm_error comm_send_irq(TComm *dev,unsigned char *data,unsigned short int length); /** @@ -431,77 +474,252 @@ comm_error comm_send_irq(TComm *dev,unsigned char *data,unsigned short int lengt * transfer being sent, it calls the corresponding DMA transmission callback * function. * - * If this function is called as a result of a byte beign sent in an IRQ driven + * If this function is called as a result of a byte being sent in an IRQ driven * transfer, this function calls the corresponding IRQ transmission callback * function. When the last byte has been sent, this function automatically stops * the current transmission, and returns a value of 0x00 to report it. + * + * The interrupt handler should use the value returned by this function to send + * the next byte or disable the transmission interrupts when the transmission + * is finished (in either case). + * + * Use the comm_get_error() function to get any possible error that happen + * during transmission. + * + * \param dev pointer to a valid TComm structure to be initialized. If + * memory is not pre-allocated before calling this function, its + * behavior is unpredictable. + * \param byte this parameter holds the value to be sent next when the function + * returns 0x00, when it is part of an IRQ driven transmission. Otherwise + * this parameter must be ignored. + * + * \return 0x00 when the transmission has ended and any other value when there + * are still some bytes to be sent. */ unsigned char comm_do_irq_send(TComm *dev,unsigned char *byte); /** * \brief Function to start an interrupt driven reception * + * This function is used to start a reception in IRQ driven mode. This function + * starts the reception process but returns immediately, reporting an error, if any. + * The received data is stored into the internal buffer, and the comm_get_received_data() + * function should be used to get it. + * + * This function calls the function of the low level communication module pointed + * by the receive_irq parameter. + * + * \param dev pointer to a valid TComm structure to be initialized. If + * memory is not pre-allocated before calling this function, its + * behavior is unpredictable. + * \param length Number of bytes to be transmitted. + * + * \return the status of the reception. The possible values are the elements + * in the comm_error enumeration. + */ comm_error comm_receive_irq(TComm *dev,unsigned short int length); /** * \brief Function to handle the reception of a single byte - * + * + * This function should be called from the interrupt handler each time a byte has + * been received. When this function is called, it first call the user defined + * function pointed by the irq_receive_cb parameter to check the received data + * (if necessary). + * + * If the callback function returns 0x00, the received byte is ignored and + * the reception process continues. If the callback function returns any other + * value, the new byte is stored into the internal buffer until all the desired + * bytes have been received. + * + * \param dev pointer to a valid TComm structure to be initialized. If + * memory is not pre-allocated before calling this function, its + * behavior is unpredictable. + * \param byte this parameter holds the last byte that has been received through + * the communication device, and it is passed to the user callback in order + * to be processed. + * + * \return 0x00 when the reception has ended and any other value when there + * are still some bytes to be received. */ unsigned char comm_do_irq_receive(TComm *dev,unsigned char byte); /** * \brief Function to get the received data - * + * + * This function is used to get the data received by an IRQ driven reception. + * This function can only be called when the reception has ended, otherwise it + * reports a busy error. + * + * Use the comm_get_error() function to get any possible error that happen + * during reception. + * + * \param dev pointer to a valid TComm structure to be initialized. If + * memory is not pre-allocated before calling this function, its + * behavior is unpredictable. + * \param data buffer where the received data will be copied to be used by the + * user application. The memory for this vector must be pre-allocated + * before calling this function. + * \param length is the number of data actually received. In case of no error, + * this value must coincide with the desired value, but it can be less + * in case of any error. + * + * \return COMM_SUCCESS in case the reception process has ended, and COMM_BUSY + * otherwise. */ comm_error comm_get_received_data(TComm *dev,unsigned char *data,unsigned short int *length); /** * \brief Function to cancel the current interrupt driven reception - * + * + * This function cancels any active IRQ driven reception. This function calls the + * function of the low level communication module pointed by the cancel_receive_irq + * parameter to actually stop and disable the reception interrupts. + * + * \param dev pointer to a valid TComm structure to be initialized. If + * memory is not pre-allocated before calling this function, its + * behavior is unpredictable. */ void comm_cancel_irq_receive(TComm *dev); /* DMA functions */ /** * \brief Function to start a DMA driven transmission * + * This function is used to start a transmission using DMA. This function calls the + * low level communication module pointed by the send_dma parameter. This function + * should configure the DMA transaction with the provided information and start it. + * + * \param dev pointer to a valid TComm structure to be initialized. If + * memory is not pre-allocated before calling this function, its + * behavior is unpredictable. + * \param data pointer to a vector where the data to be transmitted is stored. + * Memory for this parameter must be pre-allocated before calling this + * function to avoid unexpected behavior. + * \param length Number of bytes to be transmitted. + * + * \return COMM_SUCCESS if the transmission has started successfully or COMM_BUSY + * if there is an operation in progress. */ comm_error comm_send_dma(TComm *dev,unsigned char *data,unsigned short int length); /** * \brief Function to handle the transmission of all bytes * + * This function is called when the last data byte has been sent to the + * communication device. This function enables the TX interrupts to actually + * wait until the last byte has been sent to avoid possible errors. + * + * It is not until the last byte has been actually sent, that the user application + * callback function pointed by the dma_send_cb parameter is called. This function + * should handle any required action after the transmission. + * + * \param dev pointer to a valid TComm structure to be initialized. If + * memory is not pre-allocated before calling this function, its + * behavior is unpredictable. */ void comm_do_dma_send(TComm *dev); /** * \brief Function to start a DMA driven reception * + * This function is used to start a reception using DMA. This function calls the + * low level communication module pointed by the receive_dma parameter. This function + * should configure the DMA transaction with the provided information and start it. + * + * \param dev pointer to a valid TComm structure to be initialized. If + * memory is not pre-allocated before calling this function, its + * behavior is unpredictable. + * \param data pointer to a vector where the received data will be stored. + * Memory for this parameter must be pre-allocated before calling this + * function to avoid unexpected behavior. The contents of this vector + * are not valid until the operation has ended. + * \param length Number of bytes to be received. + * + * \return COMM_SUCCESS if the reception has started successfully or COMM_BUSY + * if there is an operation in progress. */ comm_error comm_receive_dma(TComm *dev,unsigned char *data,unsigned short int length); /** * \brief Function to handle the reception of all desired bytes * + * This function is called when the last data byte has been received through + * the communication device. At this point, the user application callback function + * pointed by the dma_receive_cb parameter is called. This function should + * handle any required action after the reception. + * + * \param dev pointer to a valid TComm structure to be initialized. If + * memory is not pre-allocated before calling this function, its + * behavior is unpredictable. */ void comm_do_dma_receive(TComm *dev); /** * \brief Function to cancel the current DMA driven reception - * + * + * This function is used to cancel any active DMA driven reception by calling + * the low level communication device pointed by cancel_receive_dma. When this + * function is called, the contents of the data vector passed to the comm_send_dma() + * function can not be used. + * + * \param dev pointer to a valid TComm structure to be initialized. If + * memory is not pre-allocated before calling this function, its + * behavior is unpredictable. */ void comm_cancel_dma_receive(TComm *dev); /* common functions */ /** * \brief Function to check whether the current transmission has ended or not * + * This function is used to check whether there is an active transmission or + * not. In the case the transmission has ended, this function indicates if + * there has been an error or not. In case of an error, use the comm_get_error() + * function to get it. + * + * \param dev pointer to a valid TComm structure to be initialized. If + * memory is not pre-allocated before calling this function, its + * behavior is unpredictable. + * + * \return COMM_BUSY if the transmission is still active, COMM_ERROR if the + * last transmission has ended with an error or COMM_SUCCESS if it + * ended successfully. */ comm_error comm_is_send_done(TComm *dev); /** * \brief Function to check whether the current reception has ended or not * + * This function is used to check whether there is an active reception or + * not. In the case the reception has ended, this function indicates if + * there has been an error or not. In case of an error, use the comm_get_error() + * function to get it. + * + * \param dev pointer to a valid TComm structure to be initialized. If + * memory is not pre-allocated before calling this function, its + * behavior is unpredictable. + * + * \return COMM_BUSY if the reception is still active, COMM_ERROR if the + * last reception has ended with an error or COMM_SUCCESS if it + * ended successfully. */ comm_error comm_is_receive_done(TComm *dev); /** * \brief Function to get the communication error * + * This function returns the error of the last transmission or reception + * operation. + * + * \param dev pointer to a valid TComm structure to be initialized. If + * memory is not pre-allocated before calling this function, its + * behavior is unpredictable. + * + * \return the error of the last operation or COMM_BUSY if the operation + * is still in progress. */ inline unsigned char comm_get_error(TComm *dev); /** * \brief Function to set the communication error - * + * + * The user application and the low level communications module can use this + * function to set the internal communication error at any time. The error + * set by this function can be get with the comm_get_error() function. + * + * \param dev pointer to a valid TComm structure to be initialized. If + * memory is not pre-allocated before calling this function, its + * behavior is unpredictable. + * \param error identifier of the error to be set */ inline void comm_set_error(TComm *dev, unsigned char error); diff --git a/comm/src/comm.c b/comm/src/comm.c index 7424c862051cbed1e137cff81192b2ebe18354b6..b5aa27e5049634c0479ad5f031ae3d88eed59e5e 100644 --- a/comm/src/comm.c +++ b/comm/src/comm.c @@ -330,12 +330,12 @@ comm_error comm_is_receive_done(TComm *dev) } } -inline unsigned char comm_get_error(TComm *dev) +unsigned char comm_get_error(TComm *dev) { return dev->error; } -inline void comm_set_error(TComm *dev, unsigned char error) +void comm_set_error(TComm *dev, unsigned char error) { dev->error=error; } diff --git a/dynamixel_base/Makefile b/dynamixel_base/Makefile index ad6b5947882c2d2a27c4e233946972836be03c20..fef5dc81ca91d8ee5c4e03d28c6f842ab9f5af02 100755 --- a/dynamixel_base/Makefile +++ b/dynamixel_base/Makefile @@ -13,8 +13,9 @@ COMPILE_OPTS_M3 = -mfloat-abi=softfp -mcpu=cortex-m3 COMM_PATH = ../comm UTILS_PATH = ../utils +MEMORY_PATH = ../memory -INCLUDE_DIRS = -I./include/ -I$(COMM_PATH)/include -I$(UTILS_PATH)/include +INCLUDE_DIRS = -I./include/ -I$(COMM_PATH)/include -I$(UTILS_PATH)/include -I$(MEMORY_PATH)/include DOC_DIR = ./doc diff --git a/dynamixel_base/include/dynamixel.h b/dynamixel_base/include/dynamixel.h index 263e03d87a06c2a24572d46605b96be38eb7da85..ce76479b70e5bc89402ee07379eef854986570aa 100755 --- a/dynamixel_base/include/dynamixel.h +++ b/dynamixel_base/include/dynamixel.h @@ -10,10 +10,30 @@ #define DYN_ERROR_OFF 4 #define DYN_DATA_OFF 5 +/** + * \brief + * + */ void dyn_copy_packet(unsigned char *source, unsigned char *destination); +/** + * \brief + * + */ inline unsigned char dyn_get_id(unsigned char *packet); +/** + * \brief + * + */ inline unsigned char dyn_get_length(unsigned char *packet); +/** + * \brief + * + */ inline TDynInstruction dyn_get_instruction(unsigned char *packet); +/** + * \brief + * + */ unsigned char dyn_check_checksum(unsigned char *packet); // instruction packet @@ -27,32 +47,108 @@ typedef struct{ unsigned char checksum; }TDynInst; */ +/** + * \brief + * + */ unsigned char dyn_convert_v2_inst_packet(unsigned char *source, unsigned char *destination); /* ping instruction */ +/** + * \brief + * + */ void dyn_init_ping_packet(unsigned char *packet,unsigned char id); /* read instruction */ +/** + * \brief + * + */ void dyn_init_read_packet(unsigned char *packet,unsigned char id,unsigned char address,unsigned char length); +/** + * \brief + * + */ inline unsigned char dyn_get_read_length(unsigned char *packet); +/** + * \brief + * + */ inline unsigned char dyn_get_read_address(unsigned char *packet); /* write instruction */ +/** + * \brief + * + */ void dyn_init_write_packet(unsigned char *packet,unsigned char id,unsigned char address,unsigned char length,unsigned char *data); +/** + * \brief + * + */ inline unsigned char dyn_get_write_address(unsigned char *packet); +/** + * \brief + * + */ inline unsigned char dyn_get_write_length(unsigned char *packet); +/** + * \brief + * + */ inline unsigned char dyn_get_write_data(unsigned char *packet,unsigned char *data); /* registered write instruction */ +/** + * \brief + * + */ void dyn_init_reg_write_packet(unsigned char *packet,unsigned char id,unsigned char address,unsigned char length,unsigned char *data); +/** + * \brief + * + */ inline unsigned char dyn_get_reg_write_address(unsigned char *packet); +/** + * \brief + * + */ inline unsigned char dyn_get_reg_write_length(unsigned char *packet); +/** + * \brief + * + */ unsigned char dyn_get_reg_write_data(unsigned char *packet,unsigned char *data); /* action instruction */ +/** + * \brief + * + */ void dyn_init_action_packet(unsigned char *packet); /* reset instruction */ +/** + * \brief + * + */ void dyn_init_reset_packet(unsigned char *packet,unsigned char id); /* sync write instruction */ -void dyn_init_sync_write_packet(unsigned char *packet,unsigned char num_servos,unsigned char *servo_ids,unsigned char address,unsigned char length,TWriteData *data); +/** + * \brief + * + */ +void dyn_init_sync_write_packet(unsigned char *packet,unsigned char num_servos,unsigned char *servo_ids,unsigned char address,unsigned char length,unsigned char * const data[]); +/** + * \brief + * + */ unsigned char dyn_sync_write_id_present(unsigned char *packet,unsigned char id,unsigned char *address,unsigned char *length,unsigned char *data); /* bulk read instruction */ +/** + * \brief + * + */ void dyn_init_bulk_read_packet(unsigned char *packet,unsigned char num_servos,unsigned char *servo_ids,unsigned char *address,unsigned char *length); +/** + * \brief + * + */ unsigned char dyn_bulk_read_id_present(unsigned char *packet,unsigned char id,unsigned char *address,unsigned char *length); // status packet @@ -67,11 +163,31 @@ typedef struct{ }TDynStatus; */ +/** + * \brief + * + */ unsigned char dyn_convert_v2_status_packet(unsigned char *source, unsigned char *destination); +/** + * \brief + * + */ void dyn_init_status_packet(unsigned char *packet,unsigned char id,TDynError error,unsigned char length,unsigned char *data); +/** + * \brief + * + */ inline TDynError dyn_get_status_error(unsigned char *packet); /* read instruction status packet */ +/** + * \brief + * + */ unsigned char dyn_get_read_status_data(unsigned char *packet,unsigned char *data); +/** + * \brief + * + */ inline unsigned char dyn_get_read_status_id(unsigned char *packet); #endif diff --git a/dynamixel_base/include/dynamixel2.h b/dynamixel_base/include/dynamixel2.h index a2d8e30392d74b6429393f8a3d280532fbd99755..a84821557ccf3a079fed9793e9b3c7ca3ab96133 100644 --- a/dynamixel_base/include/dynamixel2.h +++ b/dynamixel_base/include/dynamixel2.h @@ -11,11 +11,30 @@ #define DYN2_ERROR_OFF 8 #define DYN2_DATA_OFF 8 - +/** + * \brief + * + */ void dyn2_copy_packet(unsigned char *source, unsigned char *destination); +/** + * \brief + * + */ inline unsigned char dyn2_get_id(unsigned char *packet); +/** + * \brief + * + */ inline unsigned short int dyn2_get_length(unsigned char *packet); +/** + * \brief + * + */ inline TDynInstruction dyn2_get_instruction(unsigned char *packet); +/** + * \brief + * + */ unsigned char dyn2_check_checksum(unsigned char *packet); // instruction packet @@ -29,38 +48,130 @@ typedef struct{ unsigned char checksum; }TDynInst; */ +/** + * \brief + * + */ unsigned char dyn2_convert_v1_inst_packet(unsigned char *source, unsigned char *destination); /* ping instruction */ +/** + * \brief + * + */ void dyn2_init_ping_packet(unsigned char *packet,unsigned char id); /* read instruction */ +/** + * \brief + * + */ void dyn2_init_read_packet(unsigned char *packet,unsigned char id,unsigned short int address,unsigned short int length); +/** + * \brief + * + */ inline unsigned short int dyn2_get_read_length(unsigned char *packet); +/** + * \brief + * + */ inline unsigned short int dyn2_get_read_address(unsigned char *packet); /* write instruction */ +/** + * \brief + * + */ void dyn2_init_write_packet(unsigned char *packet,unsigned char id,unsigned short int address,unsigned short int length,unsigned char *data); +/** + * \brief + * + */ inline unsigned short int dyn2_get_write_address(unsigned char *packet); +/** + * \brief + * + */ inline unsigned short int dyn2_get_write_length(unsigned char *packet); +/** + * \brief + * + */ inline unsigned short int dyn2_get_write_data(unsigned char *packet,unsigned char *data); /* registered write instruction */ +/** + * \brief + * + */ void dyn2_init_reg_write_packet(unsigned char *packet,unsigned char id,unsigned short int address,unsigned short int length,unsigned char *data); +/** + * \brief + * + */ inline unsigned short int dyn2_get_reg_write_address(unsigned char *packet); +/** + * \brief + * + */ inline unsigned short int dyn2_get_reg_write_length(unsigned char *packet); +/** + * \brief + * + */ unsigned short int dyn2_get_reg_write_data(unsigned char *packet,unsigned char *data); /* action instruction */ +/** + * \brief + * + */ void dyn2_init_action_packet(unsigned char *packet); /* reset instruction */ +/** + * \brief + * + */ void dyn2_init_reset_packet(unsigned char *packet,unsigned char id,unsigned char mode); /* sync write instruction */ -void dyn2_init_sync_write_packet(unsigned char *packet,unsigned char num_servos,unsigned char *servo_ids,unsigned short int address,unsigned short int length,TWriteData *data); +/** + * \brief + * + */ +void dyn2_init_sync_write_packet(unsigned char *packet,unsigned char num_servos,unsigned char *servo_ids,unsigned short int address,unsigned short int length,unsigned char * const data[]); +/** + * \brief + * + */ unsigned char dyn2_sync_write_id_present(unsigned char *packet,unsigned char id,unsigned short int *address,unsigned short int *length,unsigned char *data); /* sync read instruction */ +/** + * \brief + * + */ void dyn2_init_sync_read_packet(unsigned char *packet,unsigned char num_servos,unsigned char *servo_ids,unsigned short int address,unsigned short int length); +/** + * \brief + * + */ unsigned char dyn2_sync_read_id_present(unsigned char *packet,unsigned char id,unsigned short int *address,unsigned short int *length); /* bulk read instruction */ +/** + * \brief + * + */ void dyn2_init_bulk_read_packet(unsigned char *packet,unsigned char num_servos,unsigned char *servo_ids,unsigned short int *address,unsigned short int *length); +/** + * \brief + * + */ unsigned char dyn2_bulk_read_id_present(unsigned char *packet,unsigned char id,unsigned short int *address,unsigned short int *length); /* bulk write instruction */ -void dyn2_init_bulk_write_packet(unsigned char *packet,unsigned char num_servos,unsigned char *servo_ids,unsigned short int *address,unsigned short int *length,TWriteData *data); +/** + * \brief + * + */ +void dyn2_init_bulk_write_packet(unsigned char *packet,unsigned char num_servos,unsigned char *servo_ids,unsigned short int *address,unsigned short int *length,unsigned char * const data[]); +/** + * \brief + * + */ unsigned char dyn2_bulk_write_id_present(unsigned char *packet,unsigned char id,unsigned short int *address,unsigned short int *length,unsigned char *data); // status packet @@ -75,11 +186,31 @@ typedef struct{ }TDynStatus; */ +/** + * \brief + * + */ unsigned char dyn2_convert_v1_status_packet(TDynInstruction inst,unsigned char *source, unsigned char *destination); +/** + * \brief + * + */ void dyn2_init_status_packet(unsigned char *packet,unsigned char id,TDynError error,unsigned short int length,unsigned char *data); +/** + * \brief + * + */ inline TDynError dyn2_get_status_error(unsigned char *packet); /* read instruction status packet */ +/** + * \brief + * + */ unsigned short int dyn2_get_read_status_data(unsigned char *packet,unsigned char *data); +/** + * \brief + * + */ inline unsigned char dyn2_get_read_status_id(unsigned char *packet); #endif diff --git a/dynamixel_base/include/dynamixel_common.h b/dynamixel_base/include/dynamixel_common.h index 7090a876973b125ccfa0017e4b694cadf6ed7e8f..1ec207d610aea42fa96b6d26c8d52ed46fe8df2b 100644 --- a/dynamixel_base/include/dynamixel_common.h +++ b/dynamixel_base/include/dynamixel_common.h @@ -1,3 +1,5 @@ +/** @file */ + #ifndef _DYNAMIXEL_COMMON_H #define _DYNAMIXEL_COMMON_H @@ -13,38 +15,55 @@ #define DYN_COMM_ERROR 4 #define DYN_BUSY 5 -typedef struct{ - unsigned char *data_addr; -}TWriteData; - -typedef enum{no_return=0x00, - return_only_read=0x01, - return_all=0x02} return_level_t; +/** + * \brief Possible return levels + * + */ +typedef enum{no_return=0x00,/*!< Only the PING command will return an aswer */ + return_only_read=0x01,/*!< Only the read commands will return an answer */ + return_all=0x02/*!< All commands will return an aswer. This is the default value */ + } return_level_t; // possible packet types -typedef enum{DYN_PING=0x01, - DYN_READ=0x02, - DYN_WRITE=0x03, - DYN_REG_WRITE=0x04, - DYN_ACTION=0x05, - DYN_RESET=0x06, - DYN_SYNC_READ=0x82, - DYN_SYNC_WRITE=0x83, - DYN_BULK_READ=0x92, - DYN_BULK_WRITE=0x93} TDynInstruction; +/** + * \brief Possible instructions + * + */ +typedef enum{DYN_PING=0x01,/*!< Ping. Instruction to detect the presence of dynamixel devices */ + DYN_READ=0x02,/*!< Read. Instruction to read a consecutive number of registers starting from a given address from a single device */ + DYN_WRITE=0x03,/*!< Write. Instruction to write a consecutive number of registers starting at a given address to a given device*/ + DYN_REG_WRITE=0x04,/*!< Registerd write. Instruction to execute a delayed write operation */ + DYN_ACTION=0x05,/*!< Action. Instruction to actually load the data of a previous registeres write instruction*/ + DYN_RESET=0x06,/*!< Reset. Instruction to reset the device to a default state */ + DYN_SYNC_READ=0x82,/*!< Synchronous read. Instruction to read a consecutive number of registers starting from a given address from multiples devices */ + DYN_SYNC_WRITE=0x83,/*!< Synchronous write. Instruction to write a consecutive number of registers starting at a given address to multiple devices */ + DYN_BULK_READ=0x92,/*!< Bulk read. Instruction to read a set of consecutive registers from multiples devices, starting at a different address for each device */ + DYN_BULK_WRITE=0x93/*!< Bulk write. Instruction to write a set of consecutive registers to multiples devices, starting at a different address for each device */ + } TDynInstruction; // boradcast ID -#define DYN_BROADCAST_ID 0xFE +#define DYN_BROADCAST_ID 0xFE/*!< Broadcast ID */ // status packet -typedef enum{DYN_NO_ERROR=0x00, - DYN_INST_ERROR=0x40, - DYN_OVERLOAD_ERROR=0x20, - DYN_CHECKSUM_ERROR=0x10, - DYN_RANGE_ERROR=0x08, - DYN_OVERTEMP_ERROR=0x04, - DYN_ANGLE_ERROR=0x02, - DYN_VOLTAGE_ERROR=0x01} TDynError; - -typedef enum{DYN_VER1=0x01,DYN_VER2=0x02} TDynVersion; +/** + * \brief Possible errors + * + */ +typedef enum{DYN_NO_ERROR=0x00,/*!< No error */ + DYN_INST_ERROR=0x40,/*!< Invalid Instruction identifier */ + DYN_OVERLOAD_ERROR=0x20,/*!< Servo exceeded the maximum torque value */ + DYN_CHECKSUM_ERROR=0x10,/*!< Invalid checksum on the instruction packet */ + DYN_RANGE_ERROR=0x08,/*!< Register value out of the allowed range */ + DYN_OVERTEMP_ERROR=0x04,/*!< Temperature exceeded the maximum value */ + DYN_ANGLE_ERROR=0x02,/*!< Target position out of range */ + DYN_VOLTAGE_ERROR=0x01/*!< Power supply voltage out of range */ + } TDynError; + +/** + * \brief Dynamixel versions + * + */ +typedef enum{DYN_VER1=0x01,/*!< Dynamixel version 1.0 */ + DYN_VER2=0x02/*!< Dynamixel version 2.0 */ + } TDynVersion; #endif diff --git a/dynamixel_base/include/dynamixel_master.h b/dynamixel_base/include/dynamixel_master.h index 3b3b4e057fd0b294a1d8c98564534d6d63fe8f8f..7da7c90be7873723a8b9b27c5db894b792ad787e 100644 --- a/dynamixel_base/include/dynamixel_master.h +++ b/dynamixel_base/include/dynamixel_master.h @@ -5,7 +5,6 @@ #include "dynamixel.h" #include "dynamixel2.h" - #ifndef MAX_DYN_MASTER_TX_BUFFER_LEN #define MAX_DYN_MASTER_TX_BUFFER_LEN 256 #endif @@ -14,42 +13,174 @@ #define MAX_DYN_MASTER_RX_BUFFER_LEN 256 #endif +/** + * \brief + * + */ typedef struct { + /** + * \brief + * + */ TComm *comm_dev; + /** + * \brief + * + */ TDynVersion version; + /** + * \brief + * + */ unsigned short int op_length; + /** + * \brief + * + */ unsigned char tx_buffer[MAX_DYN_MASTER_TX_BUFFER_LEN]; + /** + * \brief + * + */ unsigned char rx_buffer[MAX_DYN_MASTER_RX_BUFFER_LEN]; + /** + * \brief + * + */ return_level_t return_level; + /** + * \brief + * + */ unsigned char packet_ready; + /** + * \brief + * + */ unsigned char received_bytes; + /** + * \brief + * + */ void (*set_tx_mode)(void); + /** + * \brief + * + */ void (*set_rx_mode)(void); + /** + * \brief + * + */ unsigned short int rx_timeout_ms; + /** + * \brief + * + */ unsigned char rx_no_answer; + /** + * \brief + * + */ unsigned char rx_num_packets; }TDynamixelMaster; /* public functions */ +/** + * \brief + * + */ void dyn_master_init(TDynamixelMaster *master,TComm *dev,TDynVersion version); -void dyn_master_set_rx_timeout(TDynamixelMaster *master,unsigned short int timeout_ms); +/** + * \brief + * + */ +inline void dyn_master_set_rx_timeout(TDynamixelMaster *master,unsigned short int timeout_ms); +/** + * \brief + * + */ inline void dyn_master_set_return_level(TDynamixelMaster *master,return_level_t level); +/** + * \brief + * + */ inline return_level_t dyn_master_get_return_level(TDynamixelMaster *master); +/** + * \brief + * + */ void dyn_master_scan(TDynamixelMaster *master,unsigned char *num,unsigned char *ids); +/** + * \brief + * + */ unsigned char dyn_master_ping(TDynamixelMaster *master,unsigned char id); +/** + * \brief + * + */ unsigned char dyn_master_read_byte(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned char *data); +/** + * \brief + * + */ unsigned char dyn_master_read_word(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned short int *data); +/** + * \brief + * + */ unsigned char dyn_master_read_table(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned short int length,unsigned char *data); +/** + * \brief + * + */ unsigned char dyn_master_write_byte(TDynamixelMaster *master,unsigned char id, unsigned short int address, unsigned char data); +/** + * \brief + * + */ unsigned char dyn_master_write_word(TDynamixelMaster *master,unsigned char id, unsigned short int address, unsigned short int data); -unsigned char dyn_master_write_table(TDynamixelMaster *master,unsigned char id, unsigned short int address, unsigned short int length, unsigned char *data); -unsigned char dyn_master_reg_write(TDynamixelMaster *master,unsigned char id, unsigned short int address, unsigned short int length, unsigned char *data); +/** + * \brief + * + */ +unsigned char dyn_master_write_table(TDynamixelMaster *master,unsigned char id, unsigned short int address, unsigned short int length,unsigned char *data); +/** + * \brief + * + */ +unsigned char dyn_master_reg_write(TDynamixelMaster *master,unsigned char id, unsigned short int address, unsigned short int length,unsigned char *data); +/** + * \brief + * + */ unsigned char dyn_master_action(TDynamixelMaster *master); -unsigned char dyn_master_sync_write(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, TWriteData *data); -unsigned char dyn_master_sync_read(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, TWriteData *data); -unsigned char dyn_master_bulk_read(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, TWriteData *data); -unsigned char dyn_master_bulk_write(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, TWriteData *data); +/** + * \brief + * + */ +unsigned char dyn_master_sync_write(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, unsigned char * const data[]); +/** + * \brief + * + */ +unsigned char dyn_master_sync_read(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, unsigned char * const data[]); +/** + * \brief + * + */ +unsigned char dyn_master_bulk_read(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, unsigned char * const data[]); +/** + * \brief + * + */ +unsigned char dyn_master_bulk_write(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, unsigned char * const data[]); +/** + * \brief + * + */ unsigned char dyn_master_relay(TDynamixelMaster *master,unsigned char *inst_pkt,unsigned char *status_pkt); #endif diff --git a/dynamixel_base/include/dynamixel_slave.h b/dynamixel_base/include/dynamixel_slave.h index 7529f8afeff70391dcc4c84a181dfd0fdcf50190..37902b616fd26d24244cd6cfbba0adf54164e5d6 100644 --- a/dynamixel_base/include/dynamixel_slave.h +++ b/dynamixel_base/include/dynamixel_slave.h @@ -4,6 +4,8 @@ #include "comm.h" #include "dynamixel.h" #include "dynamixel2.h" +#include "dynamixel_slave_registers.h" +#include "ram.h" #ifndef MAX_DYN_SLAVE_TX_BUFFER_LEN #define MAX_DYN_SLAVE_TX_BUFFER_LEN 256 @@ -17,46 +19,209 @@ #define MAX_DYN_SLAVE_REG_BUFFER_LEN 256 #endif +/** + * \brief + * + */ typedef struct { + /** + * \brief + * + */ TComm *comm_dev; + /** + * \brief + * + */ TDynVersion version; + /** + * \brief + * + */ unsigned char address; + /** + * \brief + * + */ unsigned char return_delay; + /** + * \brief + * + */ return_level_t return_level; + /** + * \brief + * + */ unsigned char packet_ready; + /** + * \brief + * + */ unsigned char received_bytes; + /** + * \brief + * + */ unsigned short int rx_timeout_ms; + /** + * \brief + * + */ unsigned short int op_length; + /** + * \brief + * + */ unsigned char tx_buffer[MAX_DYN_SLAVE_TX_BUFFER_LEN]; + /** + * \brief + * + */ unsigned char rx_buffer[MAX_DYN_SLAVE_RX_BUFFER_LEN]; + /** + * \brief + * + */ unsigned short int reg_address; + /** + * \brief + * + */ unsigned short int reg_length; + /** + * \brief + * + */ unsigned char reg_buffer[MAX_DYN_SLAVE_REG_BUFFER_LEN]; + /** + * \brief + * + */ unsigned short int sync_bulk_address; + /** + * \brief + * + */ unsigned short int sync_bulk_length; + /** + * \brief + * + */ unsigned char sync_bulk_prev_id; + /** + * \brief + * + */ unsigned char sync_read_pending; + /** + * \brief + * + */ unsigned char bulk_read_pending; + /** + * \brief + * + */ + TMemModule mem_module; + /** + * \brief + * + */ void (*set_tx_mode)(void); + /** + * \brief + * + */ void (*set_rx_mode)(void); + /** + * \brief + * + */ + void (*set_baudrate)(TComm *comm_dev,unsigned int baudrate); + /** + * \brief + * + */ void (*on_ping)(void); + /** + * \brief + * + */ unsigned char (*on_read)(unsigned short int address,unsigned short int length,unsigned char *data); + /** + * \brief + * + */ unsigned char (*on_write)(unsigned short int address,unsigned short int length,unsigned char *data); + /** + * \brief + * + */ unsigned char (*on_reset)(void); + /** + * \brief + * + */ unsigned char (*on_relay)(TDynVersion version,unsigned char *inst_pkt,unsigned char *status_pkt); }TDynamixelSlave; /* public functions */ -void dyn_slave_init(TDynamixelSlave *slave,TComm *dev,unsigned char address,TDynVersion version); -void dyn_slave_set_rx_timeout(TDynamixelSlave *slave,unsigned short int timeout_ms); +/** + * \brief + * + */ +unsigned char dyn_slave_init(TDynamixelSlave *slave,TMemory *memory,TComm *dev,unsigned char address,TDynVersion version); +/** + * \brief + * + */ +void dyn_slave_set_baudrate(TDynamixelSlave *slave,unsigned int baudrate); +/** + * \brief + * + */ +inline void dyn_slave_set_rx_timeout(TDynamixelSlave *slave,unsigned short int timeout_ms); +/** + * \brief + * + */ inline void dyn_slave_set_address(TDynamixelSlave *slave,unsigned char address); +/** + * \brief + * + */ inline unsigned char dyn_slave_get_address(TDynamixelSlave *slave); +/** + * \brief + * + */ inline void dyn_slave_set_return_delay(TDynamixelSlave *slave,unsigned char delay); +/** + * \brief + * + */ inline unsigned char dyn_slave_get_return_delay(TDynamixelSlave *slave); +/** + * \brief + * + */ inline void dyn_slave_set_return_level(TDynamixelSlave *slave,return_level_t level); +/** + * \brief + * + */ inline return_level_t dyn_slave_get_return_level(TDynamixelSlave *slave); +/** + * \brief + * + */ inline TDynVersion dyn_slave_get_version(TDynamixelSlave *slave); +/** + * \brief + * + */ void dyn_slave_loop(TDynamixelSlave *slave); diff --git a/dynamixel_base/include/dynamixel_slave_registers.h b/dynamixel_base/include/dynamixel_slave_registers.h new file mode 100644 index 0000000000000000000000000000000000000000..8a3a01bd147a1913bc031aba1c32c97a49960954 --- /dev/null +++ b/dynamixel_base/include/dynamixel_slave_registers.h @@ -0,0 +1,45 @@ +#ifndef _DYNAMIXEL_SLAVE_REGISTERS_H +#define _DYNAMIXEL_SLAVE_REGISTERS_H + +#ifndef EEPROM_DYN_SLAVE_BASE_ADDRESS1 + #define EEPROM_DYN_SLAVE_BASE_ADDRESS1 ((unsigned short int)0x0000) +#endif + +#define EEPROM_DYN_SLAVE_LENGTH1 6 + +#ifndef EEPROM_DYN_SLAVE_BASE_ADDRESS2 + #define EEPROM_DYN_SLAVE_BASE_ADDRESS2 ((unsigned short int)0x0000) +#endif + +#define EEPROM_DYN_SLAVE_LENGTH2 1 + +#define DEVICE_MODEL (EEPROM_DYN_SLAVE_BASE_ADDRESS1) +#define FIRMWARE_VERSION (EEPROM_DYN_SLAVE_BASE_ADDRESS1+2) +#define DEVICE_ID (EEPROM_DYN_SLAVE_BASE_ADDRESS1+3) +#define BAUDRATE (EEPROM_DYN_SLAVE_BASE_ADDRESS1+4) +#define RETURN_DELAY (EEPROM_DYN_SLAVE_BASE_ADDRESS1+5) + +#define RETURN_LEVEL (EEPROM_DYN_SLAVE_BASE_ADDRESS2) + +#ifndef DEFAULT_DEVICE_MODEL + #define DEFAULT_DEVICE_MODEL 0x7300 +#endif +#ifndef DEFAULT_FIRMWARE_VERSION + #define DEFAULT_FIRMWARE_VERSION 0x0001 +#endif +#ifndef DEFAULT_DEVICE_ID + #define DEFAULT_DEVICE_ID 0x0001 +#endif +#ifndef DEFAULT_BAUDRATE + #define DEFAULT_BAUDRATE 0x0010 +#endif +#ifndef DEFAULT_RETURN_DELAY + #define DEFAULT_RETURN_DELAY 0x0000 +#endif + +#ifndef DEFAULT_RETURN_LEVEL + #define DEFAULT_RETURN_LEVEL 0x0002 +#endif + +#endif + diff --git a/dynamixel_base/src/dynamixel.c b/dynamixel_base/src/dynamixel.c index 659c3cdbd93c7c5ac5f4e8a8791c58adb4d58c7e..eb18001bc2560df2627a00445cfe060daf2cc1c3 100755 --- a/dynamixel_base/src/dynamixel.c +++ b/dynamixel_base/src/dynamixel.c @@ -40,17 +40,17 @@ void dyn_copy_packet(unsigned char *source, unsigned char *destination) destination[i]=source[i]; } -inline unsigned char dyn_get_id(unsigned char *packet) +unsigned char dyn_get_id(unsigned char *packet) { return packet[DYN_ID_OFF]; } -inline unsigned char dyn_get_length(unsigned char *packet) +unsigned char dyn_get_length(unsigned char *packet) { return packet[DYN_LENGTH_OFF]; } -inline TDynInstruction dyn_get_instruction(unsigned char *packet) +TDynInstruction dyn_get_instruction(unsigned char *packet) { return packet[DYN_INST_OFF]; } @@ -99,12 +99,12 @@ void dyn_init_read_packet(unsigned char *packet,unsigned char id,unsigned char a dyn_set_checksum(packet); } -inline unsigned char dyn_get_read_length(unsigned char *packet) +unsigned char dyn_get_read_length(unsigned char *packet) { return packet[DYN_DATA_OFF+1]; } -inline unsigned char dyn_get_read_address(unsigned char *packet) +unsigned char dyn_get_read_address(unsigned char *packet) { return packet[DYN_DATA_OFF]; } @@ -125,12 +125,12 @@ void dyn_init_write_packet(unsigned char *packet,unsigned char id,unsigned char dyn_set_checksum(packet); } -inline unsigned char dyn_get_write_address(unsigned char *packet) +unsigned char dyn_get_write_address(unsigned char *packet) { return packet[DYN_DATA_OFF]; } -inline unsigned char dyn_get_write_length(unsigned char *packet) +unsigned char dyn_get_write_length(unsigned char *packet) { return packet[DYN_LENGTH_OFF]-3; } @@ -161,12 +161,12 @@ void dyn_init_reg_write_packet(unsigned char *packet,unsigned char id,unsigned c dyn_set_checksum(packet); } -inline unsigned char dyn_get_reg_write_address(unsigned char *packet) +unsigned char dyn_get_reg_write_address(unsigned char *packet) { return packet[DYN_DATA_OFF]; } -inline unsigned char dyn_get_reg_write_length(unsigned char *packet) +unsigned char dyn_get_reg_write_length(unsigned char *packet) { return packet[DYN_LENGTH_OFF]-3; } @@ -204,7 +204,7 @@ void dyn_init_reset_packet(unsigned char *packet,unsigned char id) } /* sync write instruction */ -void dyn_init_sync_write_packet(unsigned char *packet,unsigned char num_servos,unsigned char *servo_ids,unsigned char address,unsigned char length,TWriteData *data) +void dyn_init_sync_write_packet(unsigned char *packet,unsigned char num_servos,unsigned char *servo_ids,unsigned char address,unsigned char length,unsigned char * const data[]) { unsigned char i,j; @@ -219,7 +219,7 @@ void dyn_init_sync_write_packet(unsigned char *packet,unsigned char num_servos,u { packet[DYN_DATA_OFF+2+(length+1)*i]=servo_ids[i]; for(j=0;j<length;j++) - packet[DYN_DATA_OFF+2+(length+1)*i+j+1]=data[i].data_addr[j]; + packet[DYN_DATA_OFF+2+(length+1)*i+j+1]=data[i][j]; } dyn_set_checksum(packet); } @@ -315,7 +315,7 @@ void dyn_init_status_packet(unsigned char *packet,unsigned char id,TDynError err dyn_set_checksum(packet); } -inline TDynError dyn_get_status_error(unsigned char *packet) +TDynError dyn_get_status_error(unsigned char *packet) { return packet[DYN_ERROR_OFF]; } @@ -331,7 +331,7 @@ unsigned char dyn_get_read_status_data(unsigned char *packet,unsigned char *data return packet[DYN_LENGTH_OFF]-0x02; } -inline unsigned char dyn_get_read_status_id(unsigned char *packet) +unsigned char dyn_get_read_status_id(unsigned char *packet) { return packet[DYN_ID_OFF]; } diff --git a/dynamixel_base/src/dynamixel2.c b/dynamixel_base/src/dynamixel2.c index a65f9f804f79ef9ec90c921840ef934ccb3e8c86..e645c1de97f8b22b5f2616528571bbc0c343a6bb 100644 --- a/dynamixel_base/src/dynamixel2.c +++ b/dynamixel_base/src/dynamixel2.c @@ -79,17 +79,17 @@ void dyn2_copy_packet(unsigned char *source, unsigned char *destination) destination[i]=source[i]; } -inline unsigned char dyn2_get_id(unsigned char *packet) +unsigned char dyn2_get_id(unsigned char *packet) { return packet[DYN2_ID_OFF]; } -inline unsigned short int dyn2_get_length(unsigned char *packet) +unsigned short int dyn2_get_length(unsigned char *packet) { return (packet[DYN2_LENGTH_OFF]+packet[DYN2_LENGTH_OFF+1]*256); } -inline TDynInstruction dyn2_get_instruction(unsigned char *packet) +TDynInstruction dyn2_get_instruction(unsigned char *packet) { return packet[DYN2_INST_OFF]; } @@ -145,12 +145,12 @@ void dyn2_init_read_packet(unsigned char *packet,unsigned char id,unsigned short dyn2_set_checksum(packet); } -inline unsigned short int dyn2_get_read_length(unsigned char *packet) +unsigned short int dyn2_get_read_length(unsigned char *packet) { return (packet[DYN2_DATA_OFF+2]+packet[DYN2_DATA_OFF+3]*256); } -inline unsigned short int dyn2_get_read_address(unsigned char *packet) +unsigned short int dyn2_get_read_address(unsigned char *packet) { return (packet[DYN2_DATA_OFF]+packet[DYN2_DATA_OFF+1]*256); } @@ -175,12 +175,12 @@ void dyn2_init_write_packet(unsigned char *packet,unsigned char id,unsigned shor dyn2_set_checksum(packet); } -inline unsigned short int dyn2_get_write_address(unsigned char *packet) +unsigned short int dyn2_get_write_address(unsigned char *packet) { return (packet[DYN2_DATA_OFF]+packet[DYN2_DATA_OFF+1]*256); } -inline unsigned short int dyn2_get_write_length(unsigned char *packet) +unsigned short int dyn2_get_write_length(unsigned char *packet) { return (packet[DYN2_LENGTH_OFF]+packet[DYN2_LENGTH_OFF+1]*256)-5; } @@ -216,12 +216,12 @@ void dyn2_init_reg_write_packet(unsigned char *packet,unsigned char id,unsigned dyn2_set_checksum(packet); } -inline unsigned short int dyn2_get_reg_write_address(unsigned char *packet) +unsigned short int dyn2_get_reg_write_address(unsigned char *packet) { return (packet[DYN2_DATA_OFF]+packet[DYN2_DATA_OFF+1]*256); } -inline unsigned short int dyn2_get_reg_write_length(unsigned char *packet) +unsigned short int dyn2_get_reg_write_length(unsigned char *packet) { return (packet[DYN2_LENGTH_OFF]+packet[DYN2_LENGTH_OFF+1]*256)-5; } @@ -267,7 +267,7 @@ void dyn2_init_reset_packet(unsigned char *packet,unsigned char id,unsigned char } /* sync write instruction */ -void dyn2_init_sync_write_packet(unsigned char *packet,unsigned char num_servos,unsigned char *servo_ids,unsigned short int address,unsigned short int length,TWriteData *data) +void dyn2_init_sync_write_packet(unsigned char *packet,unsigned char num_servos,unsigned char *servo_ids,unsigned short int address,unsigned short int length,unsigned char * const data[]) { unsigned char i; unsigned short int j,len; @@ -289,7 +289,7 @@ void dyn2_init_sync_write_packet(unsigned char *packet,unsigned char num_servos, { packet[DYN2_DATA_OFF+4+(length+1)*i]=servo_ids[i]; for(j=0;j<length;j++) - packet[DYN2_DATA_OFF+4+(length+1)*i+j+1]=data[i].data_addr[j]; + packet[DYN2_DATA_OFF+4+(length+1)*i+j+1]=data[i][j]; } dyn2_set_checksum(packet); } @@ -400,7 +400,7 @@ unsigned char dyn2_bulk_read_id_present(unsigned char *packet,unsigned char id,u } /* bulk write instruction */ -void dyn2_init_bulk_write_packet(unsigned char *packet,unsigned char num_servos,unsigned char *servo_ids,unsigned short int *address,unsigned short int *length,TWriteData *data) +void dyn2_init_bulk_write_packet(unsigned char *packet,unsigned char num_servos,unsigned char *servo_ids,unsigned short int *address,unsigned short int *length,unsigned char * const data[]) { unsigned char i; unsigned short int total_length=0,j; @@ -424,7 +424,7 @@ void dyn2_init_bulk_write_packet(unsigned char *packet,unsigned char num_servos, packet[DYN2_DATA_OFF+total_length+3]=length[i]%256; packet[DYN2_DATA_OFF+total_length+4]=length[i]/256; for(j=0;j<length[i];j++) - packet[DYN2_DATA_OFF+total_length+5+j]=data[i].data_addr[j]; + packet[DYN2_DATA_OFF+total_length+5+j]=data[i][j]; total_length+=length[i]+5; } dyn2_set_checksum(packet); @@ -491,7 +491,7 @@ void dyn2_init_status_packet(unsigned char *packet,unsigned char id,TDynError er dyn2_set_checksum(packet); } -inline TDynError dyn2_get_status_error(unsigned char *packet) +TDynError dyn2_get_status_error(unsigned char *packet) { return packet[DYN2_DATA_OFF]; } @@ -508,7 +508,7 @@ unsigned short int dyn2_get_read_status_data(unsigned char *packet,unsigned char return length; } -inline unsigned char dyn2_get_read_status_id(unsigned char *packet) +unsigned char dyn2_get_read_status_id(unsigned char *packet) { return packet[DYN2_ID_OFF]; } diff --git a/dynamixel_base/src/dynamixel_master.c b/dynamixel_base/src/dynamixel_master.c index cfeb073cfbbfdce3f57a719c8efebf0beea2dd13..cb3757420c2021a612d0f3acb13cf9b7b2719a63 100644 --- a/dynamixel_base/src/dynamixel_master.c +++ b/dynamixel_base/src/dynamixel_master.c @@ -1,111 +1,114 @@ #include "dynamixel_master.h" /* private functions */ -unsigned char dyn_master_irq_receive_cb(void *dyn_master,unsigned char byte) +unsigned char dyn_master_irq_receive_cb(TDynamixelMaster *dyn_master,unsigned char byte) { - TDynamixelMaster *dyn=(TDynamixelMaster *)dyn_master; - - if(dyn->comm_dev->time!=0x00000000) - time_set_timeout(dyn->comm_dev->time,dyn->rx_timeout_ms*1000); - switch(dyn->received_bytes) + if(dyn_master!=0x00000000) { - case 0: if(byte==0xFF) - { - dyn->rx_buffer[dyn->received_bytes]=byte; - dyn->received_bytes++; - } - break; - case 1: if(byte==0xFF) - { - dyn->rx_buffer[dyn->received_bytes]=byte; - dyn->received_bytes++; - } - else - dyn->received_bytes--; - break; - case 2: if(byte==0xFD)// version 2 header - { - if(dyn->version==DYN_VER2)// the module is configured for version 2 + if(dyn_master->comm_dev->time!=0x00000000) + time_set_timeout(dyn_master->comm_dev->time,dyn_master->rx_timeout_ms*1000); + switch(dyn_master->received_bytes) + { + case 0: if(byte==0xFF) { - dyn->rx_buffer[dyn->received_bytes]=byte; - dyn->received_bytes++; + dyn_master->rx_buffer[dyn_master->received_bytes]=byte; + dyn_master->received_bytes++; + } + break; + case 1: if(byte==0xFF) + { + dyn_master->rx_buffer[dyn_master->received_bytes]=byte; + dyn_master->received_bytes++; } else - dyn->received_bytes=0;// ignore packet and restart synchronization - } - else if(byte!=0xFF) - { - dyn->rx_buffer[dyn->received_bytes]=byte; - dyn->received_bytes++; - } - break; - case 3: dyn->rx_buffer[dyn->received_bytes]=byte; - if(dyn->version==DYN_VER1) - { - dyn->op_length=byte; - dyn->received_bytes=0; + dyn_master->received_bytes--; + break; + case 2: if(byte==0xFD)// version 2 header + { + if(dyn_master->version==DYN_VER2)// the module is configured for version 2 + { + dyn_master->rx_buffer[dyn_master->received_bytes]=byte; + dyn_master->received_bytes++; + } + else + dyn_master->received_bytes=0;// ignore packet and restart synchronization + } + else if(byte!=0xFF) + { + dyn_master->rx_buffer[dyn_master->received_bytes]=byte; + dyn_master->received_bytes++; + } + break; + case 3: dyn_master->rx_buffer[dyn_master->received_bytes]=byte; + if(dyn_master->version==DYN_VER1) + { + dyn_master->op_length=byte; + dyn_master->received_bytes=0; + /* finish reception by IRQ */ + comm_cancel_irq_receive(dyn_master->comm_dev); + /* enable dma RX */ + comm_receive_dma(dyn_master->comm_dev,&dyn_master->rx_buffer[4],dyn_master->op_length); + } + else + dyn_master->received_bytes++; + break; + case 4: dyn_master->rx_buffer[dyn_master->received_bytes]=byte; + dyn_master->received_bytes++; + break; + case 5: dyn_master->rx_buffer[dyn_master->received_bytes]=byte; + dyn_master->received_bytes++; + dyn_master->op_length=byte; + break; + case 6: dyn_master->rx_buffer[dyn_master->received_bytes]=byte; + dyn_master->received_bytes++; + dyn_master->op_length+=(byte<<8); + dyn_master->received_bytes=0; /* finish reception by IRQ */ - comm_cancel_irq_receive(dyn->comm_dev); + comm_cancel_irq_receive(dyn_master->comm_dev); /* enable dma RX */ - comm_receive_dma(dyn->comm_dev,&dyn->rx_buffer[4],dyn->op_length); - } - else - dyn->received_bytes++; - break; - case 4: dyn->rx_buffer[dyn->received_bytes]=byte; - dyn->received_bytes++; - break; - case 5: dyn->rx_buffer[dyn->received_bytes]=byte; - dyn->received_bytes++; - dyn->op_length=byte; - break; - case 6: dyn->rx_buffer[dyn->received_bytes]=byte; - dyn->received_bytes++; - dyn->op_length+=(byte<<8); - dyn->received_bytes=0; - /* finish reception by IRQ */ - comm_cancel_irq_receive(dyn->comm_dev); - /* enable dma RX */ - comm_receive_dma(dyn->comm_dev,&dyn->rx_buffer[7],dyn->op_length); - break; - default: break; + comm_receive_dma(dyn_master->comm_dev,&dyn_master->rx_buffer[7],dyn_master->op_length); + break; + default: break; + } } return 0x00; } -unsigned char dyn_master_dma_send_cb(void *dyn_master) +unsigned char dyn_master_dma_send_cb(TDynamixelMaster *dyn_master) { - TDynamixelMaster *dyn=(TDynamixelMaster *)dyn_master; - - dyn->set_rx_mode(); - if(dyn->rx_no_answer) - dyn->rx_no_answer=0x00; - else + if(dyn_master!=0x00000000) { - /* enable RX interrupts */ - comm_receive_irq(dyn->comm_dev,0); + dyn_master->set_rx_mode(); + if(dyn_master->rx_no_answer) + dyn_master->rx_no_answer=0x00; + else + { + /* enable RX interrupts */ + comm_receive_irq(dyn_master->comm_dev,0); + } } return 0x00; } -unsigned char dyn_master_dma_receive_cb(void *dyn_master) +unsigned char dyn_master_dma_receive_cb(TDynamixelMaster *dyn_master) { - TDynamixelMaster *dyn=(TDynamixelMaster *)dyn_master; - - dyn->rx_num_packets--; - if(dyn->rx_num_packets==0x00) - { - if(dyn->comm_dev->time!=0x00000000) - time_cancel_timeout(dyn->comm_dev->time); - dyn->packet_ready=0x01; - } - else + if(dyn_master!=0x00000000) { - /* enable RX interrupts */ - comm_receive_irq(dyn->comm_dev,0); - dyn->packet_ready=0x01; + dyn_master->rx_num_packets--; + if(dyn_master->rx_num_packets==0x00) + { + if(dyn_master->comm_dev->time!=0x00000000) + time_cancel_timeout(dyn_master->comm_dev->time); + dyn_master->packet_ready=0x01; + } + else + { + /* enable RX interrupts */ + comm_receive_irq(dyn_master->comm_dev,0); + dyn_master->packet_ready=0x01; + } } return 0x00; @@ -125,11 +128,17 @@ unsigned char dyn_master_wait_transmission(TDynamixelMaster *master) { unsigned char error; - while((error=comm_is_send_done(master->comm_dev))==COMM_BUSY); + if(master!=0x00000000) + { + while((error=comm_is_send_done(master->comm_dev))==COMM_BUSY); - if(error==COMM_SUCCESS) - return DYN_SUCCESS; - else + if(error==COMM_SUCCESS) + return DYN_SUCCESS; + else + return DYN_COMM_ERROR; + } + + else return DYN_COMM_ERROR; } @@ -137,185 +146,213 @@ unsigned char dyn_master_is_transmission_done(TDynamixelMaster *master) { unsigned char error; - error=comm_is_send_done(master->comm_dev); - if(error==COMM_BUSY) - return DYN_BUSY; - else if(error==COMM_SUCCESS) + if(master!=0x00000000) { - if(master->comm_dev->time!=0x00000000) + error=comm_is_send_done(master->comm_dev); + if(error==COMM_BUSY) + return DYN_BUSY; + else if(error==COMM_SUCCESS) { - // start the new timeout - time_set_timeout(master->comm_dev->time,master->rx_timeout_ms*1000); + if(master->comm_dev->time!=0x00000000) + { + // start the new timeout + time_set_timeout(master->comm_dev->time,master->rx_timeout_ms*1000); + } + return DYN_SUCCESS; + } + else + { + master->set_rx_mode(); + return DYN_COMM_ERROR; } - return DYN_SUCCESS; } else - { - master->set_rx_mode(); return DYN_COMM_ERROR; - } } unsigned char dyn_master_send(TDynamixelMaster *master) { unsigned char error; - // wait until any previous transmission ends - if((error=dyn_master_wait_transmission(master))!=DYN_SUCCESS) - return error; - // set the DMA transfer - if(master->version==DYN_VER1) + if(master!=0x00000000) { - comm_send_dma(master->comm_dev,master->tx_buffer,dyn_get_length(master->tx_buffer)+4); - return DYN_SUCCESS; + // wait until any previous transmission ends + if((error=dyn_master_wait_transmission(master))!=DYN_SUCCESS) + return error; + // set the DMA transfer + if(master->version==DYN_VER1) + { + comm_send_dma(master->comm_dev,master->tx_buffer,dyn_get_length(master->tx_buffer)+4); + return DYN_SUCCESS; + } + else + { + comm_send_dma(master->comm_dev,master->tx_buffer,dyn2_get_length(master->tx_buffer)+7); + return DYN_SUCCESS; + } } else - { - comm_send_dma(master->comm_dev,master->tx_buffer,dyn2_get_length(master->tx_buffer)+7); - return DYN_SUCCESS; - } + return DYN_COMM_ERROR; } unsigned char dyn_master_wait_reception(TDynamixelMaster *master) { - if(master->comm_dev->time!=0x00000000) - { - // start the new timeout - time_set_timeout(master->comm_dev->time,master->rx_timeout_ms*1000); - } - // wait for the status packet - while(!master->packet_ready) + if(master!=0x00000000) { if(master->comm_dev->time!=0x00000000) { - if(time_is_timeout(master->comm_dev->time)) + // start the new timeout + time_set_timeout(master->comm_dev->time,master->rx_timeout_ms*1000); + } + // wait for the status packet + while(!master->packet_ready) + { + if(master->comm_dev->time!=0x00000000) { - comm_cancel_irq_receive(master->comm_dev); - comm_cancel_dma_receive(master->comm_dev); - master->received_bytes=0x00; - return DYN_TIMEOUT; + if(time_is_timeout(master->comm_dev->time)) + { + comm_cancel_irq_receive(master->comm_dev); + comm_cancel_dma_receive(master->comm_dev); + master->received_bytes=0x00; + return DYN_TIMEOUT; + } } } - } - master->packet_ready=0x00; - // check the input packet checksum - if(master->version==DYN_VER1) - { - if(dyn_check_checksum(master->rx_buffer)==0xFF) - return dyn_get_status_error(master->rx_buffer); + master->packet_ready=0x00; + // check the input packet checksum + if(master->version==DYN_VER1) + { + if(dyn_check_checksum(master->rx_buffer)==0xFF) + return dyn_get_status_error(master->rx_buffer); + else + return DYN_CHECKSUM_ERROR; + } else - return DYN_CHECKSUM_ERROR; + { + if(dyn2_check_checksum(master->rx_buffer)==0x01) + return dyn2_get_status_error(master->rx_buffer); + else + return DYN_CHECKSUM_ERROR; + } } else - { - if(dyn2_check_checksum(master->rx_buffer)==0x01) - return dyn2_get_status_error(master->rx_buffer); - else - return DYN_CHECKSUM_ERROR; - } + return DYN_COMM_ERROR; } unsigned char dyn_master_is_reception_done(TDynamixelMaster *master) { - if(!master->packet_ready) + if(master!=0x00000000) { - if(master->comm_dev->time!=0x00000000) + if(!master->packet_ready) { - if(time_is_timeout(master->comm_dev->time)) + if(master->comm_dev->time!=0x00000000) { - comm_cancel_irq_receive(master->comm_dev); - comm_cancel_dma_receive(master->comm_dev); - master->received_bytes=0x00; - return DYN_TIMEOUT; + if(time_is_timeout(master->comm_dev->time)) + { + comm_cancel_irq_receive(master->comm_dev); + comm_cancel_dma_receive(master->comm_dev); + master->received_bytes=0x00; + return DYN_TIMEOUT; + } + else + return DYN_BUSY; } else return DYN_BUSY; } else - return DYN_BUSY; + { + master->packet_ready=0x00; + return DYN_SUCCESS; + } } else - { - master->packet_ready=0x00; - return DYN_SUCCESS; - } + return DYN_COMM_ERROR; } unsigned char dyn_master_start_read_table(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned short int length,unsigned char *data) { - unsigned char error; + unsigned char error=DYN_COMM_ERROR; - // generate the read packet for the desired device - if(master->version==DYN_VER1) - dyn_init_read_packet(master->tx_buffer,id,address,length); - else - dyn2_init_read_packet(master->tx_buffer,id,address,length); - master->rx_num_packets=0x01; - if(master->return_level==no_return || id==DYN_BROADCAST_ID) - master->rx_no_answer=0x01; - else - master->rx_no_answer=0x00; - // enable transmission - master->set_tx_mode(); - // send the data - if((error=dyn_master_send(master))!=DYN_SUCCESS) - master->set_rx_mode(); + if(master!=0x00000000) + { + // generate the read packet for the desired device + if(master->version==DYN_VER1) + dyn_init_read_packet(master->tx_buffer,id,address,length); + else + dyn2_init_read_packet(master->tx_buffer,id,address,length); + master->rx_num_packets=0x01; + if(master->return_level==no_return || id==DYN_BROADCAST_ID) + master->rx_no_answer=0x01; + else + master->rx_no_answer=0x00; + // enable transmission + master->set_tx_mode(); + // send the data + if((error=dyn_master_send(master))!=DYN_SUCCESS) + master->set_rx_mode(); + } return error; } unsigned char dyn_master_is_read_table_done(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned short int length,unsigned char *data) { - unsigned char error; + unsigned char error=DYN_COMM_ERROR; - if((error=dyn_master_is_transmission_done(master))==DYN_SUCCESS) + if(master!=0x00000000) { - // wait for the replay within the given timeout - if(master->return_level!=no_return && id!=DYN_BROADCAST_ID) + if((error=dyn_master_is_transmission_done(master))==DYN_SUCCESS) { - if((error=dyn_master_is_reception_done(master))==DYN_SUCCESS) + // wait for the replay within the given timeout + if(master->return_level!=no_return && id!=DYN_BROADCAST_ID) { - // check the input packet checksum - if(master->version==DYN_VER1) + if((error=dyn_master_is_reception_done(master))==DYN_SUCCESS) { - if(dyn_check_checksum(master->rx_buffer)==0xFF) + // check the input packet checksum + if(master->version==DYN_VER1) { - if((error=dyn_get_status_error(master->rx_buffer))==DYN_SUCCESS) + if(dyn_check_checksum(master->rx_buffer)==0xFF) { - if(dyn_get_read_status_data(master->rx_buffer,data)!=length)// not enough data - return DYN_INST_ERROR; - else - return DYN_SUCCESS; + if((error=dyn_get_status_error(master->rx_buffer))==DYN_SUCCESS) + { + if(dyn_get_read_status_data(master->rx_buffer,data)!=length)// not enough data + return DYN_INST_ERROR; + else + return DYN_SUCCESS; + } + else + return error; } - else - return error; + else + return DYN_CHECKSUM_ERROR; } else - return DYN_CHECKSUM_ERROR; - } - else - { - if(dyn2_check_checksum(master->rx_buffer)==0x01) { - if((error=dyn2_get_status_error(master->rx_buffer))==DYN_SUCCESS) + if(dyn2_check_checksum(master->rx_buffer)==0x01) { - if(dyn2_get_read_status_data(master->rx_buffer,data)!=length)// not enough data - return DYN_INST_ERROR; + if((error=dyn2_get_status_error(master->rx_buffer))==DYN_SUCCESS) + { + if(dyn2_get_read_status_data(master->rx_buffer,data)!=length)// not enough data + return DYN_INST_ERROR; + else + return DYN_SUCCESS; + } else - return DYN_SUCCESS; + return error; } else - return error; + return DYN_CHECKSUM_ERROR; } - else - return DYN_CHECKSUM_ERROR; } + else + return error; } else - return error; + return DYN_SUCCESS; } else - return DYN_SUCCESS; + return error; } else return error; @@ -323,318 +360,362 @@ unsigned char dyn_master_is_read_table_done(TDynamixelMaster *master,unsigned ch unsigned char dyn_master_start_write_table(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned short int length,unsigned char *data) { - unsigned char error; + unsigned char error=DYN_COMM_ERROR; - // generate the read packet for the desired device - if(master->version==DYN_VER1) - dyn_init_write_packet(master->tx_buffer,id,address,length,data); - else - dyn2_init_write_packet(master->tx_buffer,id,address,length,data); - master->rx_num_packets=0x01; - if(master->return_level==return_all && id!=DYN_BROADCAST_ID) - master->rx_no_answer=0x00; - else - master->rx_no_answer=0x01; - // enable transmission - master->set_tx_mode(); - // send the data - if((error=dyn_master_send(master))!=DYN_SUCCESS) - master->set_rx_mode(); + if(master!=0x00000000) + { + // generate the read packet for the desired device + if(master->version==DYN_VER1) + dyn_init_write_packet(master->tx_buffer,id,address,length,data); + else + dyn2_init_write_packet(master->tx_buffer,id,address,length,data); + master->rx_num_packets=0x01; + if(master->return_level==return_all && id!=DYN_BROADCAST_ID) + master->rx_no_answer=0x00; + else + master->rx_no_answer=0x01; + // enable transmission + master->set_tx_mode(); + // send the data + if((error=dyn_master_send(master))!=DYN_SUCCESS) + master->set_rx_mode(); + } return error; } unsigned char dyn_master_is_write_table_done(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned short int length,unsigned char *data) { - unsigned char error; + unsigned char error=DYN_COMM_ERROR; - if((error=dyn_master_is_transmission_done(master))==DYN_SUCCESS) + if(master!=0x00000000) { - // wait for the replay within the given timeout - if(master->return_level==return_all && id!=DYN_BROADCAST_ID) + if((error=dyn_master_is_transmission_done(master))==DYN_SUCCESS) { - if((error=dyn_master_is_reception_done(master))==DYN_SUCCESS) + // wait for the replay within the given timeout + if(master->return_level==return_all && id!=DYN_BROADCAST_ID) { - // check the input packet checksum - if(master->version==DYN_VER1) + if((error=dyn_master_is_reception_done(master))==DYN_SUCCESS) { - if(dyn_check_checksum(master->rx_buffer)==0xFF) - return dyn_get_status_error(master->rx_buffer); + // check the input packet checksum + if(master->version==DYN_VER1) + { + if(dyn_check_checksum(master->rx_buffer)==0xFF) + return dyn_get_status_error(master->rx_buffer); + else + return DYN_CHECKSUM_ERROR; + } else - return DYN_CHECKSUM_ERROR; + { + if(dyn2_check_checksum(master->rx_buffer)==0x01) + return dyn2_get_status_error(master->rx_buffer); + else + return DYN_CHECKSUM_ERROR; + } } else - { - if(dyn2_check_checksum(master->rx_buffer)==0x01) - return dyn2_get_status_error(master->rx_buffer); - else - return DYN_CHECKSUM_ERROR; - } + return error; } else - return error; + return DYN_SUCCESS; } else - return DYN_SUCCESS; + return error; } else return error; } -unsigned char dyn_master_start_sync_write(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, TWriteData *data) -{ - unsigned char error; - - // generate the read packet for the desired device - if(master->version==DYN_VER1) - dyn_init_sync_write_packet(master->tx_buffer,num,ids,address,length,data); - else - dyn2_init_sync_write_packet(master->tx_buffer,num,ids,address,length,data); - master->rx_num_packets=0x01; - master->rx_no_answer=0x00; - // enable transmission - master->set_tx_mode(); - // send the data - if((error=dyn_master_send(master))!=DYN_SUCCESS) - master->set_rx_mode(); - - return error; -} - -unsigned char dyn_master_is_sync_write_done(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, TWriteData *data) -{ - unsigned char error; - - error=comm_is_send_done(master->comm_dev); - if(error==COMM_BUSY) - return DYN_BUSY; - else if(error==COMM_SUCCESS) - return DYN_SUCCESS; - else - { - master->set_rx_mode(); - return DYN_COMM_ERROR; - } -} - -unsigned char dyn_master_start_sync_read(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, TWriteData *data) +unsigned char dyn_master_start_sync_write(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, unsigned char * const data[]) { - unsigned char error; + unsigned char error=DYN_COMM_ERROR; - // generate the read packet for the desired device - if(master->version==DYN_VER2) + if(master!=0x00000000) { - dyn2_init_sync_read_packet(master->tx_buffer,num,ids,address,length); - master->rx_num_packets=num; - if(master->return_level==no_return) - master->rx_no_answer=0x01; + // generate the read packet for the desired device + if(master->version==DYN_VER1) + dyn_init_sync_write_packet(master->tx_buffer,num,ids,address,length,data); else - master->rx_no_answer=0x00; + dyn2_init_sync_write_packet(master->tx_buffer,num,ids,address,length,data); + master->rx_num_packets=0x01; + master->rx_no_answer=0x00; // enable transmission master->set_tx_mode(); // send the data if((error=dyn_master_send(master))!=DYN_SUCCESS) master->set_rx_mode(); - - return error; } - else - return DYN_INST_ERROR; + + return error; } -unsigned char dyn_master_is_sync_read_done(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, TWriteData *data) +unsigned char dyn_master_is_sync_write_done(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length,unsigned char * const data[]) { - static unsigned char num_dev_done=0; - unsigned char error; + unsigned char error=DYN_COMM_ERROR; - // generate the read packet for the desired device - if(master->version==DYN_VER2) + if(master!=0x00000000) { - if((error=dyn_master_is_transmission_done(master))==DYN_SUCCESS) + error=comm_is_send_done(master->comm_dev); + if(error==COMM_BUSY) + return DYN_BUSY; + else if(error==COMM_SUCCESS) + return DYN_SUCCESS; + else { - // wait for the replay within the given timeout - if(master->return_level!=no_return) - { - if(num_dev_done<num) + master->set_rx_mode(); + return DYN_COMM_ERROR; + } + } + else + return error; +} + +unsigned char dyn_master_start_sync_read(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, unsigned char * const data[]) +{ + unsigned char error=DYN_COMM_ERROR; + + if(master!=0x00000000) + { + // generate the read packet for the desired device + if(master->version==DYN_VER2) + { + dyn2_init_sync_read_packet(master->tx_buffer,num,ids,address,length); + master->rx_num_packets=num; + if(master->return_level==no_return) + master->rx_no_answer=0x01; + else + master->rx_no_answer=0x00; + // enable transmission + master->set_tx_mode(); + // send the data + if((error=dyn_master_send(master))!=DYN_SUCCESS) + master->set_rx_mode(); + + return error; + } + else + return DYN_INST_ERROR; + } + else + return error; +} + +unsigned char dyn_master_is_sync_read_done(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, unsigned char * const data[]) +{ + static unsigned char num_dev_done=0; + unsigned char error=DYN_COMM_ERROR; + + if(master!=0x00000000) + { + // generate the read packet for the desired device + if(master->version==DYN_VER2) + { + if((error=dyn_master_is_transmission_done(master))==DYN_SUCCESS) + { + // wait for the replay within the given timeout + if(master->return_level!=no_return) { - if((error=dyn_master_is_reception_done(master))==DYN_SUCCESS) + if(num_dev_done<num) { - if(dyn2_check_checksum(master->rx_buffer)==0x01) + if((error=dyn_master_is_reception_done(master))==DYN_SUCCESS) { - if((error=dyn2_get_status_error(master->rx_buffer))==DYN_SUCCESS) + if(dyn2_check_checksum(master->rx_buffer)==0x01) { - while(dyn2_get_read_status_id(master->rx_buffer)!=ids[num_dev_done]) num_dev_done++; - if(dyn2_get_read_status_data(master->rx_buffer,data[num_dev_done].data_addr)!=length)// not enough data - return DYN_INST_ERROR; - else + if((error=dyn2_get_status_error(master->rx_buffer))==DYN_SUCCESS) { - num_dev_done++; - return DYN_BUSY; + while(dyn2_get_read_status_id(master->rx_buffer)!=ids[num_dev_done]) num_dev_done++; + if(dyn2_get_read_status_data(master->rx_buffer,(unsigned char *)data[num_dev_done])!=length)// not enough data + return DYN_INST_ERROR; + else + { + num_dev_done++; + return DYN_BUSY; + } } + else + return error; } else - return error; + return DYN_CHECKSUM_ERROR; } else - return DYN_CHECKSUM_ERROR; + return error; } - else - return error; + else + return DYN_SUCCESS; } - else + else return DYN_SUCCESS; } else - return DYN_SUCCESS; + return error; } else - return error; + return DYN_INST_ERROR; } - else - return DYN_INST_ERROR; + else + return error; } -unsigned char dyn_master_start_bulk_write(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, TWriteData *data) +unsigned char dyn_master_start_bulk_write(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, unsigned char * const data[]) { - unsigned char error; + unsigned char error=DYN_COMM_ERROR; - // generate the read packet for the desired device - if(master->version==DYN_VER2) + if(master!=0x00000000) { - dyn2_init_bulk_write_packet(master->tx_buffer,num,ids,address,length,data); - master->rx_num_packets=0x01; - master->rx_no_answer=0x00; - // enable transmission - master->set_tx_mode(); - // send the data - if((error=dyn_master_send(master))!=DYN_SUCCESS) - master->set_rx_mode(); - - return error; + // generate the read packet for the desired device + if(master->version==DYN_VER2) + { + dyn2_init_bulk_write_packet(master->tx_buffer,num,ids,address,length,data); + master->rx_num_packets=0x01; + master->rx_no_answer=0x00; + // enable transmission + master->set_tx_mode(); + // send the data + if((error=dyn_master_send(master))!=DYN_SUCCESS) + master->set_rx_mode(); + + return error; + } + else + return DYN_INST_ERROR; } else - return DYN_INST_ERROR; + return error; } -unsigned char dyn_master_is_bulk_write_done(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, TWriteData *data) +unsigned char dyn_master_is_bulk_write_done(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, unsigned char * const data[]) { - unsigned char error; + unsigned char error=DYN_COMM_ERROR; - if(master->version==DYN_VER2) + if(master!=0x00000000) { - error=comm_is_send_done(master->comm_dev); - if(error==COMM_BUSY) - return DYN_BUSY; - else if(error==COMM_SUCCESS) - return DYN_SUCCESS; - else + if(master->version==DYN_VER2) { - master->set_rx_mode(); - return DYN_COMM_ERROR; + error=comm_is_send_done(master->comm_dev); + if(error==COMM_BUSY) + return DYN_BUSY; + else if(error==COMM_SUCCESS) + return DYN_SUCCESS; + else + { + master->set_rx_mode(); + return DYN_COMM_ERROR; + } } + else + return DYN_INST_ERROR; } - else - return DYN_INST_ERROR; + else + return error; } -unsigned char dyn_master_start_bulk_read(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, TWriteData *data) +unsigned char dyn_master_start_bulk_read(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, unsigned char * const data[]) { - unsigned char error,i,ver1_address[255],ver1_length[255]; + unsigned char error=DYN_COMM_ERROR,i,ver1_address[255],ver1_length[255]; - // generate the read packet for the desired device - if(master->version==DYN_VER1) + if(master!=0x00000000) { - for(i=0;i<num;i++) + // generate the read packet for the desired device + if(master->version==DYN_VER1) { - ver1_address[i]=address[i]; - ver1_length[i]=length[i]; + for(i=0;i<num;i++) + { + ver1_address[i]=address[i]; + ver1_length[i]=length[i]; + } + dyn_init_bulk_read_packet(master->tx_buffer,num,ids,ver1_address,ver1_length); } - dyn_init_bulk_read_packet(master->tx_buffer,num,ids,ver1_address,ver1_length); + else + dyn2_init_bulk_read_packet(master->tx_buffer,num,ids,address,length); + master->rx_num_packets=num; + if(master->return_level==no_return) + master->rx_no_answer=0x01; + else + master->rx_no_answer=0x00; + // enable transmission + master->set_tx_mode(); + // send the data + if((error=dyn_master_send(master))!=DYN_SUCCESS) + master->set_rx_mode(); } - else - dyn2_init_bulk_read_packet(master->tx_buffer,num,ids,address,length); - master->rx_num_packets=num; - if(master->return_level==no_return) - master->rx_no_answer=0x01; - else - master->rx_no_answer=0x00; - // enable transmission - master->set_tx_mode(); - // send the data - if((error=dyn_master_send(master))!=DYN_SUCCESS) - master->set_rx_mode(); return error; } -unsigned char dyn_master_is_bulk_read_done(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, TWriteData *data) +unsigned char dyn_master_is_bulk_read_done(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, unsigned char * const data[]) { static unsigned char num_dev_done=0; - unsigned char error; + unsigned char error=DYN_COMM_ERROR; - if((error=dyn_master_is_transmission_done(master))==DYN_SUCCESS) + if(master!=0x00000000) { - // wait for the replay within the given timeout - if(master->return_level!=no_return) + if((error=dyn_master_is_transmission_done(master))==DYN_SUCCESS) { - if(num_dev_done<num) + // wait for the replay within the given timeout + if(master->return_level!=no_return) { - if((error=dyn_master_is_reception_done(master))==DYN_SUCCESS) + if(num_dev_done<num) { - // check the input packet checksum - if(master->version==DYN_VER1) + if((error=dyn_master_is_reception_done(master))==DYN_SUCCESS) { - if(dyn_check_checksum(master->rx_buffer)==0xFF) + // check the input packet checksum + if(master->version==DYN_VER1) { - if((error=dyn_get_status_error(master->rx_buffer))==DYN_SUCCESS) + if(dyn_check_checksum(master->rx_buffer)==0xFF) { - while(dyn_get_read_status_id(master->rx_buffer)!=ids[num_dev_done]) num_dev_done++; - if(dyn_get_read_status_data(master->rx_buffer,data[num_dev_done].data_addr)!=length[num_dev_done])// not enough data - return DYN_INST_ERROR; - else + if((error=dyn_get_status_error(master->rx_buffer))==DYN_SUCCESS) { - num_dev_done++; - return DYN_BUSY; + while(dyn_get_read_status_id(master->rx_buffer)!=ids[num_dev_done]) num_dev_done++; + if(dyn_get_read_status_data(master->rx_buffer,(unsigned char *)data[num_dev_done])!=length[num_dev_done])// not enough data + return DYN_INST_ERROR; + else + { + num_dev_done++; + return DYN_BUSY; + } } + else + return error; } else - return error; + return DYN_CHECKSUM_ERROR; } else - return DYN_CHECKSUM_ERROR; - } - else - { - if(dyn2_check_checksum(master->rx_buffer)==0x01) { - if((error=dyn2_get_status_error(master->rx_buffer))==DYN_SUCCESS) + if(dyn2_check_checksum(master->rx_buffer)==0x01) { - while(dyn2_get_read_status_id(master->rx_buffer)!=ids[num_dev_done]) num_dev_done++; - if(dyn2_get_read_status_data(master->rx_buffer,data[num_dev_done].data_addr)!=length[num_dev_done])// not enough data - return DYN_INST_ERROR; - else + if((error=dyn2_get_status_error(master->rx_buffer))==DYN_SUCCESS) { - num_dev_done++; - return DYN_BUSY; + while(dyn2_get_read_status_id(master->rx_buffer)!=ids[num_dev_done]) num_dev_done++; + if(dyn2_get_read_status_data(master->rx_buffer,(unsigned char *)data[num_dev_done])!=length[num_dev_done])// not enough data + return DYN_INST_ERROR; + else + { + num_dev_done++; + return DYN_BUSY; + } } + else + return error; } else - return error; + return DYN_CHECKSUM_ERROR; } - else - return DYN_CHECKSUM_ERROR; } + else + return error; } else - return error; + return DYN_SUCCESS; } else return DYN_SUCCESS; } else - return DYN_SUCCESS; + return error; } - else + else return error; } @@ -642,9 +723,9 @@ unsigned char dyn_master_is_bulk_read_done(TDynamixelMaster *master,unsigned cha void dyn_master_init(TDynamixelMaster *master,TComm *dev,TDynVersion version) { /* assign communication functions */ - dev->irq_receive_cb=dyn_master_irq_receive_cb; - dev->dma_send_cb=dyn_master_dma_send_cb; - dev->dma_receive_cb=dyn_master_dma_receive_cb; + dev->irq_receive_cb=(unsigned char (*)(void *,unsigned char))dyn_master_irq_receive_cb; + dev->dma_send_cb=(unsigned char (*)(void *))dyn_master_dma_send_cb; + dev->dma_receive_cb=(unsigned char (*)(void *))dyn_master_dma_receive_cb; master->comm_dev=dev; master->version=version; dev->data=master; @@ -664,17 +745,22 @@ void dyn_master_init(TDynamixelMaster *master,TComm *dev,TDynVersion version) void dyn_master_set_rx_timeout(TDynamixelMaster *master,unsigned short int timeout_ms) { - master->rx_timeout_ms=timeout_ms; + if(master!=0x00000000) + master->rx_timeout_ms=timeout_ms; } -inline void dyn_master_set_return_level(TDynamixelMaster *master,return_level_t level) +void dyn_master_set_return_level(TDynamixelMaster *master,return_level_t level) { - master->return_level=level; + if(master!=0x00000000) + master->return_level=level; } -inline return_level_t dyn_master_get_return_level(TDynamixelMaster *master) +return_level_t dyn_master_get_return_level(TDynamixelMaster *master) { - return master->return_level; + if(master!=0x00000000) + return master->return_level; + else + return return_all; } void dyn_master_scan(TDynamixelMaster *master,unsigned char *num,unsigned char *ids) @@ -682,43 +768,49 @@ void dyn_master_scan(TDynamixelMaster *master,unsigned char *num,unsigned char * unsigned char i; *num=0; - for(i=0;i<254;i++) + if(master!=0x00000000) { - if(dyn_master_ping(master,i)==DYN_SUCCESS)// the device exists + for(i=0;i<254;i++) { - ids[*num]=i; - (*num)++; + if(dyn_master_ping(master,i)==DYN_SUCCESS)// the device exists + { + ids[*num]=i; + (*num)++; + } } } } unsigned char dyn_master_ping(TDynamixelMaster *master,unsigned char id) { - unsigned char error; + unsigned char error=DYN_COMM_ERROR; - // generate the ping packet for the desired device - if(master->version==DYN_VER1) - dyn_init_ping_packet(master->tx_buffer,id); - else - dyn2_init_ping_packet(master->tx_buffer,id); - master->rx_num_packets=0x01; - master->rx_no_answer=0x00; - // enable transmission - master->set_tx_mode(); - // send the data - if((error=dyn_master_send(master))!=DYN_SUCCESS) - { - master->set_rx_mode(); - return error; - } - // wait for the transmission to end - if((error=dyn_master_wait_transmission(master))!=DYN_SUCCESS) + if(master!=0x00000000) { - master->set_rx_mode(); - return error; + // generate the ping packet for the desired device + if(master->version==DYN_VER1) + dyn_init_ping_packet(master->tx_buffer,id); + else + dyn2_init_ping_packet(master->tx_buffer,id); + master->rx_num_packets=0x01; + master->rx_no_answer=0x00; + // enable transmission + master->set_tx_mode(); + // send the data + if((error=dyn_master_send(master))!=DYN_SUCCESS) + { + master->set_rx_mode(); + return error; + } + // wait for the transmission to end + if((error=dyn_master_wait_transmission(master))!=DYN_SUCCESS) + { + master->set_rx_mode(); + return error; + } + // wait for the replay within the given timeout + error=dyn_master_wait_reception(master); } - // wait for the replay within the given timeout - error=dyn_master_wait_reception(master); return error; } @@ -742,49 +834,52 @@ unsigned char dyn_master_read_word(TDynamixelMaster *master,unsigned char id,uns unsigned char dyn_master_read_table(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned short int length,unsigned char *data) { - unsigned char error; + unsigned char error=DYN_COMM_ERROR; - // generate the read packet for the desired device - if(master->version==DYN_VER1) - dyn_init_read_packet(master->tx_buffer,id,address,length); - else - dyn2_init_read_packet(master->tx_buffer,id,address,length); - master->rx_num_packets=0x01; - if(master->return_level==no_return || id==DYN_BROADCAST_ID) - master->rx_no_answer=0x01; - else - master->rx_no_answer=0x00; - // enable transmission - master->set_tx_mode(); - // send the data - if((error=dyn_master_send(master))!=DYN_SUCCESS) + if(master!=0x00000000) { - master->set_rx_mode(); - return error; - } - // wait for the transmission to end - if((error=dyn_master_wait_transmission(master))!=DYN_SUCCESS) - { - master->set_rx_mode(); - return error; - } - // wait for the replay within the given timeout - if(master->return_level!=no_return && id!=DYN_BROADCAST_ID) - { - if((error=dyn_master_wait_reception(master))==DYN_SUCCESS) + // generate the read packet for the desired device + if(master->version==DYN_VER1) + dyn_init_read_packet(master->tx_buffer,id,address,length); + else + dyn2_init_read_packet(master->tx_buffer,id,address,length); + master->rx_num_packets=0x01; + if(master->return_level==no_return || id==DYN_BROADCAST_ID) + master->rx_no_answer=0x01; + else + master->rx_no_answer=0x00; + // enable transmission + master->set_tx_mode(); + // send the data + if((error=dyn_master_send(master))!=DYN_SUCCESS) + { + master->set_rx_mode(); + return error; + } + // wait for the transmission to end + if((error=dyn_master_wait_transmission(master))!=DYN_SUCCESS) { - if(master->version==DYN_VER1) - { - if(dyn_get_read_status_data(master->rx_buffer,data)!=length)// not enough data - error=DYN_INST_ERROR; - } - else + master->set_rx_mode(); + return error; + } + // wait for the replay within the given timeout + if(master->return_level!=no_return && id!=DYN_BROADCAST_ID) + { + if((error=dyn_master_wait_reception(master))==DYN_SUCCESS) { - if(dyn2_get_read_status_data(master->rx_buffer,data)!=length)// not enough data - error=DYN_INST_ERROR; + if(master->version==DYN_VER1) + { + if(dyn_get_read_status_data(master->rx_buffer,data)!=length)// not enough data + error=DYN_INST_ERROR; + } + else + { + if(dyn2_get_read_status_data(master->rx_buffer,data)!=length)// not enough data + error=DYN_INST_ERROR; + } } - } - } + } + } return error; } @@ -805,140 +900,210 @@ unsigned char dyn_master_write_word(TDynamixelMaster *master,unsigned char id, u unsigned char dyn_master_write_table(TDynamixelMaster *master,unsigned char id, unsigned short int address, unsigned short int length, unsigned char *data) { - unsigned char error; + unsigned char error=DYN_COMM_ERROR; - // generate the write packet for the desired device - if(master->version==DYN_VER1) - dyn_init_write_packet(master->tx_buffer,id,address,length,data); - else - dyn2_init_write_packet(master->tx_buffer,id,address,length,data); - master->rx_num_packets=0x01; - if(master->return_level==return_all && id!=DYN_BROADCAST_ID) - master->rx_no_answer=0x00; - else - master->rx_no_answer=0x01; - // enable transmission - master->set_tx_mode(); - // send the data - if((error=dyn_master_send(master))!=DYN_SUCCESS) - { - master->set_rx_mode(); - return error; - } - // wait for the transmission to end - if((error=dyn_master_wait_transmission(master))!=DYN_SUCCESS) + if(master!=0x00000000) { - master->set_rx_mode(); - return error; + // generate the write packet for the desired device + if(master->version==DYN_VER1) + dyn_init_write_packet(master->tx_buffer,id,address,length,data); + else + dyn2_init_write_packet(master->tx_buffer,id,address,length,data); + master->rx_num_packets=0x01; + if(master->return_level==return_all && id!=DYN_BROADCAST_ID) + master->rx_no_answer=0x00; + else + master->rx_no_answer=0x01; + // enable transmission + master->set_tx_mode(); + // send the data + if((error=dyn_master_send(master))!=DYN_SUCCESS) + { + master->set_rx_mode(); + return error; + } + // wait for the transmission to end + if((error=dyn_master_wait_transmission(master))!=DYN_SUCCESS) + { + master->set_rx_mode(); + return error; + } + // wait for the replay within the given timeout + if(master->return_level==return_all && id!=DYN_BROADCAST_ID) + error=dyn_master_wait_reception(master); } - // wait for the replay within the given timeout - if(master->return_level==return_all && id!=DYN_BROADCAST_ID) - error=dyn_master_wait_reception(master); return error; } unsigned char dyn_master_reg_write(TDynamixelMaster *master,unsigned char id, unsigned short int address, unsigned short int length, unsigned char *data) { - unsigned char error; + unsigned char error=DYN_COMM_ERROR; - // generate the registered write packet for the desired device - if(master->version==DYN_VER1) - dyn_init_reg_write_packet(master->tx_buffer,id,address,length,data); - else - dyn2_init_reg_write_packet(master->tx_buffer,id,address,length,data); - master->rx_num_packets=0x01; - if(master->return_level==return_all && id!=DYN_BROADCAST_ID) - master->rx_no_answer=0x00; - else - master->rx_no_answer=0x01; - // enable transmission - master->set_tx_mode(); - // send the data - if((error=dyn_master_send(master))!=DYN_SUCCESS) - { - master->set_rx_mode(); - return error; - } - // wait for the transmission to end - if((error=dyn_master_wait_transmission(master))!=DYN_SUCCESS) + if(master!=0x00000000) { - master->set_rx_mode(); - return error; + // generate the registered write packet for the desired device + if(master->version==DYN_VER1) + dyn_init_reg_write_packet(master->tx_buffer,id,address,length,data); + else + dyn2_init_reg_write_packet(master->tx_buffer,id,address,length,data); + master->rx_num_packets=0x01; + if(master->return_level==return_all && id!=DYN_BROADCAST_ID) + master->rx_no_answer=0x00; + else + master->rx_no_answer=0x01; + // enable transmission + master->set_tx_mode(); + // send the data + if((error=dyn_master_send(master))!=DYN_SUCCESS) + { + master->set_rx_mode(); + return error; + } + // wait for the transmission to end + if((error=dyn_master_wait_transmission(master))!=DYN_SUCCESS) + { + master->set_rx_mode(); + return error; + } + // wait for the replay within the given timeout + if(master->return_level==return_all && id!=DYN_BROADCAST_ID) + error=dyn_master_wait_reception(master); } - // wait for the replay within the given timeout - if(master->return_level==return_all && id!=DYN_BROADCAST_ID) - error=dyn_master_wait_reception(master); return error; } unsigned char dyn_master_action(TDynamixelMaster *master) { - unsigned char error; + unsigned char error=DYN_COMM_ERROR; - // generate the action packet for the desired device - if(master->version==DYN_VER1) - dyn_init_action_packet(master->tx_buffer); - else - dyn2_init_action_packet(master->tx_buffer); - master->rx_num_packets=0x01; - master->rx_no_answer=0x01; - // enable transmission - master->set_tx_mode(); - // send the data - if((error=dyn_master_send(master))!=DYN_SUCCESS) - { - master->set_rx_mode(); - return error; - } - // wait for the transmission to end - if((error=dyn_master_wait_transmission(master))!=DYN_SUCCESS) + if(master!=0x00000000) { - master->set_rx_mode(); - return error; + // generate the action packet for the desired device + if(master->version==DYN_VER1) + dyn_init_action_packet(master->tx_buffer); + else + dyn2_init_action_packet(master->tx_buffer); + master->rx_num_packets=0x01; + master->rx_no_answer=0x01; + // enable transmission + master->set_tx_mode(); + // send the data + if((error=dyn_master_send(master))!=DYN_SUCCESS) + { + master->set_rx_mode(); + return error; + } + // wait for the transmission to end + if((error=dyn_master_wait_transmission(master))!=DYN_SUCCESS) + { + master->set_rx_mode(); + return error; + } } return error; } -unsigned char dyn_master_sync_write(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, TWriteData *data) +unsigned char dyn_master_sync_write(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, unsigned char * const data[]) { - unsigned char error; + unsigned char error=DYN_COMM_ERROR; - // generate the write packet for the desired device - if(master->version==DYN_VER1) - dyn_init_sync_write_packet(master->tx_buffer,num,ids,address,length,data); - else - dyn2_init_sync_write_packet(master->tx_buffer,num,ids,address,length,data); - master->rx_num_packets=0x01; - master->rx_no_answer=0x01; - // enable transmission - master->set_tx_mode(); - // send the data - if((error=dyn_master_send(master))!=DYN_SUCCESS) + if(master!=0x00000000) { - master->set_rx_mode(); - return error; + // generate the write packet for the desired device + if(master->version==DYN_VER1) + dyn_init_sync_write_packet(master->tx_buffer,num,ids,address,length,data); + else + dyn2_init_sync_write_packet(master->tx_buffer,num,ids,address,length,data); + master->rx_num_packets=0x01; + master->rx_no_answer=0x01; + // enable transmission + master->set_tx_mode(); + // send the data + if((error=dyn_master_send(master))!=DYN_SUCCESS) + { + master->set_rx_mode(); + return error; + } + // wait for the transmission to end + if((error=dyn_master_wait_transmission(master))!=DYN_SUCCESS) + { + master->set_rx_mode(); + return error; + } } - // wait for the transmission to end - if((error=dyn_master_wait_transmission(master))!=DYN_SUCCESS) + + return error; +} + +unsigned char dyn_master_sync_read(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, unsigned char * const data[]) +{ + unsigned char error=DYN_SUCCESS,i; + + if(master!=0x00000000) { - master->set_rx_mode(); - return error; + // generate the read packet for the desired device + if(master->version==DYN_VER2) + { + dyn2_init_sync_read_packet(master->tx_buffer,num,ids,address,length); + master->rx_num_packets=num; + if(master->return_level==no_return) + master->rx_no_answer=0x01; + else + master->rx_no_answer=0x00; + // enable transmission + master->set_tx_mode(); + // send the data + if((error=dyn_master_send(master))!=DYN_SUCCESS) + { + master->set_rx_mode(); + return error; + } + // wait for the transmission to end + if((error=dyn_master_wait_transmission(master))!=DYN_SUCCESS) + { + master->set_rx_mode(); + return error; + } + // wait for the replay within the given timeout + if(master->return_level!=no_return) + { + for(i=0;i<num;i++) + { + if((error=dyn_master_wait_reception(master))==DYN_SUCCESS) + { + while(dyn2_get_read_status_id(master->rx_buffer)!=ids[i]) i++; + if(dyn2_get_read_status_data(master->rx_buffer,(unsigned char *)data[i])!=length)// not enough data + error=DYN_INST_ERROR; + } + } + } + } } return error; } -unsigned char dyn_master_sync_read(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, TWriteData *data) +unsigned char dyn_master_bulk_read(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, unsigned char * const data[]) { - unsigned char error=DYN_SUCCESS,i; + unsigned char error=DYN_COMM_ERROR,i,ver1_address[255],ver1_length[255]; - // generate the read packet for the desired device - if(master->version==DYN_VER2) + if(master!=0x00000000) { - dyn2_init_sync_read_packet(master->tx_buffer,num,ids,address,length); + // generate the read packet for the desired device + if(master->version==DYN_VER1) + { + for(i=0;i<num;i++) + { + ver1_address[i]=address[i]; + ver1_length[i]=length[i]; + } + dyn_init_bulk_read_packet(master->tx_buffer,num,ids,ver1_address,ver1_length); + } + else + dyn2_init_bulk_read_packet(master->tx_buffer,num,ids,address,length); master->rx_num_packets=num; if(master->return_level==no_return) master->rx_no_answer=0x01; @@ -965,9 +1130,18 @@ unsigned char dyn_master_sync_read(TDynamixelMaster *master,unsigned char num,un { if((error=dyn_master_wait_reception(master))==DYN_SUCCESS) { - while(dyn2_get_read_status_id(master->rx_buffer)!=ids[i]) i++; - if(dyn2_get_read_status_data(master->rx_buffer,data[i].data_addr)!=length)// not enough data - error=DYN_INST_ERROR; + if(master->version==DYN_VER1) + { + while(dyn_get_read_status_id(master->rx_buffer)!=ids[i]) i++; + if(dyn_get_read_status_data(master->rx_buffer,(unsigned char *)data[i])!=length[i])// not enough data + error=DYN_INST_ERROR; + } + else + { + while(dyn2_get_read_status_id(master->rx_buffer)!=ids[i]) i++; + if(dyn2_get_read_status_data(master->rx_buffer,(unsigned char *)data[i])!=length[i])// not enough data + error=DYN_INST_ERROR; + } } } } @@ -976,60 +1150,31 @@ unsigned char dyn_master_sync_read(TDynamixelMaster *master,unsigned char num,un return error; } -unsigned char dyn_master_bulk_read(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, TWriteData *data) +unsigned char dyn_master_bulk_write(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, unsigned char * const data[]) { - unsigned char error,i,ver1_address[255],ver1_length[255]; + unsigned char error=DYN_SUCCESS; - // generate the read packet for the desired device - if(master->version==DYN_VER1) + if(master!=0x00000000) { - for(i=0;i<num;i++) + // generate the write packet for the desired device + if(master->version==DYN_VER2) { - ver1_address[i]=address[i]; - ver1_length[i]=length[i]; - } - dyn_init_bulk_read_packet(master->tx_buffer,num,ids,ver1_address,ver1_length); - } - else - dyn2_init_bulk_read_packet(master->tx_buffer,num,ids,address,length); - master->rx_num_packets=num; - if(master->return_level==no_return) - master->rx_no_answer=0x01; - else - master->rx_no_answer=0x00; - // enable transmission - master->set_tx_mode(); - // send the data - if((error=dyn_master_send(master))!=DYN_SUCCESS) - { - master->set_rx_mode(); - return error; - } - // wait for the transmission to end - if((error=dyn_master_wait_transmission(master))!=DYN_SUCCESS) - { - master->set_rx_mode(); - return error; - } - // wait for the replay within the given timeout - if(master->return_level!=no_return) - { - for(i=0;i<num;i++) - { - if((error=dyn_master_wait_reception(master))==DYN_SUCCESS) + dyn2_init_bulk_write_packet(master->tx_buffer,num,ids,address,length,data); + master->rx_num_packets=0x01; + master->rx_no_answer=0x01; + // enable transmission + master->set_tx_mode(); + // send the data + if((error=dyn_master_send(master))!=DYN_SUCCESS) { - if(master->version==DYN_VER1) - { - while(dyn_get_read_status_id(master->rx_buffer)!=ids[i]) i++; - if(dyn_get_read_status_data(master->rx_buffer,data[i].data_addr)!=length[i])// not enough data - error=DYN_INST_ERROR; - } - else - { - while(dyn2_get_read_status_id(master->rx_buffer)!=ids[i]) i++; - if(dyn2_get_read_status_data(master->rx_buffer,data[i].data_addr)!=length[i])// not enough data - error=DYN_INST_ERROR; - } + master->set_rx_mode(); + return error; + } + // wait for the transmission to end + if((error=dyn_master_wait_transmission(master))!=DYN_SUCCESS) + { + master->set_rx_mode(); + return error; } } } @@ -1037,16 +1182,15 @@ unsigned char dyn_master_bulk_read(TDynamixelMaster *master,unsigned char num,un return error; } -unsigned char dyn_master_bulk_write(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, TWriteData *data) +unsigned char dyn_master_relay(TDynamixelMaster *master,unsigned char *inst_pkt,unsigned char *status_pkt) { - unsigned char error=DYN_SUCCESS; + unsigned char error=DYN_COMM_ERROR; - // generate the write packet for the desired device - if(master->version==DYN_VER2) + if(master!=0x00000000) { - dyn2_init_bulk_write_packet(master->tx_buffer,num,ids,address,length,data); + dyn2_copy_packet(inst_pkt,master->tx_buffer); master->rx_num_packets=0x01; - master->rx_no_answer=0x01; + master->rx_no_answer=0x00; // enable transmission master->set_tx_mode(); // send the data @@ -1061,35 +1205,10 @@ unsigned char dyn_master_bulk_write(TDynamixelMaster *master,unsigned char num,u master->set_rx_mode(); return error; } + // wait for the replay within the given timeout + error=dyn_master_wait_reception(master); + dyn2_copy_packet(master->rx_buffer,status_pkt); } return error; } - -unsigned char dyn_master_relay(TDynamixelMaster *master,unsigned char *inst_pkt,unsigned char *status_pkt) -{ - unsigned char error; - - dyn2_copy_packet(inst_pkt,master->tx_buffer); - master->rx_num_packets=0x01; - master->rx_no_answer=0x00; - // enable transmission - master->set_tx_mode(); - // send the data - if((error=dyn_master_send(master))!=DYN_SUCCESS) - { - master->set_rx_mode(); - return error; - } - // wait for the transmission to end - if((error=dyn_master_wait_transmission(master))!=DYN_SUCCESS) - { - master->set_rx_mode(); - return error; - } - // wait for the replay within the given timeout - error=dyn_master_wait_reception(master); - dyn2_copy_packet(master->rx_buffer,status_pkt); - - return error; -} diff --git a/dynamixel_base/src/dynamixel_slave.c b/dynamixel_base/src/dynamixel_slave.c index dc89a3539335ce1faad6c0773ece97399a7c5f33..e1818981fa23d44bc06a1f2ae9d8159010b2acab 100644 --- a/dynamixel_base/src/dynamixel_slave.c +++ b/dynamixel_base/src/dynamixel_slave.c @@ -1,97 +1,138 @@ #include "dynamixel_slave.h" +/* EEPROM data */ +unsigned short int dyn_slave_eeprom_data[] __attribute__ ((section (".eeprom")))={DEFAULT_DEVICE_MODEL&0x00FF,DEVICE_MODEL, + (DEFAULT_DEVICE_MODEL>>8)&0x00FF,DEVICE_MODEL+1, + DEFAULT_FIRMWARE_VERSION,FIRMWARE_VERSION, + DEFAULT_DEVICE_ID,DEVICE_ID, + DEFAULT_BAUDRATE,BAUDRATE, + DEFAULT_RETURN_DELAY,RETURN_DELAY, + DEFAULT_RETURN_LEVEL,RETURN_LEVEL}; + /* private functions */ -unsigned char dyn_slave_irq_receive_cb(void *dyn_slave,unsigned char byte) +void dyn_slave_write_cmd(TDynamixelSlave *dyn_slave,unsigned short int address,unsigned short int length,unsigned char *data) +{ + if(ram_in_range(DEVICE_ID,address,length)) + { + dyn_slave_set_address(dyn_slave,data[DEVICE_ID-address]); + ram_data[DEVICE_ID]=data[DEVICE_ID-address]; + } + if(ram_in_range(BAUDRATE,address,length)) + { + dyn_slave_set_baudrate(dyn_slave,2000000/(data[BAUDRATE-address]+1)); + ram_data[BAUDRATE]=data[BAUDRATE-address]; + } + if(ram_in_range(RETURN_DELAY,address,length)) + { + dyn_slave_set_return_delay(dyn_slave,data[RETURN_DELAY-address]); + ram_data[RETURN_DELAY]=data[RETURN_DELAY-address]; + } + if(ram_in_range(RETURN_LEVEL,address,length)) + { + dyn_slave_set_return_level(dyn_slave,data[RETURN_LEVEL-address]); + ram_data[RETURN_LEVEL]=data[RETURN_LEVEL-address]; + } +} + +void dyn_slave_read_cmd(TDynamixelSlave *dyn_slave,unsigned short int address,unsigned short int length,unsigned char *data) { - TDynamixelSlave *dyn=(TDynamixelSlave *)dyn_slave; + ram_read_table(address,length,data); +} - if(dyn->comm_dev->time!=0x00000000) - time_set_timeout(dyn->comm_dev->time,dyn->rx_timeout_ms*1000); - switch(dyn->received_bytes) +unsigned char dyn_slave_irq_receive_cb(TDynamixelSlave *dyn_slave,unsigned char byte) +{ + if(dyn_slave!=0x00000000) { - case 0: if(byte==0xFF) - { - dyn->rx_buffer[dyn->received_bytes]=byte; - dyn->received_bytes++; - } - break; - case 1: if(byte==0xFF) - { - dyn->rx_buffer[dyn->received_bytes]=byte; - dyn->received_bytes++; - } - else - dyn->received_bytes--; - break; - case 2: if(byte==0xFD)// version 2 header - { - if(dyn->version==DYN_VER2)// the module is configured for version 2 + if(dyn_slave->comm_dev->time!=0x00000000) + time_set_timeout(dyn_slave->comm_dev->time,dyn_slave->rx_timeout_ms*1000); + switch(dyn_slave->received_bytes) + { + case 0: if(byte==0xFF) + { + dyn_slave->rx_buffer[dyn_slave->received_bytes]=byte; + dyn_slave->received_bytes++; + } + break; + case 1: if(byte==0xFF) + { + dyn_slave->rx_buffer[dyn_slave->received_bytes]=byte; + dyn_slave->received_bytes++; + } + else + dyn_slave->received_bytes--; + break; + case 2: if(byte==0xFD)// version 2 header + { + if(dyn_slave->version==DYN_VER2)// the module is configured for version 2 + { + dyn_slave->rx_buffer[dyn_slave->received_bytes]=byte; + dyn_slave->received_bytes++; + } + else + dyn_slave->received_bytes=0;// ignore packet and restart synchronization + } + else if(byte!=0xFF) + { + dyn_slave->rx_buffer[dyn_slave->received_bytes]=byte; + dyn_slave->received_bytes++; + } + break; + case 3: dyn_slave->rx_buffer[dyn_slave->received_bytes]=byte; + if(dyn_slave->version==DYN_VER1) { - dyn->rx_buffer[dyn->received_bytes]=byte; - dyn->received_bytes++; + dyn_slave->op_length=byte; + dyn_slave->received_bytes=0; + /* finish reception by IRQ */ + comm_cancel_irq_receive(dyn_slave->comm_dev); + /* enable dma RX */ + comm_receive_dma(dyn_slave->comm_dev,&dyn_slave->rx_buffer[4],dyn_slave->op_length); } else - dyn->received_bytes=0;// ignore packet and restart synchronization - } - else if(byte!=0xFF) - { - dyn->rx_buffer[dyn->received_bytes]=byte; - dyn->received_bytes++; - } - break; - case 3: dyn->rx_buffer[dyn->received_bytes]=byte; - if(dyn->version==DYN_VER1) - { - dyn->op_length=byte; - dyn->received_bytes=0; + dyn_slave->received_bytes++; + break; + case 4: dyn_slave->rx_buffer[dyn_slave->received_bytes]=byte; + dyn_slave->received_bytes++; + break; + case 5: dyn_slave->rx_buffer[dyn_slave->received_bytes]=byte; + dyn_slave->received_bytes++; + dyn_slave->op_length=byte; + break; + case 6: dyn_slave->rx_buffer[dyn_slave->received_bytes]=byte; + dyn_slave->received_bytes++; + dyn_slave->op_length+=(byte<<8); + dyn_slave->received_bytes=0; /* finish reception by IRQ */ - comm_cancel_irq_receive(dyn->comm_dev); + comm_cancel_irq_receive(dyn_slave->comm_dev); /* enable dma RX */ - comm_receive_dma(dyn->comm_dev,&dyn->rx_buffer[4],dyn->op_length); - } - else - dyn->received_bytes++; - break; - case 4: dyn->rx_buffer[dyn->received_bytes]=byte; - dyn->received_bytes++; - break; - case 5: dyn->rx_buffer[dyn->received_bytes]=byte; - dyn->received_bytes++; - dyn->op_length=byte; - break; - case 6: dyn->rx_buffer[dyn->received_bytes]=byte; - dyn->received_bytes++; - dyn->op_length+=(byte<<8); - dyn->received_bytes=0; - /* finish reception by IRQ */ - comm_cancel_irq_receive(dyn->comm_dev); - /* enable dma RX */ - comm_receive_dma(dyn->comm_dev,&dyn->rx_buffer[7],dyn->op_length); - break; - default: break; + comm_receive_dma(dyn_slave->comm_dev,&dyn_slave->rx_buffer[7],dyn_slave->op_length); + break; + default: break; + } } return 0x00; } -unsigned char dyn_slave_dma_send_cb(void *dyn_slave) +unsigned char dyn_slave_dma_send_cb(TDynamixelSlave *dyn_slave) { - TDynamixelSlave *dyn=(TDynamixelSlave *)dyn_slave; - - // enable tx interrupts - dyn->set_rx_mode(); + if(dyn_slave!=0x00000000) + { + // enable tx interrupts + dyn_slave->set_rx_mode(); + } return 0x00; } -unsigned char dyn_slave_dma_receive_cb(void *dyn_slave) +unsigned char dyn_slave_dma_receive_cb(TDynamixelSlave *dyn_slave) { - TDynamixelSlave *dyn=(TDynamixelSlave *)dyn_slave; - - comm_receive_irq(dyn->comm_dev,0);// reenable reception by IRQ - if(dyn->comm_dev->time!=0x00000000) - time_cancel_timeout(dyn->comm_dev->time); - dyn->packet_ready=0x01; + if(dyn_slave!=0x00000000) + { + comm_receive_irq(dyn_slave->comm_dev,0);// reenable reception by IRQ + if(dyn_slave->comm_dev->time!=0x00000000) + time_cancel_timeout(dyn_slave->comm_dev->time); + dyn_slave->packet_ready=0x01; + } return 0x00; } @@ -106,6 +147,11 @@ void dummy_dyn_slave_set_rx_mode(void) } +void dummy_dyn_slave_set_baudrate(TComm *comm_dev,unsigned int baudrate) +{ + +} + void dummy_dyn_slave_on_ping(void) { @@ -133,28 +179,31 @@ unsigned char dummy_dyn_slave_on_relay(TDynVersion version,unsigned char *inst_p void dyn_slave_send_status_packet(TDynamixelSlave *slave,unsigned char error,unsigned short int length, unsigned char *data) { - // wait until the previous transmission has ended (if any) - while(comm_is_send_done(slave->comm_dev)==COMM_BUSY); - if(slave->return_delay>0) - if(slave->comm_dev->time!=0x00000000) - time_delay_us(slave->comm_dev->time,slave->return_delay<<1); - if(slave->version==DYN_VER1) - { - // create the status packet - dyn_init_status_packet(slave->tx_buffer,slave->address,error,length,data); - // set the tx mode, if necessary - slave->set_tx_mode(); - // start transmission by DMA - comm_send_dma(slave->comm_dev,slave->tx_buffer,dyn_get_length(slave->tx_buffer)+4); - } - else + if(slave!=0x00000000) { - // create the status packet - dyn2_init_status_packet(slave->tx_buffer,slave->address,error,length,data); - // set the tx mode, if necessary - slave->set_tx_mode(); - // start transmission by DMA - comm_send_dma(slave->comm_dev,slave->tx_buffer,dyn2_get_length(slave->tx_buffer)+7); + // wait until the previous transmission has ended (if any) + while(comm_is_send_done(slave->comm_dev)==COMM_BUSY); + if(slave->return_delay>0) + if(slave->comm_dev->time!=0x00000000) + time_delay_us(slave->comm_dev->time,slave->return_delay<<1); + if(slave->version==DYN_VER1) + { + // create the status packet + dyn_init_status_packet(slave->tx_buffer,slave->address,error,length,data); + // set the tx mode, if necessary + slave->set_tx_mode(); + // start transmission by DMA + comm_send_dma(slave->comm_dev,slave->tx_buffer,dyn_get_length(slave->tx_buffer)+4); + } + else + { + // create the status packet + dyn2_init_status_packet(slave->tx_buffer,slave->address,error,length,data); + // set the tx mode, if necessary + slave->set_tx_mode(); + // start transmission by DMA + comm_send_dma(slave->comm_dev,slave->tx_buffer,dyn2_get_length(slave->tx_buffer)+7); + } } } @@ -162,114 +211,117 @@ void dyn_v1_slave_loop(TDynamixelSlave *slave) { static unsigned char data[MAX_DYN_SLAVE_TX_BUFFER_LEN],error,length,address,prev_id,id; - id=dyn_get_id(slave->rx_buffer); - if(id==slave->address || id==DYN_BROADCAST_ID)// the packet is addressed to this device or it is a broadcast + if(slave!=0x000000000) { - // check the packet checksum - if(dyn_check_checksum(slave->rx_buffer)==0xFF)// the incomming packet is okay + id=dyn_get_id(slave->rx_buffer); + if(id==slave->address || id==DYN_BROADCAST_ID)// the packet is addressed to this device or it is a broadcast { - // process the packet - switch(dyn_get_instruction(slave->rx_buffer)) + // check the packet checksum + if(dyn_check_checksum(slave->rx_buffer)==0xFF)// the incomming packet is okay { - case DYN_PING: slave->on_ping(); - if(id!=DYN_BROADCAST_ID) - dyn_slave_send_status_packet(slave,DYN_NO_ERROR,0,data); - break; - case DYN_READ: error=slave->on_read(dyn_get_read_address(slave->rx_buffer),dyn_get_read_length(slave->rx_buffer),data); - if(slave->return_level!=no_return && id!=DYN_BROADCAST_ID) - { - if(error==DYN_NO_ERROR) - dyn_slave_send_status_packet(slave,DYN_NO_ERROR,dyn_get_read_length(slave->rx_buffer),data); - else - dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data); - } - break; - case DYN_WRITE: length=dyn_get_write_data(slave->rx_buffer,data); - error=slave->on_write(dyn_get_write_address(slave->rx_buffer),length,data); - if(slave->return_level==return_all && id!=DYN_BROADCAST_ID) - { - if(error==DYN_NO_ERROR) - dyn_slave_send_status_packet(slave,DYN_NO_ERROR,0,data); - else - dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data); - } - break; - case DYN_REG_WRITE: slave->reg_length=dyn_get_reg_write_data(slave->rx_buffer,slave->reg_buffer); - slave->reg_address=dyn_get_reg_write_address(slave->rx_buffer); - if(slave->return_level==return_all && id!=DYN_BROADCAST_ID) - dyn_slave_send_status_packet(slave,DYN_NO_ERROR,0,data); - break; - case DYN_ACTION: if(slave->reg_address!=0xFFFF) + // process the packet + switch(dyn_get_instruction(slave->rx_buffer)) + { + case DYN_PING: slave->on_ping(); + if(id!=DYN_BROADCAST_ID) + dyn_slave_send_status_packet(slave,DYN_NO_ERROR,0,data); + break; + case DYN_READ: error=slave->on_read(dyn_get_read_address(slave->rx_buffer),dyn_get_read_length(slave->rx_buffer),data); + if(slave->return_level!=no_return && id!=DYN_BROADCAST_ID) { - error=slave->on_write(slave->reg_address,slave->reg_length,slave->reg_buffer); - slave->reg_address=0xFFFF; - } - else - if(slave->return_level==return_all && id!=DYN_BROADCAST_ID) + if(error==DYN_NO_ERROR) + dyn_slave_send_status_packet(slave,DYN_NO_ERROR,dyn_get_read_length(slave->rx_buffer),data); + else dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data); + } break; - case DYN_RESET: - break; - case DYN_SYNC_READ: dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data); - break; - case DYN_SYNC_WRITE: if(dyn_sync_write_id_present(slave->rx_buffer,slave->address,&address,&length,data))// the device is addressed - error=slave->on_write(address,length,data); - break; - case DYN_BULK_READ: prev_id=dyn_bulk_read_id_present(slave->rx_buffer,slave->address,&address,&length); - if(prev_id!=0xFF) - { - if(prev_id==0x00)// first device to answer - { - error=slave->on_read(address,length,data); - if(error==DYN_NO_ERROR) - dyn_slave_send_status_packet(slave,DYN_NO_ERROR,length,data); - else - dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data); - } - else// wait for the previous device in the sequence to send its data + case DYN_WRITE: length=dyn_get_write_data(slave->rx_buffer,data); + error=slave->on_write(dyn_get_write_address(slave->rx_buffer),length,data); + if(slave->return_level==return_all && id!=DYN_BROADCAST_ID) + { + if(error==DYN_NO_ERROR) + dyn_slave_send_status_packet(slave,DYN_NO_ERROR,0,data); + else + dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data); + } + break; + case DYN_REG_WRITE: slave->reg_length=dyn_get_reg_write_data(slave->rx_buffer,slave->reg_buffer); + slave->reg_address=dyn_get_reg_write_address(slave->rx_buffer); + if(slave->return_level==return_all && id!=DYN_BROADCAST_ID) + dyn_slave_send_status_packet(slave,DYN_NO_ERROR,0,data); + break; + case DYN_ACTION: if(slave->reg_address!=0xFFFF) + { + error=slave->on_write(slave->reg_address,slave->reg_length,slave->reg_buffer); + slave->reg_address=0xFFFF; + } + else + if(slave->return_level==return_all && id!=DYN_BROADCAST_ID) + dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data); + break; + case DYN_RESET: + break; + case DYN_SYNC_READ: dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data); + break; + case DYN_SYNC_WRITE: if(dyn_sync_write_id_present(slave->rx_buffer,slave->address,&address,&length,data))// the device is addressed + error=slave->on_write(address,length,data); + break; + case DYN_BULK_READ: prev_id=dyn_bulk_read_id_present(slave->rx_buffer,slave->address,&address,&length); + if(prev_id!=0xFF) { - slave->sync_bulk_address=address; - slave->sync_bulk_length=length; - slave->sync_bulk_prev_id=prev_id; - slave->bulk_read_pending=0x01; + if(prev_id==0x00)// first device to answer + { + error=slave->on_read(address,length,data); + if(error==DYN_NO_ERROR) + dyn_slave_send_status_packet(slave,DYN_NO_ERROR,length,data); + else + dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data); + } + else// wait for the previous device in the sequence to send its data + { + slave->sync_bulk_address=address; + slave->sync_bulk_length=length; + slave->sync_bulk_prev_id=prev_id; + slave->bulk_read_pending=0x01; + } } - } - break; - case DYN_BULK_WRITE: dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data); - break; - default: - break; + break; + case DYN_BULK_WRITE: dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data); + break; + default: + break; + } + } + else + { + // send a checksum error answer + if(dyn_get_id(slave->rx_buffer)!=DYN_BROADCAST_ID) + dyn_slave_send_status_packet(slave,DYN_CHECKSUM_ERROR,0,0x00); } } else { - // send a checksum error answer - if(dyn_get_id(slave->rx_buffer)!=DYN_BROADCAST_ID) - dyn_slave_send_status_packet(slave,DYN_CHECKSUM_ERROR,0,0x00); - } - } - else - { - if(slave->bulk_read_pending) - { - if(id==slave->sync_bulk_prev_id) + if(slave->bulk_read_pending) { - slave->bulk_read_pending=0x00; - error=slave->on_read(slave->sync_bulk_address,slave->sync_bulk_length,data); - if(error==DYN_NO_ERROR) - dyn_slave_send_status_packet(slave,DYN_NO_ERROR,slave->sync_bulk_length,data); - else - dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data); + if(id==slave->sync_bulk_prev_id) + { + slave->bulk_read_pending=0x00; + error=slave->on_read(slave->sync_bulk_address,slave->sync_bulk_length,data); + if(error==DYN_NO_ERROR) + dyn_slave_send_status_packet(slave,DYN_NO_ERROR,slave->sync_bulk_length,data); + else + dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data); + } } - } - else// the packet is addressed to another device, so relay it - { - if(slave->on_relay(slave->version,slave->rx_buffer,slave->tx_buffer)==DYN_SUCCESS) + else// the packet is addressed to another device, so relay it { - // set the tx mode, if necessary - slave->set_tx_mode(); - // start transmission by DMA - comm_send_dma(slave->comm_dev,slave->tx_buffer,dyn_get_length(slave->tx_buffer)+4); + if(slave->on_relay(slave->version,slave->rx_buffer,slave->tx_buffer)==DYN_SUCCESS) + { + // set the tx mode, if necessary + slave->set_tx_mode(); + // start transmission by DMA + comm_send_dma(slave->comm_dev,slave->tx_buffer,dyn_get_length(slave->tx_buffer)+4); + } } } } @@ -280,164 +332,167 @@ void dyn_v2_slave_loop(TDynamixelSlave *slave) static unsigned char data[MAX_DYN_SLAVE_TX_BUFFER_LEN],error,prev_id,id; static unsigned short int length,address; - id=dyn2_get_id(slave->rx_buffer); - if(id==slave->address || id==DYN_BROADCAST_ID)// the packet is addressed to this device or it is a broadcast + if(slave!=0x00000000) { - // check the packet checksum - if(dyn2_check_checksum(slave->rx_buffer)==0x01)// the incomming packet is okay + id=dyn2_get_id(slave->rx_buffer); + if(id==slave->address || id==DYN_BROADCAST_ID)// the packet is addressed to this device or it is a broadcast { - // process the packet - switch(dyn2_get_instruction(slave->rx_buffer)) + // check the packet checksum + if(dyn2_check_checksum(slave->rx_buffer)==0x01)// the incomming packet is okay { - case DYN_PING: slave->on_ping(); - if(id!=DYN_BROADCAST_ID) - dyn_slave_send_status_packet(slave,DYN_NO_ERROR,0,data); - break; - case DYN_READ: error=slave->on_read(dyn2_get_read_address(slave->rx_buffer),dyn2_get_read_length(slave->rx_buffer),data); - if(slave->return_level!=no_return && id!=DYN_BROADCAST_ID) - { - if(error==DYN_NO_ERROR) - dyn_slave_send_status_packet(slave,DYN_NO_ERROR,dyn2_get_read_length(slave->rx_buffer),data); - else - dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data); - } - break; - case DYN_WRITE: length=dyn2_get_write_data(slave->rx_buffer,data); - error=slave->on_write(dyn2_get_write_address(slave->rx_buffer),length,data); - if(slave->return_level==return_all && id!=DYN_BROADCAST_ID) - { - if(error==DYN_NO_ERROR) - dyn_slave_send_status_packet(slave,DYN_NO_ERROR,0,data); - else - dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data); - } - break; - case DYN_REG_WRITE: slave->reg_length=dyn2_get_reg_write_data(slave->rx_buffer,slave->reg_buffer); - slave->reg_address=dyn2_get_reg_write_address(slave->rx_buffer); - if(slave->return_level==return_all && id!=DYN_BROADCAST_ID) - dyn_slave_send_status_packet(slave,DYN_NO_ERROR,0,data); - break; - case DYN_ACTION: if(slave->reg_address!=0xFFFF) + // process the packet + switch(dyn2_get_instruction(slave->rx_buffer)) + { + case DYN_PING: slave->on_ping(); + if(id!=DYN_BROADCAST_ID) + dyn_slave_send_status_packet(slave,DYN_NO_ERROR,0,data); + break; + case DYN_READ: error=slave->on_read(dyn2_get_read_address(slave->rx_buffer),dyn2_get_read_length(slave->rx_buffer),data); + if(slave->return_level!=no_return && id!=DYN_BROADCAST_ID) { - error=slave->on_write(slave->reg_address,slave->reg_length,slave->reg_buffer); - slave->reg_address=0xFFFF; - } - else - if(slave->return_level==return_all && id!=DYN_BROADCAST_ID) + if(error==DYN_NO_ERROR) + dyn_slave_send_status_packet(slave,DYN_NO_ERROR,dyn2_get_read_length(slave->rx_buffer),data); + else dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data); + } break; - case DYN_RESET: - break; - case DYN_SYNC_READ: prev_id=dyn2_sync_read_id_present(slave->rx_buffer,slave->address,&address,&length); - if(prev_id!=0xFF) - { - if(prev_id==0x00)// first device to answer - { - error=slave->on_read(address,length,data); - if(error==DYN_NO_ERROR) - dyn_slave_send_status_packet(slave,DYN_NO_ERROR,length,data); - else - dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data); - } - else// wait for the previous device in the sequence to send its data - { - slave->sync_bulk_address=address; - slave->sync_bulk_length=length; - slave->sync_bulk_prev_id=prev_id; - slave->sync_read_pending=0x01; - } - } - break; - case DYN_SYNC_WRITE: if(dyn2_sync_write_id_present(slave->rx_buffer,slave->address,&address,&length,data))// the device is addressed - error=slave->on_write(address,length,data); - break; - case DYN_BULK_READ: prev_id=dyn2_bulk_read_id_present(slave->rx_buffer,slave->address,&address,&length); - if(prev_id!=0xFF) - { - if(prev_id==0x00)// first device to answer + case DYN_WRITE: length=dyn2_get_write_data(slave->rx_buffer,data); + error=slave->on_write(dyn2_get_write_address(slave->rx_buffer),length,data); + if(slave->return_level==return_all && id!=DYN_BROADCAST_ID) + { + if(error==DYN_NO_ERROR) + dyn_slave_send_status_packet(slave,DYN_NO_ERROR,0,data); + else + dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data); + } + break; + case DYN_REG_WRITE: slave->reg_length=dyn2_get_reg_write_data(slave->rx_buffer,slave->reg_buffer); + slave->reg_address=dyn2_get_reg_write_address(slave->rx_buffer); + if(slave->return_level==return_all && id!=DYN_BROADCAST_ID) + dyn_slave_send_status_packet(slave,DYN_NO_ERROR,0,data); + break; + case DYN_ACTION: if(slave->reg_address!=0xFFFF) + { + error=slave->on_write(slave->reg_address,slave->reg_length,slave->reg_buffer); + slave->reg_address=0xFFFF; + } + else + if(slave->return_level==return_all && id!=DYN_BROADCAST_ID) + dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data); + break; + case DYN_RESET: + break; + case DYN_SYNC_READ: prev_id=dyn2_sync_read_id_present(slave->rx_buffer,slave->address,&address,&length); + if(prev_id!=0xFF) { - error=slave->on_read(address,length,data); - if(error==DYN_NO_ERROR) - dyn_slave_send_status_packet(slave,DYN_NO_ERROR,length,data); - else - dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data); + if(prev_id==0x00)// first device to answer + { + error=slave->on_read(address,length,data); + if(error==DYN_NO_ERROR) + dyn_slave_send_status_packet(slave,DYN_NO_ERROR,length,data); + else + dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data); + } + else// wait for the previous device in the sequence to send its data + { + slave->sync_bulk_address=address; + slave->sync_bulk_length=length; + slave->sync_bulk_prev_id=prev_id; + slave->sync_read_pending=0x01; + } } - else// wait for the previous device in the sequence to send its data + break; + case DYN_SYNC_WRITE: if(dyn2_sync_write_id_present(slave->rx_buffer,slave->address,&address,&length,data))// the device is addressed + error=slave->on_write(address,length,data); + break; + case DYN_BULK_READ: prev_id=dyn2_bulk_read_id_present(slave->rx_buffer,slave->address,&address,&length); + if(prev_id!=0xFF) { - slave->sync_bulk_address=address; - slave->sync_bulk_length=length; - slave->sync_bulk_prev_id=prev_id; - slave->bulk_read_pending=0x01; + if(prev_id==0x00)// first device to answer + { + error=slave->on_read(address,length,data); + if(error==DYN_NO_ERROR) + dyn_slave_send_status_packet(slave,DYN_NO_ERROR,length,data); + else + dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data); + } + else// wait for the previous device in the sequence to send its data + { + slave->sync_bulk_address=address; + slave->sync_bulk_length=length; + slave->sync_bulk_prev_id=prev_id; + slave->bulk_read_pending=0x01; + } } - } - break; - case DYN_BULK_WRITE: if(dyn2_bulk_write_id_present(slave->rx_buffer,slave->address,&address,&length,data)) - error=slave->on_write(address,length,data); - break; - - default: - break; + break; + case DYN_BULK_WRITE: if(dyn2_bulk_write_id_present(slave->rx_buffer,slave->address,&address,&length,data)) + error=slave->on_write(address,length,data); + break; + default: + break; + } + } + else + { + // send a checksum error answer + if(dyn_get_id(slave->rx_buffer)!=DYN_BROADCAST_ID) + dyn_slave_send_status_packet(slave,DYN_CHECKSUM_ERROR,0,0x00); } } else { - // send a checksum error answer - if(dyn_get_id(slave->rx_buffer)!=DYN_BROADCAST_ID) - dyn_slave_send_status_packet(slave,DYN_CHECKSUM_ERROR,0,0x00); - } - } - else - { - if(slave->bulk_read_pending) - { - if(id==slave->sync_bulk_prev_id) + if(slave->bulk_read_pending) { - slave->bulk_read_pending=0x00; - error=slave->on_read(slave->sync_bulk_address,slave->sync_bulk_length,data); - if(error==DYN_NO_ERROR) - dyn_slave_send_status_packet(slave,DYN_NO_ERROR,slave->sync_bulk_length,data); - else - dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data); + if(id==slave->sync_bulk_prev_id) + { + slave->bulk_read_pending=0x00; + error=slave->on_read(slave->sync_bulk_address,slave->sync_bulk_length,data); + if(error==DYN_NO_ERROR) + dyn_slave_send_status_packet(slave,DYN_NO_ERROR,slave->sync_bulk_length,data); + else + dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data); + } } - } - else if(slave->sync_read_pending) - { - if(id==slave->sync_bulk_prev_id) + else if(slave->sync_read_pending) { - slave->sync_read_pending=0x00; - error=slave->on_read(slave->sync_bulk_address,slave->sync_bulk_length,data); - if(error==DYN_NO_ERROR) - dyn_slave_send_status_packet(slave,DYN_NO_ERROR,slave->sync_bulk_length,data); - else - dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data); + if(id==slave->sync_bulk_prev_id) + { + slave->sync_read_pending=0x00; + error=slave->on_read(slave->sync_bulk_address,slave->sync_bulk_length,data); + if(error==DYN_NO_ERROR) + dyn_slave_send_status_packet(slave,DYN_NO_ERROR,slave->sync_bulk_length,data); + else + dyn_slave_send_status_packet(slave,DYN_INST_ERROR,0,data); + } } - } - else// the packet is addressed to another device, so relay it - { - if(slave->on_relay(slave->version,slave->rx_buffer,slave->tx_buffer)==DYN_SUCCESS) + else// the packet is addressed to another device, so relay it { - // set the tx mode, if necessary - slave->set_tx_mode(); - // start transmission by DMA - comm_send_dma(slave->comm_dev,slave->tx_buffer,dyn2_get_length(slave->tx_buffer)+8); + if(slave->on_relay(slave->version,slave->rx_buffer,slave->tx_buffer)==DYN_SUCCESS) + { + // set the tx mode, if necessary + slave->set_tx_mode(); + // start transmission by DMA + comm_send_dma(slave->comm_dev,slave->tx_buffer,dyn2_get_length(slave->tx_buffer)+8); + } } } } } /* public functions */ -void dyn_slave_init(TDynamixelSlave *slave,TComm *dev,unsigned char address,TDynVersion version) +unsigned char dyn_slave_init(TDynamixelSlave *slave,TMemory *memory,TComm *dev,unsigned char address,TDynVersion version) { /* assign communication functions */ - dev->irq_receive_cb=dyn_slave_irq_receive_cb; - dev->dma_send_cb=dyn_slave_dma_send_cb; - dev->dma_receive_cb=dyn_slave_dma_receive_cb; + dev->irq_receive_cb=(unsigned char (*)(void *,unsigned char))dyn_slave_irq_receive_cb; + dev->dma_send_cb=(unsigned char (*)(void *))dyn_slave_dma_send_cb; + dev->dma_receive_cb=(unsigned char (*)(void *))dyn_slave_dma_receive_cb; slave->comm_dev=dev; slave->version=version; dev->data=slave; /* initialize the internal callbacks */ slave->set_tx_mode=dummy_dyn_slave_set_tx_mode; slave->set_rx_mode=dummy_dyn_slave_set_rx_mode; + slave->set_baudrate=dummy_dyn_slave_set_baudrate; slave->on_ping=dummy_dyn_slave_on_ping; slave->on_read=dummy_dyn_slave_on_read; slave->on_write=dummy_dyn_slave_on_write; @@ -458,74 +513,112 @@ void dyn_slave_init(TDynamixelSlave *slave,TComm *dev,unsigned char address,TDyn slave->sync_read_pending=0x00; slave->bulk_read_pending=0x00; + /* initialize memory module */ + mem_module_init(&slave->mem_module); + slave->mem_module.data=slave; + slave->mem_module.write_cmd=(void(*)(void *,unsigned short int, unsigned short int,unsigned char *))dyn_slave_write_cmd; + slave->mem_module.read_cmd=(void(*)(void *,unsigned short int, unsigned short int,unsigned char *))dyn_slave_read_cmd; + if(!mem_module_add_eeprom_segment(&slave->mem_module,EEPROM_DYN_SLAVE_BASE_ADDRESS1,EEPROM_DYN_SLAVE_LENGTH1)) + return 0x00; + if(!mem_module_add_eeprom_segment(&slave->mem_module,EEPROM_DYN_SLAVE_BASE_ADDRESS2,EEPROM_DYN_SLAVE_LENGTH2)) + return 0x00; + if(!mem_add_module(memory,&slave->mem_module)) + return 0x00; + slave->set_rx_mode(); /* enable reception by IRQ */ comm_receive_irq(slave->comm_dev,0); + + return 0x01; +} + +void dyn_slave_set_baudrate(TDynamixelSlave *slave,unsigned int baudrate) +{ + slave->set_baudrate(slave->comm_dev,baudrate); } void dyn_slave_set_rx_timeout(TDynamixelSlave *slave,unsigned short int timeout_ms) { - slave->rx_timeout_ms=timeout_ms; + if(slave!=0x00000000) + slave->rx_timeout_ms=timeout_ms; } -inline void dyn_slave_set_address(TDynamixelSlave *slave,unsigned char address) +void dyn_slave_set_address(TDynamixelSlave *slave,unsigned char address) { - slave->address=address; + if(slave!=0x00000000) + slave->address=address; } -inline unsigned char dyn_slave_get_address(TDynamixelSlave *slave) +unsigned char dyn_slave_get_address(TDynamixelSlave *slave) { - return slave->address; + if(slave!=0x00000000) + return slave->address; + else + return 0xFF; } -inline void dyn_slave_set_return_delay(TDynamixelSlave *slave,unsigned char delay) +void dyn_slave_set_return_delay(TDynamixelSlave *slave,unsigned char delay) { - slave->return_delay=delay; + if(slave!=0x00000000) + slave->return_delay=delay; } -inline unsigned char dyn_slave_get_return_delay(TDynamixelSlave *slave) +unsigned char dyn_slave_get_return_delay(TDynamixelSlave *slave) { - return slave->return_delay; + if(slave!=0x00000000) + return slave->return_delay; + else + return 0xFF; } -inline void dyn_slave_set_return_level(TDynamixelSlave *slave,return_level_t level) +void dyn_slave_set_return_level(TDynamixelSlave *slave,return_level_t level) { - slave->return_level=level; + if(slave!=0x00000000) + slave->return_level=level; } -inline return_level_t dyn_slave_get_return_level(TDynamixelSlave *slave) +return_level_t dyn_slave_get_return_level(TDynamixelSlave *slave) { - return slave->return_level; + if(slave!=0x00000000) + return slave->return_level; + else + return return_all; } -inline TDynVersion dyn_slave_get_version(TDynamixelSlave *slave) +TDynVersion dyn_slave_get_version(TDynamixelSlave *slave) { - return slave->version; + if(slave!=0x00000000) + return slave->version; + else + return DYN_VER1; } void dyn_slave_loop(TDynamixelSlave *slave) { - if(slave->packet_ready)// check if a new instruction packet has been received + if(slave!=0x00000000) { - slave->packet_ready=0x00; - // check address - if(slave->version==DYN_VER1) - dyn_v1_slave_loop(slave); + if(slave->packet_ready)// check if a new instruction packet has been received + { + slave->packet_ready=0x00; + // check address + if(slave->version==DYN_VER1) + dyn_v1_slave_loop(slave); + else + dyn_v2_slave_loop(slave); + } else - dyn_v2_slave_loop(slave); - } - else - { - if(slave->comm_dev->time!=0x00000000) { - if(time_is_timeout(slave->comm_dev->time)) - { - /* cancel any IRQ or DMA reception */ - comm_cancel_irq_receive(slave->comm_dev); - comm_cancel_dma_receive(slave->comm_dev); - slave->received_bytes=0; - /* enable reception by IRQ */ - comm_receive_irq(slave->comm_dev,0); + if(slave->comm_dev->time!=0x00000000) + { + if(time_is_timeout(slave->comm_dev->time)) + { + /* cancel any IRQ or DMA reception */ + comm_cancel_irq_receive(slave->comm_dev); + comm_cancel_dma_receive(slave->comm_dev); + slave->received_bytes=0; + /* enable reception by IRQ */ + comm_receive_irq(slave->comm_dev,0); + } } } } diff --git a/dynamixel_manager/Makefile b/dynamixel_manager/Makefile index ab1e59b5b6dea777f90b49ffc73ebf51ef73e345..4ebb37992b0d459fc28f3864365f430b99e11ee1 100755 --- a/dynamixel_manager/Makefile +++ b/dynamixel_manager/Makefile @@ -14,8 +14,9 @@ COMPILE_OPTS_M3 = -mfloat-abi=softfp -mcpu=cortex-m3 COMM_PATH = ../comm UTILS_PATH = ../utils DYN_BASE_PATH = ../dynamixel_base +MEMORY_PATH = ../memory -INCLUDE_DIRS = -I./include/ -I$(COMM_PATH)/include -I$(UTILS_PATH)/include -I$(DYN_BASE_PATH)/include +INCLUDE_DIRS = -I./include/ -I./include/modules -I$(COMM_PATH)/include -I$(UTILS_PATH)/include -I$(DYN_BASE_PATH)/include -I$(MEMORY_PATH)/include TCHAIN_PREFIX=arm-none-eabi- @@ -32,6 +33,8 @@ DYNAMIXEL_OUT_M3 = ./lib/dynamixel_manager_m3.a SRC_DIR=./src/ SRC=$(wildcard $(SRC_DIR)*.c) +SRC_DIR_MODULES=./src/modules/ +SRC+=$(wildcard $(SRC_DIR_MODULES)*.c) DYNAMIXEL_M4_FPU_OBJ_DIR=build/m4_fpu/ DYNAMIXEL_M4_FPU_OBJS_TMP = $(notdir $(SRC:.c=.o)) @@ -52,12 +55,20 @@ DYNAMIXEL_M3_OBJS = $(patsubst %,$(DYNAMIXEL_M3_OBJ_DIR)%,$(DYNAMIXEL_M3_OBJS_TM all: $(DYNAMIXEL_OUT_M4_FPU) $(DYNAMIXEL_OUT_M0) $(DYNAMIXEL_OUT_M0plus) $(DYNAMIXEL_OUT_M3) $(DYNAMIXEL_M4_FPU_OBJ_DIR)%.o: $(SRC_DIR)%.c $(CC) -c $(CFLAGS) $(COMPILE_OPTS_M4_FPU) -o $@ $< +$(DYNAMIXEL_M4_FPU_OBJ_DIR)%.o: $(SRC_DIR_MODULES)%.c + $(CC) -c $(CFLAGS) $(COMPILE_OPTS_M4_FPU) -o $@ $< $(DYNAMIXEL_M0_OBJ_DIR)%.o: $(SRC_DIR)%.c $(CC) -c $(CFLAGS) $(COMPILE_OPTS_M0) -o $@ $< +$(DYNAMIXEL_M0_OBJ_DIR)%.o: $(SRC_DIR_MODULES)%.c + $(CC) -c $(CFLAGS) $(COMPILE_OPTS_M0) -o $@ $< $(DYNAMIXEL_M0plus_OBJ_DIR)%.o: $(SRC_DIR)%.c $(CC) -c $(CFLAGS) $(COMPILE_OPTS_M0plus) -o $@ $< +$(DYNAMIXEL_M0plus_OBJ_DIR)%.o: $(SRC_DIR_MODULES)%.c + $(CC) -c $(CFLAGS) $(COMPILE_OPTS_M0plus) -o $@ $< $(DYNAMIXEL_M3_OBJ_DIR)%.o: $(SRC_DIR)%.c $(CC) -c $(CFLAGS) $(COMPILE_OPTS_M3) -o $@ $< +$(DYNAMIXEL_M3_OBJ_DIR)%.o: $(SRC_DIR_MODULES)%.c + $(CC) -c $(CFLAGS) $(COMPILE_OPTS_M3) -o $@ $< mkdir_build: mkdir -p build/m4_fpu mkdir -p build/m0 diff --git a/dynamixel_manager/include/dyn_devices.h b/dynamixel_manager/include/dyn_devices.h index c10582ff5b9f007c2e6aaf3538290bacaa46489d..e6bbe5225e1497dcd79d05c746a5c2e88e136077 100644 --- a/dynamixel_manager/include/dyn_devices.h +++ b/dynamixel_manager/include/dyn_devices.h @@ -19,9 +19,13 @@ #define SERVO_MX106 0x0140 #define SERVO_XL320 0x015E -#define IS_SERVO(model) (model==SERVO_DX113 || model==SERVO_DX116 || model==SERVO_DX117 || model==SERVO_AX12A \ - model==SERVO_AX12W || model==SERVO_AX18A || model==SERVO_RX10 || model==SERVO_MX12W \ - model==SERVO_MX28 || model==SERVO_RX24F || model==SERVO_RX28 || model==SERVO_RX64 \ +#define IS_SERVO(model) (model==SERVO_DX113 || model==SERVO_DX116 || model==SERVO_DX117 || model==SERVO_AX12A || \ + model==SERVO_AX12W || model==SERVO_AX18A || model==SERVO_RX10 || model==SERVO_MX12W || \ + model==SERVO_MX28 || model==SERVO_RX24F || model==SERVO_RX28 || model==SERVO_RX64 || \ model==SERVO_MX64 || model==SERVO_EX106 || model==SERVO_MX106 || model==SERVO_XL320) +#define HAS_BULK_READ(model) (model==SERVO_MX12W || model==SERVO_MX28 || model==SERVO_MX64 || model==SERVO_MX106 || model==SERVO_XL320) +#define HAS_BULK_WRITE(model) (model==SERVO_XL320) +#define HAS_SYNC_READ(model) (model==SERVO_XL320) + #endif diff --git a/dynamixel_manager/include/dyn_manager.h b/dynamixel_manager/include/dyn_manager.h index ab0c336134eb7f4993ff6633da608afb789247ed..b9f03388a2dcf3461dcad06f3d654ff32a2c3adf 100644 --- a/dynamixel_manager/include/dyn_manager.h +++ b/dynamixel_manager/include/dyn_manager.h @@ -1,14 +1,28 @@ #ifndef _DYN_MANAGER_H #define _DYN_MANAGER_H +#include "dyn_manager_registers.h" #include "dynamixel_master.h" - -#define DYN_MANAGER_MAX_NUM_MASTERS 4 -#define DYN_MANAGER_MAX_NUM_MODULES 8 -#define DYN_MANAGER_MAX_NUM_DEVICES 32 -#define DYN_MANAGER_MAX_NUM_SINGLE_OP 16 -#define DYN_MANAGER_MAX_NUM_SYNC_OP 4 -#define DYN_MANAGER_MAX_NUM_BULK_OP 4 +#include "memory.h" + +#ifndef DYN_MANAGER_MAX_NUM_MASTERS + #define DYN_MANAGER_MAX_NUM_MASTERS 4 +#endif +#ifndef DYN_MANAGER_MAX_NUM_MODULES + #define DYN_MANAGER_MAX_NUM_MODULES 8 +#endif +#ifndef DYN_MANAGER_MAX_NUM_DEVICES + #define DYN_MANAGER_MAX_NUM_DEVICES 32 +#endif +#ifndef DYN_MANAGER_MAX_NUM_SINGLE_OP + #define DYN_MANAGER_MAX_NUM_SINGLE_OP 16 +#endif +#ifndef DYN_MANAGER_MAX_NUM_SYNC_OP + #define DYN_MANAGER_MAX_NUM_SYNC_OP 4 +#endif +#ifndef DYN_MANAGER_MAX_NUM_BULK_OP + #define DYN_MANAGER_MAX_NUM_BULK_OP 4 +#endif #define DYN_MANAGER_MAX_NUM_OP ((DYN_MANAGER_MAX_NUM_SINGLE_OP+DYN_MANAGER_MAX_NUM_SYNC_OP+DYN_MANAGER_MAX_NUM_BULK_OP)*DYN_MANAGER_MAX_NUM_MASTERS) #define DYN_MANAGER_PERIODIC 0xFF @@ -58,7 +72,7 @@ typedef struct{ unsigned char ids[DYN_MANAGER_MAX_NUM_DEVICES]; unsigned short int address; unsigned short int length; - TWriteData data[DYN_MANAGER_MAX_NUM_DEVICES]; + unsigned char *data[DYN_MANAGER_MAX_NUM_DEVICES]; }TDynManagerSyncOp; typedef struct{ @@ -67,7 +81,7 @@ typedef struct{ unsigned char ids[DYN_MANAGER_MAX_NUM_DEVICES]; unsigned short int address[DYN_MANAGER_MAX_NUM_DEVICES]; unsigned short int length[DYN_MANAGER_MAX_NUM_DEVICES]; - TWriteData data[DYN_MANAGER_MAX_NUM_DEVICES]; + unsigned char *data[DYN_MANAGER_MAX_NUM_DEVICES]; }TDynManagerBulkOp; typedef struct{ @@ -96,13 +110,21 @@ typedef struct{ unsigned char num_devices; OP_HANDLE op_handles[DYN_MANAGER_MAX_NUM_OP]; unsigned char num_ops; + unsigned short int period_us; + void (*init_timer)(void); + void (*set_period)(unsigned short int period_us); + TMemModule mem_module; }TDynManager; // public functions -void dyn_manager_init(TDynManager *manager); +unsigned char dyn_manager_init(TDynManager *manager,TMemory *memory); +void dyn_manager_set_period(TDynManager *manager,unsigned short int period_us); +unsigned short int dyn_manager_get_period(TDynManager *manager); void dyn_manager_scan(TDynManager *manager); void dyn_manager_add_master(TDynManager *manager,TDynamixelMaster *master); unsigned char dyn_manager_get_num_masters(TDynManager *manager); +TDynamixelMaster *dyn_manager_get_device_master(TDynManager *manager,unsigned char device_id); +unsigned short int dyn_manager_get_device_model(TDynManager *manager,unsigned char device_id); void dyn_manager_add_module(TDynManager *manager,struct TDynModule *module); unsigned char dyn_manager_get_num_modules(TDynManager *manager); void dyn_manager_delete_op(TDynManager *manager,OP_HANDLE *op); @@ -117,19 +139,19 @@ void dyn_manager_single_op_change_address(TDynManager *manager,OP_HANDLE *op,uns void dyn_manager_single_op_change_length(TDynManager *manager,OP_HANDLE *op,unsigned short int length); void dyn_manager_single_op_change_data(TDynManager *manager,OP_HANDLE *op,unsigned char *data); // sync operation functions -OP_HANDLE *dyn_manager_sync_op_new(TDynManager *manager,unsigned char mode,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, TWriteData *data); -void dyn_manager_sync_op_add_devices(TDynManager *manager,OP_HANDLE *op,unsigned char num,unsigned char *ids,TWriteData *data); +OP_HANDLE *dyn_manager_sync_op_new(TDynManager *manager,unsigned char mode,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, unsigned char * const data[]); +void dyn_manager_sync_op_add_devices(TDynManager *manager,OP_HANDLE *op,unsigned char num,unsigned char *ids,unsigned char * const data[]); void dyn_manager_sync_op_remove_devices(TDynManager *manager,OP_HANDLE *op,unsigned char num,unsigned char *ids); void dyn_manager_sync_op_change_address(TDynManager *manager,OP_HANDLE *op,unsigned short int address); void dyn_manager_sync_op_change_length(TDynManager *manager,OP_HANDLE *op,unsigned short int length); -void dyn_manager_sync_op_change_data(TDynManager *manager,OP_HANDLE *op,unsigned char num,unsigned char *ids,TWriteData *data); +void dyn_manager_sync_op_change_data(TDynManager *manager,OP_HANDLE *op,unsigned char num,unsigned char *ids,unsigned char * const data[]); // bulk operation functions -OP_HANDLE *dyn_manager_bulk_op_new(TDynManager *manager,unsigned char mode,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, TWriteData *data); -void dyn_manager_bulk_op_add_devices(TDynManager *manager,OP_HANDLE *op,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length,TWriteData *data); +OP_HANDLE *dyn_manager_bulk_op_new(TDynManager *manager,unsigned char mode,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, unsigned char * const data[]); +void dyn_manager_bulk_op_add_devices(TDynManager *manager,OP_HANDLE *op,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length,unsigned char * const data[]); void dyn_manager_bulk_op_remove_devices(TDynManager *manager,OP_HANDLE *op,unsigned char num,unsigned char *ids); void dyn_manager_bulk_op_change_address(TDynManager *manager,OP_HANDLE *op,unsigned char num,unsigned char *ids,unsigned short int *address); void dyn_manager_bulk_op_change_length(TDynManager *manager,OP_HANDLE *op,unsigned char num,unsigned char *ids,unsigned short int *length); -void dyn_manager_bulk_op_change_data(TDynManager *manager,OP_HANDLE *op,unsigned char num,unsigned char *ids,TWriteData *data); +void dyn_manager_bulk_op_change_data(TDynManager *manager,OP_HANDLE *op,unsigned char num,unsigned char *ids,unsigned char * const data[]); void dyn_manager_loop(TDynManager *manager); #endif diff --git a/dynamixel_manager/include/dyn_manager_registers.h b/dynamixel_manager/include/dyn_manager_registers.h new file mode 100644 index 0000000000000000000000000000000000000000..7099cc7d5ce6a4292d3a7b7c2c96164bbe2e0264 --- /dev/null +++ b/dynamixel_manager/include/dyn_manager_registers.h @@ -0,0 +1,26 @@ +#ifndef _DYN_MANAGER_REGISTERS_H +#define _DYN_MANAGER_REGISTERS_H + +#ifndef RAM_DYN_MANAGER_BASE_ADDRESS + #define RAM_DYN_MANAGER_BASE_ADDRESS ((unsigned short int)0x0000) +#endif + +#ifndef EEPROM_DYN_MANAGER_BASE_ADDRESS + #define EEPROM_DYN_MANAGER_BASE_ADDRESS ((unsigned short int)0x0000) +#endif + +#define RAM_DYN_MANAGER_LENGTH 2 + +#define EEPROM_DYN_MANAGER_LENGTH 2 + +#define DYN_MANAGER_PERIOD EEPROM_DYN_MANAGER_BASE_ADDRESS + +#define DYN_MANAGER_NUM_MODULES RAM_DYN_MANAGER_BASE_ADDRESS +#define DYN_MANAGER_NUM_MASTERS (RAM_DYN_MANAGER_BASE_ADDRESS+1) + +#ifndef DEFAULT_DYN_MANAGER_PERIOD + #define DEFAULT_DYN_MANAGER_PERIOD 0x1E78 +#endif + +#endif + diff --git a/dynamixel_manager/include/dyn_module.h b/dynamixel_manager/include/dyn_module.h index 95ba55eaba912bb0d4f2c8dbbed74f7635f91caf..f4c6d44fad8636bfe75fb69f09f4484344900090 100644 --- a/dynamixel_manager/include/dyn_module.h +++ b/dynamixel_manager/include/dyn_module.h @@ -1,9 +1,12 @@ #ifndef _DYN_MODULE_H #define _DYN_MODULE_H -#define MODULE_MAX_NUM_MODELS 32 +#ifndef MODULE_MAX_NUM_MODELS + #define MODULE_MAX_NUM_MODELS 32 +#endif #include "dyn_manager.h" +#include "memory.h" typedef struct TDynModule{ TDynManager *manager; @@ -12,26 +15,27 @@ typedef struct TDynModule{ unsigned char assigned_ids[DYN_MANAGER_MAX_NUM_DEVICES]; unsigned char num_assigned_ids; unsigned char period_count; - void (*init)(void); - void (*add_device)(unsigned char id,unsigned short int model); - void (*setup)(void); - void (*pre_process)(void); - void (*post_process)(void); + unsigned char enabled; + void (*add_device)(void *module_data,unsigned char id,unsigned short int model); + void (*set_period)(void *module_data,unsigned short int period_us); + void (*setup)(void *module_data); + void (*pre_process)(void *module_data); + void (*post_process)(void *module_data); + void *data; + TMemModule *mem_module; }TDynModule; //public functions -void dyn_module_init(TDynModule *module); +unsigned char dyn_module_init(TDynModule *module); +inline void dyn_module_enable(TDynModule *module); +inline void dyn_module_disable(TDynModule *module); +inline unsigned char dyn_module_is_enabled(TDynModule *module); void dyn_module_add_model(TDynModule *module,unsigned short int model); -unsigned char dyn_module_get_num_models(TDynModule *module); +inline unsigned char dyn_module_get_num_models(TDynModule *module); void dyn_module_add_device(TDynModule *module,unsigned char id, unsigned short int model); -unsigned char dyn_module_get_num_assigned_devices(TDynModule *module); -unsigned char dyn_moudle_device_is_assigned(TDynModule *module,unsigned char id); +inline unsigned char dyn_module_get_num_assigned_devices(TDynModule *module); +unsigned char dyn_module_device_is_assigned(TDynModule *module,unsigned char id); void dyn_module_set_period(TDynModule *module,unsigned char period); -unsigned char dyn_module_get_period(TDynModule *module); -void dyn_module_set_init_fctn(TDynModule *module,void (*init)(void)); -void dyn_module_set_add_device_fctn(TDynModule *module,void (*add_device)(unsigned char id,unsigned short int model)); -void dyn_module_set_setup_fctn(TDynModule *module,void (*setup)(void)); -void dyn_module_set_pre_process_fctn(TDynModule *module,void (*pre_process)(void)); -void dyn_module_set_post_process_fctn(TDynModule *module,void (*post_process)(void)); +inline unsigned char dyn_module_get_period(TDynModule *module); #endif diff --git a/dynamixel_manager/include/dyn_module_registers.h b/dynamixel_manager/include/dyn_module_registers.h new file mode 100644 index 0000000000000000000000000000000000000000..c4c7afdf264b1289021c6f3478da61db963cfe8e --- /dev/null +++ b/dynamixel_manager/include/dyn_module_registers.h @@ -0,0 +1,19 @@ +#ifndef _DYN_MODULE_REGISTERS_H +#define _DYN_MODULE_REGISTERS_H + +#define RAM_DYN_MODULE_LENGTH 3 + +#define EEPROM_DYN_MODULE_LENGTH 1 + +#define DYN_MODULE_PERIOD EEPROM_DYN_MODULE_BASE_ADDRESS + +#define DYN_MODULE_CNTRL RAM_DYN_MODULE_BASE_ADDRESS +#define DYN_MODULE_NUM_MODELS (RAM_DYN_MODULE_BASE_ADDRESS+1) +#define DYN_MODULE_NUM_DEVICES (RAM_DYN_MODULE_BASE_ADDRESS+2) + +#ifndef DEFAULT_DYN_MODULE_PERIOD + #define DEFAULT_DYN_MODULE_PERIOD 0x01 +#endif + +#endif + diff --git a/dynamixel_manager/include/modules/action.h b/dynamixel_manager/include/modules/action.h new file mode 100755 index 0000000000000000000000000000000000000000..506b11d5ab837a58f7d08b45041c49cf0994aeb2 --- /dev/null +++ b/dynamixel_manager/include/modules/action.h @@ -0,0 +1,25 @@ +#ifndef _ACTION_H +#define _ACTION_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include "motion_pages.h" +#include "motion_module.h" + +// public functions +void action_init(void); +TMotionModule *action_get_module(void); +unsigned char action_load_page(unsigned char page_id); +void action_start_page(void); +void action_stop_page(void); +unsigned char action_is_running(void); +unsigned char action_get_current_page(void); +unsigned char action_get_current_step(void); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/dynamixel_manager/include/modules/dyn_servos.h b/dynamixel_manager/include/modules/dyn_servos.h new file mode 100755 index 0000000000000000000000000000000000000000..07d4e55521b0279fda66c9fe001c91c7623ce551 --- /dev/null +++ b/dynamixel_manager/include/modules/dyn_servos.h @@ -0,0 +1,125 @@ +#ifndef _DYN_SERVOS_H +#define _DYN_SERVOS_H + +// Servo register map +typedef enum{ + P_MODEL_NUMBER_L = 0, + P_MODEL_NUMBER_H = 1, + P_VERSION = 2, + P_ID = 3, + P_BAUD_RATE = 4, + P_RETURN_DELAY_TIME = 5, + P_CW_ANGLE_LIMIT_L = 6, + P_CW_ANGLE_LIMIT_H = 7, + P_CCW_ANGLE_LIMIT_L = 8, + P_CCW_ANGLE_LIMIT_H = 9, + P_SYSTEM_DATA2 = 10, + P_HIGH_LIMIT_TEMPERATURE = 11, + P_LOW_LIMIT_VOLTAGE = 12, + P_HIGH_LIMIT_VOLTAGE = 13, + P_MAX_TORQUE_L = 14, + P_MAX_TORQUE_H = 15, + P_RETURN_LEVEL = 16, + P_ALARM_LED = 17, + P_ALARM_SHUTDOWN = 18, + P_OPERATING_MODE = 19, + P_LOW_CALIBRATION_L = 20, + P_LOW_CALIBRATION_H = 21, + P_HIGH_CALIBRATION_L = 22, + P_HIGH_CALIBRATION_H = 23, + P_TORQUE_ENABLE = 24, + P_LED = 25, + P_CW_COMPLIANCE_MARGIN = 26, + P_CCW_COMPLIANCE_MARGIN = 27, + P_CW_COMPLIANCE_SLOPE = 28, + P_CCW_COMPLIANCE_SLOPE = 29, + P_D_GAIN = 26, + P_I_GAIN = 27, + P_P_GAIN = 28, + P_RESERVED = 29, + P_GOAL_POSITION_L = 30, + P_GOAL_POSITION_H = 31, + P_MOVING_SPEED_L = 32, + P_MOVING_SPEED_H = 33, + P_TORQUE_LIMIT_L = 34, + P_TORQUE_LIMIT_H = 35, + P_PRESENT_POSITION_L = 36, + P_PRESENT_POSITION_H = 37, + P_PRESENT_SPEED_L = 38, + P_PRESENT_SPEED_H = 39, + P_PRESENT_LOAD_L = 40, + P_PRESENT_LOAD_H = 41, + P_PRESENT_VOLTAGE = 42, + P_PRESENT_TEMPERATURE = 43, + P_REGISTERED_INSTRUCTION = 44, + P_PAUSE_TIME = 45, + P_MOVING = 46, + P_LOCK = 47, + P_PUNCH_L = 48, + P_PUNCH_H = 49, + P_RESERVED4 = 50, + P_RESERVED5 = 51, + P_POT_L = 52, + P_POT_H = 53, + P_PWM_OUT_L = 54, + P_PWM_OUT_H = 55, + P_P_ERROR_L = 56, + P_P_ERROR_H = 57, + P_I_ERROR_L = 58, + P_I_ERROR_H = 59, + P_D_ERROR_L = 60, + P_D_ERROR_H = 61, + P_P_ERROR_OUT_L = 62, + P_P_ERROR_OUT_H = 63, + P_I_ERROR_OUT_L = 64, + P_I_ERROR_OUT_H = 65, + P_D_ERROR_OUT_L = 66, + P_D_ERROR_OUT_H = 67}TDynServo; + +typedef enum{ + XL_MODEL_NUMBER_L = 0, + XL_MODEL_NUMBER_H = 1, + XL_VERSION = 2, + XL_ID = 3, + XL_BAUD_RATE = 4, + XL_RETURN_DELAY_TIME = 5, + XL_CW_ANGLE_LIMIT_L = 6, + XL_CW_ANGLE_LIMIT_H = 7, + XL_CCW_ANGLE_LIMIT_L = 8, + XL_CCW_ANGLE_LIMIT_H = 9, + XL_SYSTEM_DATA2 = 10, + XL_CONTROL_MODE = 11, + XL_HIGH_LIMIT_TEMPERATURE = 12, + XL_LOW_LIMIT_VOLTAGE = 13, + XL_HIGH_LIMIT_VOLTAGE = 14, + XL_MAX_TORQUE_L = 15, + XL_MAX_TORQUE_H = 16, + XL_RETURN_LEVEL = 17, + XL_ALARM_SHUTDOWN = 18, + XL_TORQUE_ENABLE = 24, + XL_LED = 25, + XL_D_GAIN = 27, + XL_I_GAIN = 28, + XL_P_GAIN = 29, + XL_GOAL_POSITION_L = 30, + XL_GOAL_POSITION_H = 31, + XL_MOVING_SPEED_L = 32, + XL_MOVING_SPEED_H = 33, + XL_TORQUE_LIMIT_L = 35, + XL_TORQUE_LIMIT_H = 36, + XL_PRESENT_POSITION_L = 37, + XL_PRESENT_POSITION_H = 38, + XL_PRESENT_SPEED_L = 39, + XL_PRESENT_SPEED_H = 40, + XL_PRESENT_LOAD_L = 41, + XL_PRESENT_LOAD_H = 42, + XL_PRESENT_VOLTAGE = 45, + XL_PRESENT_TEMPERATURE = 46, + XL_REGISTERED_INSTRUCTION = 47, + XL_MOVING = 49, + XL_HARDWARE_ERROR_STATUS = 50, + XL_PUNCH_L = 51, + XL_PUNCH_H = 51}TXLServo; + + +#endif diff --git a/dynamixel_manager/include/modules/motion_manager.h b/dynamixel_manager/include/modules/motion_manager.h new file mode 100644 index 0000000000000000000000000000000000000000..b5d3bbe2e32678e5d0c055bb3bd27d08c02fa9c8 --- /dev/null +++ b/dynamixel_manager/include/modules/motion_manager.h @@ -0,0 +1,70 @@ +#ifndef _MOTION_MANAGER_H +#define _MOTION_MANAGER_H + +#include "dyn_module.h" + +#ifndef MM_MAX_NUM_MOTION_MODULES + #define MM_MAX_NUM_MOTION_MODULES 8 +#endif +#ifdef MM_MAX_NUM_MODELS + #define MODULE_MAX_NUM_MODELS MM_MAX_NUM_MODELS +#endif + +typedef enum {MM_NONE = -1, + MM_ACTION = 0, + MM_WALKING = 1, + MM_JOINTS = 2, + MM_HEAD = 3} TModules; + +typedef struct{ + unsigned short int model; + unsigned short int encoder_resolution; + unsigned char gear_ratio; + unsigned short int max_angle; + unsigned short int center_angle; + unsigned short int max_speed; + long long int cw_angle_limit; + long long int ccw_angle_limit; +}TServoConfig; + +typedef struct{ + unsigned char cw_compliance; + unsigned char ccw_compliance; + unsigned short int target_value; + long long int target_angle; + unsigned short int current_value; + long long int current_angle; + TModules module; + unsigned char enabled; + char offset; +}TServoValues; + +struct TMotionModule; + +typedef struct { + TDynModule dyn_module; + struct TMotionModule *modules[MM_MAX_NUM_MOTION_MODULES]; + unsigned char num_modules; + TServoConfig servo_configs[DYN_MANAGER_MAX_NUM_DEVICES]; + TServoValues servo_values[DYN_MANAGER_MAX_NUM_DEVICES]; + OP_HANDLE *enable_op; + OP_HANDLE *motion_op; + OP_HANDLE *feedback_op; + unsigned char balance_enabled; + void (*balance)(short int offsets[DYN_MANAGER_MAX_NUM_DEVICES]); +}TMotionManager; + +unsigned char mmanager_init(TMotionManager *mmanager,TMemory *memory); +TDynModule *mmanager_get_dyn_module(TMotionManager *mmanager); +void mmanager_enable_balance(TMotionManager *mmanager); +void mmanager_disable_balance(TMotionManager *mmanager); +unsigned char mmanager_is_balance_enabled(TMotionManager *mmanager); +void mmanager_set_module(TMotionManager *mmanager,unsigned char servo_id,TModules module); +TModules mmanager_get_module(TMotionManager *mmanager,unsigned char servo_id); +void mmanager_enable_servo(TMotionManager *mmanager,unsigned char servo_id); +void mmanager_disable_servo(TMotionManager *mmanager,unsigned char servo_id); +unsigned char mmanager_is_servo_enabled(TMotionManager *mmanager,unsigned char servo_id); +void mmanager_set_offset(TMotionManager *mmanager,unsigned char servo_id,char offset); +void mmanager_add_module(TMotionManager *mmanager,struct TMotionModule *module); + +#endif diff --git a/dynamixel_manager/include/modules/motion_manager_registers.h b/dynamixel_manager/include/modules/motion_manager_registers.h new file mode 100644 index 0000000000000000000000000000000000000000..208e6b810965e8a28d47268820f151b25498137e --- /dev/null +++ b/dynamixel_manager/include/modules/motion_manager_registers.h @@ -0,0 +1,62 @@ +#ifndef _MOTION_MANAGER_REGISTERS_H +#define _MOTION_MANAGER_REGISTERS_H + +#include "dyn_module_registers.h" + +#ifndef RAM_MM_BASE_ADDRESS + #define RAM_MM_BASE_ADDRESS ((unsigned short int)0x0000) +#endif + +#ifndef EEPROM_MM_BASE_ADDRESS + #define EEPROM_MM_BASE_ADDRESS ((unsigned short int)0x0000) +#endif + +#define RAM_MM_LENGTH (RAM_DYN_MODULE_LENGTH + ) + +#define EEPROM_MM_LENGTH (EEPROM_DYN_MODULE_LENGTH + ) + +#define MM_PERIOD DYN_MODULE_PERIOD + +#define MM_CNTRL DYN_MODULE_CNTRL +#define MM_NUM_MODELS DYN_MODULE_NUM_MODELS +#define MM_NUM_DEVICES DYN_MODULE_NUM_DEVICES + +#ifndef DEFAULT_MM_PERIOD + #define DEFAULT_MM_PERIOD DEFAULT_DYN_MODULE_PERIOD +#endif +#define MM_SERVO0_OFFSET ((unsigned short int)0x0011) +#define MM_SERVO1_OFFSET ((unsigned short int)0x0012) +#define MM_SERVO2_OFFSET ((unsigned short int)0x0013) +#define MM_SERVO3_OFFSET ((unsigned short int)0x0014) +#define MM_SERVO4_OFFSET ((unsigned short int)0x0015) +#define MM_SERVO5_OFFSET ((unsigned short int)0x0016) +#define MM_SERVO6_OFFSET ((unsigned short int)0x0017) +#define MM_SERVO7_OFFSET ((unsigned short int)0x0018) +#define MM_SERVO8_OFFSET ((unsigned short int)0x0019) +#define MM_SERVO9_OFFSET ((unsigned short int)0x001A) +#define MM_SERVO10_OFFSET ((unsigned short int)0x001B) +#define MM_SERVO11_OFFSET ((unsigned short int)0x001C) +#define MM_SERVO12_OFFSET ((unsigned short int)0x001D) +#define MM_SERVO13_OFFSET ((unsigned short int)0x001E) +#define MM_SERVO14_OFFSET ((unsigned short int)0x001F) +#define MM_SERVO15_OFFSET ((unsigned short int)0x0020) +#define MM_SERVO16_OFFSET ((unsigned short int)0x0021) +#define MM_SERVO17_OFFSET ((unsigned short int)0x0022) +#define MM_SERVO18_OFFSET ((unsigned short int)0x0023) +#define MM_SERVO19_OFFSET ((unsigned short int)0x0024) +#define MM_SERVO20_OFFSET ((unsigned short int)0x0025) +#define MM_SERVO21_OFFSET ((unsigned short int)0x0026) +#define MM_SERVO22_OFFSET ((unsigned short int)0x0027) +#define MM_SERVO23_OFFSET ((unsigned short int)0x0028) +#define MM_SERVO24_OFFSET ((unsigned short int)0x0029) +#define MM_SERVO25_OFFSET ((unsigned short int)0x002A) +#define MM_SERVO26_OFFSET ((unsigned short int)0x002B) +#define MM_SERVO27_OFFSET ((unsigned short int)0x002C) +#define MM_SERVO28_OFFSET ((unsigned short int)0x002D) +#define MM_SERVO29_OFFSET ((unsigned short int)0x002E) +#define MM_SERVO30_OFFSET ((unsigned short int)0x002F) +#define MM_SERVO31_OFFSET ((unsigned short int)0x0030) + + +#endif + diff --git a/dynamixel_manager/include/modules/motion_module.h b/dynamixel_manager/include/modules/motion_module.h new file mode 100644 index 0000000000000000000000000000000000000000..bb4c6136e19f3992691d9fdf120627bfedd99aed --- /dev/null +++ b/dynamixel_manager/include/modules/motion_module.h @@ -0,0 +1,18 @@ +#ifndef _MOTION_MODULE_H +#define _MOTION_MODULE_H + +#include "motion_manager.h" + +typedef struct TMotionModule{ + TModules id; + TMotionManager *manager; + void (*set_period)(void *module,unsigned short int period_us); + void (*set_module)(void *module,unsigned char servo_id); + void (*pre_process)(void *module); + void (*post_process)(void *module); + void *data; +}TMotionModule; + +void mmodule_init(TMotionModule *module); + +#endif diff --git a/dynamixel_manager/include/modules/motion_pages.h b/dynamixel_manager/include/modules/motion_pages.h new file mode 100755 index 0000000000000000000000000000000000000000..e871850961238d47b2f0557a836b129e1a2bb767 --- /dev/null +++ b/dynamixel_manager/include/modules/motion_pages.h @@ -0,0 +1,58 @@ +#ifndef _MOTION_PAGES_H +#define _MOTION_PAGES_H + +#ifdef __cplusplus +extern "C" { +#endif + +#define MAX_PAGES 256 +#define PAGE_MAX_NUM_SERVOS 31 +#define POSE_NUMBER_OF_POSES_PER_PAGE 7 + +typedef struct // Header Structure (total 64unsigned char) +{ + unsigned char name[14]; // Name 0~13 + unsigned char reserved1; // Reserved1 14 + unsigned char repeat; // Repeat count 15 + unsigned char schedule; // schedule 16 + unsigned char reserved2[3]; // reserved2 17~19 + unsigned char stepnum; // Number of step 20 + unsigned char reserved3; // reserved3 21 + unsigned char speed; // Speed 22 + unsigned char reserved4; // reserved4 23 + unsigned char accel; // Acceleration time 24 + unsigned char next; // Link to next 25 + unsigned char exit; // Link to exit 26 + unsigned char reserved5[4]; // reserved5 27~30 + unsigned char checksum; // checksum 31 + unsigned char slope[PAGE_MAX_NUM_SERVOS]; // CW/CCW compliance slope 32~62 + unsigned char reserved6; // reserved6 63 +}TPageHeader; + +typedef struct // Step Structure (total 64unsigned char) +{ + short int position[PAGE_MAX_NUM_SERVOS]; // Joint position 0~61 + unsigned char pause; // Pause time 62 + unsigned char time; // Time 63 +}TStep; + +typedef struct // Page Structure (total 512unsigned char) +{ + TPageHeader header; // Page header 0~64 + TStep steps[POSE_NUMBER_OF_POSES_PER_PAGE]; // Page step 65~511 +}TPage; + +extern TPage motion_pages[MAX_PAGES]; + +// public functions +void pages_get_page(unsigned char page_id,TPage *page); +unsigned char pages_check_checksum(TPage *page); +void pages_clear_page(TPage *page); +void pages_copy_page(TPage *src,TPage *dst); +unsigned char pages_get_slope(TPage *page,unsigned char servo_id); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/dynamixel_manager/src/dyn_manager.c b/dynamixel_manager/src/dyn_manager.c index e304245b98bdaefc501864f3bc7e186bafd651ee..8499d2c1a5130a45251b9d02167c47bba4e6338d 100644 --- a/dynamixel_manager/src/dyn_manager.c +++ b/dynamixel_manager/src/dyn_manager.c @@ -1,22 +1,45 @@ #include "dyn_manager.h" #include "dyn_module.h" +#include "ram.h" extern unsigned char dyn_master_start_read_table(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned short int length,unsigned char *data); extern unsigned char dyn_master_is_read_table_done(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned short int length,unsigned char *data); extern unsigned char dyn_master_start_write_table(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned short int length,unsigned char *data); extern unsigned char dyn_master_is_write_table_done(TDynamixelMaster *master,unsigned char id,unsigned short int address,unsigned short int length,unsigned char *data); -extern unsigned char dyn_master_start_sync_write(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, TWriteData *data); -extern unsigned char dyn_master_is_sync_write_done(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, TWriteData *data); -extern unsigned char dyn_master_start_sync_read(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, TWriteData *data); -extern unsigned char dyn_master_is_sync_read_done(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, TWriteData *data); -extern unsigned char dyn_master_start_bulk_write(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, TWriteData *data); -extern unsigned char dyn_master_is_bulk_write_done(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, TWriteData *data); -extern unsigned char dyn_master_start_bulk_read(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, TWriteData *data); -extern unsigned char dyn_master_is_bulk_read_done(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, TWriteData *data); +extern unsigned char dyn_master_start_sync_write(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, unsigned char * const data[]); +extern unsigned char dyn_master_is_sync_write_done(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, unsigned char * const data[]); +extern unsigned char dyn_master_start_sync_read(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, unsigned char * const data[]); +extern unsigned char dyn_master_is_sync_read_done(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, unsigned char * const data[]); +extern unsigned char dyn_master_start_bulk_write(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, unsigned char * const data[]); +extern unsigned char dyn_master_is_bulk_write_done(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, unsigned char * const data[]); +extern unsigned char dyn_master_start_bulk_read(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, unsigned char * const data[]); +extern unsigned char dyn_master_is_bulk_read_done(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, unsigned char * const data[]); typedef enum {start_single_ops,wait_single_ops,start_sync_ops,wait_sync_ops,start_bulk_ops,wait_bulk_ops,ops_done} dyn_manager_states_t; // private functions +void dyn_manager_write_cmd(TDynManager *module,unsigned short int address,unsigned short int length,unsigned char *data) +{ + unsigned short int uint16_value; + unsigned char *data_ptr; + + if(ram_in_window(DYN_MANAGER_PERIOD,2,address,length)) + { + uint16_value=dyn_manager_get_period(module); + data_ptr=(unsigned char *)&uint16_value; + if(ram_in_range(DYN_MANAGER_PERIOD,address,length)) + data_ptr[0]=data[DYN_MANAGER_PERIOD-address]; + if(ram_in_range(DYN_MANAGER_PERIOD+1,address,length)) + data_ptr[1]=data[DYN_MANAGER_PERIOD+1-address]; + dyn_manager_set_period(module,uint16_value); + } +} + +void dyn_manager_read_cmd(TDynManager *module,unsigned short int address,unsigned short int length,unsigned char *data) +{ + ram_read_table(address,length,data); +} + unsigned char dyn_manager_check_id(TDynManager *manager,unsigned char id) { if(id>=DYN_MANAGER_MAX_NUM_DEVICES) @@ -115,7 +138,7 @@ void dyn_manager_delete_op_single_master(TDynManager *manager,unsigned char mast for(k=0;k<manager->operations[master_index].sync_op[j].num_devices;k++) { manager->operations[master_index].sync_op[j].ids[k]=manager->operations[master_index].sync_op[j+1].ids[k]; - manager->operations[master_index].sync_op[j].data[k].data_addr=manager->operations[master_index].sync_op[j+1].data[k].data_addr; + manager->operations[master_index].sync_op[j].data[k]=manager->operations[master_index].sync_op[j+1].data[k]; } } } @@ -131,7 +154,7 @@ void dyn_manager_delete_op_single_master(TDynManager *manager,unsigned char mast for(k=0;k<manager->operations[master_index].sync_op[op->op_index[master_index]].num_devices;k++) { manager->operations[master_index].sync_op[op->op_index[master_index]].ids[k]=0xFF; - manager->operations[master_index].sync_op[op->op_index[master_index]].data[k].data_addr=0x00000000; + manager->operations[master_index].sync_op[op->op_index[master_index]].data[k]=0x00000000; } } manager->operations[master_index].num_sync_op--; @@ -150,7 +173,7 @@ void dyn_manager_delete_op_single_master(TDynManager *manager,unsigned char mast manager->operations[master_index].bulk_op[j].address[k]=manager->operations[master_index].bulk_op[j+1].address[k]; manager->operations[master_index].bulk_op[j].length[k]=manager->operations[master_index].bulk_op[j+1].length[k]; manager->operations[master_index].bulk_op[j].ids[k]=manager->operations[master_index].bulk_op[j+1].ids[k]; - manager->operations[master_index].bulk_op[j].data[k].data_addr=manager->operations[master_index].bulk_op[j+1].data[k].data_addr; + manager->operations[master_index].bulk_op[j].data[k]=manager->operations[master_index].bulk_op[j+1].data[k]; } } } @@ -166,7 +189,7 @@ void dyn_manager_delete_op_single_master(TDynManager *manager,unsigned char mast manager->operations[master_index].bulk_op[op->op_index[master_index]].address[k]=0x0000; manager->operations[master_index].bulk_op[op->op_index[master_index]].length[k]=0x0000; manager->operations[master_index].bulk_op[op->op_index[master_index]].ids[k]=0xFF; - manager->operations[master_index].bulk_op[op->op_index[master_index]].data[k].data_addr=0x00000000; + manager->operations[master_index].bulk_op[op->op_index[master_index]].data[k]=0x00000000; } } manager->operations[master_index].num_bulk_op--; @@ -188,7 +211,7 @@ void dyn_manager_delete_op_single_master(TDynManager *manager,unsigned char mast } // public functions -void dyn_manager_init(TDynManager *manager) +unsigned char dyn_manager_init(TDynManager *manager,TMemory *memory) { unsigned char i,j,k; @@ -217,7 +240,7 @@ void dyn_manager_init(TDynManager *manager) for(k=0;k<DYN_MANAGER_MAX_NUM_DEVICES;k++) { manager->operations[i].sync_op[j].ids[k]=0xFF; - manager->operations[i].sync_op[j].data[k].data_addr=0x00000000; + manager->operations[i].sync_op[j].data[k]=0x00000000; } manager->operations[i].sync_op[j].address=0x0000; manager->operations[i].sync_op[j].length=0x0000; @@ -235,7 +258,7 @@ void dyn_manager_init(TDynManager *manager) manager->operations[i].bulk_op[j].ids[k]=0xFF; manager->operations[i].bulk_op[j].address[k]=0x0000; manager->operations[i].bulk_op[j].length[k]=0x0000; - manager->operations[i].bulk_op[j].data[k].data_addr=0x00000000; + manager->operations[i].bulk_op[j].data[k]=0x00000000; } } manager->operations[i].num_bulk_op=0; @@ -258,6 +281,45 @@ void dyn_manager_init(TDynManager *manager) manager->op_handles[i].handle_index=i; } manager->num_ops=0x00; + /* initialize timer */ + if(manager->init_timer!=0x00000000) + manager->init_timer(); + manager->set_period(DEFAULT_DYN_MANAGER_PERIOD); + + /* initialize memory module */ + mem_module_init(&manager->mem_module); + manager->mem_module.data=manager; + manager->mem_module.write_cmd=(void(*)(void *,unsigned short int, unsigned short int,unsigned char *))dyn_manager_write_cmd; + manager->mem_module.read_cmd=(void(*)(void *,unsigned short int, unsigned short int,unsigned char *))dyn_manager_read_cmd; + if(!mem_module_add_ram_segment(&manager->mem_module,RAM_DYN_MANAGER_BASE_ADDRESS,RAM_DYN_MANAGER_LENGTH)) + return 0x00; + if(!mem_module_add_eeprom_segment(&manager->mem_module,EEPROM_DYN_MANAGER_BASE_ADDRESS,EEPROM_DYN_MANAGER_LENGTH)) + return 0x00; + if(!mem_add_module(memory,&manager->mem_module)) + return 0x00; + + return 0x01; +} + +void dyn_manager_set_period(TDynManager *manager,unsigned short int period_us) +{ + unsigned char i; + + if(manager->set_period!=0x00000000) + { + manager->set_period(period_us); + for(i=0;i<DYN_MANAGER_MAX_NUM_MODULES;i++) + if(manager->modules[i]!=0x00000000 && manager->modules[i]->set_period!=0x00000000) + manager->modules[i]->set_period(manager->modules[i]->data,period_us); + manager->period_us=period_us; + ram_data[DYN_MANAGER_PERIOD]=period_us%256; + ram_data[DYN_MANAGER_PERIOD+1]=period_us/256; + } +} + +unsigned short int dyn_manager_get_period(TDynManager *manager) +{ + return manager->period_us; } void dyn_manager_scan(TDynManager *manager) @@ -266,11 +328,6 @@ void dyn_manager_scan(TDynManager *manager) unsigned char i,j,k,num=0; unsigned short int model; - // call all the modules init function - for(i=0;i<manager->num_modules;i++) - if(manager->modules[i]->init!=0x00000000) - manager->modules[i]->init(); - // scan all dynamixel buses for(i=0;i<manager->num_masters;i++) { @@ -287,8 +344,9 @@ void dyn_manager_scan(TDynManager *manager) manager->devices[servo_ids[j]].master_index=i; manager->num_devices++; // pass the infor to each of the modules - for(k=0;k<manager->num_modules;k++) - dyn_module_add_device(manager->modules[k],servo_ids[j],model); + for(k=0;k<DYN_MANAGER_MAX_NUM_MODULES;k++) + if(manager->modules[k]!=0x00000000) + dyn_module_add_device(manager->modules[k],servo_ids[j],model); } // else ignore device } @@ -296,7 +354,7 @@ void dyn_manager_scan(TDynManager *manager) // configure the operations for(i=0;i<manager->num_modules;i++) if(manager->modules[i]->setup!=0x00000000) - manager->modules[i]->setup(); + manager->modules[i]->setup(manager->modules[i]->data); } void dyn_manager_add_master(TDynManager *manager,TDynamixelMaster *master) @@ -310,6 +368,7 @@ void dyn_manager_add_master(TDynManager *manager,TDynamixelMaster *master) { manager->masters[i]=master; manager->num_masters++; + ram_data[DYN_MANAGER_NUM_MASTERS]++; } } @@ -318,6 +377,19 @@ unsigned char dyn_manager_get_num_masters(TDynManager *manager) return manager->num_masters; } +TDynamixelMaster *dyn_manager_get_device_master(TDynManager *manager,unsigned char device_id) +{ + if(dyn_manager_check_id(manager,device_id)!=0xFF) + return manager->masters[dyn_manager_check_id(manager,device_id)]; + else + return 0x00000000; +} + +unsigned short int dyn_manager_get_device_model(TDynManager *manager,unsigned char device_id) +{ + return manager->devices[device_id].model; +} + void dyn_manager_add_module(TDynManager *manager,TDynModule *module) { unsigned char i; @@ -330,6 +402,7 @@ void dyn_manager_add_module(TDynManager *manager,TDynModule *module) manager->modules[i]=module; manager->num_modules++; module->manager=manager; + ram_data[DYN_MANAGER_NUM_MODULES]++; } } @@ -489,7 +562,7 @@ void dyn_manager_single_op_change_data(TDynManager *manager,OP_HANDLE *op,unsign } // sync operation functions -OP_HANDLE *dyn_manager_sync_op_new(TDynManager *manager,unsigned char mode,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, TWriteData *data) +OP_HANDLE *dyn_manager_sync_op_new(TDynManager *manager,unsigned char mode,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, unsigned char * const data[]) { unsigned char i,master_index,current_op_index; TDynManagerSyncOp *current_op; @@ -522,7 +595,7 @@ OP_HANDLE *dyn_manager_sync_op_new(TDynManager *manager,unsigned char mode,unsig current_op->address=address; current_op->length=length; current_op->ids[current_op->num_devices]=ids[i]; - current_op->data[current_op->num_devices].data_addr=data[i].data_addr; + current_op->data[current_op->num_devices]=data[i]; current_op->num_devices++; manager->operations[master_index].num_sync_op++; } @@ -540,7 +613,7 @@ OP_HANDLE *dyn_manager_sync_op_new(TDynManager *manager,unsigned char mode,unsig if(dyn_manager_id_present(current_op->num_devices,current_op->ids,ids[i])==0x00) { current_op->ids[current_op->num_devices]=ids[i]; - current_op->data[current_op->num_devices].data_addr=data[i].data_addr; + current_op->data[current_op->num_devices]=data[i]; current_op->num_devices++; } } @@ -549,7 +622,7 @@ OP_HANDLE *dyn_manager_sync_op_new(TDynManager *manager,unsigned char mode,unsig return op_handle; } -void dyn_manager_sync_op_add_devices(TDynManager *manager,OP_HANDLE *op,unsigned char num,unsigned char *ids,TWriteData *data) +void dyn_manager_sync_op_add_devices(TDynManager *manager,OP_HANDLE *op,unsigned char num,unsigned char *ids,unsigned char * const data[]) { TDynManagerSyncOp *current_op; unsigned char i,j; @@ -566,7 +639,7 @@ void dyn_manager_sync_op_add_devices(TDynManager *manager,OP_HANDLE *op,unsigned if(current_op->num_devices<DYN_MANAGER_MAX_NUM_DEVICES) { current_op->ids[current_op->num_devices]=ids[i]; - current_op->data[current_op->num_devices].data_addr=data[i].data_addr; + current_op->data[current_op->num_devices]=data[i]; current_op->num_devices++; } } @@ -594,13 +667,13 @@ void dyn_manager_sync_op_remove_devices(TDynManager *manager,OP_HANDLE *op,unsig for(k=device_index;k<current_op->num_devices;k++) { current_op->ids[k]=current_op->ids[k+1]; - current_op->data[k].data_addr=current_op->data[k+1].data_addr; + current_op->data[k]=current_op->data[k+1]; } } else { current_op->ids[device_index]=0xFF; - current_op->data[device_index].data_addr=0x00000000; + current_op->data[device_index]=0x00000000; } current_op->num_devices--; } @@ -633,7 +706,7 @@ void dyn_manager_sync_op_change_length(TDynManager *manager,OP_HANDLE *op,unsign } } -void dyn_manager_sync_op_change_data(TDynManager *manager,OP_HANDLE *op,unsigned char num,unsigned char *ids,TWriteData *data) +void dyn_manager_sync_op_change_data(TDynManager *manager,OP_HANDLE *op,unsigned char num,unsigned char *ids,unsigned char * const data[]) { TDynManagerSyncOp *current_op; unsigned char i,j,device_index; @@ -646,14 +719,14 @@ void dyn_manager_sync_op_change_data(TDynManager *manager,OP_HANDLE *op,unsigned for(j=0;j<num;j++) { if((device_index=dyn_manager_id_present(current_op->num_devices,current_op->ids,ids[j]))!=0x00) - current_op->data[device_index].data_addr=data[j].data_addr; + current_op->data[device_index]=data[j]; } } } } // bulk operation functions -OP_HANDLE *dyn_manager_bulk_op_new(TDynManager *manager,unsigned char mode,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, TWriteData *data) +OP_HANDLE *dyn_manager_bulk_op_new(TDynManager *manager,unsigned char mode,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, unsigned char * const data[]) { unsigned char i,master_index,current_op_index; TDynManagerBulkOp *current_op; @@ -686,7 +759,7 @@ OP_HANDLE *dyn_manager_bulk_op_new(TDynManager *manager,unsigned char mode,unsig current_op->address[current_op->num_devices]=address[i]; current_op->length[current_op->num_devices]=length[i]; current_op->ids[current_op->num_devices]=ids[i]; - current_op->data[current_op->num_devices].data_addr=data[i].data_addr; + current_op->data[current_op->num_devices]=data[i]; current_op->num_devices++; manager->operations[master_index].num_bulk_op++; } @@ -706,7 +779,7 @@ OP_HANDLE *dyn_manager_bulk_op_new(TDynManager *manager,unsigned char mode,unsig current_op->address[current_op->num_devices]=address[i]; current_op->length[current_op->num_devices]=length[i]; current_op->ids[current_op->num_devices]=ids[i]; - current_op->data[current_op->num_devices].data_addr=data[i].data_addr; + current_op->data[current_op->num_devices]=data[i]; current_op->num_devices++; } } @@ -715,7 +788,7 @@ OP_HANDLE *dyn_manager_bulk_op_new(TDynManager *manager,unsigned char mode,unsig return op_handle; } -void dyn_manager_bulk_op_add_devices(TDynManager *manager,OP_HANDLE *op,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length,TWriteData *data) +void dyn_manager_bulk_op_add_devices(TDynManager *manager,OP_HANDLE *op,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length,unsigned char * const data[]) { TDynManagerBulkOp *current_op; unsigned char i,j; @@ -734,7 +807,7 @@ void dyn_manager_bulk_op_add_devices(TDynManager *manager,OP_HANDLE *op,unsigned current_op->ids[current_op->num_devices]=ids[i]; current_op->address[current_op->num_devices]=address[i]; current_op->length[current_op->num_devices]=length[i]; - current_op->data[current_op->num_devices].data_addr=data[i].data_addr; + current_op->data[current_op->num_devices]=data[i]; current_op->num_devices++; } } @@ -764,7 +837,7 @@ void dyn_manager_bulk_op_remove_devices(TDynManager *manager,OP_HANDLE *op,unsig current_op->ids[k]=current_op->ids[k+1]; current_op->address[k]=current_op->address[k+1]; current_op->length[k]=current_op->length[k+1]; - current_op->data[k].data_addr=current_op->data[k+1].data_addr; + current_op->data[k]=current_op->data[k+1]; } } else @@ -772,7 +845,7 @@ void dyn_manager_bulk_op_remove_devices(TDynManager *manager,OP_HANDLE *op,unsig current_op->ids[device_index]=0xFF; current_op->address[device_index]=0x0000; current_op->length[device_index]=0x0000; - current_op->data[device_index].data_addr=0x00000000; + current_op->data[device_index]=0x00000000; } current_op->num_devices--; } @@ -821,7 +894,7 @@ void dyn_manager_bulk_op_change_length(TDynManager *manager,OP_HANDLE *op,unsign } } -void dyn_manager_bulk_op_change_data(TDynManager *manager,OP_HANDLE *op,unsigned char num,unsigned char *ids,TWriteData *data) +void dyn_manager_bulk_op_change_data(TDynManager *manager,OP_HANDLE *op,unsigned char num,unsigned char *ids,unsigned char * const data[]) { TDynManagerBulkOp *current_op; unsigned char i,j,device_index; @@ -834,7 +907,7 @@ void dyn_manager_bulk_op_change_data(TDynManager *manager,OP_HANDLE *op,unsigned for(j=0;j<num;j++) { if((device_index=dyn_manager_id_present(current_op->num_devices,current_op->ids,ids[j]))!=0x00) - current_op->data[device_index].data_addr=data[j].data_addr; + current_op->data[device_index]=data[j]; } } } @@ -867,11 +940,14 @@ void dyn_manager_loop(TDynManager *manager) // execute the pre_process functions for all modules for(i=0;i<manager->num_modules;i++) { - modules_period_count[i]++; - if(modules_period_count[i]==manager->modules[i]->period_count) + if(manager->modules[i]->enabled) { - if(manager->modules[i]->pre_process!=0x00000000) - manager->modules[i]->pre_process(); + modules_period_count[i]++; + if(modules_period_count[i]==manager->modules[i]->period_count) + { + if(manager->modules[i]->pre_process!=0x00000000) + manager->modules[i]->pre_process(manager->modules[i]->data); + } } } @@ -882,7 +958,7 @@ void dyn_manager_loop(TDynManager *manager) switch(states[i]) { case start_single_ops: if(current_single_op[i]>=manager->operations[i].num_single_op) - states[i]=start_sync_ops; + states[i]=start_bulk_ops; else { single_op_period_count[current_single_op[i]]++; @@ -921,7 +997,10 @@ void dyn_manager_loop(TDynManager *manager) } break; case start_sync_ops: if(current_sync_op[i]>=manager->operations[i].num_sync_op) - states[i]=start_bulk_ops; + { + states[i]=ops_done; + loop_done--; + } else { sync_op_period_count[current_sync_op[i]]++; @@ -960,10 +1039,7 @@ void dyn_manager_loop(TDynManager *manager) } break; case start_bulk_ops: if(current_bulk_op[i]>=manager->operations[i].num_bulk_op) - { - states[i]=ops_done; - loop_done--; - } + states[i]=start_sync_ops; else { bulk_op_period_count[current_bulk_op[i]]++; @@ -1010,11 +1086,14 @@ void dyn_manager_loop(TDynManager *manager) // execute the pre_process functions for all modules for(i=0;i<manager->num_modules;i++) { - if(modules_period_count[i]==manager->modules[i]->period_count) + if(manager->modules[i]->enabled) { - if(manager->modules[i]->post_process!=0x00000000) - manager->modules[i]->post_process(); - modules_period_count[i]=0; + if(modules_period_count[i]==manager->modules[i]->period_count) + { + if(manager->modules[i]->post_process!=0x00000000) + manager->modules[i]->post_process(manager->modules[i]->data); + modules_period_count[i]=0; + } } } } diff --git a/dynamixel_manager/src/dyn_module.c b/dynamixel_manager/src/dyn_module.c index a84e3451ca878eb4b5f45860ad27cda97a92f04e..4207dee7a4b61ba1383b94fe7d4d4b9441326fe2 100644 --- a/dynamixel_manager/src/dyn_module.c +++ b/dynamixel_manager/src/dyn_module.c @@ -1,6 +1,6 @@ #include "dyn_module.h" -void dyn_module_init(TDynModule *module) +unsigned char dyn_module_init(TDynModule *module) { unsigned char i; @@ -11,11 +11,29 @@ void dyn_module_init(TDynModule *module) module->assigned_ids[i]=0x00; module->num_assigned_ids=0x00; module->period_count=0x00; - module->init=0x00000000; + module->enabled=0x00; module->add_device=0x00000000; module->setup=0x00000000; module->pre_process=0x00000000; module->post_process=0x00000000; + module->data=0x00000000; + + return 0x01; +} + +void dyn_module_enable(TDynModule *module) +{ + module->enabled=0x01; +} + +void dyn_module_disable(TDynModule *module) +{ + module->enabled=0x00; +} + +unsigned char dyn_module_is_enabled(TDynModule *module) +{ + return module->enabled; } void dyn_module_add_model(TDynModule *module,unsigned short int model) @@ -45,12 +63,11 @@ void dyn_module_add_device(TDynModule *module,unsigned char id, unsigned short i { if(model==module->models[i]) { + module->assigned_ids[id]=0x01; + module->num_assigned_ids++; if(module->add_device!=0x00000000) - { - module->assigned_ids[id]=0x01; - module->num_assigned_ids++; - module->add_device(id,model); - } + module->add_device(module->data,id,model); + break; } } } @@ -81,28 +98,4 @@ unsigned char dyn_module_get_period(TDynModule *module) return module->period_count; } -void dyn_module_set_init_fctn(TDynModule *module,void (*init)(void)) -{ - module->init=init; -} - -void dyn_module_set_add_device_fctn(TDynModule *module,void (*add_device)(unsigned char id,unsigned short int model)) -{ - module->add_device=add_device; -} - -void dyn_module_set_setup_fctn(TDynModule *module,void (*setup)(void)) -{ - module->setup=setup; -} - -void dyn_module_set_pre_process_fctn(TDynModule *module,void (*pre_process)(void)) -{ - module->pre_process=pre_process; -} - -void dyn_module_set_post_process_fctn(TDynModule *module,void (*post_process)(void)) -{ - module->post_process=post_process; -} diff --git a/dynamixel_manager/src/modules/action.c b/dynamixel_manager/src/modules/action.c new file mode 100755 index 0000000000000000000000000000000000000000..ea816ab70fd8b5fb941a6d98c1ecd37825016141 --- /dev/null +++ b/dynamixel_manager/src/modules/action.c @@ -0,0 +1,556 @@ +#include "action.h" +#include "motion_pages.h" + +#define SPEED_BASE_SCHEDULE 0x00 +#define TIME_BASE_SCHEDULE 0x0A + +// private variables +typedef enum {ACTION_PRE,ACTION_MAIN,ACTION_POST,ACTION_PAUSE} action_states; +TMotionModule action_module; + +TPage action_next_page; +TPage action_current_page; +unsigned char action_current_step_index; +unsigned char action_current_page_index; +// angle variables +long long int action_moving_angles[PAGE_MAX_NUM_SERVOS];// fixed point 48|16 format +long long int action_start_angles[PAGE_MAX_NUM_SERVOS];// fixed point 48|16 format +// speed variables +long long int action_start_speed[PAGE_MAX_NUM_SERVOS];// fixed point 48|16 format +long long int action_main_speed[PAGE_MAX_NUM_SERVOS];// fixed point 48|16 format +// control variables +unsigned char action_end,action_stop; +unsigned char action_zero_speed_finish[PAGE_MAX_NUM_SERVOS]; +unsigned char action_next_index; +unsigned char action_running; +// time variables (in time units (7.8 ms each time unit)) +long long int action_total_time;// fixed point 48|16 format +long long int action_pre_time;// fixed point 48|16 format +long long int action_main_time;// fixed point 48|16 format +long long int action_step_time;// fixed point 48|16 format +long long int action_pause_time;// fixed point 48|16 format +long long int action_current_time;// fixed point 48|16 format +long long int action_section_time;// fixed point 48|16 format +long long int action_period; + +// private functions +void action_load_next_step(void) +{ + // page and step information + static char num_repetitions=0; + // angle variables + short int next_angle;// information from the flash memory (fixed point 9|7 format) + long long int max_angle;// fixed point format 48|16 format + long long int tmp_angle;// fixed point format 48|16 format + long long int angle;// fixed point format 48|16 format + long long int target_angles;// fixed point 48|16 format + long long int next_angles;// fixed point 48|16 format + long long int action_pre_time_initial_speed_angle;// fixed point 48|16 format + long long int moving_angle;// fixed point 48|16 format + // time variables + long long int accel_time_num;// fixed point 48|16 format + long long int zero_speed_divider;// fixed point 48|16 format + long long int non_zero_speed_divider;// fixed point 48|16 format + // control variables + unsigned char dir_change; + // control information + unsigned char i=0; + + action_current_step_index++; + if(action_current_step_index>=action_current_page.header.stepnum)// the last step has been executed + { + if(action_current_page.header.stepnum==1) + { + if(action_stop) + action_next_index=action_current_page.header.exit; + else + { + num_repetitions--; + if(num_repetitions>0) + action_next_index=action_current_page_index; + else + action_next_index=action_current_page.header.next; + } + if(action_next_index==0) + action_end=0x01; + else + { + if(action_next_index!=action_current_page_index) + { + pages_get_page(action_next_index,&action_next_page); + if(!pages_check_checksum(&action_next_page)) + action_end=0x01; + } + if(action_next_page.header.repeat==0 || action_next_page.header.stepnum==0) + action_end=0x01; + } + } + pages_copy_page(&action_next_page,&action_current_page);// make the next page active + if(action_current_page_index!=action_next_index) + num_repetitions=action_current_page.header.repeat; + action_current_step_index=0; + action_current_page_index=action_next_index; + } + else if(action_current_step_index==action_current_page.header.stepnum-1)// it is the last step + { + if(action_stop) + action_next_index=action_current_page.header.exit; + else + { + num_repetitions--; + if(num_repetitions>0) + action_next_index=action_current_page_index; + else + action_next_index=action_current_page.header.next; + } + if(action_next_index==0) + action_end=0x01; + else + { + if(action_next_index!=action_current_page_index) + { + pages_get_page(action_next_index,&action_next_page); + if(!pages_check_checksum(&action_next_page)) + action_end=0x01; + } + if(action_next_page.header.repeat==0 || action_next_page.header.stepnum==0) + action_end=0x01; + } + } + // compute trajectory + action_pause_time=(action_current_page.steps[action_current_step_index].pause*action_current_page.header.speed)>>5; + action_pause_time=action_pause_time<<9; + action_step_time=(action_current_page.steps[action_current_step_index].time*action_current_page.header.speed)>>5; + action_step_time=action_step_time<<9; + if(action_step_time<action_period) + action_step_time=action_period;// 0.078 in 16|16 format + max_angle=0; + for(i=0;i<PAGE_MAX_NUM_SERVOS;i++) + { + if(mmanager_get_module(action_module.manager,i)==MM_ACTION) + { + angle=action_current_page.steps[action_current_step_index].position[i]; + if(angle==0x5A00)// bigger than 180 + target_angles=action_module.manager->servo_values[i].target_angle; + else + target_angles=angle<<9; + action_moving_angles[i]=target_angles-action_module.manager->servo_values[i].target_angle; + if(action_end) + next_angles=target_angles; + else + { + if(action_current_step_index==action_current_page.header.stepnum-1) + next_angle=action_next_page.steps[0].position[i]; + else + next_angle=action_current_page.steps[action_current_step_index+1].position[i]; + if(next_angle==0x5A00) + next_angles=target_angles; + else + next_angles=next_angle<<9; + } + // check changes in direction + if(((action_module.manager->servo_values[i].target_angle < target_angles) && (target_angles < next_angles)) || + ((action_module.manager->servo_values[i].target_angle > target_angles) && (target_angles > next_angles))) + dir_change=0x00; + else + dir_change=0x01; + // check the type of ending + if(dir_change || action_pause_time>0 || action_end) + action_zero_speed_finish[i]=0x01; + else + action_zero_speed_finish[i]=0x00; + // find the maximum angle of motion in speed base schedule + if(action_current_page.header.schedule==SPEED_BASE_SCHEDULE)// speed base schedule + { + // fer el valor absolut + if(action_moving_angles[i]<0) + tmp_angle=-action_moving_angles[i]; + else + tmp_angle=action_moving_angles[i]; + if(tmp_angle>max_angle) + max_angle=tmp_angle; + } + action_module.manager->servo_values[i].cw_compliance=(1<<(action_current_page.header.slope[i]&0x0F)); + action_module.manager->servo_values[i].ccw_compliance=(1<<((action_current_page.header.slope[i]&0xF0)>>4)); + } + } + if(action_current_page.header.schedule==SPEED_BASE_SCHEDULE) + action_step_time=(max_angle*256)/(action_current_page.steps[action_current_step_index].time*720); + accel_time_num=action_current_page.header.accel; + accel_time_num=accel_time_num<<9; + if(action_step_time<=(accel_time_num<<1)) + { + if(action_step_time==0) + accel_time_num=0; + else + { + accel_time_num=(action_step_time-action_period)>>1; + if(accel_time_num==0) + action_step_time=0; + } + } + action_total_time=action_step_time; + action_pre_time=accel_time_num; + action_main_time=action_total_time-action_pre_time; + non_zero_speed_divider=(action_pre_time>>1)+action_main_time; + if(non_zero_speed_divider<action_period) + non_zero_speed_divider=action_period; + zero_speed_divider=action_main_time; + if(zero_speed_divider<action_period) + zero_speed_divider=action_period; + for(i=0;i<PAGE_MAX_NUM_SERVOS;i++) + { + if(mmanager_get_module(action_module.manager,i)==MM_ACTION) + { + action_pre_time_initial_speed_angle=(action_start_speed[i]*action_pre_time)>>1; + moving_angle=action_moving_angles[i]<<16; + if(action_zero_speed_finish[i]) + action_main_speed[i]=(moving_angle-action_pre_time_initial_speed_angle)/zero_speed_divider; + else + action_main_speed[i]=(moving_angle-action_pre_time_initial_speed_angle)/non_zero_speed_divider; +// if((action_main_speed[i]>>16)>info.max_speed) +// action_main_speed[i]=(info.max_speed*65536); +// else if((action_main_speed[i]>>16)<-info.max_speed) +// action_main_speed[i]=-(info.max_speed*65536); + } + } +} + +void action_finish(void) +{ + // set the internal state machine to the idle state + action_end=0x00; + // change the internal state + action_running=0x00; +} + +void action_set_period(void *module,unsigned short int period_us) +{ + // load the current period + action_period=(period_us<<16)/1000000; +} + +void action_set_module(void *module,unsigned char servo_id) +{ + +} + +void action_pre_process(void *module) +{ + unsigned char i; + // angle variables + static long long int accel_angles[PAGE_MAX_NUM_SERVOS];// fixed point 48|16 format + static long long int main_angles[PAGE_MAX_NUM_SERVOS];// fixed point 48|16 format + // speed variables (in controller units) + static long long int current_speed[PAGE_MAX_NUM_SERVOS]={0};// fixed point 48|16 format + long long int delta_speed; + // control variables + static action_states state=ACTION_PAUSE; + + if(action_running==0x01) + { + action_current_time+=action_period; + switch(state) + { + case ACTION_PRE: if(action_current_time>action_section_time) + { + for(i=0;i<PAGE_MAX_NUM_SERVOS;i++) + { + if(mmanager_get_module(action_module.manager,i)==MM_ACTION) + { + /* the last segment of the pre section */ + delta_speed=(action_main_speed[i]-action_start_speed[i]); + current_speed[i]=action_start_speed[i]+delta_speed; + accel_angles[i]=(action_start_speed[i]*action_section_time+((delta_speed*action_section_time)>>1))>>16; + action_module.manager->servo_values[i].target_angle=action_start_angles[i]+accel_angles[i]; + /* update of the state */ + if(!action_zero_speed_finish[i]) + { + if((action_step_time-action_pre_time)==0) + main_angles[i]=0; + else + main_angles[i]=((action_moving_angles[i]-accel_angles[i])*(action_step_time-(action_pre_time<<1)))/(action_step_time-action_pre_time); + action_start_angles[i]=action_module.manager->servo_values[i].target_angle; + } + else + { + main_angles[i]=action_moving_angles[i]-accel_angles[i]-(((action_main_speed[i]*action_pre_time)>>1)>>16); + action_start_angles[i]=action_module.manager->servo_values[i].target_angle; + } + /* the first step of the main section */ + action_module.manager->servo_values[i].target_angle=action_start_angles[i]+(main_angles[i]*(action_current_time-action_section_time))/(action_step_time-(action_pre_time<<1)); + current_speed[i]=action_main_speed[i]; + } + } + action_current_time=action_current_time-action_section_time; + action_section_time=action_step_time-(action_pre_time<<1); + state=ACTION_MAIN; + } + else + { + for(i=0;i<PAGE_MAX_NUM_SERVOS;i++) + { + if(mmanager_get_module(action_module.manager,i)==MM_ACTION) + { + delta_speed=((action_main_speed[i]-action_start_speed[i])*action_current_time)/action_section_time; + current_speed[i]=action_start_speed[i]+delta_speed; + accel_angles[i]=((action_start_speed[i]*action_current_time)+((delta_speed*action_current_time)>>1))>>16; + action_module.manager->servo_values[i].target_angle=action_start_angles[i]+accel_angles[i]; + } + } + state=ACTION_PRE; + } + break; + case ACTION_MAIN: if(action_current_time>action_section_time) + { + for(i=0;i<PAGE_MAX_NUM_SERVOS;i++) + { + if(mmanager_get_module(action_module.manager,i)==MM_ACTION) + { + /* last segment of the main section */ + action_module.manager->servo_values[i].target_angle=action_start_angles[i]+main_angles[i]; + current_speed[i]=action_main_speed[i]; + /* update state */ + action_start_angles[i]=action_module.manager->servo_values[i].target_angle; + main_angles[i]=action_moving_angles[i]-main_angles[i]-accel_angles[i]; + /* first segment of the post section */ + if(action_zero_speed_finish[i]) + { + delta_speed=((0.0-action_main_speed[i])*(action_current_time-action_section_time))/action_pre_time; + current_speed[i]=action_main_speed[i]+delta_speed; + action_module.manager->servo_values[i].target_angle=action_start_angles[i]+(((action_main_speed[i]*(action_current_time-action_section_time))+((delta_speed*(action_current_time-action_section_time))>>1))>>16); + } + else + { + action_module.manager->servo_values[i].target_angle=action_start_angles[i]+((main_angles[i]*(action_current_time-action_section_time))/action_pre_time); + current_speed[i]=action_main_speed[i]; + } + } + } + action_current_time=action_current_time-action_section_time; + action_section_time=action_pre_time; + state=ACTION_POST; + } + else + { + for(i=0;i<PAGE_MAX_NUM_SERVOS;i++) + { + if(mmanager_get_module(action_module.manager,i)==MM_ACTION) + { + action_module.manager->servo_values[i].target_angle=action_start_angles[i]+(main_angles[i]*action_current_time)/action_section_time; + current_speed[i]=action_main_speed[i]; + } + } + state=ACTION_MAIN; + } + break; + case ACTION_POST: if(action_current_time>action_section_time) + { + for(i=0;i<PAGE_MAX_NUM_SERVOS;i++) + { + if(mmanager_get_module(action_module.manager,i)==MM_ACTION) + { + /* last segment of the post section */ + if(action_zero_speed_finish[i]) + { + delta_speed=-action_main_speed[i]; + current_speed[i]=action_main_speed[i]+delta_speed; + action_module.manager->servo_values[i].target_angle=action_start_angles[i]+(((action_main_speed[i]*action_section_time)+((delta_speed*action_section_time)>>1))>>16); + } + else + { + action_module.manager->servo_values[i].target_angle=action_start_angles[i]+main_angles[i]; + current_speed[i]=action_main_speed[i]; + } + /* update state */ + action_start_angles[i]=action_module.manager->servo_values[i].target_angle; + action_start_speed[i]=current_speed[i]; + } + } + /* load the next step */ + if(action_pause_time==0) + { + if(action_end) + { + state=ACTION_PAUSE; + action_finish(); + } + else + { + action_load_next_step(); + /* first step of the next section */ + for(i=0;i<PAGE_MAX_NUM_SERVOS;i++) + { + if(mmanager_get_module(action_module.manager,i)==MM_ACTION) + { + delta_speed=((action_main_speed[i]-action_start_speed[i])*(action_current_time-action_section_time))/action_pre_time; + current_speed[i]=action_start_speed[i]+delta_speed; + accel_angles[i]=(((action_start_speed[i]*(action_current_time-action_section_time))+((delta_speed*(action_current_time-action_section_time))>>1))>>16); + action_module.manager->servo_values[i].target_angle=action_start_angles[i]+accel_angles[i]; + } + } + action_current_time=action_current_time-action_section_time; + action_section_time=action_pre_time; + state=ACTION_PRE; + } + } + else + { + action_current_time=action_current_time-action_section_time; + action_section_time=action_pause_time; + state=ACTION_PAUSE; + } + } + else + { + for(i=0;i<PAGE_MAX_NUM_SERVOS;i++) + { + if(mmanager_get_module(action_module.manager,i)==MM_ACTION) + { + if(action_zero_speed_finish[i]) + { + delta_speed=((0.0-action_main_speed[i])*action_current_time)/action_section_time; + current_speed[i]=action_main_speed[i]+delta_speed; + action_module.manager->servo_values[i].target_angle=action_start_angles[i]+(((action_main_speed[i]*action_current_time)+((delta_speed*action_current_time)>>1))>>16); + } + else + { + action_module.manager->servo_values[i].target_angle=action_start_angles[i]+(main_angles[i]*action_current_time)/action_section_time; + current_speed[i]=action_main_speed[i]; + } + } + } + state=ACTION_POST; + } + break; + case ACTION_PAUSE: if(action_current_time>action_section_time) + { + if(action_end) + { + state=ACTION_PAUSE; + action_finish(); + } + else + { + // find next page to execute + action_load_next_step(); + /* first step of the next section */ + for(i=0;i<PAGE_MAX_NUM_SERVOS;i++) + { + if(mmanager_get_module(action_module.manager,i)==MM_ACTION) + { + delta_speed=((action_main_speed[i]-action_start_speed[i])*(action_current_time-action_section_time))/action_pre_time; + current_speed[i]=action_start_speed[i]+delta_speed; + accel_angles[i]=(((action_start_speed[i]*(action_current_time-action_section_time))+((delta_speed*(action_current_time-action_section_time))>>1))>>16); + action_module.manager->servo_values[i].target_angle=action_start_angles[i]+accel_angles[i]; + } + } + action_current_time=action_current_time-action_section_time; + action_section_time=action_pre_time; + state=ACTION_PRE; + } + } + else// pause section still active + state=ACTION_PAUSE; + break; + default: break; + } + } +} + +// public functions +void action_init(void) +{ + unsigned char i; + + /* initialize the motion module */ + mmodule_init(&action_module); + action_module.id=MM_ACTION; + action_module.set_period=action_set_period; + action_module.set_module=action_set_module; + action_module.pre_process=action_pre_process; + + // init manager_current_angles with the current position of the servos + for(i=0;i<PAGE_MAX_NUM_SERVOS;i++) + { + // angle variables + action_moving_angles[i]=0;// fixed point 48|16 format + action_start_angles[i]=0; + // speed variables + action_start_speed[i]=0;// fixed point 48|16 format + action_main_speed[i]=0;// fixed point 48|16 format + // control variables + action_zero_speed_finish[i]=0x00; + } + // clear the contents of the next page + pages_clear_page(&action_next_page); + pages_clear_page(&action_current_page); + action_current_page_index=0; + action_current_step_index=-1; + // control variables + action_end=0x00; + action_stop=0x00; + action_running=0x00; + action_next_index=0; + // time variables (in time units (7.8 ms each time unit)) + action_total_time=0;// fixed point 48|16 format + action_pre_time=0;// fixed point 48|16 format + action_main_time=0;// fixed point 48|16 format + action_step_time=0;// fixed point 48|16 format + action_pause_time=0;// fixed point 48|16 format + action_current_time=0;// fixed point 48|16 format + action_section_time=0;// fixed point 48|16 format + action_period=(DYN_MANAGER_DEFAULT_PERIOD_US<<16)/1000000; +} + +TMotionModule *action_get_module(void) +{ + return &action_module; +} + +unsigned char action_load_page(unsigned char page_id) +{ + action_next_index=page_id; + pages_get_page(action_next_index,&action_current_page); + pages_get_page(action_current_page.header.next,&action_next_page); + if(!pages_check_checksum(&action_current_page)) + return 0x00; + if(!pages_check_checksum(&action_next_page)) + return 0x00; + + return 0x01; +} + +void action_start_page(void) +{ + unsigned char i; + + for(i=0;i<PAGE_MAX_NUM_SERVOS;i++) + action_start_angles[i]=action_module.manager->servo_values[i].target_angle; + action_stop=0x00; + action_current_time=0; + action_section_time=0; + action_current_step_index=-1; + /* clear the interrupt flag */ + action_running=0x01; +} + +void action_stop_page(void) +{ + action_stop=0x01; +} + +unsigned char action_is_running(void) +{ + return action_running; +} + +unsigned char action_get_current_page(void) +{ + return action_current_page_index; +} + +unsigned char action_get_current_step(void) +{ + return action_current_step_index; +} diff --git a/dynamixel_manager/src/modules/motion_manager.c b/dynamixel_manager/src/modules/motion_manager.c new file mode 100644 index 0000000000000000000000000000000000000000..9c8c7dc992e7f8098e6ab3ba4c4c4fcedf20625f --- /dev/null +++ b/dynamixel_manager/src/modules/motion_manager.c @@ -0,0 +1,449 @@ +#include "motion_manager.h" +#include "motion_module.h" +#include "dyn_devices.h" +#include "dyn_servos.h" + +/* private variables */ +uint16_t mm_eeprom_data[] __attribute__ ((section (".eeprom")))={DEFAULT_ADC_PERIOD,ADC_PERIOD}; + +/* private functions */ +void mmanager_read_cmd(void *module,unsigned short int address,unsigned short int length,unsigned char *data) +{ + +} + +void mmanager_write_cmd(void *module,unsigned short int address,unsigned short int length,unsigned char *data) +{ + +} + +unsigned short int mmanager_angle_to_value(TMotionManager *mmanager,unsigned char servo_id,short int angle); +inline unsigned short int mmanager_angle_to_value(TMotionManager *mmanager,unsigned char servo_id,short int angle) +{ + return ((angle+mmanager->servo_configs[servo_id].center_angle)*mmanager->servo_configs[servo_id].encoder_resolution)/mmanager->servo_configs[servo_id].max_angle; +} + +short int mmanager_value_to_angle(TMotionManager *mmanager,unsigned char servo_id,unsigned short int value); +inline short int mmanager_value_to_angle(TMotionManager *mmanager,unsigned char servo_id,unsigned short int value) +{ + return (((short int)((value*mmanager->servo_configs[servo_id].max_angle)/mmanager->servo_configs[servo_id].encoder_resolution))-mmanager->servo_configs[servo_id].center_angle); +} + +void mmanager_compute_targets(TMotionManager *mmanager) +{ + short int offsets[DYN_MANAGER_MAX_NUM_DEVICES]={0}; + unsigned char i; + short int angle; + + // compute the balance offsets + if(mmanager->balance_enabled==0x01) + mmanager->balance(offsets); + // convert the angles to digital values + for(i=0;i<DYN_MANAGER_MAX_NUM_DEVICES;i++) + { + if(mmanager->servo_values[i].enabled)// servo is enabled and present + { + angle=(mmanager->servo_values[i].target_angle>>9)+offsets[i]+(mmanager->servo_values[i].offset<<3); + //>>16 from the action codification, <<7 from the manager codification + mmanager->servo_values[i].target_value=mmanager_angle_to_value(mmanager,i,angle); + } + } +} + +void mmanager_compute_angles(TMotionManager *mmanager) +{ + unsigned char i; + + // convert the digital values to angles + for(i=0;i<DYN_MANAGER_MAX_NUM_DEVICES;i++) + { + if(mmanager->servo_values[i].enabled)// servo is enabled and present + mmanager->servo_values[i].current_angle=(mmanager_value_to_angle(mmanager,i,mmanager->servo_values[i].current_value)<<9); + } +} + +void mmanager_add_device(TMotionManager *mmanager,unsigned char id,unsigned short int model) +{ + unsigned short int cw_value,ccw_value; + + if(id<DYN_MANAGER_MAX_NUM_DEVICES) + { + switch(model) + { + case SERVO_DX113: mmanager->servo_configs[id].encoder_resolution=1023; + mmanager->servo_configs[id].gear_ratio=193; + mmanager->servo_configs[id].max_angle=300<<7; + mmanager->servo_configs[id].center_angle=150<<7; + mmanager->servo_configs[id].max_speed=324; + break; + case SERVO_DX116: mmanager->servo_configs[id].encoder_resolution=1023; + mmanager->servo_configs[id].gear_ratio=143; + mmanager->servo_configs[id].max_angle=300<<7; + mmanager->servo_configs[id].center_angle=150<<7; + mmanager->servo_configs[id].max_speed=354; + break; + case SERVO_DX117: mmanager->servo_configs[id].encoder_resolution=1023; + mmanager->servo_configs[id].gear_ratio=193; + mmanager->servo_configs[id].max_angle=300<<7; + mmanager->servo_configs[id].center_angle=150<<7; + mmanager->servo_configs[id].max_speed=510; + break; + case SERVO_AX12A: mmanager->servo_configs[id].encoder_resolution=1023; + mmanager->servo_configs[id].gear_ratio=254; + mmanager->servo_configs[id].max_angle=300<<7; + mmanager->servo_configs[id].center_angle=150<<7; + mmanager->servo_configs[id].max_speed=354; + break; + case SERVO_AX12W: mmanager->servo_configs[id].encoder_resolution=1023; + mmanager->servo_configs[id].gear_ratio=32; + mmanager->servo_configs[id].max_angle=300<<7; + mmanager->servo_configs[id].center_angle=150<<7; + mmanager->servo_configs[id].max_speed=2830; + break; + case SERVO_AX18A: mmanager->servo_configs[id].encoder_resolution=1023; + mmanager->servo_configs[id].gear_ratio=254; + mmanager->servo_configs[id].max_angle=300<<7; + mmanager->servo_configs[id].center_angle=150<<7; + mmanager->servo_configs[id].max_speed=582; + break; + case SERVO_RX10: mmanager->servo_configs[id].encoder_resolution=1023; + mmanager->servo_configs[id].gear_ratio=193; + mmanager->servo_configs[id].max_angle=300<<7; + mmanager->servo_configs[id].center_angle=150<<7; + mmanager->servo_configs[id].max_speed=324; + break; + case SERVO_MX12W: mmanager->servo_configs[id].encoder_resolution=4095; + mmanager->servo_configs[id].gear_ratio=32; + mmanager->servo_configs[id].max_angle=360<<7; + mmanager->servo_configs[id].center_angle=180<<7; + mmanager->servo_configs[id].max_speed=2820; + break; + case SERVO_MX28: mmanager->servo_configs[id].encoder_resolution=4095; + mmanager->servo_configs[id].gear_ratio=193; + mmanager->servo_configs[id].max_angle=360<<7; + mmanager->servo_configs[id].center_angle=180<<7; + mmanager->servo_configs[id].max_speed=330; + break; + case SERVO_RX24F: mmanager->servo_configs[id].encoder_resolution=1023; + mmanager->servo_configs[id].gear_ratio=193; + mmanager->servo_configs[id].max_angle=300<<7; + mmanager->servo_configs[id].center_angle=150<<7; + mmanager->servo_configs[id].max_speed=756; + break; + case SERVO_RX28: mmanager->servo_configs[id].encoder_resolution=1023; + mmanager->servo_configs[id].gear_ratio=193; + mmanager->servo_configs[id].max_angle=300<<7; + mmanager->servo_configs[id].center_angle=150<<7; + mmanager->servo_configs[id].max_speed=402; + break; + case SERVO_RX64: mmanager->servo_configs[id].encoder_resolution=1023; + mmanager->servo_configs[id].gear_ratio=200; + mmanager->servo_configs[id].max_angle=300<<7; + mmanager->servo_configs[id].center_angle=150<<7; + mmanager->servo_configs[id].max_speed=294; + break; + case SERVO_MX64: mmanager->servo_configs[id].encoder_resolution=4095; + mmanager->servo_configs[id].gear_ratio=200; + mmanager->servo_configs[id].max_angle=360<<7; + mmanager->servo_configs[id].center_angle=180<<7; + mmanager->servo_configs[id].max_speed=378; + break; + case SERVO_EX106: mmanager->servo_configs[id].encoder_resolution=1023; + mmanager->servo_configs[id].gear_ratio=184; + mmanager->servo_configs[id].max_angle=250<<7; + mmanager->servo_configs[id].center_angle=125<<7; + mmanager->servo_configs[id].max_speed=414; + break; + case SERVO_MX106: mmanager->servo_configs[id].encoder_resolution=4095; + mmanager->servo_configs[id].gear_ratio=225; + mmanager->servo_configs[id].max_angle=360<<7; + mmanager->servo_configs[id].center_angle=180<<7; + mmanager->servo_configs[id].max_speed=270; + break; + case SERVO_XL320: mmanager->servo_configs[id].encoder_resolution=1023; + mmanager->servo_configs[id].gear_ratio=238; + mmanager->servo_configs[id].max_angle=300<<7; + mmanager->servo_configs[id].center_angle=150<<7; + mmanager->servo_configs[id].max_speed=684; + break; + default: break; + } + // get the servo's current position + if(model==SERVO_XL320) + { + dyn_master_read_word(dyn_manager_get_device_master(mmanager->dyn_module.manager,id),id,XL_PRESENT_POSITION_L,&mmanager->servo_values[id].current_value); + dyn_master_read_word(dyn_manager_get_device_master(mmanager->dyn_module.manager,id),id,XL_CW_ANGLE_LIMIT_L,&cw_value); + dyn_master_read_word(dyn_manager_get_device_master(mmanager->dyn_module.manager,id),id,XL_CCW_ANGLE_LIMIT_L,&ccw_value); + } + else + { + dyn_master_read_word(dyn_manager_get_device_master(mmanager->dyn_module.manager,id),id,P_PRESENT_POSITION_L,&mmanager->servo_values[id].current_value); + dyn_master_read_word(dyn_manager_get_device_master(mmanager->dyn_module.manager,id),id,P_CW_ANGLE_LIMIT_L,&cw_value); + dyn_master_read_word(dyn_manager_get_device_master(mmanager->dyn_module.manager,id),id,P_CCW_ANGLE_LIMIT_L,&ccw_value); + } + mmanager->servo_values[id].target_value=mmanager->servo_values[id].current_value; + mmanager->servo_values[id].current_angle=(mmanager_value_to_angle(mmanager,id,mmanager->servo_values[id].current_value)<<9); + mmanager->servo_values[id].target_angle=mmanager->servo_values[id].current_angle; + // read the servo limits + mmanager->servo_configs[id].cw_angle_limit=(mmanager_value_to_angle(mmanager,id,cw_value)<<9); + mmanager->servo_configs[id].ccw_angle_limit=(mmanager_value_to_angle(mmanager,id,ccw_value)<<9); + } +} + +void mmanager_setup(TMotionManager *mmanager) +{ + unsigned char * data[DYN_MANAGER_MAX_NUM_DEVICES]; + unsigned char ids[DYN_MANAGER_MAX_NUM_DEVICES]; + unsigned short int address[DYN_MANAGER_MAX_NUM_DEVICES]; + unsigned short int length[DYN_MANAGER_MAX_NUM_DEVICES]; + unsigned char i,num=0; + + // initialize the motion operation + for(i=0;i<DYN_MANAGER_MAX_NUM_DEVICES;i++) + { + if(mmanager->dyn_module.assigned_ids[i]==0x01) + { + data[num]=&mmanager->servo_values[i].cw_compliance; + ids[num]=i; + num++; + } + } + mmanager->motion_op=dyn_manager_sync_op_new(mmanager->dyn_module.manager,DYN_MANAGER_WRITE,num,ids,P_CW_COMPLIANCE_SLOPE,4,(unsigned char * const*)&data); + /* create a feedback operation with the servos that support bulk read */ + num=0; + for(i=0;i<DYN_MANAGER_MAX_NUM_DEVICES;i++) + { + if(mmanager->dyn_module.assigned_ids[i]==0x01) + { + if(HAS_BULK_READ(dyn_manager_get_device_model(mmanager->dyn_module.manager,i))) + { + if(dyn_manager_get_device_model(mmanager->dyn_module.manager,i)==SERVO_XL320) + address[num]=XL_PRESENT_POSITION_L; + else + address[num]=P_PRESENT_POSITION_L; + data[num]=(unsigned char *)&mmanager->servo_values[i].current_value; + length[num]=2; + ids[num]=i; + num++; + } + } + } + mmanager->feedback_op=dyn_manager_bulk_op_new(mmanager->dyn_module.manager,DYN_MANAGER_READ,num,ids,address,length,(unsigned char * const*)data); +} + +void mmanager_set_period(TMotionManager *mmanager,unsigned short int period_us) +{ + unsigned char i; + + for(i=0;i<MAX_NUM_MOTION_MODULES;i++) + if(mmanager->modules[i]->set_period!=0x00000000) + mmanager->modules[i]->set_period(mmanager->modules[i]->data,period_us*mmanager->dyn_module.period_count); +} + +void mmanager_pre_process(TMotionManager *mmanager) +{ + unsigned char i; + + for(i=0;i<MAX_NUM_MOTION_MODULES;i++) + { + if(mmanager->modules[i]!=0x00000000 && mmanager->modules[i]->pre_process!=0x00000000) + mmanager->modules[i]->pre_process(mmanager->modules[i]->data); + } + /* call the balance function */ + mmanager_compute_targets(mmanager); +} + +void mmanager_post_process(TMotionManager *mmanager) +{ + unsigned char i; + + /* transform the current values to angles */ + mmanager_compute_angles(mmanager); + for(i=0;i<MAX_NUM_MOTION_MODULES;i++) + { + if(mmanager->modules[i]!=0x00000000 && mmanager->modules[i]->post_process!=0x00000000) + mmanager->modules[i]->post_process(mmanager->modules[i]->data); + } +} + +/* public functions */ +unsigned char mmanager_init(TMotionManager *mmanager,TMemory *memory) +{ + unsigned char i; + + /* initialize the base module */ + dyn_module_init(&mmanager->dyn_module); + /* add all known servo models */ + dyn_module_add_model(&mmanager->dyn_module,SERVO_DX113); + dyn_module_add_model(&mmanager->dyn_module,SERVO_DX116); + dyn_module_add_model(&mmanager->dyn_module,SERVO_DX117); + dyn_module_add_model(&mmanager->dyn_module,SERVO_AX12A); + dyn_module_add_model(&mmanager->dyn_module,SERVO_AX12W); + dyn_module_add_model(&mmanager->dyn_module,SERVO_AX18A); + dyn_module_add_model(&mmanager->dyn_module,SERVO_RX10); + dyn_module_add_model(&mmanager->dyn_module,SERVO_MX12W); + dyn_module_add_model(&mmanager->dyn_module,SERVO_MX28); + dyn_module_add_model(&mmanager->dyn_module,SERVO_RX24F); + dyn_module_add_model(&mmanager->dyn_module,SERVO_RX28); + dyn_module_add_model(&mmanager->dyn_module,SERVO_RX64); + dyn_module_add_model(&mmanager->dyn_module,SERVO_MX64); + dyn_module_add_model(&mmanager->dyn_module,SERVO_EX106); + dyn_module_add_model(&mmanager->dyn_module,SERVO_MX106); + dyn_module_add_model(&mmanager->dyn_module,SERVO_XL320); + mmanager->dyn_module.data=mmanager; + mmanager->dyn_module.add_device=(void (*)(void *,unsigned char,unsigned short int))mmanager_add_device; + mmanager->dyn_module.setup=(void (*)(void *))mmanager_setup; + mmanager->dyn_module.set_period=(void (*)(void *,unsigned short int))mmanager_set_period; + mmanager->dyn_module.pre_process=(void (*)(void *))mmanager_pre_process; + mmanager->dyn_module.post_process=(void (*)(void *))mmanager_post_process; + /* initialize internal attributes */ + mmanager->balance_enabled=0x00; + mmanager->num_modules=0x00; + /* initialize motion module base attributes */ + for(i=0;i<MAX_NUM_MOTION_MODULES;i++) + mmanager->modules[i]=0x00000000; + for(i=0;i<DYN_MANAGER_MAX_NUM_DEVICES;i++) + { + /* servo configuration */ + mmanager->servo_configs[i].model=0x0000; + mmanager->servo_configs[i].encoder_resolution=0x0000; + mmanager->servo_configs[i].gear_ratio=0x00; + mmanager->servo_configs[i].max_angle=0x0000; + mmanager->servo_configs[i].center_angle=0x0000; + mmanager->servo_configs[i].max_speed=0x0000; + mmanager->servo_configs[i].cw_angle_limit=0; + mmanager->servo_configs[i].ccw_angle_limit=0; + /* servo values */ + mmanager->servo_values[i].cw_compliance=0x00; + mmanager->servo_values[i].ccw_compliance=0x00; + mmanager->servo_values[i].target_value=0x0000; + mmanager->servo_values[i].target_angle=0; + mmanager->servo_values[i].current_value=0x0000; + mmanager->servo_values[i].current_angle=0; + mmanager->servo_values[i].module=MM_NONE; + mmanager->servo_values[i].enabled=0x00; + mmanager->servo_values[i].offset=0x00; + } + mmanager->enable_op=0x00000000; + mmanager->motion_op=0x00000000; + mmanager->feedback_op=0x00000000; + mmanager->balance=0x00000000; + + /* initialize memory module */ + mem_module_init(&manager->mem_module); + manager->mem_module.data=manager; + manager->mem_module.write_cmd=mmanager_write_cmd; + manager->mem_module.read_cmd=mmanager_read_cmd; + if(!mem_module_add_ram_segment(&manager->mem_module,RAM_MM_BASE_ADDRESS,RAM_MM_LENGTH)) + return 0x00; + if(!mem_module_add_eeprom_segment(&manager->mem_module,EEPROM_MM_BASE_ADDRESS,EEPROM_MM_LENGTH)) + return 0x00; + if(!mem_add_module(memory,&manager->mem_module)) + return 0x00; + + return 0x01; +} + +TDynModule *mmanager_get_dyn_module(TMotionManager *mmanager) +{ + return &mmanager->dyn_module; +} + +void mmanager_enable_balance(TMotionManager *mmanager) +{ + if(mmanager->balance!=0x00000000) + mmanager->balance_enabled=0x01; +} + +void mmanager_disable_balance(TMotionManager *mmanager) +{ + mmanager->balance_enabled=0x00; +} + +unsigned char mmanager_is_balance_enabled(TMotionManager *mmanager) +{ + return mmanager->balance_enabled; +} + +void mmanager_set_module(TMotionManager *mmanager,unsigned char servo_id,TModules module) +{ + if(servo_id<DYN_MANAGER_MAX_NUM_DEVICES) + { + if(module!=mmanager->servo_values[servo_id].module) + { + if(module!=MM_NONE && mmanager->modules[module]->set_module!=0x00000000) + mmanager->modules[module]->set_module(mmanager->modules[module]->data,servo_id); + mmanager->servo_values[servo_id].module=module; + } + } +} + +TModules mmanager_get_module(TMotionManager *mmanager,unsigned char servo_id) +{ + if(servo_id<DYN_MANAGER_MAX_NUM_DEVICES) + return mmanager->servo_values[servo_id].module; + else + return MM_NONE; +} + +void mmanager_enable_servo(TMotionManager *mmanager,unsigned char servo_id) +{ + unsigned char * const data=&(mmanager->servo_values[servo_id].enabled); + + if(servo_id<DYN_MANAGER_MAX_NUM_DEVICES) + { + mmanager->servo_values[servo_id].enabled=0x01; + /* add an operation to enable the servo */ + if(mmanager->enable_op==0x00000000 || mmanager->enable_op->op_type==no_op) + { + mmanager->enable_op=dyn_manager_sync_op_new(mmanager->dyn_module.manager,DYN_MANAGER_WRITE,1,&servo_id,P_TORQUE_ENABLE,1,&data); + dyn_manager_set_op_repetitions(mmanager->dyn_module.manager,mmanager->enable_op,1); + } + else + dyn_manager_sync_op_add_devices(mmanager->dyn_module.manager,mmanager->enable_op,1,&servo_id,&data); + } +} + +void mmanager_disable_servo(TMotionManager *mmanager,unsigned char servo_id) +{ + unsigned char * const data=&(mmanager->servo_values[servo_id].enabled); + + if(servo_id<DYN_MANAGER_MAX_NUM_DEVICES) + { + mmanager->servo_values[servo_id].enabled=0x00; + /* add an operation to enable the servo */ + if(mmanager->enable_op==0x00000000 || mmanager->enable_op->op_type==no_op) + { + mmanager->enable_op=dyn_manager_sync_op_new(mmanager->dyn_module.manager,DYN_MANAGER_WRITE,1,&servo_id,P_TORQUE_ENABLE,1,&data); + dyn_manager_set_op_repetitions(mmanager->dyn_module.manager,mmanager->enable_op,1); + } + else + dyn_manager_sync_op_add_devices(mmanager->dyn_module.manager,mmanager->enable_op,1,&servo_id,&data); + } +} + +unsigned char mmanager_is_servo_enabled(TMotionManager *mmanager,unsigned char servo_id) +{ + if(servo_id<DYN_MANAGER_MAX_NUM_DEVICES) + return mmanager->servo_values[servo_id].enabled; + else + return 0x00; +} + +void mmanager_set_offset(TMotionManager *mmanager,unsigned char servo_id,char offset) +{ + if(servo_id<DYN_MANAGER_MAX_NUM_DEVICES) + mmanager->servo_values[servo_id].offset=offset; +} + +void mmanager_add_module(TMotionManager *mmanager,TMotionModule *module) +{ + if(module->id!=MM_NONE) + { + if(mmanager->modules[module->id]==0x00000000) + mmanager->num_modules++; + mmanager->modules[module->id]=module; + module->manager=mmanager; + } +} + diff --git a/dynamixel_manager/src/modules/motion_module.c b/dynamixel_manager/src/modules/motion_module.c new file mode 100644 index 0000000000000000000000000000000000000000..96199bd032887d4f941f45a07651fc078ca7a27d --- /dev/null +++ b/dynamixel_manager/src/modules/motion_module.c @@ -0,0 +1,12 @@ +#include "motion_module.h" + +void mmodule_init(TMotionModule *module) +{ + module->id=MM_NONE; + module->manager=0x00000000; + module->set_period=0x00000000; + module->set_module=0x00000000; + module->pre_process=0x00000000; + module->post_process=0x00000000; + module->data=0x00000000; +} diff --git a/dynamixel_manager/src/modules/motion_pages.c b/dynamixel_manager/src/modules/motion_pages.c new file mode 100755 index 0000000000000000000000000000000000000000..bf4f7d8a10551d3545493e0aa698a5088e136666 --- /dev/null +++ b/dynamixel_manager/src/modules/motion_pages.c @@ -0,0 +1,45 @@ +#include "motion_pages.h" + +void pages_get_page(unsigned char page_id,TPage *page) +{ + unsigned short int i=0; + + for(i=0;i<sizeof(TPage);i++) + ((unsigned char *)page)[i]=((unsigned char *)&motion_pages[page_id])[i]; +} + +unsigned char pages_check_checksum(TPage *page) +{ + unsigned char checksum=0x00; + unsigned short int i=0; + + for(i=0;i<sizeof(TPage);i++) + checksum+=((unsigned char *)page)[i]; + if(checksum==0x00) + return 0x01; + else + return 0x00; +} + +void pages_clear_page(TPage *page) +{ + unsigned short int i=0; + + for(i=0;i<sizeof(TPage);i++) + ((unsigned char *)page)[i]=0x00; +} + +void pages_copy_page(TPage *src,TPage *dst) +{ + unsigned short int i=0; + + for(i=0;i<sizeof(TPage);i++) + ((unsigned char *)dst)[i]=((unsigned char *)src)[i]; +} + +inline unsigned char pages_get_slope(TPage *page,unsigned char servo_id) +{ + return 0x01<<(page->header.slope[servo_id]&0x0F); +} + +TPage motion_pages[MAX_PAGES] __attribute__ ((section (".pages"))) __attribute__((weak)) ; diff --git a/dynamixel_manager/test/Makefile b/dynamixel_manager/test/Makefile index c57e9c1ab38c150a85bc5f1300b63950813da197..5ebaf504ae096678463832ddd32da27cbd77bb57 100755 --- a/dynamixel_manager/test/Makefile +++ b/dynamixel_manager/test/Makefile @@ -2,19 +2,20 @@ # modified by zerom for WinARM 8/2010 PROJECT_NAME=dynamixel_manager_test -TARGET_FILES=$(wildcard *.c) +TARGET_FILES+=$(wildcard *.c) +TARGET_FILES+=$(wildcard ../src/modules/*.c) TARGET_FILES+=$(wildcard ../src/*.c) TARGET_FILES+=$(wildcard ../../utils/src/*.c) TARGET_FILES+=$(wildcard ../../comm/src/*.c) BUILD_PATH=build BIN_PATH=bin -INCLUDE_DIRS = -I../include -I../../utils/include -I../../comm/include -I../../dynamixel_base/include +INCLUDE_DIRS = -I../include -I../include/modules -I../../utils/include -I../../comm/include -I../../dynamixel_base/include CC = gcc -UTILS_OBJS_TMP = $(notdir $(TARGET_FILES:.c=.o)) -UTILS_OBJS = $(patsubst %,$(BUILD_PATH)/%,$(UTILS_OBJS_TMP)) +TEST_OBJS_TMP = $(notdir $(TARGET_FILES:.c=.o)) +TEST_OBJS = $(patsubst %,$(BUILD_PATH)/%,$(TEST_OBJS_TMP)) OUT_FILE=$(BIN_PATH)/$(PROJECT_NAME) @@ -30,14 +31,17 @@ $(BUILD_PATH)/%.o: %.c $(BUILD_PATH)/%.o: ../src/%.c $(CC) -c -g $(INCLUDE_DIRS) -o $@ $< +$(BUILD_PATH)/%.o: ../src/modules/%.c + $(CC) -c -g $(INCLUDE_DIRS) -o $@ $< + $(BUILD_PATH)/%.o: ../../utils/src/%.c $(CC) -c -g $(INCLUDE_DIRS) -o $@ $< $(BUILD_PATH)/%.o: ../../comm/src/%.c $(CC) -c -g $(INCLUDE_DIRS) -o $@ $< -$(OUT_FILE): make_dirs $(UTILS_OBJS) - $(CC) -g $(UTILS_OBJS) -lpthread --output $@ +$(OUT_FILE): make_dirs $(TEST_OBJS) + $(CC) -g $(TEST_OBJS) -lpthread --output $@ clean: -rm -rf $(BUILD_PATH) diff --git a/dynamixel_manager/test/dyn_man_test.c b/dynamixel_manager/test/dyn_man_test.c index 359cf3ff5999a3f1b8fe55af6ee2468d5656afcd..ccaf642ef95ed60437a0632979739dd30f405b56 100644 --- a/dynamixel_manager/test/dyn_man_test.c +++ b/dynamixel_manager/test/dyn_man_test.c @@ -3,6 +3,9 @@ #include "dyn_devices.h" #include "dyn_module.h" #include "dyn_manager.h" +#include "motion_manager.h" +#include "action.h" +#include "dyn_servos.h" TDynamixelMaster dyn_master1; TDynamixelMaster dyn_master2; @@ -97,11 +100,23 @@ unsigned char dyn_master_read_word(TDynamixelMaster *master,unsigned char id,uns { if(address==0x0000) (*data)=SERVO_AX12A; + else if(address==0x0006) + (*data)=0x0000; + else if(address==0x0008) + (*data)=0x03FF; + else if(address==0x0024) + (*data)=0x01FF; } if(id>10 && id<21) { if(address==0x0000) (*data)=SERVO_MX28; + else if(address==0x0006) + (*data)=0x0000; + else if(address==0x0008) + (*data)=0x0FFF; + else if(address==0x0024) + (*data)=0x07FF; } } else if(master==&dyn_master2) @@ -110,6 +125,12 @@ unsigned char dyn_master_read_word(TDynamixelMaster *master,unsigned char id,uns { if(address==0x0000) (*data)=SERVO_XL320; + else if(address==0x0006) + (*data)=0x0000; + else if(address==0x0008) + (*data)=0x03FF; + else if(address==0x0025) + (*data)=0x01FF; } } } @@ -151,7 +172,7 @@ unsigned char dyn_master_is_write_table_done(TDynamixelMaster *master,unsigned c return DYN_SUCCESS; } -unsigned char dyn_master_start_sync_write(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, TWriteData *data) +unsigned char dyn_master_start_sync_write(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, unsigned char * const data[]) { unsigned short int i,j; @@ -164,18 +185,24 @@ unsigned char dyn_master_start_sync_write(TDynamixelMaster *master,unsigned char { printf(" servo %d data:\n",ids[j]); for(i=0;i<length;i++) - printf(" 0x%x\n",data[j].data_addr[i]); + printf(" 0x%x\n",data[j][i]); } +/* if(master==&dyn_master1 && address==P_CW_COMPLIANCE_SLOPE && length==4) + { + for(i=0;i<num;i++) + printf("%d,",data[i][2]+(data[i][3]<<8)); + printf("\n"); + }*/ return DYN_SUCCESS; } -unsigned char dyn_master_is_sync_write_done(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, TWriteData *data) +unsigned char dyn_master_is_sync_write_done(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, unsigned char * const data[]) { return DYN_SUCCESS; } -unsigned char dyn_master_start_sync_read(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, TWriteData *data) +unsigned char dyn_master_start_sync_read(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, unsigned char * const data[]) { unsigned short int i; @@ -190,12 +217,12 @@ unsigned char dyn_master_start_sync_read(TDynamixelMaster *master,unsigned char return DYN_SUCCESS; } -unsigned char dyn_master_is_sync_read_done(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, TWriteData *data) +unsigned char dyn_master_is_sync_read_done(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int address, unsigned short int length, unsigned char * const data[]) { return DYN_SUCCESS; } -unsigned char dyn_master_start_bulk_write(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, TWriteData *data) +unsigned char dyn_master_start_bulk_write(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, unsigned char * const data[]) { unsigned short int i,j; @@ -208,18 +235,18 @@ unsigned char dyn_master_start_bulk_write(TDynamixelMaster *master,unsigned char { printf(" servo %d at address %d with length %d and data:\n",ids[j],address[j],length[j]); for(i=0;i<length[j];i++) - printf(" 0x%x\n",data[j].data_addr[i]); + printf(" 0x%x\n",data[j][i]); } return DYN_SUCCESS; } -unsigned char dyn_master_is_bulk_write_done(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, TWriteData *data) +unsigned char dyn_master_is_bulk_write_done(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, unsigned char * const data[]) { return DYN_SUCCESS; } -unsigned char dyn_master_start_bulk_read(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, TWriteData *data) +unsigned char dyn_master_start_bulk_read(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, unsigned char * const data[]) { unsigned short int i,j; @@ -234,160 +261,27 @@ unsigned char dyn_master_start_bulk_read(TDynamixelMaster *master,unsigned char return DYN_SUCCESS; } -unsigned char dyn_master_is_bulk_read_done(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, TWriteData *data) +unsigned char dyn_master_is_bulk_read_done(TDynamixelMaster *master,unsigned char num,unsigned char *ids,unsigned short int *address, unsigned short int *length, unsigned char * const data[]) { return DYN_SUCCESS; } -// module functions -unsigned char single_op_data[8]; -unsigned char sync_op_data[64]; -unsigned char bulk_op_data[64]; - -void module1_init(void) -{ - unsigned int i; - - printf("module1: init function\n"); - for(i=0;i<8;i++) - single_op_data[i]=i; - for(i=0;i<64;i++) - { - sync_op_data[i]=i; - bulk_op_data[i]=i; - } -} - -void module1_add_device(unsigned char id,unsigned short int model) -{ - printf("module1: add_device function\n"); - printf(" ID: %d model: %d\n",id,model); -} - -void module1_setup(void) -{ - TWriteData data[8]; - unsigned char servo_ids[8]; - unsigned short int address[8],length[8]; - - servo_ids[0]=12; - servo_ids[1]=21; - - data[0].data_addr=&sync_op_data[4]; - data[1].data_addr=&sync_op_data[8]; - - printf("module1: setup function\n"); - dyn_manager_single_op_new(&manager,DYN_MANAGER_WRITE,11,0x0000,2,&single_op_data[0]); - dyn_manager_single_op_new(&manager,DYN_MANAGER_READ,11,0x0020,4,&single_op_data[0]); - dyn_manager_single_op_new(&manager,DYN_MANAGER_WRITE,21,0x0050,6,&single_op_data[0]); - dyn_manager_single_op_new(&manager,DYN_MANAGER_READ,22,0x0070,8,&single_op_data[0]); - dyn_manager_sync_op_new(&manager,DYN_MANAGER_WRITE,2,servo_ids,0x0000,2,data); - dyn_manager_sync_op_new(&manager,DYN_MANAGER_READ,2,servo_ids,0x0000,2,data); - servo_ids[0]=12; - servo_ids[1]=13; - servo_ids[2]=14; - servo_ids[3]=15; - servo_ids[4]=21; - servo_ids[5]=22; - servo_ids[6]=23; - servo_ids[7]=24; - data[0].data_addr=&sync_op_data[0]; - address[0]=0; - length[0]=1; - data[1].data_addr=&sync_op_data[1]; - address[1]=1; - length[1]=2; - data[2].data_addr=&sync_op_data[3]; - address[2]=3; - length[2]=4; - data[3].data_addr=&sync_op_data[7]; - address[3]=7; - length[3]=8; - data[4].data_addr=&sync_op_data[15]; - address[4]=15; - length[4]=8; - data[5].data_addr=&sync_op_data[23]; - address[5]=23; - length[5]=4; - data[6].data_addr=&sync_op_data[27]; - address[6]=27; - length[6]=2; - data[7].data_addr=&sync_op_data[29]; - address[7]=29; - length[7]=1; - dyn_manager_bulk_op_new(&manager,DYN_MANAGER_WRITE,8,servo_ids,address,length,data); - dyn_manager_bulk_op_new(&manager,DYN_MANAGER_READ,8,servo_ids,address,length,data); -} - -void module1_pre_process(void) -{ - printf("module1: pre_process function\n"); -} - -void module1_post_process(void) -{ - printf("module1: post_process function\n"); -} - -void module2_init(void) -{ - printf("module2: init function\n"); -} - -void module2_add_device(unsigned char id,unsigned short int model) -{ - printf("module2: add_device function\n"); - printf(" ID: %d model: %d\n",id,model); -} - -void module2_setup(void) -{ - printf("module2: setup function\n"); - dyn_manager_single_op_new(&manager,DYN_MANAGER_WRITE,1,0x0050,6,&single_op_data[0]); - dyn_manager_single_op_new(&manager,DYN_MANAGER_READ,2,0x0070,8,&single_op_data[0]); -} - -void module2_pre_process(void) -{ - printf("module2: pre_process function\n"); -} - -void module2_post_process(void) -{ - printf("module2: post_process function\n"); -} - int main(void) { int i; - TDynModule module1; - TDynModule module2; + TMotionManager mmanager; TTime dyn_master1_timer; TComm dyn_master1_comm; TTime dyn_master2_timer; TComm dyn_master2_comm; // init modules - dyn_module_init(&module1); - dyn_module_add_model(&module1,SERVO_MX28); - dyn_module_add_model(&module1,SERVO_MX28);// this should be ignored - dyn_module_add_model(&module1,SERVO_XL320); - dyn_module_set_init_fctn(&module1,module1_init); - dyn_module_set_add_device_fctn(&module1,module1_add_device); - dyn_module_set_setup_fctn(&module1,module1_setup); - dyn_module_set_pre_process_fctn(&module1,module1_pre_process); - dyn_module_set_post_process_fctn(&module1,module1_post_process); - dyn_module_set_period(&module1,1); - - dyn_module_init(&module2); - dyn_module_add_model(&module2,SERVO_AX12A); - dyn_module_set_init_fctn(&module2,module1_init); - dyn_module_set_add_device_fctn(&module2,module1_add_device); - dyn_module_set_setup_fctn(&module2,module1_setup); - dyn_module_set_pre_process_fctn(&module2,module1_pre_process); - dyn_module_set_post_process_fctn(&module2,module1_post_process); - dyn_module_set_period(&module2,1); - + mmanager_init(&mmanager); + action_init(); + mmanager_add_module(&mmanager,action_get_module()); + dyn_module_set_period(mmanager_get_dyn_module(&mmanager),1); + dyn_module_enable(mmanager_get_dyn_module(&mmanager)); + // init masters time_init(&dyn_master1_timer,time_get_counts_per_us(),time_get_counts); comm_init(&dyn_master1_comm,0x01,&dyn_master1_timer); @@ -399,10 +293,22 @@ int main(void) dyn_manager_init(&manager); dyn_manager_add_master(&manager,&dyn_master1); dyn_manager_add_master(&manager,&dyn_master2); - dyn_manager_add_module(&manager,&module1); - dyn_manager_add_module(&manager,&module2); + dyn_manager_add_module(&manager,mmanager_get_dyn_module(&mmanager)); dyn_manager_scan(&manager); + /* enable all servos and assign them to the ACTION module */ + for(i=1;i<25;i++) + { + mmanager_set_module(&mmanager,i,MM_ACTION); + mmanager_enable_servo(&mmanager,i); + } + /* load and start a page */ + if(action_load_page(9)) + action_start_page(); + for(i=0;i<10;i++) + { + printf("***** Iteration %d *****\n",i); dyn_manager_loop(&manager); + } } diff --git a/dynamixel_manager/test/pages.c b/dynamixel_manager/test/pages.c new file mode 100755 index 0000000000000000000000000000000000000000..cd69e24f6a5f5434fb0063789bece68e3dd768e2 --- /dev/null +++ b/dynamixel_manager/test/pages.c @@ -0,0 +1,2673 @@ +#include "motion_pages.h" + +TPage motion_pages[MAX_PAGES] __attribute__ ((section (".pages")))= +{ + // page 0 + { + { + {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}, + 0x00, + 0x00, + 0x0a, + {0x00,0x00,0x00}, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + {0x00,0x00,0x00,0x00}, + 0xf6, + {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}, + 0x00 + }, + { + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + } + } + }, + // page 1 (int) + { + { + {0x69,0x6e,0x74,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}, + 0x00, + 0x01, + 0x0a, + {0x00,0x00,0x00}, + 0x01, + 0x00, + 0x20, + 0x00, + 0x20, + 0x00, + 0x00, + {0x00,0x00,0x00,0x00}, + 0x87, + {0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00}, + 0x00 + }, + { + { + {0x6000,0xe719,0x18c1,0xf2d1,0x0d09,0x0465,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0000,0x056b,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x7d + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + } + } + }, + // page 2 (ok) + { + { + {0x6f,0x6b,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}, + 0x00, + 0x01, + 0x0a, + {0x00,0x00,0x00}, + 0x05, + 0x00, + 0x20, + 0x00, + 0x20, + 0x00, + 0x00, + {0x00,0x00,0x00,0x00}, + 0x22, + {0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00}, + 0x00 + }, + { + { + {0x6000,0xe719,0x18c1,0xf2d1,0x0d09,0x0465,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0000,0x0cbe,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0xe719,0x18c1,0xf2d1,0x0d09,0x0465,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0000,0xfe19,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0xe719,0x18c1,0xf2d1,0x0d09,0x0465,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0000,0x0cbe,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0xe719,0x18c1,0xf2d1,0x0d09,0x0465,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0000,0xfe19,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0xe719,0x18c1,0xf2d1,0x0d09,0x0465,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0000,0x056b,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x7d + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + } + } + }, + // page 3 (no) + { + { + {0x6e,0x6f,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}, + 0x00, + 0x01, + 0x0a, + {0x00,0x00,0x00}, + 0x05, + 0x00, + 0x20, + 0x00, + 0x20, + 0x00, + 0x00, + {0x00,0x00,0x00,0x00}, + 0x23, + {0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00}, + 0x00 + }, + { + { + {0x6000,0xe719,0x18c1,0xf2d1,0x0d09,0x0465,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xf8ad,0x056b,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0xe719,0x18c1,0xf2d1,0x0d09,0x0465,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0753,0x056b,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0xe719,0x18c1,0xf2d1,0x0d09,0x0465,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xf8ad,0x056b,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0xe719,0x18c1,0xf2d1,0x0d09,0x0465,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0753,0x056b,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0xe719,0x18c1,0xf2d1,0x0d09,0x0465,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0000,0x056b,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x7d + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + } + } + }, + // page 4 (hi) + { + { + {0x68,0x69,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}, + 0x00, + 0x01, + 0x0a, + {0x00,0x00,0x00}, + 0x04, + 0x00, + 0x20, + 0x00, + 0x20, + 0x00, + 0x00, + {0x00,0x00,0x00,0x00}, + 0x5c, + {0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00}, + 0x00 + }, + { + { + {0x6000,0xdfc6,0x2014,0xf5bf,0x0a1b,0x101d,0xefbe,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0000,0x056b,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0xdfc6,0x2014,0xf5bf,0x0a1b,0x101d,0xefbe,0x0000,0x0000,0x0000,0x0000,0xf736,0x08a4,0x02ee,0xfd12,0x00bb,0xff45,0x0000,0x0000,0x0000,0xf9b4,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x18, + 0x7d + }, + { + {0x6000,0xdfc6,0x2014,0xf5bf,0x0a1b,0x101d,0xefbe,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0000,0x056b,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x7d + }, + { + {0x6000,0xe719,0x18c1,0xf2d1,0x0d09,0x0465,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0000,0x056b,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x7d + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + } + } + }, + // page 5 (??) + { + { + {0x3f,0x3f,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}, + 0x00, + 0x01, + 0x0a, + {0x00,0x00,0x00}, + 0x03, + 0x00, + 0x20, + 0x00, + 0x20, + 0x00, + 0x00, + {0x00,0x00,0x00,0x00}, + 0x31, + {0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00}, + 0x00 + }, + { + { + {0x6000,0xe719,0x189c,0xf2d1,0x0d09,0x0465,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xf135,0x0bb8,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0xe719,0x189c,0xf2d1,0x0d09,0x0465,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xf135,0x0bb8,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x7d + }, + { + {0x6000,0xe6f4,0x189c,0xf2ac,0x0d09,0x043f,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0000,0x0546,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x7d + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + } + } + }, + // page 6 (talk1) + { + { + {0x74,0x61,0x6c,0x6b,0x31,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}, + 0x00, + 0x01, + 0x0a, + {0x00,0x00,0x00}, + 0x07, + 0x00, + 0x20, + 0x00, + 0x20, + 0x00, + 0x00, + {0x00,0x00,0x00,0x00}, + 0x79, + {0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00}, + 0x00 + }, + { + { + {0x6000,0x137b,0x0b6d,0xee92,0x0d2f,0xf7a7,0x0697,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0151,0xffdb,0x0000,0x0096,0xff45,0x0000,0x0000,0xf7cc,0x0697,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x7d + }, + { + {0x6000,0x137b,0x0b6d,0xee92,0x0d2f,0xf7a7,0x0697,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0151,0xffdb,0x0000,0x0096,0xff45,0x0000,0x0000,0xf7cc,0x0232,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x7d + }, + { + {0x6000,0x137b,0x0b6d,0xee92,0x0d2f,0xf7a7,0x0697,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0151,0xffdb,0x0000,0x0096,0xff45,0x0000,0x0000,0xf7cc,0x0697,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x18, + 0x7d + }, + { + {0x6000,0xf736,0xfcc7,0xecaa,0x0ecb,0xf75c,0x07e9,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0151,0xffdb,0x0000,0x0096,0xff45,0x0000,0x0000,0x02a3,0x0859,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x7d + }, + { + {0x6000,0xf736,0xfcc7,0xecaa,0x0ecb,0xf75c,0x07e9,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0151,0xffdb,0x0000,0x0096,0xff45,0x0000,0x0000,0x02a3,0x020d,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x7d + }, + { + {0x6000,0xf736,0xfcc7,0xecaa,0x0ecb,0xf75c,0x07e9,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0151,0xffdb,0x0000,0x0096,0xff45,0x0000,0x0000,0x02a3,0x0859,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x18, + 0x7d + }, + { + {0x6000,0xe719,0x18c1,0xf2d1,0x0d09,0x0465,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0000,0x056b,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x7d + } + } + }, + // page 7 (walk_ready) + { + { + {0x77,0x61,0x6c,0x6b,0x72,0x65,0x61,0x64,0x79,0x00,0x00,0x00,0x00,0x00}, + 0x00, + 0x01, + 0x0a, + {0x00,0x00,0x00}, + 0x01, + 0x00, + 0x20, + 0x00, + 0x20, + 0x00, + 0x00, + {0x00,0x00,0x00,0x00}, + 0x81, + {0x55,0x55,0x55,0x77,0x77,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00}, + 0x00 + }, + { + { + {0x1500,0xe7d5,0x14a7,0xf711,0x08ca,0x0ea6,0xf135,0x0000,0x0000,0x0025,0xffdb,0xef02,0x10fe,0x1a38,0xe5c8,0x0fac,0xf054,0x004b,0xffb5,0x0000,0x13ec,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500}, + 0x00, + 0x7d + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + } + } + }, + // page 8 (f_up) + { + { + {0x66,0x20,0x75,0x70,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}, + 0x00, + 0x01, + 0x0a, + {0x00,0x00,0x00}, + 0x05, + 0x00, + 0x20, + 0x00, + 0x20, + 0x00, + 0x00, + {0x00,0x00,0x00,0x00}, + 0x9b, + {0x55,0x55,0x55,0x77,0x77,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00}, + 0x00 + }, + { + { + {0x6000,0xe082,0x1bfa,0xf67b,0x0a41,0x21b1,0xdcb3,0xffdb,0xff6a,0x0000,0x0000,0xdf30,0x1f0e,0x1f0e,0xe0f2,0x0b6d,0xf2ac,0x00bb,0xff90,0x0000,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x18 + }, + { + {0x6000,0x0a1b,0xf261,0xf67b,0x0a41,0x2302,0xdc8d,0xffdb,0xff6a,0x0000,0x0000,0xcd9c,0x3138,0x2cad,0xd16b,0x0bdd,0xf216,0x00bb,0xff90,0x0000,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x18 + }, + { + {0x6000,0x1042,0xef98,0xedfc,0x1204,0xe05c,0x1de2,0xffdb,0xff6a,0x0000,0x0000,0xcf5e,0x2d1e,0x4155,0xbdf0,0x23be,0xd99f,0x0000,0x0000,0x0000,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x63 + }, + { + {0x6000,0x0697,0xf6eb,0xedfc,0x1204,0xe05c,0x1de2,0xffdb,0xff6a,0x0000,0x0000,0xcf5e,0x2f50,0x3e41,0xc093,0x197d,0xe57d,0x0000,0x0000,0x0000,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x7d + }, + { + {0x1500,0xe7d5,0x14a7,0xf711,0x08ca,0x0ea6,0xf135,0x0000,0x0000,0x0025,0xffdb,0xef02,0x10fe,0x1a38,0xe5c8,0x0fac,0xf054,0x004b,0xffb5,0x0000,0x13ec,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500}, + 0x00, + 0x7d + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + } + } + }, + // page 9 (b_up) + { + { + {0x62,0x20,0x75,0x70,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}, + 0x00, + 0x01, + 0x0a, + {0x00,0x00,0x00}, + 0x06, + 0x00, + 0x20, + 0x00, + 0x20, + 0x00, + 0x00, + {0x00,0x00,0x00,0x00}, + 0xcd, + {0x55,0x55,0x55,0x77,0x77,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00}, + 0x00 + }, + { + { + {0x6000,0xb677,0x4963,0xe997,0x13c6,0x1c90,0xe325,0xffdb,0xff6a,0x0000,0x0000,0xd2e2,0x2c3d,0x331f,0xcad4,0x1194,0xec85,0x00bb,0xff90,0x0000,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x18 + }, + { + {0x6000,0xb677,0x4963,0xe997,0x13c6,0xff45,0x0070,0xffdb,0xff6a,0x0000,0x0000,0xd2e2,0x2c3d,0x331f,0xcad4,0x101d,0xee21,0x00bb,0xff90,0x0000,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0xbbe3,0x4099,0xf67b,0x0a41,0xe5ed,0x1ace,0xffdb,0xff6a,0x0000,0x0000,0x0f61,0xef98,0x1d4c,0xe2b4,0x0000,0x0000,0x00bb,0xff90,0x0000,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x18, + 0x31 + }, + { + {0x6000,0xd3e9,0x25a5,0xecf5,0x0f3c,0xdbd2,0x2848,0xffdb,0xff6a,0x0000,0x0000,0x0fac,0xef4d,0x3507,0xc982,0x2742,0xd7b8,0x00bb,0xff90,0x0000,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x31, + 0x4a + }, + { + {0x6000,0x0ea6,0xf15a,0xecf5,0x0f3c,0xdbd2,0x2848,0xffdb,0xff6a,0x0000,0x0000,0x0fac,0xef4d,0x2db4,0xd0d5,0x2742,0xd7b8,0x00bb,0xff90,0x0000,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x18, + 0x63 + }, + { + {0x1500,0xe7d5,0x14a7,0xf711,0x08ca,0x0ea6,0xf135,0x0000,0x0000,0x0025,0xffdb,0xef02,0x10fe,0x1a38,0xe5c8,0x0fac,0xf054,0x004b,0xffb5,0x0000,0x13ec,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500}, + 0x00, + 0x7d + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + } + } + }, + // page a (rk) + { + { + {0x72,0x6b,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}, + 0x00, + 0x01, + 0x0a, + {0x00,0x00,0x00}, + 0x07, + 0x00, + 0x20, + 0x00, + 0x20, + 0x00, + 0x00, + {0x00,0x00,0x00,0x00}, + 0x9d, + {0x55,0x55,0x55,0x77,0x77,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x44,0x44,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00}, + 0x00 + }, + { + { + {0x6000,0xe890,0x13a1,0xf6c6,0x08ca,0x0e5b,0xf135,0xffb5,0x0025,0x0000,0x0000,0xec14,0x13ec,0x1644,0xe9e2,0x0a41,0xf448,0xffb5,0xff45,0x0000,0x137b,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0xe3e0,0x1068,0xf6c6,0x0a8c,0x0ecb,0xf15a,0x0025,0x0025,0x0070,0xffdb,0xea2d,0x161e,0x1c20,0xe42b,0x0e10,0xf23b,0x07c3,0x04fb,0x0025,0x1437,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x18 + }, + { + {0x6000,0xe3e0,0x1068,0xf6c6,0x0a8c,0x0ecb,0xf15a,0x0025,0x0025,0x035e,0xfe64,0xe50c,0x19c8,0x2ac6,0xe42b,0x1563,0xf23b,0x07c3,0x04fb,0x0025,0x1437,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x09 + }, + { + {0x6000,0xe0f2,0x079e,0xf54f,0x0a8c,0x0025,0xe719,0x0025,0x0025,0x035e,0xfe64,0xe0a7,0x1563,0x0ef1,0xe42b,0xf6a0,0xf2f7,0x04d5,0x0384,0x0025,0x1437,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x12, + 0x09 + }, + { + {0x6000,0xe845,0x14cd,0xf6c6,0x0a8c,0x0ecb,0xf15a,0x0025,0x0025,0x035e,0xfe64,0xe21e,0x16da,0x2ac6,0xe42b,0x1563,0xf23b,0x04d5,0x043f,0x0025,0x1437,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x09 + }, + { + {0x1500,0xe7d5,0x14a7,0xf711,0x08ca,0x0ea6,0xf135,0x0000,0x0000,0x0025,0xffdb,0xef02,0x10fe,0x1a38,0xe5c8,0x0fac,0xf054,0x004b,0xffb5,0x0000,0x13ec,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500}, + 0x00, + 0x09 + }, + { + {0x1500,0xe7d5,0x14a7,0xf711,0x08ca,0x0ea6,0xf135,0x0000,0x0000,0x0025,0xffdb,0xef02,0x10fe,0x1a38,0xe5c8,0x0fac,0xf054,0x004b,0xffb5,0x0000,0x13ec,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500}, + 0x00, + 0x3d + } + } + }, + // page b (lk) + { + { + {0x6c,0x6b,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}, + 0x00, + 0x01, + 0x0a, + {0x00,0x00,0x00}, + 0x07, + 0x00, + 0x20, + 0x00, + 0x20, + 0x00, + 0x00, + {0x00,0x00,0x00,0x00}, + 0xbb, + {0x55,0x55,0x55,0x77,0x77,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x44,0x44,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00}, + 0x00 + }, + { + { + {0x6000,0xe890,0x13a1,0xf6c6,0x08ca,0x0e5b,0xf135,0xffb5,0x0025,0x0000,0x0000,0xec14,0x13ec,0x1644,0xe9e2,0x0a41,0xf448,0xffb5,0xff45,0x0000,0x137b,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0xef73,0x1bfa,0xf54f,0x0915,0x0e80,0xf10f,0xffb5,0xffb5,0x0000,0xff6a,0xe9bc,0x15ae,0x1baf,0xe3bb,0x0d9f,0xf1cb,0xfae0,0xf817,0x0025,0x1437,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x18 + }, + { + {0x6000,0xef73,0x1bfa,0xf54f,0x0915,0x0e80,0xf10f,0xffb5,0xffb5,0x0177,0xfc7c,0xe613,0x1ace,0x1baf,0xd515,0x0d9f,0xea78,0xfae0,0xf817,0x0025,0x1437,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x09 + }, + { + {0x6000,0xf83d,0x1ee8,0xf54f,0x0a8c,0x18c1,0xffb5,0xffb5,0xffb5,0x0177,0xfc7c,0xea78,0x1f33,0x1baf,0xf0ea,0x0ce4,0x093a,0xfc57,0xfb05,0x0025,0x1437,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x12, + 0x09 + }, + { + {0x6000,0xeb0e,0x1795,0xf54f,0x0915,0x0e80,0xf10f,0xffb5,0xffb5,0x0177,0xfc7c,0xe901,0x1dbc,0x1baf,0xd515,0x0d9f,0xea78,0xfb9b,0xfb05,0x0025,0x1437,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x09 + }, + { + {0x1500,0xe7d5,0x14a7,0xf711,0x08ca,0x0ea6,0xf135,0x0000,0x0000,0x0025,0xffdb,0xef02,0x10fe,0x1a38,0xe5c8,0x0fac,0xf054,0x004b,0xffb5,0x0000,0x13ec,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500}, + 0x00, + 0x09 + }, + { + {0x1500,0xe7d5,0x14a7,0xf711,0x08ca,0x0ea6,0xf135,0x0000,0x0000,0x0025,0xffdb,0xef02,0x10fe,0x1a38,0xe5c8,0x0fac,0xf054,0x004b,0xffb5,0x0000,0x13ec,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500}, + 0x00, + 0x3d + } + } + }, + // page c (sit_down) + { + { + {0x73,0x69,0x74,0x20,0x64,0x6f,0x77,0x6e,0x00,0x00,0x00,0x00,0x00,0x00}, + 0x00, + 0x01, + 0x0a, + {0x00,0x00,0x00}, + 0x01, + 0x00, + 0x20, + 0x00, + 0x20, + 0x00, + 0x00, + {0x00,0x00,0x00,0x00}, + 0x81, + {0x55,0x55,0x55,0x77,0x77,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00}, + 0x00 + }, + { + { + {0x6000,0xe7fa,0x14a7,0xf6a0,0x0a66,0x0ea6,0xf135,0xffdb,0xff6a,0x0070,0xffdb,0xde2a,0x20f5,0x4074,0xbf1c,0x2302,0xdc8d,0x0151,0xff90,0x0025,0x0591,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x7d + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + } + } + }, + // page d (stand_up) + { + { + {0x73,0x74,0x61,0x6e,0x64,0x20,0x75,0x70,0x00,0x00,0x00,0x00,0x00,0x00}, + 0x00, + 0x01, + 0x0a, + {0x00,0x00,0x00}, + 0x01, + 0x00, + 0x20, + 0x00, + 0x20, + 0x00, + 0x00, + {0x00,0x00,0x00,0x00}, + 0x26, + {0x55,0x55,0x55,0x77,0x77,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00}, + 0x00 + }, + { + { + {0x1500,0xe7d5,0x14a7,0xf711,0x08ca,0x0ea6,0xf135,0x0000,0x0000,0x0025,0xffdb,0xef02,0x10fe,0x1a38,0xe5c8,0x0fac,0xf054,0x004b,0xffb5,0x0000,0x13ec,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500}, + 0x00, + 0x7d + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + } + } + }, + // page e (mul1) + { + { + {0x6d,0x75,0x6c,0x31,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}, + 0x00, + 0x01, + 0x0a, + {0x00,0x00,0x00}, + 0x07, + 0x00, + 0x20, + 0x00, + 0x20, + 0x12, + 0x00, + {0x00,0x00,0x00,0x00}, + 0x40, + {0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00}, + 0x00 + }, + { + { + {0x6000,0x079e,0xf9ff,0xee92,0x11b9,0xe926,0x09d0,0xffdb,0xff6a,0x0000,0x0000,0xdcd8,0x226c,0x4074,0xbf67,0x2328,0xdcb3,0x00bb,0xff90,0x0000,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x7d + }, + { + {0x6000,0x137b,0xea9d,0xecaa,0x14cd,0xffb5,0xff45,0xffdb,0xff6a,0x0000,0x0000,0xcf13,0x2f2b,0x30ed,0xce7d,0x2328,0xdcb3,0x00bb,0xff90,0x0000,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x7d + }, + { + {0x6000,0x27d8,0xd6b1,0xecaa,0x14a7,0x1c6b,0xe13d,0xffdb,0xff6a,0x0000,0x0000,0xcf39,0x2f50,0xff45,0xffb5,0x2328,0xdcb3,0x00bb,0xff90,0x0000,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x7d + }, + { + {0x6000,0x4ada,0xb500,0xecaa,0x14a7,0x1c6b,0xe13d,0xffdb,0xff6a,0x0000,0x0000,0xcf39,0x2f50,0xff45,0xffb5,0x2328,0xdcb3,0x00bb,0xff90,0x0000,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x7d + }, + { + {0x6000,0x4ada,0xb500,0x129a,0xec14,0xdfa1,0x24c4,0xffdb,0xff6a,0x0000,0x0000,0xcf39,0x2f50,0xff45,0xffb5,0x2328,0xdcb3,0x00bb,0xff90,0x0000,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x7d + }, + { + {0x6000,0x0915,0xf493,0x168f,0xe78a,0xd0b0,0x2c3d,0xffdb,0xff6a,0x0000,0x0000,0xcf39,0x2f50,0xff45,0xffb5,0x2328,0xdcb3,0x00bb,0xff90,0x0000,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x7d + }, + { + {0x6000,0x027d,0xfda8,0xe901,0x13ec,0x16ff,0xea07,0xffdb,0xff6a,0x0000,0x0000,0xcf39,0x2f50,0xff45,0xffb5,0x2328,0xdcb3,0x00bb,0xff90,0x0000,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x7d + } + } + }, + // page f (mul2) + { + { + {0x6d,0x75,0x6c,0x32,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}, + 0x00, + 0x01, + 0x0a, + {0x00,0x00,0x00}, + 0x07, + 0x00, + 0x20, + 0x00, + 0x20, + 0x13, + 0x00, + {0x00,0x00,0x00,0x00}, + 0xd3, + {0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00}, + 0x00 + }, + { + { + {0x6000,0x027d,0xfda8,0xe901,0x13ec,0x16ff,0xea07,0xffdb,0xff6a,0x0000,0x0000,0x0f87,0x2f50,0x27b2,0xffb5,0x1e07,0xdcb3,0x00bb,0xff90,0x0000,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x7d + }, + { + {0x6000,0x0e5b,0xf0ea,0xe901,0x13ec,0x16ff,0xea07,0xffdb,0xff6a,0x0000,0x0000,0x0f61,0xef73,0x0dea,0xf054,0xea07,0x12c0,0x00bb,0xff90,0x0000,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x7d + }, + { + {0x6000,0x1a13,0xe532,0xe901,0x13ec,0x0cbe,0xf448,0xffdb,0xff6a,0x0000,0x0000,0xfdce,0x0106,0x00bb,0xfefa,0xea07,0x12c0,0x00bb,0xff90,0x0000,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x7d + }, + { + {0x6000,0x1a13,0xe532,0xe901,0x13ec,0x0cbe,0xf448,0xffdb,0xff6a,0x1bfa,0xe28f,0xfdce,0x0106,0x00bb,0xfefa,0xea07,0x12c0,0x00bb,0xff90,0x0000,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x7d + }, + { + {0x6000,0x1a13,0xe532,0xe901,0x13ec,0x0cbe,0xf448,0xffdb,0xff6a,0x0000,0x0000,0xfdce,0x0106,0x00bb,0xfefa,0xea07,0x12c0,0x00bb,0xff90,0x0000,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x7d + }, + { + {0x6000,0x1a13,0xe532,0xe901,0x13ec,0x0cbe,0xf448,0xffdb,0xff6a,0x1bfa,0xe28f,0xfdce,0x0106,0x00bb,0xfefa,0xea07,0x12c0,0x00bb,0xff90,0x0000,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x7d + }, + { + {0x6000,0x1a13,0xe532,0xe901,0x13ec,0x0cbe,0xf448,0xffdb,0xff6a,0x0000,0x0000,0xfdce,0x0106,0x00bb,0xfefa,0xea07,0x12c0,0x00bb,0xff90,0x0000,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x7d + } + } + }, + // page 10 (mul3) + { + { + {0x6d,0x75,0x6c,0x33,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}, + 0x00, + 0x01, + 0x0a, + {0x00,0x00,0x00}, + 0x06, + 0x00, + 0x20, + 0x00, + 0x20, + 0x00, + 0x00, + {0x00,0x00,0x00,0x00}, + 0x59, + {0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00}, + 0x00 + }, + { + { + {0x6000,0x027d,0xfda8,0xe901,0x13ec,0x16ff,0xea07,0xffdb,0xff6a,0x0000,0x0000,0x0f87,0x2f50,0x27b2,0xffb5,0x1e07,0xeb59,0x00bb,0xff90,0x0000,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x7d + }, + { + {0x6000,0x027d,0xfda8,0xe901,0x13ec,0x16ff,0xea07,0xffdb,0xff6a,0x0000,0x0000,0xcf39,0x2f50,0xff45,0xffb5,0x11df,0xebc9,0x00bb,0xff90,0x0000,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x7d + }, + { + {0x6000,0x02c8,0xfe19,0xecaa,0x11df,0x20f5,0xdddf,0xffdb,0xff6a,0x0000,0x0000,0xcf39,0x2f50,0x1bfa,0xe42b,0x2328,0xdcb3,0x00bb,0xff90,0x0000,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x7d + }, + { + {0x6000,0x1042,0xef98,0xedfc,0x1204,0xe05c,0x1de2,0xffdb,0xff6a,0x0000,0x0000,0xcf5e,0x2d1e,0x4155,0xbdf0,0x23be,0xd99f,0x0000,0x0000,0x0000,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x7d + }, + { + {0x6000,0x0697,0xf6eb,0xedfc,0x1204,0xe05c,0x1de2,0xffdb,0xff6a,0x0000,0x0000,0xcf5e,0x2f50,0x412f,0xbda5,0x197d,0xe57d,0x0000,0x0000,0x0000,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x7d + }, + { + {0x1500,0xe7d5,0x14a7,0xf711,0x08ca,0x0ea6,0xf135,0x0000,0x0000,0x0025,0xffdb,0xef02,0x10fe,0x1a38,0xe5c8,0x0fac,0xf054,0x004b,0xffb5,0x0000,0x13ec,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500}, + 0x00, + 0x7d + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + } + } + }, + // page 11 (d1) + { + { + {0x64,0x31,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}, + 0x00, + 0x01, + 0x0a, + {0x00,0x00,0x00}, + 0x04, + 0x00, + 0x15, + 0x00, + 0x20, + 0x00, + 0x00, + {0x00,0x00,0x00,0x00}, + 0x2a, + {0x55,0x66,0x66,0x66,0x66,0x66,0x66,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00}, + 0x00 + }, + { + { + {0x6000,0x2e6f,0x0e80,0xf574,0x0d2f,0xf54f,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0465,0x0859,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x7d + }, + { + {0x6000,0xf31c,0x0e80,0xf054,0x0d2f,0x211b,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfb9b,0x0465,0x02ee,0xfd12,0x00bb,0xff45,0x0000,0x0000,0x0465,0xfc57,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x4a + }, + { + {0x6000,0xffdb,0x0e80,0xf574,0x0d2f,0x130b,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0465,0xff90,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0xe719,0x18c1,0xf2d1,0x0d09,0x0465,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0000,0x056b,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x7d + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + } + } + }, + // page 12 (d2) + { + { + {0x64,0x32,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}, + 0x00, + 0x01, + 0x0a, + {0x00,0x00,0x00}, + 0x05, + 0x00, + 0x20, + 0x00, + 0x20, + 0x19, + 0x00, + {0x00,0x00,0x00,0x00}, + 0x82, + {0x55,0x66,0x66,0x66,0x66,0x66,0x66,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00}, + 0x00 + }, + { + { + {0x6000,0x3be9,0xc543,0xf75c,0x0ea6,0xf15a,0x0a1b,0xf8ad,0xf8ad,0x0000,0x0000,0x0000,0xfd12,0x0000,0x0000,0xff45,0xfdce,0x0000,0x0000,0xf448,0x16ff,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0xfa + }, + { + {0x6000,0x3be9,0xc543,0xe163,0x249f,0xf15a,0x0a1b,0xf8ad,0xf8ad,0x0000,0x0000,0x0000,0xfd12,0x0000,0x0000,0xff45,0xfdce,0x0000,0x0000,0xf448,0x16ff,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0x3be9,0xc543,0xf75c,0x0ea6,0xf15a,0x0a1b,0xf8ad,0xf8ad,0x0000,0x0000,0x0000,0xfd12,0x0000,0x0000,0xff45,0xfdce,0x0000,0x0000,0xf448,0x16ff,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0x3be9,0xc543,0xe163,0x249f,0xf15a,0x0a1b,0xf8ad,0xf8ad,0x0000,0x0000,0x0000,0xfd12,0x0000,0x0000,0xff45,0xfdce,0x0000,0x0000,0xf448,0x16ff,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0x3be9,0xc543,0xf75c,0x0ea6,0xf15a,0x0a1b,0xf8ad,0xf8ad,0x0000,0x0000,0x0000,0xfd12,0x0000,0x0000,0xff45,0xfdce,0x0000,0x0000,0xf448,0x16ff,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + } + } + }, + // page 13 (d2) + { + { + {0x64,0x32,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}, + 0x00, + 0x01, + 0x0a, + {0x00,0x00,0x00}, + 0x06, + 0x00, + 0x20, + 0x00, + 0x20, + 0x00, + 0x00, + {0x00,0x00,0x00,0x00}, + 0x67, + {0x55,0x66,0x66,0x66,0x66,0x66,0x66,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00}, + 0x00 + }, + { + { + {0x6000,0x3be9,0xc543,0xf009,0x0753,0xf15a,0x0a1b,0x0753,0x0753,0x0000,0x0000,0x02ee,0x0000,0x0000,0x0000,0x0232,0x00bb,0x0000,0x0000,0x0bb8,0x16ff,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0xba + }, + { + {0x6000,0x3be9,0xc543,0xda10,0x1d4c,0xf15a,0x0a1b,0x0753,0x0753,0x0000,0x0000,0x02ee,0x0000,0x0000,0x0000,0x0232,0x00bb,0x0000,0x0000,0x0bb8,0x16ff,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0x3be9,0xc543,0xf009,0x0753,0xf15a,0x0a1b,0x0753,0x0753,0x0000,0x0000,0x02ee,0x0000,0x0000,0x0000,0x0232,0x00bb,0x0000,0x0000,0x0bb8,0x16ff,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0x3be9,0xc543,0xda10,0x1d4c,0xf15a,0x0a1b,0x0753,0x0753,0x0000,0x0000,0x02ee,0x0000,0x0000,0x0000,0x0232,0x00bb,0x0000,0x0000,0x0bb8,0x16ff,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0x3be9,0xc543,0xf009,0x0753,0xf15a,0x0a1b,0x0753,0x0753,0x0000,0x0000,0x02ee,0x0000,0x0000,0x0000,0x0232,0x00bb,0x0000,0x0000,0x0bb8,0x16ff,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0xe719,0x18c1,0xf2d1,0x0d09,0x0465,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0000,0x056b,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0xba + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + } + } + }, + // page 14 (d3) + { + { + {0x64,0x33,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}, + 0x00, + 0x01, + 0x0a, + {0x00,0x00,0x00}, + 0x05, + 0x00, + 0x20, + 0x00, + 0x20, + 0x00, + 0x00, + {0x00,0x00,0x00,0x00}, + 0x89, + {0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00}, + 0x00 + }, + { + { + {0x6000,0x3183,0x18c1,0x004b,0x0ce4,0x1a5e,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfa24,0x05dc,0x02ee,0xfd12,0x00bb,0xff45,0x0000,0x0000,0xf2f7,0x004b,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x7d + }, + { + {0x6000,0x1d71,0x18c1,0xf31c,0x0ce4,0x1e52,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfa24,0x05dc,0x02ee,0xfd12,0x00bb,0xff45,0x0000,0x0000,0xf969,0xf83d,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0x3183,0x18c1,0x004b,0x0ce4,0x1a5e,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfa24,0x05dc,0x02ee,0xfd12,0x00bb,0xff45,0x0000,0x0000,0xf2f7,0x004b,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0x1d71,0x18c1,0xf31c,0x0ce4,0x1e52,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfa24,0x05dc,0x02ee,0xfd12,0x00bb,0xff45,0x0000,0x0000,0xf969,0xf83d,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0xe719,0x18c1,0xf2d1,0x0d09,0x0465,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0000,0x056b,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x7d + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + } + } + }, + // page 15 (talk2) + { + { + {0x74,0x61,0x6c,0x6b,0x32,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}, + 0x00, + 0x01, + 0x0a, + {0x00,0x00,0x00}, + 0x05, + 0x00, + 0x15, + 0x00, + 0x20, + 0x1e, + 0x00, + {0x00,0x00,0x00,0x00}, + 0xc4, + {0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00}, + 0x00 + }, + { + { + {0x6000,0x0601,0x0e35,0xf009,0x0cbe,0xfcc7,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0753,0x056b,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x7d + }, + { + {0x6000,0x0601,0x0e35,0xf009,0x0cbe,0xfcc7,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0753,0x03f4,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0x0601,0x0e35,0xf009,0x0cbe,0xfcc7,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0753,0x06e2,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0x0601,0x0e35,0xf009,0x0cbe,0xfcc7,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0753,0x03f4,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0x0601,0x0e35,0xf009,0x0cbe,0xfcc7,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0753,0x06e2,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + } + } + }, + // page 16 (talk2) + { + { + {0x74,0x61,0x6c,0x6b,0x32,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}, + 0x00, + 0x01, + 0x0a, + {0x00,0x00,0x00}, + 0x05, + 0x00, + 0x15, + 0x00, + 0x20, + 0x00, + 0x00, + {0x00,0x00,0x00,0x00}, + 0x78, + {0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00}, + 0x00 + }, + { + { + {0x6000,0xf6eb,0xfd12,0xef98,0x0d7a,0xfc57,0x0384,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xf8ad,0x056b,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x7d + }, + { + {0x6000,0xf6eb,0xfd12,0xef98,0x0d7a,0xfc57,0x0384,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xf8ad,0x03f4,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0xf6eb,0xfd12,0xef98,0x0d7a,0xfc57,0x0384,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xf8ad,0x06e2,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0xf6eb,0xfd12,0xef98,0x0d7a,0xfc57,0x0384,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xf8ad,0x03f4,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0xf6eb,0xfd12,0xef98,0x0d7a,0xfc57,0x0384,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xf8ad,0x06e2,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + } + } + }, + // page 17 (d4) + { + { + {0x64,0x34,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}, + 0x00, + 0x01, + 0x0a, + {0x00,0x00,0x00}, + 0x06, + 0x00, + 0x20, + 0x00, + 0x20, + 0x00, + 0x00, + {0x00,0x00,0x00,0x00}, + 0x24, + {0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00}, + 0x00 + }, + { + { + {0x6000,0x3112,0xcea3,0x05b6,0xf67b,0xd1b6,0x2dd9,0x0000,0x0000,0x0000,0x0000,0xfe64,0x0177,0x0753,0xf8ad,0x0520,0xfae0,0x0000,0x0000,0x0000,0x056b,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x7d + }, + { + {0x6000,0x3865,0xd5f6,0x218b,0xdec0,0xe05c,0x2dd9,0x0000,0x0000,0x0000,0x0000,0x0e80,0xf2d1,0x0a41,0xf5bf,0x0c73,0xf38d,0x0000,0x0000,0x0000,0xf8f8,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x7d + }, + { + {0x6000,0x3865,0xd5f6,0x05b6,0xf67b,0xe05c,0x2dd9,0x0000,0x0000,0x0000,0x0000,0xf135,0x101d,0x0a41,0xf5bf,0x0232,0xfdce,0x0000,0x0000,0x0000,0x0dea,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x7d + }, + { + {0x6000,0x29bf,0xc750,0x218b,0xdec0,0xd1b6,0x1f33,0x0000,0x0000,0x0000,0x0000,0x0e80,0xf2d1,0x0a41,0xf5bf,0x0c73,0xf38d,0x0000,0x0000,0x0000,0xf8f8,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x7d + }, + { + {0x6000,0x29bf,0xc750,0x05b6,0xf67b,0xd1b6,0x1f33,0x0000,0x0000,0x0000,0x0000,0xf135,0x101d,0x0a41,0xf5bf,0x00bb,0xff45,0x0000,0x0000,0x0000,0x0dea,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x7d + }, + { + {0x6000,0xe719,0x18c1,0xf2d1,0x0d09,0x0465,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0000,0x056b,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x7d + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + } + } + }, + // page 18 (d2) + { + { + {0x64,0x32,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}, + 0x00, + 0x01, + 0x0a, + {0x00,0x00,0x00}, + 0x05, + 0x00, + 0x20, + 0x00, + 0x20, + 0x27, + 0x00, + {0x00,0x00,0x00,0x00}, + 0x15, + {0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00}, + 0x00 + }, + { + { + {0x6000,0x3be9,0x09d0,0xf75c,0x0f87,0xf15a,0x09f6,0xf8ad,0xf8ad,0x0000,0x0000,0xfd12,0x0000,0x0000,0x0000,0xff45,0xfdce,0x0000,0x0000,0xf448,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0xfa + }, + { + {0x6000,0x3be9,0x09d0,0xe8b6,0x0f87,0xe971,0x09f6,0xf8ad,0xf8ad,0x0000,0x0000,0xfd12,0x0000,0x0000,0x0000,0xff45,0xfdce,0x0000,0x0000,0xf448,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0x3be9,0x09d0,0xf75c,0x0f87,0xf15a,0x09f6,0xf8ad,0xf8ad,0x0000,0x0000,0xfd12,0x0000,0x0000,0x0000,0xff45,0xfdce,0x0000,0x0000,0xf448,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0x3be9,0x09d0,0xe8b6,0x0f87,0xe971,0x09f6,0xf8ad,0xf8ad,0x0000,0x0000,0xfd12,0x0000,0x0000,0x0000,0xff45,0xfdce,0x0000,0x0000,0xf448,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0x3be9,0x09d0,0xf75c,0x0f87,0xf15a,0x09f6,0xf8ad,0xf8ad,0x0000,0x0000,0xfd12,0x0000,0x0000,0x0000,0xff45,0xfdce,0x0000,0x0000,0xf448,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + } + } + }, + // page 19 (d2) + { + { + {0x64,0x32,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}, + 0x00, + 0x01, + 0x0a, + {0x00,0x00,0x00}, + 0x06, + 0x00, + 0x20, + 0x00, + 0x20, + 0x00, + 0x00, + {0x00,0x00,0x00,0x00}, + 0xce, + {0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00}, + 0x00 + }, + { + { + {0x6000,0x3be9,0x09d0,0xe8b6,0x0f87,0xf15a,0x09f6,0x0753,0x0753,0x0000,0x0000,0x0000,0x02ee,0x0000,0x0000,0x0232,0x00bb,0x0000,0x0000,0x0bb8,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x7d + }, + { + {0x6000,0x3be9,0x09d0,0xda10,0x0f87,0xea2d,0x09f6,0x0753,0x0753,0x0000,0x0000,0x0000,0x02ee,0x0000,0x0000,0x0232,0x00bb,0x0000,0x0000,0x0bb8,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0x3be9,0x09d0,0xe8b6,0x0f87,0xf15a,0x09f6,0x0753,0x0753,0x0000,0x0000,0x0000,0x02ee,0x0000,0x0000,0x0232,0x00bb,0x0000,0x0000,0x0bb8,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0x3be9,0x09d0,0xda10,0x0f87,0xea2d,0x09f6,0x0753,0x0753,0x0000,0x0000,0x0000,0x02ee,0x0000,0x0000,0x0232,0x00bb,0x0000,0x0000,0x0bb8,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0x3be9,0x09d0,0xe8b6,0x0f87,0xf15a,0x09f6,0x0753,0x0753,0x0000,0x0000,0x0000,0x02ee,0x0000,0x0000,0x0232,0x00bb,0x0000,0x0000,0x0bb8,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0xe719,0x18c1,0xf2d1,0x0d09,0x0465,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0000,0x056b,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0xba + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + } + } + }, + // page 1a (talk2) + { + { + {0x74,0x61,0x6c,0x6b,0x32,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}, + 0x00, + 0x01, + 0x0a, + {0x00,0x00,0x00}, + 0x03, + 0x00, + 0x15, + 0x00, + 0x20, + 0x2a, + 0x00, + {0x00,0x00,0x00,0x00}, + 0x44, + {0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00}, + 0x00 + }, + { + { + {0x6000,0xe5a2,0x1a38,0xf2d1,0x0d09,0x0753,0xf888,0x0000,0x0000,0x0000,0x0000,0xf8ad,0x0753,0x02ee,0xfd12,0x00bb,0xff45,0x0000,0x0000,0x0000,0xfe19,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x63 + }, + { + {0x6000,0xe5a2,0x1a38,0xf2d1,0x0d09,0x0753,0xf888,0x0000,0x0000,0x0000,0x0000,0xfa24,0x05dc,0x02ee,0xfd12,0x00bb,0xff45,0x0000,0x0000,0xfb9b,0x056b,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0xe5a2,0x1a38,0xf2d1,0x0d09,0x0753,0xf888,0x0000,0x0000,0x0000,0x0000,0xfb9b,0x0465,0x02ee,0xfd12,0x00bb,0xff45,0x0000,0x0000,0xfb9b,0x0859,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + } + } + }, + // page 1b (talk2) + { + { + {0x74,0x61,0x6c,0x6b,0x32,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}, + 0x00, + 0x01, + 0x0a, + {0x00,0x00,0x00}, + 0x05, + 0x00, + 0x15, + 0x00, + 0x20, + 0x2b, + 0x00, + {0x00,0x00,0x00,0x00}, + 0xb1, + {0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00}, + 0x00 + }, + { + { + {0x6000,0xf54f,0x06e2,0x08a4,0x0a66,0x20aa,0x03a9,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x01c2,0x06e2,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x7d + }, + { + {0x6000,0xf54f,0x06e2,0x08a4,0x0a66,0x20aa,0x03a9,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x01c2,0x027d,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0xf54f,0x06e2,0x08a4,0x0a66,0x20aa,0x03a9,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x01c2,0x06e2,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0xf54f,0x06e2,0x08a4,0x0a66,0x20aa,0x03a9,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x01c2,0x027d,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0xf54f,0x06e2,0x08a4,0x0a66,0x20aa,0x03a9,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x01c2,0x06e2,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + } + } + }, + // page 1c (talk2) + { + { + {0x74,0x61,0x6c,0x6b,0x32,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}, + 0x00, + 0x01, + 0x0a, + {0x00,0x00,0x00}, + 0x05, + 0x00, + 0x20, + 0x00, + 0x20, + 0x2c, + 0x00, + {0x00,0x00,0x00,0x00}, + 0xab, + {0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00}, + 0x00 + }, + { + { + {0x6000,0x0601,0x0e35,0xf009,0x0cbe,0xfcc7,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0753,0x056b,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x7d + }, + { + {0x6000,0x0601,0x0e35,0xf009,0x0cbe,0xfcc7,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0753,0x03f4,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0x0601,0x0e35,0xf009,0x0cbe,0xfcc7,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0753,0x06e2,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0x0601,0x0e35,0xf009,0x0cbe,0xfcc7,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0753,0x03f4,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0x0601,0x0e35,0xf009,0x0cbe,0xfcc7,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0753,0x06e2,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + } + } + }, + // page 1d (talk2) + { + { + {0x74,0x61,0x6c,0x6b,0x32,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}, + 0x00, + 0x01, + 0x0a, + {0x00,0x00,0x00}, + 0x05, + 0x00, + 0x15, + 0x00, + 0x20, + 0x2d, + 0x00, + {0x00,0x00,0x00,0x00}, + 0x4b, + {0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00}, + 0x00 + }, + { + { + {0x6000,0xf6eb,0xfd12,0xef98,0x0d7a,0xfc57,0x0384,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xf8ad,0x056b,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x7d + }, + { + {0x6000,0xf6eb,0xfd12,0xef98,0x0d7a,0xfc57,0x0384,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xf8ad,0x03f4,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0xf6eb,0xfd12,0xef98,0x0d7a,0xfc57,0x0384,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xf8ad,0x06e2,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0xf6eb,0xfd12,0xef98,0x0d7a,0xfc57,0x0384,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xf8ad,0x03f4,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0xf6eb,0xfd12,0xef98,0x0d7a,0xfc57,0x0384,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xf8ad,0x06e2,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + } + } + }, + // page 1e (talk2) + { + { + {0x74,0x61,0x6c,0x6b,0x32,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}, + 0x00, + 0x01, + 0x0a, + {0x00,0x00,0x00}, + 0x07, + 0x00, + 0x20, + 0x00, + 0x20, + 0x2e, + 0x00, + {0x00,0x00,0x00,0x00}, + 0xe8, + {0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00}, + 0x00 + }, + { + { + {0x6000,0x0ad7,0xfa4a,0xf4de,0x0859,0xff1f,0x064c,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xf5bf,0x0859,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x7d + }, + { + {0x6000,0x0ad7,0xfa4a,0xf4de,0x0859,0xff1f,0x064c,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xf5bf,0x04b0,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0x0ad7,0xfa4a,0xf4de,0x0859,0xff1f,0x064c,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xf5bf,0x0859,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0x0ad7,0xfa4a,0xf4de,0x0859,0xff1f,0x064c,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xf5bf,0x04b0,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0x0ad7,0xfa4a,0xf4de,0x0859,0xff1f,0x064c,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xf5bf,0x0859,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0x0ad7,0xfa4a,0xf4de,0x0859,0xff1f,0x064c,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xf5bf,0x04b0,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0x0ad7,0xfa4a,0xf4de,0x0859,0xff1f,0x064c,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xf5bf,0x0859,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + } + } + }, + // page 1f (talk2) + { + { + {0x74,0x61,0x6c,0x6b,0x32,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}, + 0x00, + 0x01, + 0x0a, + {0x00,0x00,0x00}, + 0x05, + 0x00, + 0x20, + 0x00, + 0x20, + 0x2f, + 0x00, + {0x00,0x00,0x00,0x00}, + 0xe6, + {0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00}, + 0x00 + }, + { + { + {0x6000,0x0313,0x041a,0xf2f7,0x0d2f,0xedd6,0x0c28,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xf8ad,0x056b,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x7d + }, + { + {0x6000,0x0313,0x041a,0xf2f7,0x0d2f,0xedd6,0x0c28,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xf8ad,0x03f4,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0x0313,0x041a,0xf2f7,0x0d2f,0xedd6,0x0c28,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xf8ad,0x06e2,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0x0313,0x041a,0xf2f7,0x0d2f,0xedd6,0x0c28,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xf8ad,0x03f4,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0x0313,0x041a,0xf2f7,0x0d2f,0xedd6,0x0c28,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xf8ad,0x06e2,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + } + } + }, + // page 20 (talk2) + { + { + {0x74,0x61,0x6c,0x6b,0x32,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}, + 0x00, + 0x01, + 0x0a, + {0x00,0x00,0x00}, + 0x06, + 0x00, + 0x20, + 0x00, + 0x20, + 0x00, + 0x00, + {0x00,0x00,0x00,0x00}, + 0x96, + {0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00}, + 0x00 + }, + { + { + {0x6000,0x0601,0x0e35,0xf009,0x0cbe,0xfcc7,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0753,0x056b,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x7d + }, + { + {0x6000,0x0601,0x0e35,0xf009,0x0cbe,0xfcc7,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0753,0x03f4,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0x0601,0x0e35,0xf009,0x0cbe,0xfcc7,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0753,0x06e2,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0x0601,0x0e35,0xf009,0x0cbe,0xfcc7,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0753,0x03f4,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0x0601,0x0e35,0xf009,0x0cbe,0xfcc7,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0753,0x06e2,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0xe719,0x18c1,0xf2d1,0x0d09,0x0465,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0000,0x056b,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x7d + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + } + } + }, + // page 21 (int) + { + { + {0x69,0x6e,0x74,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}, + 0x00, + 0x01, + 0x0a, + {0x00,0x00,0x00}, + 0x02, + 0x00, + 0x20, + 0x00, + 0x20, + 0x37, + 0x00, + {0x00,0x00,0x00,0x00}, + 0x38, + {0x55,0x77,0x77,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x77,0x77,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00}, + 0x00 + }, + { + { + {0x6000,0x3219,0xcbda,0xf2d1,0x0d7a,0x2085,0xdcfe,0xffb5,0x0000,0xffb5,0x0025,0x0106,0xfe3e,0xff90,0x0025,0x0106,0xfe64,0x0025,0xff45,0xf8ad,0x0c4e,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x7d + }, + { + {0x6000,0x4210,0xbc79,0xf2f7,0x0dc5,0xe49c,0x19ed,0x0000,0x0000,0x0000,0x0000,0x0177,0xfe89,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xf8ad,0x0cbe,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x7d + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + } + } + }, + // page 22 (int) + { + { + {0x69,0x6e,0x74,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}, + 0x00, + 0x01, + 0x0a, + {0x00,0x00,0x00}, + 0x06, + 0x00, + 0x10, + 0x00, + 0x20, + 0x38, + 0x00, + {0x00,0x00,0x00,0x00}, + 0xaf, + {0x55,0x77,0x77,0x77,0x77,0x77,0x77,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x77,0x77,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00}, + 0x00 + }, + { + { + {0x6000,0x4210,0xbc79,0xe1ae,0x1c90,0xe49c,0x19ed,0x0000,0x0000,0x0000,0x0000,0x0177,0xfe89,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xf8ad,0x0cbe,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0x4210,0xbc79,0xf83d,0x0591,0xe49c,0x19ed,0x0000,0x0000,0x0000,0x0000,0x0177,0xfe89,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xfa24,0x0cbe,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0x4210,0xbc79,0xe1ae,0x1c90,0xe49c,0x19ed,0x0000,0x0000,0x0000,0x0000,0x0177,0xfe89,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xfb9b,0x0cbe,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0x4210,0xbc79,0xf83d,0x0591,0xe49c,0x19ed,0x0000,0x0000,0x0000,0x0000,0x0177,0xfe89,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xfd12,0x0cbe,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0x4210,0xbc79,0xe1ae,0x1c90,0xe49c,0x19ed,0x0000,0x0000,0x0000,0x0000,0x0177,0xfe89,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xfe89,0x0cbe,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0x4210,0xbc79,0xf83d,0x0591,0xe49c,0x19ed,0x0000,0x0000,0x0000,0x0000,0x0177,0xfe89,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0000,0x0cbe,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + } + } + }, + // page 23 (int) + { + { + {0x69,0x6e,0x74,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}, + 0x00, + 0x01, + 0x0a, + {0x00,0x00,0x00}, + 0x06, + 0x00, + 0x10, + 0x00, + 0x20, + 0x3a, + 0x00, + {0x00,0x00,0x00,0x00}, + 0xc2, + {0x55,0x66,0x66,0x66,0x66,0x66,0x66,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x77,0x77,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00}, + 0x00 + }, + { + { + {0x6000,0x4210,0xbc79,0xe188,0x1d26,0xe49c,0x19ed,0x0000,0x0000,0x0000,0x0000,0x0177,0xfe89,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0177,0x0cbe,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0x4210,0xbc79,0xf83d,0x0591,0xe49c,0x19ed,0x0000,0x0000,0x0000,0x0000,0x0177,0xfe89,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x02ee,0x0cbe,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0x4210,0xbc79,0xe188,0x1d26,0xe49c,0x19ed,0x0000,0x0000,0x0000,0x0000,0x0177,0xfe89,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0465,0x0cbe,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0x4210,0xbc79,0xf83d,0x0591,0xe49c,0x19ed,0x0000,0x0000,0x0000,0x0000,0x0177,0xfe89,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x05dc,0x0cbe,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0x4210,0xbc79,0xe188,0x1d26,0xe49c,0x19ed,0x0000,0x0000,0x0000,0x0000,0x0177,0xfe89,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0753,0x0cbe,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0x4210,0xbc79,0xf83d,0x0591,0xe49c,0x19ed,0x0000,0x0000,0x0000,0x0000,0x0177,0xfe89,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x05dc,0x0cbe,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + } + } + }, + // page 24 (int) + { + { + {0x69,0x6e,0x74,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}, + 0x00, + 0x01, + 0x0a, + {0x00,0x00,0x00}, + 0x06, + 0x00, + 0x10, + 0x00, + 0x20, + 0x3a, + 0x00, + {0x00,0x00,0x00,0x00}, + 0x04, + {0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x66,0x66,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00}, + 0x00 + }, + { + { + {0x6000,0x4210,0xbc79,0xe188,0x1c6b,0xe49c,0x19ed,0x0000,0x0000,0x0000,0x0000,0x0177,0xfe89,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0465,0x0cbe,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0x4210,0xbc79,0xf83d,0x0591,0xe49c,0x19ed,0x0000,0x0000,0x0000,0x0000,0x0177,0xfe89,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x02ee,0x0cbe,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0x4210,0xbc79,0xe188,0x1c6b,0xe49c,0x19ed,0x0000,0x0000,0x0000,0x0000,0x0177,0xfe89,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0177,0x0cbe,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0x4210,0xbc79,0xf83d,0x0591,0xe49c,0x19ed,0x0000,0x0000,0x0000,0x0000,0x0177,0xfe89,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0000,0x0cbe,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0x4210,0xbc79,0xe188,0x1c6b,0xe49c,0x19ed,0x0000,0x0000,0x0000,0x0000,0x0177,0xfe89,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xfe89,0x0cbe,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0x4210,0xbc79,0xf83d,0x0591,0xe49c,0x19ed,0x0000,0x0000,0x0000,0x0000,0x0177,0xfe89,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xfd12,0x0cbe,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + } + } + }, + // page 25 (int) + { + { + {0x69,0x6e,0x74,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}, + 0x00, + 0x01, + 0x0a, + {0x00,0x00,0x00}, + 0x01, + 0x00, + 0x20, + 0x00, + 0x20, + 0x00, + 0x00, + {0x00,0x00,0x00,0x00}, + 0x06, + {0x55,0x77,0x77,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00}, + 0x00 + }, + { + { + {0x6000,0xe719,0x18c1,0xf2d1,0x0d09,0x0465,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0000,0x056b,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0xba + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + } + } + }, + // page 26 (rPASS) + { + { + {0x72,0x50,0x41,0x53,0x53,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}, + 0x00, + 0x01, + 0x0a, + {0x00,0x00,0x00}, + 0x07, + 0x00, + 0x20, + 0x00, + 0x20, + 0x00, + 0x00, + {0x00,0x00,0x00,0x00}, + 0x49, + {0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x44,0x44,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00}, + 0x00 + }, + { + { + {0x6000,0xdc8d,0x1068,0xf6c6,0x0a8c,0x0025,0xf15a,0x0025,0x0025,0x0070,0xffdb,0xe971,0x16da,0x1c20,0xe42b,0x0e10,0xf23b,0x0ab1,0x043f,0x0025,0x1437,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x18 + }, + { + {0x6000,0xd828,0x1068,0xf9b4,0x0a8c,0x0025,0xf15a,0x0ecb,0x0025,0x035e,0xfe64,0xe50c,0x19c8,0x2ac6,0xe42b,0x1563,0xf23b,0x07c3,0x043f,0x0025,0x1437,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x09 + }, + { + {0x6000,0xd99f,0x079e,0xf54f,0x0a8c,0x0025,0xe719,0x11b9,0x0025,0xfefa,0xfe64,0xe0a7,0x1563,0x0ef1,0xe42b,0xf6a0,0xf2f7,0x04d5,0x043f,0x0025,0x1437,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x09 + }, + { + {0x6000,0xd24c,0x079e,0xf54f,0x0a8c,0x0025,0xe719,0x0d54,0x0025,0xed66,0xfe64,0xe0a7,0x1563,0x0ef1,0xe42b,0xf6a0,0xf2f7,0x04d5,0x043f,0x0025,0x1437,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x0b, + 0x09 + }, + { + {0x6000,0xd99f,0x079e,0xf54f,0x0a8c,0x0025,0xe719,0x11b9,0x0025,0xfefa,0xfe64,0xe0a7,0x1563,0x0ef1,0xe42b,0xf6a0,0xf2f7,0x04d5,0x043f,0x0025,0x1437,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x09 + }, + { + {0x6000,0xe845,0x14cd,0xf6c6,0x0a8c,0x0ecb,0xf15a,0x0025,0x0025,0x01e7,0xfe64,0xe21e,0x16da,0x2ac6,0xe42b,0x1563,0xf23b,0x04d5,0x0384,0x0025,0x1437,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x18 + }, + { + {0x6000,0xe7fa,0x14cd,0xf6c6,0x0a8c,0x0ecb,0xf15a,0x0025,0x0025,0x0070,0xffdb,0xe971,0x16da,0x1c20,0xe42b,0x0e10,0xf23b,0x0070,0xffdb,0x0025,0x1437,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x09 + } + } + }, + // page 27 (lPASS) + { + { + {0x6c,0x50,0x41,0x53,0x53,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}, + 0x00, + 0x01, + 0x0a, + {0x00,0x00,0x00}, + 0x07, + 0x00, + 0x20, + 0x00, + 0x20, + 0x00, + 0x00, + {0x00,0x00,0x00,0x00}, + 0x3c, + {0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x44,0x44,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00}, + 0x00 + }, + { + { + {0x6000,0xef73,0x234d,0xf54f,0x0915,0x0e80,0xffb5,0xffb5,0xffb5,0x0000,0xff6a,0xe901,0x1669,0x1baf,0xe3bb,0x0d9f,0xf1cb,0xfb9b,0xf529,0x0025,0x1437,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x18 + }, + { + {0x6000,0xef73,0x27b2,0xf54f,0x0627,0x0e80,0xffb5,0xffb5,0xf10f,0x0177,0xfc7c,0xe613,0x1ace,0x1baf,0xd515,0x0d9f,0xea78,0xfb9b,0xf817,0x0025,0x1437,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x09 + }, + { + {0x6000,0xf83d,0x263b,0xf54f,0x0a8c,0x18c1,0xffb5,0xffb5,0xee21,0x0177,0x00e1,0xea78,0x1f33,0x1baf,0xf0ea,0x0ce4,0x093a,0xfb9b,0xfb05,0x0025,0x1437,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x09 + }, + { + {0x6000,0xf83d,0x2d8e,0xf54f,0x0a8c,0x18c1,0xffb5,0xffb5,0xf286,0x0177,0x1275,0xea78,0x1f33,0x1baf,0xf0ea,0x0ce4,0x093a,0xfb9b,0xfb05,0x0025,0x1437,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x0b, + 0x09 + }, + { + {0x6000,0xf83d,0x263b,0xf54f,0x0a8c,0x18c1,0xffb5,0xffb5,0xee21,0x0177,0x00e1,0xea78,0x1f33,0x1baf,0xf0ea,0x0ce4,0x093a,0xfb9b,0xfb05,0x0025,0x1437,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x09 + }, + { + {0x6000,0xeb0e,0x1795,0xf54f,0x0915,0x0e80,0xf10f,0xffb5,0xffb5,0x0177,0xfdf3,0xe901,0x1dbc,0x1baf,0xd515,0x0d9f,0xea78,0xfc57,0xfb05,0x0025,0x1437,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x18 + }, + { + {0x6000,0xe7fa,0x14cd,0xf6c6,0x0a8c,0x0ecb,0xf15a,0x0025,0x0025,0x0070,0xffdb,0xe971,0x16da,0x1c20,0xe42b,0x0e10,0xf23b,0x0070,0xffdb,0x0025,0x1437,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x09 + } + } + }, + // page 28 (lie_down) + { + { + {0x6c,0x69,0x65,0x20,0x64,0x6f,0x77,0x6e,0x00,0x00,0x00,0x00,0x00,0x00}, + 0x00, + 0x01, + 0x0a, + {0x00,0x00,0x00}, + 0x04, + 0x00, + 0x20, + 0x00, + 0x20, + 0x00, + 0x00, + {0x00,0x00,0x00,0x00}, + 0x63, + {0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00}, + 0x00 + }, + { + { + {0x6000,0x15f9,0xeba4,0xea78,0x13ec,0xdb3c,0x1e07,0x0025,0x0025,0x0070,0xffdb,0xd24c,0x2dd9,0x3f93,0xbe86,0x263b,0xd909,0x0070,0xffdb,0x0025,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x7d + }, + { + {0x6000,0x0ff7,0xedd6,0xea78,0x1411,0x20aa,0xdd23,0x0025,0x0025,0x0070,0xffdb,0xeb7e,0x12c0,0x0ad7,0xf59a,0x2661,0xd899,0x0070,0xffdb,0x0025,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x7d + }, + { + {0x6000,0xe50c,0x1932,0xea78,0x1411,0x20aa,0xdd23,0x0025,0x0025,0x0070,0xffdb,0xeb7e,0x12c0,0x0ad7,0xf59a,0x2661,0xd899,0x0070,0xffdb,0x0025,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x7d + }, + { + {0x6000,0xe7fa,0x14cd,0xf6c6,0x0a8c,0x0ecb,0xf15a,0x0025,0x0025,0x0070,0xffdb,0xe971,0x16da,0x1c20,0xe42b,0x0e10,0xf23b,0x0070,0xffdb,0x0025,0x1437,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x7d + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + } + } + }, + // page 29 (lie_up) + { + { + {0x6c,0x69,0x65,0x20,0x75,0x70,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}, + 0x00, + 0x01, + 0x0a, + {0x00,0x00,0x00}, + 0x03, + 0x00, + 0x20, + 0x00, + 0x20, + 0x00, + 0x00, + {0x00,0x00,0x00,0x00}, + 0xd8, + {0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00}, + 0x00 + }, + { + { + {0x6000,0xd3e9,0x25a5,0xecf5,0x0f3c,0xdbd2,0x2848,0xffdb,0xff6a,0x0000,0x0000,0x0fac,0xef4d,0x3219,0xcc70,0x2742,0xd7b8,0x00bb,0xff90,0x0000,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x7d + }, + { + {0x6000,0xb677,0x4963,0xe997,0x0ef1,0x0960,0xf2d1,0xffdb,0xff6a,0x0000,0x0000,0x0f16,0xf054,0x101d,0xedfc,0xee6c,0x0fac,0x00bb,0xff90,0x0000,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x7d + }, + { + {0x6000,0xe7fa,0x14cd,0xf6c6,0x0a8c,0x0ecb,0xf15a,0x0025,0x0025,0x0070,0xffdb,0xe971,0x16da,0x1c20,0xe42b,0x0e10,0xf23b,0x0070,0xffdb,0x0025,0x1437,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x7d + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + } + } + }, + // page 2a (sit_down) + { + { + {0x73,0x69,0x74,0x20,0x64,0x6f,0x77,0x6e,0x00,0x00,0x00,0x00,0x00,0x00}, + 0x00, + 0x06, + 0x0a, + {0x00,0x00,0x00}, + 0x02, + 0x00, + 0x0c, + 0x00, + 0x20, + 0x00, + 0x00, + {0x00,0x00,0x00,0x00}, + 0xda, + {0x55,0x55,0x55,0x77,0x77,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00}, + 0x00 + }, + { + { + {0x6000,0xe7fa,0x14a7,0xf6a0,0x0a66,0x0ea6,0xf135,0xffdb,0xff6a,0x0070,0xffdb,0xe57d,0x19a2,0x2904,0xd68c,0x15d3,0xe9bc,0x0151,0xff90,0x0025,0x0591,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0xe890,0x13a1,0xf6c6,0x08ca,0x0e5b,0xf135,0xffb5,0x0025,0x0000,0x0000,0xec14,0x13ec,0x1644,0xe9e2,0x093a,0xf54f,0xffb5,0xff45,0x0000,0x093a,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + } + } + }, + // page 2b (sit_down) + { + { + {0x73,0x69,0x74,0x20,0x64,0x6f,0x77,0x6e,0x00,0x00,0x00,0x00,0x00,0x00}, + 0x00, + 0x04, + 0x0a, + {0x00,0x00,0x00}, + 0x02, + 0x00, + 0x0c, + 0x00, + 0x20, + 0xf0, + 0x00, + {0x00,0x00,0x00,0x00}, + 0xeb, + {0x55,0x55,0x55,0x77,0x77,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00}, + 0x00 + }, + { + { + {0x6000,0xe7fa,0x14a7,0xf6a0,0x0a66,0x0ea6,0xf135,0xffdb,0xff6a,0x0070,0xffdb,0xe57d,0x19a2,0x2328,0xdc68,0x116e,0xee21,0x0151,0xff90,0x0025,0x0591,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x6000,0xe890,0x13a1,0xf6c6,0x08ca,0x0e5b,0xf135,0xffb5,0x0025,0x0000,0x0000,0xec14,0x13ec,0x1644,0xe9e2,0x093a,0xf54f,0xffb5,0xff45,0x0000,0x093a,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x31 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + } + } + }, + // page 2c (sit_down) + { + { + {0x73,0x69,0x74,0x20,0x64,0x6f,0x77,0x6e,0x00,0x00,0x00,0x00,0x00,0x00}, + 0x00, + 0x04, + 0x0a, + {0x00,0x00,0x00}, + 0x02, + 0x00, + 0x0c, + 0x00, + 0x20, + 0xf1, + 0x00, + {0x00,0x00,0x00,0x00}, + 0x04, + {0x55,0x55,0x55,0x77,0x77,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00}, + 0x00 + }, + { + { + {0x6000,0xe7fa,0x14a7,0xf6a0,0x0a66,0x0ea6,0xf135,0xffdb,0xff6a,0x0070,0xffdb,0xe57d,0x19a2,0x2328,0xdc68,0x116e,0xee21,0x0151,0xff90,0x0025,0x0591,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x24 + }, + { + {0x6000,0xe890,0x13a1,0xf6c6,0x08ca,0x0e5b,0xf135,0xffb5,0x0025,0x0000,0x0000,0xec14,0x13ec,0x1644,0xe9e2,0x093a,0xf54f,0xffb5,0xff45,0x0000,0x093a,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x24 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + } + } + }, + // page 2d (sit_down) + { + { + {0x73,0x69,0x74,0x20,0x64,0x6f,0x77,0x6e,0x00,0x00,0x00,0x00,0x00,0x00}, + 0x00, + 0x14, + 0x0a, + {0x00,0x00,0x00}, + 0x02, + 0x00, + 0x0c, + 0x00, + 0x20, + 0x00, + 0x00, + {0x00,0x00,0x00,0x00}, + 0xfd, + {0x55,0x55,0x55,0x77,0x77,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00}, + 0x00 + }, + { + { + {0x6000,0xe7fa,0x14a7,0xf6a0,0x0a66,0x0ea6,0xf135,0xffdb,0xff6a,0x0070,0xffdb,0xe57d,0x19a2,0x2328,0xdc68,0x116e,0xee21,0x0151,0xff90,0x0025,0x0591,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x18 + }, + { + {0x6000,0xe890,0x13a1,0xf6c6,0x08ca,0x0e5b,0xf135,0xffb5,0x0025,0x0000,0x0000,0xec14,0x13ec,0x1644,0xe9e2,0x093a,0xf54f,0xffb5,0xff45,0x0000,0x093a,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500}, + 0x00, + 0x18 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + }, + { + {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000}, + 0x00, + 0x00 + } + } + }, +}; diff --git a/f1/usart/include/usart1.h b/f1/usart/include/usart1.h index 67621a03121af056ba1d0002e2237983b2752878..58e6d28b584d80469d404b0968dd5cda92901810 100644 --- a/f1/usart/include/usart1.h +++ b/f1/usart/include/usart1.h @@ -9,6 +9,7 @@ void usart1_init(TComm *comm_dev,UART_InitTypeDef *conf,TUSART_IRQ_Priorities *priorities); void usart1_config(TComm *comm_dev,UART_InitTypeDef *conf); void usart1_set_priorities(TComm *comm_dev,TUSART_IRQ_Priorities *priorities); +void usart1_set_baudrate(TComm *comm_dev,unsigned int baudrate); /* IRQ functions */ unsigned char usart1_send_irq(unsigned char first_byte); unsigned char usart1_enable_tx_irq(void); diff --git a/f1/usart/include/usart2.h b/f1/usart/include/usart2.h index 9889d0be2c0416414e568e19b458fcd1e560d187..21891f0f7f9d9f6fc388ad877d6dd588e2aed613 100644 --- a/f1/usart/include/usart2.h +++ b/f1/usart/include/usart2.h @@ -9,6 +9,7 @@ void usart2_init(TComm *comm_dev,UART_InitTypeDef *conf,TUSART_IRQ_Priorities *priorities); void usart2_config(TComm *comm_dev,UART_InitTypeDef *conf); void usart2_set_priorities(TComm *comm_dev,TUSART_IRQ_Priorities *priorities); +void usart2_set_baudrate(TComm *comm_dev,unsigned int baudrate); /* IRQ functions */ unsigned char usart2_send_irq(unsigned char first_byte); unsigned char usart2_enable_tx_irq(void); diff --git a/f1/usart/include/usart3.h b/f1/usart/include/usart3.h index fbc0c3b7063419b2d12b0bf128e6948e8af97300..23e8ac4501d31fadebe3b16abf2792353c4a85f7 100644 --- a/f1/usart/include/usart3.h +++ b/f1/usart/include/usart3.h @@ -9,6 +9,7 @@ void usart3_init(TComm *comm_dev,UART_InitTypeDef *conf,TUSART_IRQ_Priorities *priorities); void usart3_config(TComm *comm_dev,UART_InitTypeDef *conf); void usart3_set_priorities(TComm *comm_dev,TUSART_IRQ_Priorities *priorities); +void usart3_set_baudrate(TComm *comm_dev,unsigned int baudrate); /* IRQ functions */ unsigned char usart3_send_irq(unsigned char first_byte); unsigned char usart3_enable_tx_irq(void); diff --git a/f1/usart/src/usart1.c b/f1/usart/src/usart1.c index 2284366a2ade504e68fc779b1d2b2e2a3ea33786..7a087719a9ac2e54cb0fee8e483b14636d1c4476 100644 --- a/f1/usart/src/usart1.c +++ b/f1/usart/src/usart1.c @@ -273,6 +273,12 @@ void usart1_set_priorities(TComm *comm_dev,TUSART_IRQ_Priorities *priorities) } } +void usart1_set_baudrate(TComm *comm_dev,unsigned int baudrate) +{ + Uart1Handle.Init.BaudRate = baudrate; + HAL_UART_Init(&Uart1Handle); +} + /* IRQ functions */ unsigned char usart1_send_irq(unsigned char first_byte) { diff --git a/f1/usart/src/usart2.c b/f1/usart/src/usart2.c index 3783a81ea4d0a697c66698fc5f35df3fde58d162..48066858e1c57ff47d055d899fc629d8c4569042 100644 --- a/f1/usart/src/usart2.c +++ b/f1/usart/src/usart2.c @@ -273,6 +273,12 @@ void usart2_set_priorities(TComm *comm_dev,TUSART_IRQ_Priorities *priorities) } } +void usart2_set_baudrate(TComm *comm_dev,unsigned int baudrate) +{ + Uart2Handle.Init.BaudRate = baudrate; + HAL_UART_Init(&Uart2Handle); +} + /* IRQ functions */ unsigned char usart2_send_irq(unsigned char first_byte) { diff --git a/f1/usart/src/usart3.c b/f1/usart/src/usart3.c index 322b771cafa94c8d325d7710394e0dc6ceac7761..4b6fc90a51f52df608927137a6373994a581c2d3 100644 --- a/f1/usart/src/usart3.c +++ b/f1/usart/src/usart3.c @@ -273,6 +273,12 @@ void usart3_set_priorities(TComm *comm_dev,TUSART_IRQ_Priorities *priorities) } } +void usart3_set_baudrate(TComm *comm_dev,unsigned int baudrate) +{ + Uart3Handle.Init.BaudRate = baudrate; + HAL_UART_Init(&Uart3Handle); +} + /* IRQ functions */ unsigned char usart3_send_irq(unsigned char first_byte) { diff --git a/f4/usart/include/usart1.h b/f4/usart/include/usart1.h index 4c5f41eebcd5d2205f0e88f030e2d1bdb911f815..94ad8f0787ef49a1cbf78370b6f8e3390471b559 100644 --- a/f4/usart/include/usart1.h +++ b/f4/usart/include/usart1.h @@ -9,6 +9,7 @@ void usart1_init(TComm *comm_dev,UART_InitTypeDef *conf,TUSART_IRQ_Priorities *priorities); void usart1_config(TComm *comm_dev,UART_InitTypeDef *conf); void usart1_set_priorities(TComm *comm_dev,TUSART_IRQ_Priorities *priorities); +void usart1_set_baudrate(TComm *comm_dev,unsigned int baudrate); /* IRQ functions */ unsigned char usart1_send_irq(unsigned char first_byte); unsigned char usart1_enable_tx_irq(void); diff --git a/f4/usart/include/usart2.h b/f4/usart/include/usart2.h index 65bbfaea37e42f7987a7c62ffe0f996b625a4864..162c39146373d643ab7646f4d41c5f2db8de0f83 100644 --- a/f4/usart/include/usart2.h +++ b/f4/usart/include/usart2.h @@ -9,6 +9,7 @@ void usart2_init(TComm *comm_dev,UART_InitTypeDef *conf,TUSART_IRQ_Priorities *priorities); void usart2_config(TComm *comm_dev,UART_InitTypeDef *conf); void usart2_set_priorities(TComm *comm_dev,TUSART_IRQ_Priorities *priorities); +void usart2_set_baudrate(TComm *comm_dev,unsigned int baudrate); /* IRQ functions */ unsigned char usart2_send_irq(unsigned char first_byte); unsigned char usart2_enable_tx_irq(void); diff --git a/f4/usart/include/usart3.h b/f4/usart/include/usart3.h index 7d659d93cd037e0225abdc12bc6bd103c1a8187e..4d96a995bda7b0b5c59712ea80eee3281750660d 100644 --- a/f4/usart/include/usart3.h +++ b/f4/usart/include/usart3.h @@ -9,6 +9,7 @@ void usart3_init(TComm *comm_dev,UART_InitTypeDef *conf,TUSART_IRQ_Priorities *priorities); void usart3_config(TComm *comm_dev,UART_InitTypeDef *conf); void usart3_set_priorities(TComm *comm_dev,TUSART_IRQ_Priorities *priorities); +void usart3_set_baudrate(TComm *comm_dev,unsigned int baudrate); /* IRQ functions */ unsigned char usart3_send_irq(unsigned char first_byte); unsigned char usart3_enable_tx_irq(void); diff --git a/f4/usart/include/usart4.h b/f4/usart/include/usart4.h index 78213da290f03c2c9769082e76f6625ef558b146..21370af9e17c4e1b78906f7fc36a4eb63a3f16c4 100644 --- a/f4/usart/include/usart4.h +++ b/f4/usart/include/usart4.h @@ -9,6 +9,7 @@ void usart4_init(TComm *comm_dev,UART_InitTypeDef *conf,TUSART_IRQ_Priorities *priorities); void usart4_config(TComm *comm_dev,UART_InitTypeDef *conf); void usart4_set_priorities(TComm *comm_dev,TUSART_IRQ_Priorities *priorities); +void usart4_set_baudrate(TComm *comm_dev,unsigned int baudrate); /* IRQ functions */ unsigned char usart4_send_irq(unsigned char first_byte); unsigned char usart4_enable_tx_irq(void); diff --git a/f4/usart/src/usart1.c b/f4/usart/src/usart1.c index 1622ccfd6e2d6e00906f044f30bc4243f20462a4..ca3e17cf23d42c943c3998fe9b15a4877338a20a 100644 --- a/f4/usart/src/usart1.c +++ b/f4/usart/src/usart1.c @@ -277,6 +277,12 @@ void usart1_set_priorities(TComm *comm_dev,TUSART_IRQ_Priorities *priorities) } } +void usart1_set_baudrate(TComm *comm_dev,unsigned int baudrate) +{ + Uart1Handle.Init.BaudRate = baudrate; + HAL_UART_Init(&Uart1Handle); +} + /* IRQ functions */ unsigned char usart1_send_irq(unsigned char first_byte) { diff --git a/f4/usart/src/usart2.c b/f4/usart/src/usart2.c index ee1cc5562244f5717523f9479216bb3873d0f5a5..44190b812114be10d0d8540a5cee57a16a27007c 100644 --- a/f4/usart/src/usart2.c +++ b/f4/usart/src/usart2.c @@ -277,6 +277,12 @@ void usart2_set_priorities(TComm *comm_dev,TUSART_IRQ_Priorities *priorities) } } +void usart2_set_baudrate(TComm *comm_dev,unsigned int baudrate) +{ + Uart2Handle.Init.BaudRate = baudrate; + HAL_UART_Init(&Uart2Handle); +} + /* IRQ functions */ unsigned char usart2_send_irq(unsigned char first_byte) { diff --git a/f4/usart/src/usart3.c b/f4/usart/src/usart3.c index 921746cd4ead2ee457dbf592cf21524119d51954..5e5a6e38ca1a659005605b07f0465b5b055f70cb 100644 --- a/f4/usart/src/usart3.c +++ b/f4/usart/src/usart3.c @@ -277,6 +277,12 @@ void usart3_set_priorities(TComm *comm_dev,TUSART_IRQ_Priorities *priorities) } } +void usart3_set_baudrate(TComm *comm_dev,unsigned int baudrate) +{ + Uart3Handle.Init.BaudRate = baudrate; + HAL_UART_Init(&Uart3Handle); +} + /* IRQ functions */ unsigned char usart3_send_irq(unsigned char first_byte) { diff --git a/f4/usart/src/usart4.c b/f4/usart/src/usart4.c index 301468416126104a99bf0ee869b01d5824fd4050..12413be95528c422f84d7de56fee0f3852b3b90a 100644 --- a/f4/usart/src/usart4.c +++ b/f4/usart/src/usart4.c @@ -1,9 +1,9 @@ #include "usart4.h" -#define USART USART4 -#define USART_ENABLE_CLK __HAL_RCC_USART4_CLK_ENABLE() -#define USART_IRQn USART4_IRQn -#define USART_IRQHandler USART4_IRQHandler +#define USART UART4 +#define USART_ENABLE_CLK __HAL_RCC_UART4_CLK_ENABLE() +#define USART_IRQn UART4_IRQn +#define USART_IRQHandler UART4_IRQHandler #define USART_TX_PIN GPIO_PIN_10 #define USART_TX_GPIO_PORT GPIOC @@ -182,8 +182,8 @@ void usart4_init(TComm *comm_dev,UART_InitTypeDef *conf,TUSART_IRQ_Priorities *p GPIO_InitStructure.Pin = USART_TX_PIN; GPIO_InitStructure.Mode = GPIO_MODE_AF_PP; GPIO_InitStructure.Pull = GPIO_PULLUP; - GPIO_InitStructure.Speed = GPIO_SPEED_LOW; - GPIO_InitStructure.Alternate = GPIO_AF7_USART4; + GPIO_InitStructure.Speed = GPIO_SPEED_FREQ_VERY_HIGH; + GPIO_InitStructure.Alternate = GPIO_AF8_UART4; HAL_GPIO_Init(USART_TX_GPIO_PORT, &GPIO_InitStructure); GPIO_InitStructure.Pin = USART_RX_PIN; @@ -277,6 +277,12 @@ void usart4_set_priorities(TComm *comm_dev,TUSART_IRQ_Priorities *priorities) } } +void usart4_set_baudrate(TComm *comm_dev,unsigned int baudrate) +{ + Uart4Handle.Init.BaudRate = baudrate; + HAL_UART_Init(&Uart4Handle); +} + /* IRQ functions */ unsigned char usart4_send_irq(unsigned char first_byte) { diff --git a/f4/usb/src/usb.c b/f4/usb/src/usb.c index c9bec4f4e75b031efcff2de12eb64a0482220403..1821df26a6896333ca80f2f11ad747f85157dd6b 100644 --- a/f4/usb/src/usb.c +++ b/f4/usb/src/usb.c @@ -5,7 +5,7 @@ #define TX_DATA_SIZE 1024 // private variables -extern PCD_HandleTypeDef PCDHandle; +extern PCD_HandleTypeDef hpcd_USB_OTG_FS; USBD_HandleTypeDef USBHandle; TComm *usb_comm_dev; uint8_t usb_rx_buffer[RX_DATA_SIZE]; @@ -178,7 +178,7 @@ void OTG_FS_IRQHandler(void) /* USER CODE BEGIN USB_LP_CAN1_RX0_IRQn 0 */ /* USER CODE END USB_LP_CAN1_RX0_IRQn 0 */ - HAL_PCD_IRQHandler(&PCDHandle); + HAL_PCD_IRQHandler(&hpcd_USB_OTG_FS); /* USER CODE BEGIN USB_LP_CAN1_RX0_IRQn 1 */ /* USER CODE END USB_LP_CAN1_RX0_IRQn 1 */ @@ -215,12 +215,12 @@ void usb_init(TComm *comm_dev,USBD_DescriptorsTypeDef *pdesc,USBD_ClassTypeDef * void usb_disconnect(void) { - HAL_PCD_DevDisconnect(&PCDHandle); + HAL_PCD_DevDisconnect(&hpcd_USB_OTG_FS); } void usb_connect(void) { - HAL_PCD_DevConnect(&PCDHandle); + HAL_PCD_DevConnect(&hpcd_USB_OTG_FS); } /* IRQ functions */ diff --git a/memory/Makefile b/memory/Makefile index 5acf059ad88e32dd3f13880565cd1dfa31fed021..8c10981525a8f64771fb427596b3e8be3729e841 100755 --- a/memory/Makefile +++ b/memory/Makefile @@ -2,8 +2,8 @@ # modified by zerom for WinARM 8/2010 COMPILE_OPTS = -mlittle-endian -mthumb -mthumb-interwork -COMPILE_OPTS += -Wall -O2 -fno-common -#COMPILE_OPTS += -Wall -g -fno-common +#COMPILE_OPTS += -Wall -O2 -fno-common +COMPILE_OPTS += -Wall -g -fno-common COMPILE_OPTS += -ffreestanding -nostdlib COMPILE_OPTS_M4_FPU = -mfloat-abi=hard -mfpu=fpv4-sp-d16 -mcpu=cortex-m4 diff --git a/memory/include/mem_module.h b/memory/include/mem_module.h index 50c085ea6a9a2ebf4c3a074079301f49705e4784..ae07cc71aaddda066c603f50d964257633ce7a82 100644 --- a/memory/include/mem_module.h +++ b/memory/include/mem_module.h @@ -15,8 +15,9 @@ typedef struct TMemSegment ram_segments[MAX_NUM_SEGMENTS]; unsigned char num_eeprom_segments; TMemSegment eeprom_segments[MAX_NUM_SEGMENTS]; - void (*write_cmd)(unsigned short int address,unsigned short int length,unsigned char *data); - void (*read_cmd)(unsigned short int address,unsigned short int length,unsigned char *data); + void (*write_cmd)(void *module,unsigned short int address,unsigned short int length,unsigned char *data); + void (*read_cmd)(void *module,unsigned short int address,unsigned short int length,unsigned char *data); + void *data; }TMemModule; void mem_module_init(TMemModule *module); diff --git a/memory/include/memory.h b/memory/include/memory.h index 21214c5b272a3b2f3beb7dcca18f8939bb361db6..1829fee81dbbb353df3cc2791584e24bde51e00a 100644 --- a/memory/include/memory.h +++ b/memory/include/memory.h @@ -4,8 +4,12 @@ #include "mem_module.h" #define MAX_NUM_MEM_MODULES 16 -#define EEPROM_SIZE 32 -#define RAM_SIZE 256 +#ifndef EEPROM_SIZE + #define EEPROM_SIZE 32 +#endif +#ifndef RAM_SIZE + #define RAM_SIZE 256 +#endif typedef struct { diff --git a/memory/include/ram.h b/memory/include/ram.h new file mode 100644 index 0000000000000000000000000000000000000000..ecc663695a1d6fa81089cbc8ee53c41889b9e683 --- /dev/null +++ b/memory/include/ram.h @@ -0,0 +1,34 @@ +#ifndef _RAM_H +#define _RAM_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include "memory.h" + +#define RAM_SUCCESS 0 +#define RAM_BAD_ADDRESS -1 +#define RAM_WRITE_ERROR -2 +#define RAM_BAD_BIT -3 +#define RAM_BAD_ACCESS -4 + +extern unsigned char ram_data[RAM_SIZE]; + +unsigned char ram_init(TMemory *memory); +inline void ram_read_byte(unsigned short int address, unsigned char *data); +inline void ram_read_word(unsigned short int address, unsigned short int *data); +unsigned char ram_read_table(unsigned short int address, unsigned short int length,unsigned char *data); +unsigned char ram_set_bit(unsigned short int address, unsigned char bit); +unsigned char ram_clear_bit(unsigned short int address, unsigned char bit); +unsigned char ram_write_byte(unsigned short int address, unsigned char data); +unsigned char ram_write_word(unsigned short int address, unsigned short int data); +unsigned char ram_write_table(unsigned short int address, unsigned short int length,unsigned char *data); +inline unsigned char ram_in_range(unsigned short int reg,unsigned short int address,unsigned short int length); +unsigned char ram_in_window(unsigned short int start_reg,unsigned short int reg_length,unsigned short int start_address,unsigned short int address_length); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/memory/src/mem_module.c b/memory/src/mem_module.c index cf158d18dbc8dee96ea47d60c519f3e5c8f23d10..e97238840ad1c7030db65bcf3ca494fa56507c16 100644 --- a/memory/src/mem_module.c +++ b/memory/src/mem_module.c @@ -17,6 +17,7 @@ void mem_module_init(TMemModule *module) } module->write_cmd=0x00000000; module->read_cmd=0x00000000; + module->data=0x00000000; } unsigned char mem_module_add_ram_segment(TMemModule *module,unsigned short int start_address, unsigned short int length) diff --git a/memory/src/memory.c b/memory/src/memory.c index c2b2a70def0e0538ab847145e2062ed0f1e33f91..68fbc806be5d15e471023bd8665b03b89caab7fb 100644 --- a/memory/src/memory.c +++ b/memory/src/memory.c @@ -66,7 +66,7 @@ void mem_initialize_data(TMemory *memory) actual_length=EEPROM_SIZE-mem_module->eeprom_segments[j].start_address; if((actual_address+actual_length)>(mem_module->eeprom_segments[j].start_address+mem_module->eeprom_segments[j].length)) actual_length-=((actual_address+actual_length)-(mem_module->eeprom_segments[j].start_address+mem_module->eeprom_segments[j].length)); - mem_module->write_cmd(actual_address,actual_length,&eeprom_data[actual_address]); + mem_module->write_cmd(mem_module->data,actual_address,actual_length,&eeprom_data[actual_address]); } } } @@ -171,7 +171,7 @@ void mem_do_write(TMemory *memory,unsigned short int start_address,unsigned shor } if((actual_address+actual_length)>(mem_module->ram_segments[j].start_address+mem_module->ram_segments[j].length)) actual_length-=((actual_address+actual_length)-(mem_module->ram_segments[j].start_address+mem_module->ram_segments[j].length)); - mem_module->write_cmd(actual_address,actual_length,&data[actual_address-start_address]); + mem_module->write_cmd(mem_module->data,actual_address,actual_length,&data[actual_address-start_address]); } } } @@ -192,7 +192,7 @@ void mem_do_write(TMemory *memory,unsigned short int start_address,unsigned shor if((actual_address+actual_length)>(mem_module->eeprom_segments[j].start_address+mem_module->eeprom_segments[j].length)) actual_length-=((actual_address+actual_length)-(mem_module->eeprom_segments[j].start_address+mem_module->eeprom_segments[j].length)); if(mem_module->write_cmd!=0x00000000) - mem_module->write_cmd(actual_address,actual_length,&data[actual_address-start_address]); + mem_module->write_cmd(mem_module->data,actual_address,actual_length,&data[actual_address-start_address]); if(memory->eeprom_write_data!=0x00000000) for(k=actual_address;k<(actual_address+actual_length);k++) memory->eeprom_write_data(k,data[k-start_address]); @@ -228,7 +228,7 @@ void mem_do_read(TMemory *memory,unsigned short int start_address,unsigned short } if((actual_address+actual_length)>(mem_module->ram_segments[j].start_address+mem_module->ram_segments[j].length)) actual_length-=((actual_address+actual_length)-(mem_module->ram_segments[j].start_address+mem_module->ram_segments[j].length)); - mem_module->read_cmd(actual_address,actual_length,&data[actual_address-start_address]); + mem_module->read_cmd(mem_module->data,actual_address,actual_length,&data[actual_address-start_address]); } } } @@ -250,7 +250,7 @@ void mem_do_read(TMemory *memory,unsigned short int start_address,unsigned short } if((actual_address+actual_length)>(mem_module->eeprom_segments[j].start_address+mem_module->eeprom_segments[j].length)) actual_length-=((actual_address+actual_length)-(mem_module->eeprom_segments[j].start_address+mem_module->eeprom_segments[j].length)); - mem_module->read_cmd(actual_address,actual_length,&data[actual_address-start_address]); + mem_module->read_cmd(mem_module->data,actual_address,actual_length,&data[actual_address-start_address]); } } } diff --git a/memory/src/ram.c b/memory/src/ram.c new file mode 100644 index 0000000000000000000000000000000000000000..39f442a6abee2795534bc20e2d95c5225c6ad8fe --- /dev/null +++ b/memory/src/ram.c @@ -0,0 +1,110 @@ +#include "ram.h" + +unsigned char ram_data[RAM_SIZE]={0}; + +unsigned char ram_init(TMemory *memory) +{ + mem_initialize_data(memory); + + return 0x01; +} + +inline void ram_read_byte(unsigned short int address,unsigned char *data) +{ + (*data)=ram_data[address]; +} + +inline void ram_read_word(unsigned short int address, unsigned short int *data) +{ + (*data)=ram_data[address]; + (*data)+=ram_data[address+1]*256; +} + +unsigned char ram_read_table(unsigned short int address, unsigned short int length,unsigned char *data) +{ + unsigned short int i; + + if((address+length)<=(RAM_SIZE-1)) + { + for(i=0;i<length;i++) + data[i]=ram_data[address+i]; + return RAM_SUCCESS; + } + else + return RAM_BAD_ADDRESS; +} + +unsigned char ram_set_bit(unsigned short int address, unsigned char bit) +{ + if(bit>=0 && bit<8) + { + ram_data[address]|=(0x01<<bit); + return RAM_SUCCESS; + } + else + return RAM_BAD_BIT; +} + +unsigned char ram_clear_bit(unsigned short int address, unsigned char bit) +{ + if(bit>=0 && bit<8) + { + ram_data[address]&=(~(0x01<<bit)); + return RAM_SUCCESS; + } + else + return RAM_BAD_BIT; +} + +unsigned char ram_write_byte(unsigned short int address, unsigned char data) +{ + ram_data[address]=data; + + return RAM_SUCCESS; +} + +unsigned char ram_write_word(unsigned short int address, unsigned short int data) +{ + if(address < (RAM_SIZE-1)) + { + ram_data[address]=data%256; + ram_data[address+1]=data/256; + + return RAM_SUCCESS; + } + else + return RAM_BAD_ADDRESS; +} + +unsigned char ram_write_table(unsigned short int address, unsigned short int length,unsigned char *data) +{ + unsigned short int i; + + if((address+length)<RAM_SIZE) + { + for(i=0;i<length;i++) + ram_data[address+i]=data[i]; + return RAM_SUCCESS; + } + else + return RAM_BAD_ADDRESS; +} + +inline unsigned char ram_in_range(unsigned short int reg,unsigned short int address,unsigned short int length) +{ + if(reg>=address && reg<(address+length)) + return 0x01; + else + return 0x00; +} + +unsigned char ram_in_window(unsigned short int start_reg,unsigned short int reg_length,unsigned short int start_address,unsigned short int address_length) +{ + unsigned short int end_reg=start_reg+reg_length-1; + unsigned short int end_address=start_address+address_length-1; + + if((start_reg>=start_address && start_reg<=end_address) || (end_reg>=start_address && end_reg<=end_address)) + return 0x01; + else + return 0x00; +} diff --git a/utils/include/buffer.h b/utils/include/buffer.h index a2a5abd582a6c7fb39cd2aab1279027566f33c67..69a3274385fbd520334684dbfea4e0fe9df39d91 100644 --- a/utils/include/buffer.h +++ b/utils/include/buffer.h @@ -1,4 +1,4 @@ -/** @file */ +/** @file */ #ifndef _BUFFER_H #define _BUFFER_H @@ -151,7 +151,7 @@ unsigned char buffer_write_byte(TBuffer *buffer,unsigned char data); * behavior is unpredictable. * \param data pointer to a byte variable to store the buffer data. Memory for * this parameter must be pre-allocated before calling this function - * to avoid unexpected behaviour. + * to avoid unexpected behavior. * * \return This function returns 1 on success and 0 otherwise. */ @@ -172,11 +172,11 @@ unsigned char buffer_read_byte(TBuffer *buffer,unsigned char *data); * behavior is unpredictable. * \param data pointer to a byte vector variable with the data to be written to * the buffer. Memory for this parameter must be pre-allocated before calling - * this function to avoid unexpected behaviour. + * this function to avoid unexpected behavior. * \param length number of bytes to be written into the buffer. * * \return number of bytes actually written into the buffer. This value can be used - * to check wether the function was successfull or not. + * to check whether the function was successful or not. */ unsigned short int buffer_write(TBuffer *buffer,unsigned char *data,unsigned short int length); /** @@ -194,11 +194,11 @@ unsigned short int buffer_write(TBuffer *buffer,unsigned char *data,unsigned sho * behavior is unpredictable. * \param data pointer to a byte vector variable to store the buffer data. Memory for * this parameter must be pre-allocated before calling this function to avoid - * unexpected behaviour. + * unexpected behavior. * \param length number of bytes to be read from the buffer. * * \return number of bytes actually read from the buffer. This value can be used - * to check wether the function was successfull or not. + * to check whether the function was successful or not. */ unsigned short int buffer_read(TBuffer *buffer,unsigned char *data,unsigned short int length); diff --git a/utils/include/stm32_time.h b/utils/include/stm32_time.h index 2eed9f5908db54df8e8366294fe629d51a884477..22d7f45268c7274bda8d58ab8f1d41dfcbe700f5 100644 --- a/utils/include/stm32_time.h +++ b/utils/include/stm32_time.h @@ -1,4 +1,4 @@ -/** @file */ +/** @file */ #ifndef _STM32_TIME_H #define _STM32_TIME_H @@ -17,7 +17,7 @@ * * A time structure is first initialized by calling the time_init() function. * Using this structure, it is possible to implement delays in - * seconds (time_delay_s()), milli-seconds (time_delay_ms()) and micro-seconds + * seconds (time_delay_s()), Milli-seconds (time_delay_ms()) and micro-seconds * (time_delay_us()), and also start (time_set_timeout()) and cancel * (time_cancel_timeout()) timeout, as well as checks its status (time_is_timeout()). * @@ -55,7 +55,7 @@ typedef struct * and returns the accumulated number of timer counts since the last reset as an * unsigned long long variable (8 bytes in most architectures). * - * This pointer must be provided at initilaization time when the time_init() function + * This pointer must be provided at initialization time when the time_init() function * is called, and should not be modified afterwards. * */ @@ -83,7 +83,7 @@ void time_init(TTime *time,int counts_per_us,unsigned long long int(*get_time)(v * \brief Function to set the desired timeout in micro-seconds * * This functions sets the desired time to generate a timeout event if it is not - * canceller by the time_cancel_timeout() function. After calling this function, + * canceled by the time_cancel_timeout() function. After calling this function, * the time_is_timeout() function will return true if the current time is past * the time when this function was called plus the desired timeout value. * @@ -129,9 +129,9 @@ void time_cancel_timeout(TTime *time); /** * \brief Function to wait a given time in micro-seconds * - * This function blocks until the specifierd time in micro-seconds has elapsed. + * This function blocks until the specified time in micro-seconds has elapsed. * If this function is called when there is an active timeout, the timeout is - * automatically cancelled. + * automatically canceled. * * \param time pointer to a valid TTime structure to be initialized. If * memory is not pre-allocated before calling this function, its @@ -140,24 +140,24 @@ void time_cancel_timeout(TTime *time); */ void time_delay_us(TTime *time,int delay_us); /** - * \brief Function to wait a given time in milli-seconds + * \brief Function to wait a given time in Milli-seconds * - * This function blocks until the specifierd time in milli-seconds has elapsed. + * This function blocks until the specified time in Milli-seconds has elapsed. * If this function is called when there is an active timeout, the timeout is - * automatically cancelled. + * automatically canceled. * * \param time pointer to a valid TTime structure to be initialized. If * memory is not pre-allocated before calling this function, its * behavior is unpredictable. - * \param delay_ms value of the desired delay in milli-seconds + * \param delay_ms value of the desired delay in Milli-seconds */ void time_delay_ms(TTime *time,int delay_ms); /** * \brief Function to wait a given time in seconds * - * This function blocks until the specifierd time in seconds has elapsed. + * This function blocks until the specified time in seconds has elapsed. * If this function is called when there is an active timeout, the timeout is - * automatically cancelled. + * automatically canceled. * * \param time pointer to a valid TTime structure to be initialized. If * memory is not pre-allocated before calling this function, its diff --git a/utils/src/buffer.c b/utils/src/buffer.c index b3db1744d00265b7393040dc869319642f86bd27..8d0045b86c72a21e3e232ca0845338553ded77d1 100644 --- a/utils/src/buffer.c +++ b/utils/src/buffer.c @@ -14,7 +14,7 @@ void buffer_flush(TBuffer *buffer) buffer->num_data=0; } -inline unsigned short int buffer_get_num_data(TBuffer *buffer) +unsigned short int buffer_get_num_data(TBuffer *buffer) { return buffer->num_data; }